Georgios Tsamis / Mbed 2 deprecated VirtualForces_debug

Navigate to a given point using the OGM and virtual forces

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

main.cpp@18:6a3b14e284ee, 2017-03-27 (annotated)

Committer:
geotsam
Date:
Mon Mar 27 17:07:10 2017 +0000
Revision:
18:6a3b14e284ee
Parent:
17:caf393b63e27 
added a small delay between the new positions - but it doesn't work, it's just spinning

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
geotsam 12:3c0ca2350624 9 int goToPointWithAngle(float target_x, float target_y, float target_angle);
AurelienBernier 8:109314be5b68 10
AurelienBernier 11:e641aa08c92e 11 int updateSonarValues();
AurelienBernier 11:e641aa08c92e 12
AurelienBernier 8:109314be5b68 13 float alpha; //angle error
AurelienBernier 8:109314be5b68 14 float rho; //distance from target
AurelienBernier 8:109314be5b68 15 float beta;
AurelienBernier 11:e641aa08c92e 16 float kRho=12, ka=30, kb=-13; //Kappa values
AurelienBernier 8:109314be5b68 17 float linear, angular, angular_left, angular_right;
AurelienBernier 8:109314be5b68 18 float dt=0.5;
AurelienBernier 8:109314be5b68 19 float temp;
AurelienBernier 8:109314be5b68 20 float d2;
AurelienBernier 8:109314be5b68 21
geotsam 14:d58f2bdbf42e 22 //bool tooClose = false;
AurelienBernier 11:e641aa08c92e 23
AurelienBernier 11:e641aa08c92e 24 int leftMm;
AurelienBernier 11:e641aa08c92e 25 int frontMm;
AurelienBernier 11:e641aa08c92e 26 int rightMm;
AurelienBernier 11:e641aa08c92e 27
AurelienBernier 8:109314be5b68 28 //Diameter of a wheel and distance between the 2
AurelienBernier 8:109314be5b68 29 float r=3.25, b=7.2;
AurelienBernier 8:109314be5b68 30
AurelienBernier 8:109314be5b68 31 int speed=999; // Max speed at beggining of movement
AurelienBernier 8:109314be5b68 32
AurelienBernier 8:109314be5b68 33 //Target example x,y values
geotsam 17:caf393b63e27 34 float target_x, target_y, target_angle;
AurelienBernier 8:109314be5b68 35
AurelienBernier 4:8c56c3ba6e54 36 //Timeout time;
geotsam 0:8bffb51cc345 37 int main(){
geotsam 17:caf393b63e27 38 target_x=200*rand();
geotsam 17:caf393b63e27 39 target_y=200*rand();
geotsam 17:caf393b63e27 40 target_angle=3.1416*2*rand()-3.1416;
geotsam 17:caf393b63e27 41
geotsam 13:41f75c132135 42 i2c1.frequency(100000);
AurelienBernier 2:ea61e801e81f 43 initRobot(); //Initializing the robot
geotsam 0:8bffb51cc345 44 pc.baud(9600); // baud for the pc communication
geotsam 0:8bffb51cc345 45
geotsam 13:41f75c132135 46 measure_always_on();
geotsam 13:41f75c132135 47 wait_ms(50);
geotsam 13:41f75c132135 48
AurelienBernier 2:ea61e801e81f 49 //Resetting coordinates before moving
AurelienBernier 2:ea61e801e81f 50 theta=0;
geotsam 0:8bffb51cc345 51 X=0;
geotsam 0:8bffb51cc345 52 Y=0;
geotsam 0:8bffb51cc345 53
AurelienBernier 4:8c56c3ba6e54 54 alpha = atan2((target_y-Y),(target_x-X))-theta;
AurelienBernier 4:8c56c3ba6e54 55 alpha = atan(sin(alpha)/cos(alpha));
AurelienBernier 4:8c56c3ba6e54 56 rho = dist(X, Y, target_x, target_y);
AurelienBernier 6:afde4b08166b 57
AurelienBernier 4:8c56c3ba6e54 58 beta = -alpha-theta+target_angle;
AurelienBernier 6:afde4b08166b 59 //beta = atan(sin(beta)/cos(beta));
AurelienBernier 8:109314be5b68 60
geotsam 17:caf393b63e27 61 while(1){
geotsam 17:caf393b63e27 62 goToPointWithAngle(target_x, target_y, target_angle);
geotsam 18:6a3b14e284ee 63 wait(1);
geotsam 17:caf393b63e27 64 target_x=200*rand();
geotsam 17:caf393b63e27 65 target_y=200*rand();
geotsam 17:caf393b63e27 66 target_angle=3.1416*2*rand()-3.1416;
geotsam 17:caf393b63e27 67 }
geotsam 17:caf393b63e27 68
AurelienBernier 8:109314be5b68 69 //Stop at the end
AurelienBernier 8:109314be5b68 70 leftMotor(1,0);
AurelienBernier 8:109314be5b68 71 rightMotor(1,0);
AurelienBernier 8:109314be5b68 72
AurelienBernier 8:109314be5b68 73 pc.printf("\n\r %f -- arrived!", rho);
AurelienBernier 8:109314be5b68 74 }
AurelienBernier 8:109314be5b68 75
AurelienBernier 8:109314be5b68 76 //Distance computation function
AurelienBernier 8:109314be5b68 77 float dist(float robot_x, float robot_y, float target_x, float target_y){
AurelienBernier 8:109314be5b68 78 return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
AurelienBernier 8:109314be5b68 79 }
AurelienBernier 8:109314be5b68 80
AurelienBernier 11:e641aa08c92e 81 //Updates sonar values
AurelienBernier 11:e641aa08c92e 82 int updateSonarValues() {
AurelienBernier 11:e641aa08c92e 83 leftMm = get_distance_left_sensor();
AurelienBernier 11:e641aa08c92e 84 frontMm = get_distance_front_sensor();
AurelienBernier 11:e641aa08c92e 85 rightMm = get_distance_right_sensor();
AurelienBernier 11:e641aa08c92e 86 return 0;
AurelienBernier 11:e641aa08c92e 87 }
AurelienBernier 11:e641aa08c92e 88
geotsam 12:3c0ca2350624 89 int goToPointWithAngle(float target_x, float target_y, float target_angle) {
AurelienBernier 8:109314be5b68 90 do {
geotsam 0:8bffb51cc345 91 pc.printf("\n\n\r entered while");
AurelienBernier 2:ea61e801e81f 92
AurelienBernier 6:afde4b08166b 93 //Timer stuff
AurelienBernier 6:afde4b08166b 94 dt = t.read();
AurelienBernier 6:afde4b08166b 95 t.reset();
AurelienBernier 6:afde4b08166b 96 t.start();
AurelienBernier 6:afde4b08166b 97
geotsam 14:d58f2bdbf42e 98 //Updating X,Y and theta with the odometry values
geotsam 14:d58f2bdbf42e 99 Odometria();
geotsam 14:d58f2bdbf42e 100
AurelienBernier 11:e641aa08c92e 101 updateSonarValues();
AurelienBernier 11:e641aa08c92e 102 if (leftMm < 100 || frontMm < 100 || rightMm < 100) {
geotsam 14:d58f2bdbf42e 103 break;
AurelienBernier 11:e641aa08c92e 104 }
AurelienBernier 11:e641aa08c92e 105
AurelienBernier 4:8c56c3ba6e54 106 alpha = atan2((target_y-Y),(target_x-X))-theta;
AurelienBernier 4:8c56c3ba6e54 107 alpha = atan(sin(alpha)/cos(alpha));
AurelienBernier 4:8c56c3ba6e54 108 rho = dist(X, Y, target_x, target_y);
AurelienBernier 6:afde4b08166b 109 d2 = rho;
AurelienBernier 5:dea05b8f30d0 110 beta = -alpha-theta+target_angle;
AurelienBernier 6:afde4b08166b 111 //beta = atan(sin(beta)/cos(beta));
AurelienBernier 6:afde4b08166b 112
AurelienBernier 6:afde4b08166b 113
AurelienBernier 2:ea61e801e81f 114 //Computing angle error and distance towards the target value
AurelienBernier 4:8c56c3ba6e54 115 rho += dt*(-kRho*cos(alpha)*rho);
AurelienBernier 4:8c56c3ba6e54 116 temp = alpha;
AurelienBernier 6:afde4b08166b 117 alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta);
AurelienBernier 6:afde4b08166b 118 beta += dt*(-kRho*sin(temp));
AurelienBernier 6:afde4b08166b 119 pc.printf("\n\r d2=%f", d2);
AurelienBernier 6:afde4b08166b 120 pc.printf("\n\r dt=%f", dt);
geotsam 0:8bffb51cc345 121
AurelienBernier 2:ea61e801e81f 122 //Computing linear and angular velocities
AurelienBernier 4:8c56c3ba6e54 123 if(alpha>=-1.5708 && alpha<=1.5708){
AurelienBernier 4:8c56c3ba6e54 124 linear=kRho*rho;
AurelienBernier 4:8c56c3ba6e54 125 angular=ka*alpha+kb*beta;
geotsam 3:1e0f4cb93eda 126 }
geotsam 3:1e0f4cb93eda 127 else{
AurelienBernier 4:8c56c3ba6e54 128 linear=-kRho*rho;
AurelienBernier 4:8c56c3ba6e54 129 angular=-ka*alpha-kb*beta;
geotsam 3:1e0f4cb93eda 130 }
geotsam 0:8bffb51cc345 131 angular_left=(linear-0.5*b*angular)/r;
geotsam 0:8bffb51cc345 132 angular_right=(linear+0.5*b*angular)/r;
geotsam 0:8bffb51cc345 133
AurelienBernier 2:ea61e801e81f 134 //Slowing down at the end for more precision
AurelienBernier 6:afde4b08166b 135 if (d2<25) {
AurelienBernier 6:afde4b08166b 136 speed = d2*30;
geotsam 0:8bffb51cc345 137 }
AurelienBernier 2:ea61e801e81f 138
AurelienBernier 2:ea61e801e81f 139 //Normalize speed for motors
geotsam 0:8bffb51cc345 140 if(angular_left>angular_right) {
geotsam 0:8bffb51cc345 141 angular_right=speed*angular_right/angular_left;
geotsam 0:8bffb51cc345 142 angular_left=speed;
geotsam 0:8bffb51cc345 143 } else {
geotsam 0:8bffb51cc345 144 angular_left=speed*angular_left/angular_right;
geotsam 0:8bffb51cc345 145 angular_right=speed;
geotsam 0:8bffb51cc345 146 }
geotsam 0:8bffb51cc345 147
geotsam 0:8bffb51cc345 148 pc.printf("\n\r X=%f", X);
geotsam 0:8bffb51cc345 149 pc.printf("\n\r Y=%f", Y);
AurelienBernier 15:44ab4626f1ad 150 pc.printf("\n\r leftMm=%f", leftMm);
AurelienBernier 15:44ab4626f1ad 151 pc.printf("\n\r frontMm=%f", frontMm);
AurelienBernier 15:44ab4626f1ad 152 pc.printf("\n\r rightMm=%f", rightMm);
geotsam 0:8bffb51cc345 153
AurelienBernier 2:ea61e801e81f 154 //Updating motor velocities
AurelienBernier 1:f0807d5c5a4b 155 leftMotor(1,angular_left);
AurelienBernier 1:f0807d5c5a4b 156 rightMotor(1,angular_right);
geotsam 0:8bffb51cc345 157
AurelienBernier 7:c94070f9af78 158 wait(0.2);
AurelienBernier 6:afde4b08166b 159 //Timer stuff
AurelienBernier 6:afde4b08166b 160 t.stop();
geotsam 14:d58f2bdbf42e 161 } while(d2>1);
AurelienBernier 8:109314be5b68 162
AurelienBernier 8:109314be5b68 163 return 0;
AurelienBernier 6:afde4b08166b 164 }