Aurélien Bernier Levalois / Mbed 2 deprecated Mapping

Mapping for TP2

Dependencies:   ISR_Mini-explorer mbed

Fork of GoToPointWithAngle by Georgios Tsamis

main.cpp@5:dea05b8f30d0, 2017-03-24 (annotated)

Committer:
AurelienBernier
Date:
Fri Mar 24 16:38:58 2017 +0000
Revision:
5:dea05b8f30d0
Parent:
4:8c56c3ba6e54
Child:
6:afde4b08166b
v2 with nice kappas kappa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
geotsam 0:8bffb51cc345 1 #include "mbed.h"
geotsam 0:8bffb51cc345 2 #include "robot.h" // Initializes the robot. This include should be used in all main.cpp!
geotsam 0:8bffb51cc345 3 #include "math.h"
geotsam 0:8bffb51cc345 4
AurelienBernier 4:8c56c3ba6e54 5
geotsam 0:8bffb51cc345 6 float dist(float robot_x, float robot_y, float target_x, float target_y);
geotsam 0:8bffb51cc345 7
AurelienBernier 4:8c56c3ba6e54 8 //Timeout time;
geotsam 0:8bffb51cc345 9 int main(){
AurelienBernier 2:ea61e801e81f 10 initRobot(); //Initializing the robot
geotsam 0:8bffb51cc345 11 pc.baud(9600); // baud for the pc communication
geotsam 0:8bffb51cc345 12
AurelienBernier 2:ea61e801e81f 13 //Target example x,y values
geotsam 3:1e0f4cb93eda 14 float target_x=46.8, target_y=78.6, target_angle=0;
geotsam 0:8bffb51cc345 15
AurelienBernier 4:8c56c3ba6e54 16 float alpha; //angle error
AurelienBernier 4:8c56c3ba6e54 17 float rho; //distance from target
geotsam 3:1e0f4cb93eda 18 float beta;
geotsam 3:1e0f4cb93eda 19 //float k_linear=10, k_angular=200;
AurelienBernier 5:dea05b8f30d0 20 float kRho=1, ka=8, kb=-2;
geotsam 0:8bffb51cc345 21 float linear, angular, angular_left, angular_right;
geotsam 3:1e0f4cb93eda 22 float dt=0.5;
geotsam 3:1e0f4cb93eda 23 float temp;
geotsam 0:8bffb51cc345 24
AurelienBernier 2:ea61e801e81f 25 //Diameter of a wheel and distance between the 2
geotsam 0:8bffb51cc345 26 float r=3.25, b=7.2;
geotsam 0:8bffb51cc345 27
AurelienBernier 2:ea61e801e81f 28 int speed=999; // Max speed at beggining of movement
geotsam 0:8bffb51cc345 29
AurelienBernier 2:ea61e801e81f 30 //Resetting coordinates before moving
AurelienBernier 2:ea61e801e81f 31 theta=0;
geotsam 0:8bffb51cc345 32 X=0;
geotsam 0:8bffb51cc345 33 Y=0;
geotsam 0:8bffb51cc345 34
AurelienBernier 4:8c56c3ba6e54 35 alpha = atan2((target_y-Y),(target_x-X))-theta;
AurelienBernier 4:8c56c3ba6e54 36 alpha = atan(sin(alpha)/cos(alpha));
AurelienBernier 4:8c56c3ba6e54 37 rho = dist(X, Y, target_x, target_y);
AurelienBernier 4:8c56c3ba6e54 38 beta = -alpha-theta+target_angle;
geotsam 3:1e0f4cb93eda 39
geotsam 0:8bffb51cc345 40 do {
geotsam 0:8bffb51cc345 41 pc.printf("\n\n\r entered while");
AurelienBernier 2:ea61e801e81f 42
AurelienBernier 2:ea61e801e81f 43 //Updating X,Y and theta with the odometry values
geotsam 0:8bffb51cc345 44 Odometria();
geotsam 3:1e0f4cb93eda 45
AurelienBernier 4:8c56c3ba6e54 46 alpha = atan2((target_y-Y),(target_x-X))-theta;
AurelienBernier 4:8c56c3ba6e54 47 alpha = atan(sin(alpha)/cos(alpha));
AurelienBernier 4:8c56c3ba6e54 48 rho = dist(X, Y, target_x, target_y);
AurelienBernier 5:dea05b8f30d0 49 //beta = -alpha-theta;
AurelienBernier 5:dea05b8f30d0 50 beta = -alpha-theta+target_angle;
geotsam 0:8bffb51cc345 51
AurelienBernier 2:ea61e801e81f 52 //Computing angle error and distance towards the target value
AurelienBernier 4:8c56c3ba6e54 53 rho += dt*(-kRho*cos(alpha)*rho);
AurelienBernier 4:8c56c3ba6e54 54 temp = alpha;
AurelienBernier 4:8c56c3ba6e54 55 alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*b);
AurelienBernier 4:8c56c3ba6e54 56 b += dt*(-kRho*sin(temp));
AurelienBernier 4:8c56c3ba6e54 57 pc.printf("\n\r rho=%f", rho);
geotsam 0:8bffb51cc345 58
AurelienBernier 2:ea61e801e81f 59 //Computing linear and angular velocities
AurelienBernier 4:8c56c3ba6e54 60 if(alpha>=-1.5708 && alpha<=1.5708){
AurelienBernier 4:8c56c3ba6e54 61 linear=kRho*rho;
AurelienBernier 4:8c56c3ba6e54 62 angular=ka*alpha+kb*beta;
geotsam 3:1e0f4cb93eda 63 }
geotsam 3:1e0f4cb93eda 64 else{
AurelienBernier 4:8c56c3ba6e54 65 linear=-kRho*rho;
AurelienBernier 4:8c56c3ba6e54 66 angular=-ka*alpha-kb*beta;
geotsam 3:1e0f4cb93eda 67 }
geotsam 0:8bffb51cc345 68 angular_left=(linear-0.5*b*angular)/r;
geotsam 0:8bffb51cc345 69 angular_right=(linear+0.5*b*angular)/r;
geotsam 0:8bffb51cc345 70
AurelienBernier 2:ea61e801e81f 71 //Slowing down at the end for more precision
AurelienBernier 4:8c56c3ba6e54 72 if (rho<25) {
AurelienBernier 4:8c56c3ba6e54 73 speed = rho*30;
geotsam 0:8bffb51cc345 74 }
AurelienBernier 2:ea61e801e81f 75
AurelienBernier 2:ea61e801e81f 76 //Normalize speed for motors
geotsam 0:8bffb51cc345 77 if(angular_left>angular_right) {
geotsam 0:8bffb51cc345 78 angular_right=speed*angular_right/angular_left;
geotsam 0:8bffb51cc345 79 angular_left=speed;
geotsam 0:8bffb51cc345 80 } else {
geotsam 0:8bffb51cc345 81 angular_left=speed*angular_left/angular_right;
geotsam 0:8bffb51cc345 82 angular_right=speed;
geotsam 0:8bffb51cc345 83 }
geotsam 0:8bffb51cc345 84
geotsam 0:8bffb51cc345 85 pc.printf("\n\r X=%f", X);
geotsam 0:8bffb51cc345 86 pc.printf("\n\r Y=%f", Y);
geotsam 0:8bffb51cc345 87
AurelienBernier 2:ea61e801e81f 88 //Updating motor velocities
AurelienBernier 1:f0807d5c5a4b 89 leftMotor(1,angular_left);
AurelienBernier 1:f0807d5c5a4b 90 rightMotor(1,angular_right);
geotsam 0:8bffb51cc345 91
geotsam 3:1e0f4cb93eda 92 wait(dt);
AurelienBernier 4:8c56c3ba6e54 93 } while(rho>1);
geotsam 0:8bffb51cc345 94
AurelienBernier 2:ea61e801e81f 95 //Stop at the end
geotsam 0:8bffb51cc345 96 leftMotor(1,0);
geotsam 0:8bffb51cc345 97 rightMotor(1,0);
geotsam 0:8bffb51cc345 98
AurelienBernier 4:8c56c3ba6e54 99 pc.printf("\n\r %f -- arrived!", rho);
geotsam 0:8bffb51cc345 100 }
geotsam 0:8bffb51cc345 101
AurelienBernier 2:ea61e801e81f 102 //Distance computation function
geotsam 0:8bffb51cc345 103 float dist(float robot_x, float robot_y, float target_x, float target_y){
geotsam 0:8bffb51cc345 104 return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
AurelienBernier 4:8c56c3ba6e54 105 }
AurelienBernier 4:8c56c3ba6e54 106
AurelienBernier 4:8c56c3ba6e54 107 /*static double diffclock(clock_t clock1,clock_t clock2)
AurelienBernier 4:8c56c3ba6e54 108 {
AurelienBernier 4:8c56c3ba6e54 109 double diffticks=clock1-clock2;
AurelienBernier 4:8c56c3ba6e54 110 double diffms=(diffticks)/(CLOCKS_PER_SEC/1000);
AurelienBernier 4:8c56c3ba6e54 111 return diffms;
AurelienBernier 4:8c56c3ba6e54 112 }*/