![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Mapping for TP2
Dependencies: ISR_Mini-explorer mbed
Fork of GoToPointWithAngle by
Diff: main.cpp
- Revision:
- 6:afde4b08166b
- Parent:
- 5:dea05b8f30d0
- Child:
- 7:c94070f9af78
diff -r dea05b8f30d0 -r afde4b08166b main.cpp --- a/main.cpp Fri Mar 24 16:38:58 2017 +0000 +++ b/main.cpp Fri Mar 24 17:39:24 2017 +0000 @@ -1,7 +1,8 @@ #include "mbed.h" #include "robot.h" // Initializes the robot. This include should be used in all main.cpp! #include "math.h" - + +Timer t; float dist(float robot_x, float robot_y, float target_x, float target_y); @@ -17,10 +18,11 @@ float rho; //distance from target float beta; //float k_linear=10, k_angular=200; - float kRho=1, ka=8, kb=-2; + float kRho=12, ka=25, kb=-15; float linear, angular, angular_left, angular_right; float dt=0.5; float temp; + float d2; //Diameter of a wheel and distance between the 2 float r=3.25, b=7.2; @@ -35,26 +37,37 @@ alpha = atan2((target_y-Y),(target_x-X))-theta; alpha = atan(sin(alpha)/cos(alpha)); rho = dist(X, Y, target_x, target_y); + beta = -alpha-theta+target_angle; + //beta = atan(sin(beta)/cos(beta)); do { pc.printf("\n\n\r entered while"); + //Timer stuff + dt = t.read(); + t.reset(); + t.start(); + //Updating X,Y and theta with the odometry values Odometria(); alpha = atan2((target_y-Y),(target_x-X))-theta; alpha = atan(sin(alpha)/cos(alpha)); rho = dist(X, Y, target_x, target_y); + d2 = rho; //beta = -alpha-theta; beta = -alpha-theta+target_angle; - + //beta = atan(sin(beta)/cos(beta)); + + //Computing angle error and distance towards the target value rho += dt*(-kRho*cos(alpha)*rho); temp = alpha; - alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*b); - b += dt*(-kRho*sin(temp)); - pc.printf("\n\r rho=%f", rho); + alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta); + beta += dt*(-kRho*sin(temp)); + pc.printf("\n\r d2=%f", d2); + pc.printf("\n\r dt=%f", dt); //Computing linear and angular velocities if(alpha>=-1.5708 && alpha<=1.5708){ @@ -69,8 +82,8 @@ angular_right=(linear+0.5*b*angular)/r; //Slowing down at the end for more precision - if (rho<25) { - speed = rho*30; + if (d2<25) { + speed = d2*30; } //Normalize speed for motors @@ -89,8 +102,10 @@ leftMotor(1,angular_left); rightMotor(1,angular_right); - wait(dt); - } while(rho>1); + wait(0.5); + //Timer stuff + t.stop(); + } while(d2>2); //Stop at the end leftMotor(1,0); @@ -102,11 +117,4 @@ //Distance computation function float dist(float robot_x, float robot_y, float target_x, float target_y){ return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2)); -} - -/*static double diffclock(clock_t clock1,clock_t clock2) -{ - double diffticks=clock1-clock2; - double diffms=(diffticks)/(CLOCKS_PER_SEC/1000); - return diffms; -}*/ \ No newline at end of file +} \ No newline at end of file