Georgios Tsamis / Mbed 2 deprecated OneFileToRuleThemAll

All the lab works are here!

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

main.cpp@6:afde4b08166b, 2017-03-24 (annotated)

Committer:
AurelienBernier
Date:
Fri Mar 24 17:39:24 2017 +0000
Revision:
6:afde4b08166b
Parent:
5:dea05b8f30d0
Child:
7:c94070f9af78
functional shit;

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