Carole-Anne TRUDEL 26953794

Ran Huo 26436064

Steeve Tchatchouang 21072765

MECH 472

Mechatronics and Automation

Report Project

Presented to

Brandon Gordon

May 5th 2016

Concordia University
 Figure 1: CAD of the required design to achieve the competition

We initially had a design that was on “two floors” laser cut with plexiglass, but the target
could not fit at the required height to let the other team reach it. Therefore, we changed to the
design in figure 1 with minor changed we 3D printed our frame that could fit everything on one-
level.
 Figure 2: Process Map of the Robot with yellow as the starting point

After a brief summary of how’d we programed our robot let’s take a detailed look the
sequence of our programing method.

ALIGN WITH THE TARGET

First we want our robot to align with the target, our program always starts with align with
the target, we used the equation had been given in the sample programing, e = r – theta, e is the
error that we are using to turn our robot, r is the target angle and theta is the robot angle. The two
circles forms the theta angle represents the colors on the robot, and circle that is further away
represent the target color. But the problem is that it is not working for scenario 3 and 4 refers to
figure 3.
 For the scenario 3 our robot angle theta is greater than 90 degrees and less than 180 degrees,
our target angle is less than -90 degrees and greater than -180 degrees. In this case our error
equation becomes
𝑒 = (180 − 𝑡ℎ𝑒𝑡𝑎) − (−180 − 𝑟) = 360 + (𝑟 − 𝑡ℎ𝑒𝑡𝑎) = 3.14159 + (𝑟 − 𝑡ℎ𝑒𝑡𝑎) 𝑖𝑛 𝑟𝑎𝑑𝑖𝑎𝑛𝑠
For the scenario 4 our robot angle theta is less than -90 degrees and greater than -180 degrees, our
target angle is greater than 90 degrees and less than 180 degrees. In this case or error equation
becomes
𝑒 = (−180 − 𝑡ℎ𝑒𝑡𝑎) − (180 − 𝑟) = −360 + (𝑟 − 𝑡ℎ𝑒𝑡𝑎)
= −3.14159 + (𝑟 − 𝑡ℎ𝑒𝑡𝑎) 𝑖𝑛 𝑟𝑎𝑑𝑖𝑎𝑛𝑠
Apply those equation in our program our robot has been able to align with the target in all the
directions, once the align problem has been solved then we can move on to the obstacle avoiding
method.

OBSTACLE DETECTION

In order to avoid obstacle we decided as a team to check the perpendicular distance of the
line connecting the robot (Rxy) and the target (Txy) with the Radius (Oxy) of the obstacle. So we
derived the equation of the line from robot to target as below;

2
𝐿𝑖𝑛𝑒 𝑜𝑓 𝑅𝑜𝑏𝑜𝑡 𝑡𝑜 𝑇𝑎𝑟𝑔𝑒𝑡 (𝑅𝑥𝑦 , 𝑇𝑥𝑦 ) = √(𝑇𝑦 + 𝑅𝑦 ) + (𝑇𝑥 + 𝑅𝑥 )2
 From the line connecting the robot and target, the equation of the perpendicular distance
is described below;

𝑃𝑒𝑟𝑝𝑒𝑛𝑑𝑖𝑐𝑢𝑙𝑎𝑟 𝑑𝑖𝑠𝑡𝑎𝑛𝑐𝑒 𝑜𝑓 𝑂𝑏𝑠𝑡𝑎𝑐𝑙𝑒 𝑡𝑜 𝐿𝑖𝑛𝑒(𝑅𝑥𝑦 , 𝑇𝑥𝑦 , 𝑂𝑥𝑦 )

|(𝑇𝑦 + 𝑅𝑦 )𝑂𝑥 − (𝑇𝑥 + 𝑅𝑥 )𝑂𝑦 + 𝑇𝑥 𝑅𝑦 − 𝑅𝑥 𝑇𝑦 |
=
2
√(𝑇𝑦 + 𝑅𝑦 ) + (𝑇𝑥 + 𝑅𝑥 )2

If this perpendicular distance from obstacle is greater that the radius of the obstacle, then
it means that there is no obstacle between the robot and target. In other words if this
perpendicular distance is less than the radius of obstacle, then it means that there is an obstacle
between the robot and target as seen in the figures below.

Figure 4: No obstacle between the Robot and Target.

 Figure 5: Obstacle detected between the Robot and Target.

Figure 6: Line of code for obstacle detection.

If there was no obstacle between the robot and the target the robot would turn on the laser
and then move forward. The purpose of moving forward was to make sure we would get closer to
the target each time in case it changed direction we would have been closer therefore we would
have been able to adjust faster to the new position of the target. At first, we had issue to move
forward with a smooth motion, the robot was moving toward the target with saccade movement.
We programmed for 3 obstacles and for 2 targets with the same color, but with the approach
we used we could not find an “easy” way to program is.
First for the 3 obstacles, we had the perpendicular distance of the 3 obstacles from the line
linking the robot and the target. If that perpendicular distance was shorter than the radius of the
obstacle that meant there was an obstacle in the way. But then, our robot had to turn 30 degrees
and move forward which was not a good sequence for this. The geometry of 3 obstacles on the
board was way more complex and hardly suitable to the programing we had been working with.
We wanted to use the appropriate angle, but as we encountered issues doing an exact angle (which
was replaced by fixed 30 degrees) we could not get a specific angle for this purpose as well.
Second for the 2 targets, we were taking both centroid and finding the one that belonged to us by
analyzing the distance from one color of our robot. This part was working fine with the assumption
that because we were playing laser tag the other team would not try to come to close to our robot
which would have confused our target determination.

TURNING 30 DEGREES AFTER DETECTING AN OBSTICLE

Our program is designed in a way that after align with the target we use the point to line
equation to calculate the distance from the obstacle centroid to the line between the target and the
robot. If the distance is less than the obstacle radius then our program can recognize the obstacle
in between then our robot will turn 30 degrees to the left or right depending on the location of our
robot. The turning sequence is defined in the figure 4. By programing this way we will avoid the
increase in the angle value will make the robot turn over the x- axis, if it does then the positive
angle value will become a negative value, and this will give us a problem.

Once there is an obstacle in between our robot and the target, our robot can identify the
obstacle right away, sometimes it turns really well but some time it turns very slowly.

IMAGE PROCESSING AND THRESHOLD VALUE CHOICE

From the image processing, we encountered problems due to the shadow the robot
produced which was problematic since the RGB of the shadow is close to some of the desired
colors of interest. In other to resolve this problem, we had to use the Background function from
assignment with n (number of regions) between 15 to 17 and a threshold value of about 30.
In conclusion the project was a challenging one, and if more time was given to us we
would have solved most of the issues we encountered during testing.
 CODE APPENDIX

int level_one(image &rgb0, image &a, image &label, i2byte nlabel)

{

// initialize pointers
ibyte *pa, *p_rgb0;
i2byte *pl;

i4byte width, height;

//i4byte i, j;

int i = 0;
char ch;
double th, th_obstacle, e, ed, ei, dt, dx_robot, dy_robot, dy_target, dx_target,
dy_obstacle, dx_obstacle, kp, ki, kd, u; //counter;
double r, rd, ep, r_temp;
unsigned char uR, uL, uLas; // one byte value for servo angle (0-255)
unsigned int count1;
int count2;
static int count3;
double count4;
unsigned char count5;

// variables for serial communication

HANDLE h1;
const int NMAX = 64;
char buffer_in[NMAX], buffer_out[NMAX];
int n;

double t, t0, t1, t2;

// note: the serial port driver should be set to the same

// data (baud) rate in the device manager com port settings
// and the Arduino program.
open_serial("COM7", h1);

// note: open_serial will reset the Arduino board

// and start the program
// synchronize serial port communication

printf("\npress c key to continue");

while (!KEY('C')) Sleep(1);

Sleep(100);

// set actuator to neutral position

uR = 92.0;
uL = 92.0;
uLas = 65.0;

// send input to Arduino using serial communication

buffer_out[0] = uR;
buffer_out[1] = uL;
buffer_out[2] = uLas;
 buffer_out[3] = 0;//laser on/off
n = 4;
serial_send(buffer_out, n, h1);

// wait for servo to reach neutral position

Sleep(1000);

int number_of_labels;

double r_obstacle = 0;
double ic_DB = 0;
double jc_DB = 0;
double ic_R = 0;
double jc_R = 0;
double ic_PK = 0;
double jc_PK = 0;
double ic_G = 0;
double jc_G = 0;
double ic_P = 0;
double jc_P = 0;
double ic_LB = 0;
double jc_LB = 0;
double ic_DO = 0;
double jc_DO = 0;

p_rgb0 = rgb0.pdata;
pl = (i2byte *)label.pdata;
pa = a.pdata;

// number of pixels
width = rgb0.width;
height = rgb0.height;

// initialize variables
ei = 0.0; // integral of the error
dt = 0.1; // estimate of the sampling rate
ep = 0.0; // error for previous sample

// put this just before the main loop

t0 = high_resolution_time();

while (1)
{

while (1)
{

find_object(nlabel, number_of_labels);
//Robot Back
position_Dark_Blue_Object(rgb0, a, label, number_of_labels, ic_DB,
jc_DB);

//green tagets (2) G1 is my Robot, G2 is opponents

position_Green_Object(rgb0, a, label, number_of_labels, ic_G, jc_G);
//purple obdtacle (3)
position_Purple_Object(rgb0, a, label, number_of_labels, ic_P, jc_P);

position_Light_Blue_Object(rgb0, a, label, number_of_labels, ic_LB, jc_LB, r_obstacle);

//Robot Front
position_Dark_Orange_Object(rgb0, a, label, number_of_labels, ic_DO, jc_DO);

// position_Orange_Object(rgb0, a, label, number_of_labels, ic_O, jc_O);

// position_Yellow_Object(rgb0, a, label, number_of_labels, ic_Y, jc_Y);

view_image(rgb0);

dy_robot = jc_DO - jc_DB;

dx_robot = ic_DO - ic_DB;
dy_target = jc_G - jc_DB;
dx_target = ic_G - ic_DB;

t = high_resolution_time() - t0;

th = atan2(dy_robot, dx_robot);

r = atan2(dy_target, dx_target);

printf("\n1st align");

rd = 0.0;

e = th - r; // control error (rad)

if (th > 1.570796 && r < -1.570796)

{
e = -3.14159 * 2 - (th - r);
}
else if (th < -1.570796 && r > 1.570796)
{
e = 3.14159 * 2 - (th - r);
}

kp = 0.06; // proportional gain

ki = 0.0005; // integral gain
kd = 0.00; // derivative gain

ed = (e - ep) / dt;
ep = e;
ei += e*dt;

// calculate control input

// note: normalize input to range from -1 to 1
u = kp*ep + ki*ei + kd*ed;

// saturate control input

if (u > 1.0) u = 1.0;
if (u < -1.0) u = -1.0;
uR = 92 + 90 * u;
uL = 92 + 90 * u;
uLas = 65;

// send input to Arduino using serial communication

buffer_out[0] = uR;
buffer_out[1] = uL;
buffer_out[2] = uLas;
buffer_out[3] = 0;
n = 4;
serial_send(buffer_out, n, h1);

//uR >= 91.5 && uR <= 92.5 && uL >= 91.5 && uL <= 92.5
if (uR == 92 && uL == 92)
{

uR = 92.0;
uL = 92.0;
uLas = 65.0;

// send input to Arduino using serial communication

buffer_out[0] = uR;
buffer_out[1] = uL;
buffer_out[2] = uLas;
buffer_out[3] = 0;
n = 4;
serial_send(buffer_out, n, h1);

break;
}// end if (Ur==92...)

} // end while for align object

find_object(nlabel, number_of_labels);

position_Dark_Blue_Object(rgb0, a, label, number_of_labels, ic_DB, jc_DB);

//green tagets (2) G1 is my Robot, G2 is opponents

position_Green_Object(rgb0, a, label, number_of_labels, ic_G, jc_G);

//purple obdtacle (3)

position_Purple_Object(rgb0, a, label, number_of_labels, ic_P, jc_P);

position_Light_Blue_Object(rgb0, a, label, number_of_labels, ic_LB, jc_LB, r_obstacle);

position_Dark_Orange_Object(rgb0, a, label, number_of_labels, ic_DO, jc_DO);

view_image(rgb0);

dy_robot = jc_DO - jc_DB;

dx_robot = ic_DO - ic_DB;
dy_target = jc_G - jc_DB;
dx_target = ic_G - ic_DB;

double ic_G_to_DB = ic_G - ic_DB;

double jc_G_to_DB = jc_G - jc_DB;

double ic_DB_to_obstacle = ic_P - ic_DB;

double jc_DB_to_obstacle = jc_P - jc_DB;

double dist_DB_to_G = sqrt(ic_G_to_DB*ic_G_to_DB + jc_G_to_DB*jc_G_to_DB);

double dist_DB_to_obstacle = sqrt(ic_DB_to_obstacle*ic_DB_to_obstacle +

jc_DB_to_obstacle*jc_DB_to_obstacle);

double absolute = abs(jc_G_to_DB*ic_P - ic_G_to_DB*jc_P + ic_G*jc_DB - jc_G*ic_DB);

double perpendicular_dist_from_P_to_line = absolute / dist_DB_to_G;

double radius_obstacle = 32.50;// check this value all the time

if (perpendicular_dist_from_P_to_line <= radius_obstacle)

{
dy_robot = jc_DO - jc_DB;
dx_robot = ic_DO - ic_DB;

double th_initial = atan2(dy_robot, dx_robot);

while (perpendicular_dist_from_P_to_line < radius_obstacle)

{

find_object(nlabel, number_of_labels);

position_Dark_Blue_Object(rgb0, a, label, number_of_labels, ic_DB, jc_DB);

//green tagets (2) G1 is my Robot, G2 is opponents

position_Green_Object(rgb0, a, label, number_of_labels, ic_G, jc_G);

//purple obdtacle (3)

position_Purple_Object(rgb0, a, label, number_of_labels, ic_P, jc_P);

position_Light_Blue_Object(rgb0, a, label, number_of_labels, ic_LB, jc_LB, r_obstacle);

position_Dark_Orange_Object(rgb0, a, label, number_of_labels, ic_DO, jc_DO);

view_image(rgb0);

printf("\nin while perpendicular <= radius");

dy_robot = jc_DO - jc_DB;
dx_robot = ic_DO - ic_DB;
dy_target = jc_G - jc_DB;
dx_target = ic_G - ic_DB;
dy_obstacle = jc_P - jc_DB;
dx_obstacle = ic_P - ic_DB;

t = high_resolution_time() - t0;
 th = atan2(dy_robot, dx_robot); // use atan2 to get the
correct quadrant

r = atan2(dy_target, dx_target);
th_obstacle = atan2(dy_obstacle, dx_obstacle);

//Quadrant Selection for robot Rotation

if (th_obstacle > 0 && th_obstacle < 1.570796 && r > 0 && r <
1.570796)

{
r = 0.61087 + th_initial;
}

else if (th_obstacle > 1.570796 && th_obstacle < 3.14159 && r > 1.570796 && r < 3.14159)

{
r = -0.61087 + th_initial;
}

else if (th_obstacle < 0 && th_obstacle > -1.570796 && r < 0 && r > -1.570796)

{
r = -0.61087 + th_initial;
}

else if (th_obstacle < -1.570796 && th_obstacle > -3.14159 && r < -1.570796 && r > -
3.14159)

{
r = 0.61087 + th_initial;
}

rd = 0.0;

e = th - r;// control error(rad)

if (th > 1.570796 && r < -1.570796)

{
e = -3.14159 * 2 - (th - r);
}
else if (th < -1.570796 && r > 1.570796)
{
e = 3.14159 * 2 - (th - r);
}

printf("\n\ne (to turn 30) = %lf", e / 3.14159 * 180);

kp = 0.065; // proportional gain

ki = 0.0005; // integral gain
kd = 0.0; // derivative gain

ed = (e - ep) / dt;
ep = e;

ei += e*dt;

u = kp*ep + ki*ei + kd*ed;

 // saturate control input
if (u > 1.0) u = 1.0;
if (u < -1.0) u = -1.0;

printf("\ntrying to turn 30");

uR = 92 + 90 * u;
uL = 92 + 90 * u;
uLas = 65;
//printf("\n\nuR = %lf, uL = %lf", (float)uR, (float)uL);

// send input to Arduino using serial communication

buffer_out[0] = uR;
buffer_out[1] = uL;
buffer_out[2] = uLas;
buffer_out[3] = 0;
n = 4;
serial_send(buffer_out, n, h1);
printf("\n\nuR=%lf,uL=%lf", (float)uR, (float)uL);

if (uR == 92 && uL == 92)

{

find_object(nlabel, number_of_labels);

position_Dark_Blue_Object(rgb0, a, label, number_of_labels, ic_DB, jc_DB);

//green tagets (2) G1 is my Robot, G2 is opponents

position_Green_Object(rgb0, a, label, number_of_labels, ic_G, jc_G);

//purple obdtacle (3)

position_Purple_Object(rgb0, a, label, number_of_labels, ic_P, jc_P);

position_Light_Blue_Object(rgb0, a, label, number_of_labels, ic_LB, jc_LB, r_obstacle);

position_Dark_Orange_Object(rgb0, a, label, number_of_labels, ic_DO, jc_DO);

view_image(rgb0);

// declare and initialize ic_DB and jc_DB before the loop

double ic_DB_initial = ic_DB;
double jc_DB_initial = jc_DB;

//dist_DB_to_purple should not change change therefore we only calculate it once

//dist_to_travel should not change neither therefore we only calculate it once
double ic_DB_to_G = ic_G - ic_DB;
double jc_DB_to_G = jc_G - jc_DB;
double dist_DB_to_G = sqrt(ic_DB_to_G*ic_DB_to_G + jc_DB_to_G*jc_DB_to_G);
//new variable
double distance_to_travel = dist_DB_to_G * cos(r);
double distance_travelled = 0;

while (1)
{
find_object(nlabel, number_of_labels);
 position_Dark_Blue_Object(rgb0, a, label, number_of_labels, ic_DB, jc_DB);

//green tagets (2) G1 is my Robot, G2 is opponents

position_Green_Object(rgb0, a, label, number_of_labels, ic_G, jc_G);

//purple obdtacle (3)

position_Purple_Object(rgb0, a, label, number_of_labels, ic_P, jc_P);

position_Light_Blue_Object(rgb0, a, label, number_of_labels, ic_LB, jc_LB, r_obstacle);

position_Dark_Orange_Object(rgb0, a, label, number_of_labels, ic_DO, jc_DO);

view_image(rgb0);

// this will change as we travelled it is current distance - initial distance

distance_travelled = sqrt(ic_DB*ic_DB + jc_DB*jc_DB) - sqrt(ic_DB_initial*ic_DB_initial +

jc_DB_initial*jc_DB_initial);

e = abs(distance_to_travel - distance_travelled);

uR = 180;
uL = 4;
uLas = 65;

// send input to Arduino using serial communication

buffer_out[0] = uR;
buffer_out[1] = uL;
buffer_out[2] = uLas;
buffer_out[3] = 0;
n = 4;
serial_send(buffer_out, n, h1);

printf("\nTravelling!");
printf("\n\nuR=%lf,uL=%lf", (float)uR,
(float)uL);

if (abs(e) < 20)//align

{

while (1)
{
find_object(nlabel, number_of_labels);

position_Dark_Blue_Object(rgb0, a, label, number_of_labels, ic_DB, jc_DB);

//green tagets (2) G1 is my Robot, G2 is opponents

position_Green_Object(rgb0, a, label, number_of_labels, ic_G, jc_G);

//purple obdtacle (3)

position_Purple_Object(rgb0, a, label, number_of_labels, ic_P, jc_P);

position_Light_Blue_Object(rgb0, a, label, number_of_labels, ic_LB, jc_LB, r_obstacle);

position_Dark_Orange_Object(rgb0, a, label, number_of_labels, ic_DO, jc_DO);

view_image(rgb0);

printf("\nrealign (after moving forward!)");

dy_robot = jc_DO - jc_DB;

dx_robot = ic_DO - ic_DB;
dy_target = jc_G - jc_DB;
dx_target = ic_G - ic_DB;

t = high_resolution_time() - t0;

th = atan2(dy_robot, dx_robot);

r = atan2(dy_target, dx_target);

rd = 0.0;

e = th - r; // control error (rad)

if (th > 1.570796 && r < -1.570796)

{
e = -3.14159 * 2 - (th - r);
}
else if (th < -1.570796 && r > 1.570796)
{
e = 3.14159 * 2 - (th - r);
}

kp = 0.069; // proportional gain

ki = 0.0005; // integral gain
kd = 0.00; // derivative gain

ed = (e - ep) / dt;
ep = e;
ei += e*dt;

// calculate control input

// note: normalize input to range from -1 to 1
u = kp*ep + ki*ei + kd*ed;

// saturate control input

if (u > 1.0) u = 1.0;
if (u < -1.0) u = -1.0;

uR = 92 + 90 * u;
uL = 92 + 90 * u;
uLas = 65;

// send input to Arduino using serial communication

buffer_out[0] = uR;
buffer_out[1] = uL;
buffer_out[2] = uLas;
buffer_out[3] = 0;
n = 4;
serial_send(buffer_out, n, h1);
//uR >= 91.5 && uR <= 92.5 && uL >= 91.5 && uL <= 92.5
if (uR == 92 && uL == 92)
{

uR = 92.0 ;
uL = 92.0 ;
uLas = 65;

// send input to Arduino using serial communication

buffer_out[0] = uR;
buffer_out[1] = uL;
buffer_out[2] = uLas;
buffer_out[3] = 0;
n = 4;
serial_send(buffer_out, n, h1);

break;
}//end if (UR==92...)
}// end while for align object

//break;
} // end if(abs(e)<1)

break;// break out move forward

}// end while(1) to move forward

}// end if(uR....)

find_object(nlabel, number_of_labels);

position_Dark_Blue_Object(rgb0, a, label, number_of_labels, ic_DB, jc_DB);

//green tagets (2) G1 is my Robot, G2 is opponents

position_Green_Object(rgb0, a, label, number_of_labels, ic_G, jc_G);

//purple obdtacle (3)

position_Purple_Object(rgb0, a, label, number_of_labels, ic_P, jc_P);

position_Light_Blue_Object(rgb0, a, label, number_of_labels, ic_LB, jc_LB, r_obstacle);

position_Dark_Orange_Object(rgb0, a, label, number_of_labels, ic_DO, jc_DO);

// position_Orange_Object(rgb0, a, label, number_of_labels, ic_O, jc_O);

// position_Yellow_Object(rgb0, a, label, number_of_labels, ic_Y, jc_Y);
//copy(a, rgb); // convert to RGB image format

ic_G_to_DB = ic_G - ic_DB;

jc_G_to_DB = jc_G - jc_DB;

ic_DB_to_obstacle = ic_P - ic_DB;

jc_DB_to_obstacle = jc_P - jc_DB;

dist_DB_to_G = sqrt(ic_G_to_DB*ic_G_to_DB + jc_G_to_DB*jc_G_to_DB);

dist_DB_to_obstacle = sqrt(ic_DB_to_obstacle*ic_DB_to_obstacle +
jc_DB_to_obstacle*jc_DB_to_obstacle);

absolute = abs(jc_G_to_DB*ic_P - ic_G_to_DB*jc_P + ic_G*jc_DB - jc_G*ic_DB);

perpendicular_dist_from_P_to_line = absolute / dist_DB_to_G;

}// end while (perpendicular < radius)

}//end if

else if (perpendicular_dist_from_P_to_line >= radius_obstacle)

{
while (perpendicular_dist_from_P_to_line >= radius_obstacle)
{
//i++;

find_object(nlabel, number_of_labels);

position_Dark_Blue_Object(rgb0, a, label, number_of_labels, ic_DB, jc_DB);

//green tagets (2) G1 is my Robot, G2 is opponents

position_Green_Object(rgb0, a, label, number_of_labels, ic_G, jc_G);

//purple obdtacle (3)

position_Purple_Object(rgb0, a, label, number_of_labels, ic_P, jc_P);

position_Light_Blue_Object(rgb0, a, label, number_of_labels, ic_LB, jc_LB, r_obstacle);

position_Dark_Orange_Object(rgb0, a, label, number_of_labels, ic_DO, jc_DO);

view_image(rgb0);

dy_robot = jc_DO - jc_DB;

dx_robot = ic_DO - ic_DB;
dy_target = jc_G - jc_DB;
dx_target = ic_G - ic_DB;

t = high_resolution_time() - t0;

th = atan2(dy_robot, dx_robot); // use atan2 to get the correct quadrant

//printf("\n\nt = %lf, th(deg) = %lf", t, th / 3.14159 * 180);

r = atan2(dy_target, dx_target);
//printf("\n\nt = %lf, r(deg) = %lf", t, r / 3.14159 * 180);
printf("\nalign 1st else if");

rd = 0.0;

e = th - r; // control error (rad)

if (th > 1.570796 && r < -1.570796)

{
e = -3.14159 * 2 - (th - r);
}
else if (th < -1.570796 && r > 1.570796)
{
e = 3.14159 * 2 - (th - r);
}

//printf("\n\ne (deg) = %lf", e / 3.14159 * 180);

// kp = 0.2 still fast and not alligned

kp = 0.07; // proportional gain

ki = 0.0005; // integral gain
kd = 0.00; // derivative gain

ed = (e - ep) / dt;
ep = e;
ei += e*dt;

// calculate control input

// note: normalize input to range from -1 to 1
u = kp*ep + ki*ei + kd*ed;

// saturate control input

if (u > 1.0) u = 1.0;
if (u < -1.0) u = -1.0;

uR = 92 + 90 * u;
uL = 92 + 90 * u;
uLas = 65;

// send input to Arduino using serial communication

buffer_out[0] = uR;
buffer_out[1] = uL;
buffer_out[2] = uLas;
buffer_out[3] = 0;
n = 4;
serial_send(buffer_out, n, h1);

while (abs(e) < 1.14)

{
u = 0.35;
uR = 92 + 90 * u;
uL = 92 + 90 * -u;
uLas = 65;

// send input to Arduino using serial communication

buffer_out[0] = uR;
buffer_out[1] = uL;
buffer_out[2] = uLas;
buffer_out[3] = 1;
n = 4;
serial_send(buffer_out, n, h1);

//check laser angle

if (uLas == 70)
{
uLas = 55;
}
uLas++;
find_object(nlabel, number_of_labels);

position_Dark_Blue_Object(rgb0, a, label, number_of_labels, ic_DB, jc_DB);

//purple obdtacle (3)

position_Purple_Object(rgb0, a, label, number_of_labels, ic_P, jc_P);

position_Light_Blue_Object(rgb0, a, label, number_of_labels, ic_LB, jc_LB, r_obstacle);

position_Dark_Orange_Object(rgb0, a, label, number_of_labels, ic_DO, jc_DO);

position_Green_Object(rgb0, a, label, number_of_labels, ic_G, jc_G);

view_image(rgb0);

dy_robot = jc_DO - jc_DB;

dx_robot = ic_DO - ic_DB;
dy_target = jc_G - jc_DB;
dx_target = ic_G - ic_DB;

t = high_resolution_time() - t0;

th = atan2(dy_robot, dx_robot); // use atan2 to get the correct quadrant

r = atan2(dy_target, dx_target);
//printf("\n\nt = %lf, r(deg) = %lf", t, r / 3.14159 * 180);
printf("\nAbs error else if ");

rd = 0.0;

e = th - r; // control error (rad)

if (th > 1.570796 && r < -1.570796)

{
e = -3.14159 * 2 - (th - r);
}
else if (th < -1.570796 && r > 1.570796)
{
e = 3.14159 * 2 - (th - r);
}

//break;
}// end of while abs(e<=0.2)

find_object(nlabel, number_of_labels);

position_Dark_Blue_Object(rgb0, a, label, number_of_labels, ic_DB, jc_DB);

//robot tracker
position_Red_Object(rgb0, a, label, number_of_labels, ic_R, jc_R);
position_Pink_Object(rgb0, a, label, number_of_labels, ic_PK, jc_PK);

//green tagets (2) G1 is my Robot, G2 is opponents

position_Green_Object(rgb0, a, label, number_of_labels, ic_G, jc_G);

//purple obdtacle (3)

position_Purple_Object(rgb0, a, label, number_of_labels, ic_P, jc_P);

position_Light_Blue_Object(rgb0, a, label, number_of_labels, ic_LB, jc_LB, r_obstacle);

position_Dark_Orange_Object(rgb0, a, label, number_of_labels, ic_DO, jc_DO);

ic_G_to_DB = ic_G - ic_DB;

jc_G_to_DB = jc_G - jc_DB;

ic_DB_to_obstacle = ic_P - ic_DB;

jc_DB_to_obstacle = jc_P - jc_DB;

dist_DB_to_G = sqrt(ic_G_to_DB*ic_G_to_DB + jc_G_to_DB*jc_G_to_DB);

dist_DB_to_obstacle = sqrt(ic_DB_to_obstacle*ic_DB_to_obstacle +
jc_DB_to_obstacle*jc_DB_to_obstacle);

absolute = abs(jc_G_to_DB*ic_P - ic_G_to_DB*jc_P + ic_G*jc_DB - jc_G*ic_DB);

perpendicular_dist_from_P_to_line = absolute / dist_DB_to_G;

}//end of while(perpendicular else if)

}// end of else if

}//end of while(1)

close_serial(h1);

return 0;