Mapping for TP2
Dependencies: ISR_Mini-explorer mbed
Fork of GoToPointWithAngle by
Diff: main.cpp
- Revision:
- 3:1e0f4cb93eda
- Parent:
- 2:ea61e801e81f
- Child:
- 4:8c56c3ba6e54
diff -r ea61e801e81f -r 1e0f4cb93eda main.cpp --- a/main.cpp Tue Mar 21 16:17:56 2017 +0000 +++ b/main.cpp Fri Mar 24 15:46:46 2017 +0000 @@ -9,12 +9,16 @@ pc.baud(9600); // baud for the pc communication //Target example x,y values - float target_x=46.8, target_y=78.6; + float target_x=46.8, target_y=78.6, target_angle=0; - float angle_error; //angle error + float a; //angle error float d; //distance from target - float k_linear=10, k_angular=200; + float beta; + //float k_linear=10, k_angular=200; + float kr=3, ka=8, kb=-2; float linear, angular, angular_left, angular_right; + float dt=0.5; + float temp; //Diameter of a wheel and distance between the 2 float r=3.25, b=7.2; @@ -26,21 +30,38 @@ X=0; Y=0; + a = atan2((target_y-Y),(target_x-X))-theta; + a = atan(sin(a)/cos(a)); + d = dist(X, Y, target_x, target_y); + beta = -a-theta+target_angle; + do { pc.printf("\n\n\r entered while"); //Updating X,Y and theta with the odometry values Odometria(); + + a = atan2((target_y-Y),(target_x-X))-theta; + a = atan(sin(a)/cos(a)); + d = dist(X, Y, target_x, target_y); + beta = -a-theta+target_angle; //Computing angle error and distance towards the target value - angle_error = atan2((target_y-Y),(target_x-X))-theta; - angle_error = atan(sin(angle_error)/cos(angle_error)); - d=dist(X, Y, target_x, target_y); + d += dt*(-kr*cos(a)*d); + a = temp; + a += dt*(kr*sin(a)-ka*a-kb*b); + b += dt*(-kr*sin(temp)); pc.printf("\n\r d=%f", d); //Computing linear and angular velocities - linear=k_linear*d; - angular=k_angular*angle_error; + if(a>=-1.5708 && a<=1.5708){ + linear=kr*d; + angular=ka*a+kb*beta; + } + else{ + linear=-kr*d; + angular=-ka*a-kb*beta; + } angular_left=(linear-0.5*b*angular)/r; angular_right=(linear+0.5*b*angular)/r; @@ -65,7 +86,7 @@ leftMotor(1,angular_left); rightMotor(1,angular_right); - wait(0.5); + wait(dt); } while(d>1); //Stop at the end