Aurélien Bernier Levalois / Mbed 2 deprecated Mapping

Mapping for TP2

Dependencies:   ISR_Mini-explorer mbed

Fork of GoToPointWithAngle by Georgios Tsamis

main.cpp@9:b7138acdf4ac, 2017-03-27 (annotated)

Committer:
geotsam
Date:
Mon Mar 27 16:26:41 2017 +0000
Revision:
9:b7138acdf4ac
Parent:
8:109314be5b68
Child:
10:a7fd80e79e80
added target_angle in the function

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