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@19:dbc5fbad4975, 2017-04-03 (annotated)

Committer:
AurelienBernier
Date:
Mon Apr 03 17:01:13 2017 +0000
Revision:
19:dbc5fbad4975
Parent:
15:44ab4626f1ad
Child:
20:6a9062d54eb0
groundwork and clean structure, needs to actually do the equations in computeObstacle

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 19:dbc5fbad4975 11 int randomizeAndMap();
AurelienBernier 11:e641aa08c92e 12 int updateSonarValues();
AurelienBernier 19:dbc5fbad4975 13 int computeObstacle();
AurelienBernier 11:e641aa08c92e 14
AurelienBernier 19:dbc5fbad4975 15 bool map[25][25];
AurelienBernier 8:109314be5b68 16 float alpha; //angle error
AurelienBernier 8:109314be5b68 17 float rho; //distance from target
AurelienBernier 8:109314be5b68 18 float beta;
AurelienBernier 11:e641aa08c92e 19 float kRho=12, ka=30, kb=-13; //Kappa values
AurelienBernier 8:109314be5b68 20 float linear, angular, angular_left, angular_right;
AurelienBernier 8:109314be5b68 21 float dt=0.5;
AurelienBernier 8:109314be5b68 22 float temp;
AurelienBernier 8:109314be5b68 23 float d2;
AurelienBernier 8:109314be5b68 24
AurelienBernier 11:e641aa08c92e 25 bool tooClose = false;
AurelienBernier 11:e641aa08c92e 26
AurelienBernier 11:e641aa08c92e 27 int leftMm;
AurelienBernier 11:e641aa08c92e 28 int frontMm;
AurelienBernier 11:e641aa08c92e 29 int rightMm;
AurelienBernier 11:e641aa08c92e 30
AurelienBernier 19:dbc5fbad4975 31
AurelienBernier 8:109314be5b68 32 //Diameter of a wheel and distance between the 2
AurelienBernier 8:109314be5b68 33 float r=3.25, b=7.2;
AurelienBernier 8:109314be5b68 34
AurelienBernier 8:109314be5b68 35 int speed=999; // Max speed at beggining of movement
AurelienBernier 8:109314be5b68 36
AurelienBernier 8:109314be5b68 37 //Target example x,y values
AurelienBernier 19:dbc5fbad4975 38 float target_x=46.8, target_y=78.6, target_angle=1.57;
AurelienBernier 8:109314be5b68 39
AurelienBernier 4:8c56c3ba6e54 40 //Timeout time;
geotsam 0:8bffb51cc345 41 int main(){
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);
AurelienBernier 19:dbc5fbad4975 48
AurelienBernier 19:dbc5fbad4975 49 //Fill map
AurelienBernier 19:dbc5fbad4975 50 for (int i = 0; i<25; i++) {
AurelienBernier 19:dbc5fbad4975 51 for (int j = 0; j<25; j++) {
AurelienBernier 19:dbc5fbad4975 52 map[i][j] = false;
AurelienBernier 19:dbc5fbad4975 53 }
AurelienBernier 19:dbc5fbad4975 54 }
geotsam 13:41f75c132135 55
AurelienBernier 2:ea61e801e81f 56 //Resetting coordinates before moving
AurelienBernier 2:ea61e801e81f 57 theta=0;
geotsam 0:8bffb51cc345 58 X=0;
geotsam 0:8bffb51cc345 59 Y=0;
geotsam 0:8bffb51cc345 60
AurelienBernier 19:dbc5fbad4975 61 /*alpha = atan2((target_y-Y),(target_x-X))-theta;
AurelienBernier 4:8c56c3ba6e54 62 alpha = atan(sin(alpha)/cos(alpha));
AurelienBernier 4:8c56c3ba6e54 63 rho = dist(X, Y, target_x, target_y);
AurelienBernier 6:afde4b08166b 64
AurelienBernier 19:dbc5fbad4975 65 beta = -alpha-theta+target_angle;*/
AurelienBernier 19:dbc5fbad4975 66 for (int i = 0; i<10; i++) {
AurelienBernier 19:dbc5fbad4975 67 randomizeAndMap();
AurelienBernier 19:dbc5fbad4975 68 }
AurelienBernier 8:109314be5b68 69
AurelienBernier 8:109314be5b68 70 //Stop at the end
AurelienBernier 8:109314be5b68 71 leftMotor(1,0);
AurelienBernier 8:109314be5b68 72 rightMotor(1,0);
AurelienBernier 8:109314be5b68 73
AurelienBernier 8:109314be5b68 74 pc.printf("\n\r %f -- arrived!", rho);
AurelienBernier 8:109314be5b68 75 }
AurelienBernier 8:109314be5b68 76
AurelienBernier 8:109314be5b68 77 //Distance computation function
AurelienBernier 8:109314be5b68 78 float dist(float robot_x, float robot_y, float target_x, float target_y){
AurelienBernier 8:109314be5b68 79 return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
AurelienBernier 8:109314be5b68 80 }
AurelienBernier 8:109314be5b68 81
AurelienBernier 11:e641aa08c92e 82 //Updates sonar values
AurelienBernier 11:e641aa08c92e 83 int updateSonarValues() {
AurelienBernier 11:e641aa08c92e 84 leftMm = get_distance_left_sensor();
AurelienBernier 11:e641aa08c92e 85 frontMm = get_distance_front_sensor();
AurelienBernier 11:e641aa08c92e 86 rightMm = get_distance_right_sensor();
AurelienBernier 11:e641aa08c92e 87 return 0;
AurelienBernier 11:e641aa08c92e 88 }
AurelienBernier 11:e641aa08c92e 89
AurelienBernier 19:dbc5fbad4975 90 int randomizeAndMap() {
AurelienBernier 19:dbc5fbad4975 91 target_x = (rand()%2500)/10;//for decimal precision
AurelienBernier 19:dbc5fbad4975 92 target_y = (rand()%2500)/10;
AurelienBernier 19:dbc5fbad4975 93 target_angle = ((rand()%31416)-15708)/1000;
AurelienBernier 19:dbc5fbad4975 94 pc.printf("\n\r targ_X=%f", target_x);
AurelienBernier 19:dbc5fbad4975 95 pc.printf("\n\r targ_Y=%f", target_y);
AurelienBernier 19:dbc5fbad4975 96 pc.printf("\n\r targ_Angle=%f", target_angle);
AurelienBernier 19:dbc5fbad4975 97 goToPointWithAngle(target_x, target_y, target_angle);
AurelienBernier 19:dbc5fbad4975 98 return 0;
AurelienBernier 19:dbc5fbad4975 99 }
AurelienBernier 19:dbc5fbad4975 100
AurelienBernier 19:dbc5fbad4975 101 int computeObstacle() {
AurelienBernier 19:dbc5fbad4975 102 //get the sensor values
AurelienBernier 19:dbc5fbad4975 103 //compute the probabilistic shit of empty/non-empty in the direction of the sensor below 10 cm
AurelienBernier 19:dbc5fbad4975 104 //update the map object with true where it's likely to be obstacled
AurelienBernier 19:dbc5fbad4975 105 int xObstacle, yObstacle;
AurelienBernier 19:dbc5fbad4975 106 if (leftMm < 10) {
AurelienBernier 19:dbc5fbad4975 107
AurelienBernier 19:dbc5fbad4975 108 } else if (frontMm < 10) {
AurelienBernier 19:dbc5fbad4975 109
AurelienBernier 19:dbc5fbad4975 110 } else if (rightMm < 10) {
AurelienBernier 19:dbc5fbad4975 111
AurelienBernier 19:dbc5fbad4975 112 }
AurelienBernier 19:dbc5fbad4975 113
AurelienBernier 19:dbc5fbad4975 114 map[xObstacle][yObstacle] = true;
AurelienBernier 19:dbc5fbad4975 115 return 0;
AurelienBernier 19:dbc5fbad4975 116 }
AurelienBernier 19:dbc5fbad4975 117
geotsam 12:3c0ca2350624 118 int goToPointWithAngle(float target_x, float target_y, float target_angle) {
AurelienBernier 8:109314be5b68 119 do {
geotsam 0:8bffb51cc345 120 pc.printf("\n\n\r entered while");
AurelienBernier 2:ea61e801e81f 121
AurelienBernier 6:afde4b08166b 122 //Timer stuff
AurelienBernier 6:afde4b08166b 123 dt = t.read();
AurelienBernier 6:afde4b08166b 124 t.reset();
AurelienBernier 6:afde4b08166b 125 t.start();
AurelienBernier 6:afde4b08166b 126
AurelienBernier 11:e641aa08c92e 127 updateSonarValues();
AurelienBernier 11:e641aa08c92e 128 if (leftMm < 100 || frontMm < 100 || rightMm < 100) {
AurelienBernier 11:e641aa08c92e 129 tooClose = true;
AurelienBernier 11:e641aa08c92e 130 }
AurelienBernier 11:e641aa08c92e 131
AurelienBernier 2:ea61e801e81f 132 //Updating X,Y and theta with the odometry values
geotsam 0:8bffb51cc345 133 Odometria();
geotsam 3:1e0f4cb93eda 134
AurelienBernier 4:8c56c3ba6e54 135 alpha = atan2((target_y-Y),(target_x-X))-theta;
AurelienBernier 4:8c56c3ba6e54 136 alpha = atan(sin(alpha)/cos(alpha));
AurelienBernier 4:8c56c3ba6e54 137 rho = dist(X, Y, target_x, target_y);
AurelienBernier 6:afde4b08166b 138 d2 = rho;
AurelienBernier 19:dbc5fbad4975 139 beta = -alpha-theta+target_angle;
AurelienBernier 6:afde4b08166b 140
AurelienBernier 2:ea61e801e81f 141 //Computing angle error and distance towards the target value
AurelienBernier 4:8c56c3ba6e54 142 rho += dt*(-kRho*cos(alpha)*rho);
AurelienBernier 4:8c56c3ba6e54 143 temp = alpha;
AurelienBernier 6:afde4b08166b 144 alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta);
AurelienBernier 6:afde4b08166b 145 beta += dt*(-kRho*sin(temp));
AurelienBernier 6:afde4b08166b 146 pc.printf("\n\r d2=%f", d2);
AurelienBernier 6:afde4b08166b 147 pc.printf("\n\r dt=%f", dt);
geotsam 0:8bffb51cc345 148
AurelienBernier 2:ea61e801e81f 149 //Computing linear and angular velocities
AurelienBernier 4:8c56c3ba6e54 150 if(alpha>=-1.5708 && alpha<=1.5708){
AurelienBernier 4:8c56c3ba6e54 151 linear=kRho*rho;
AurelienBernier 4:8c56c3ba6e54 152 angular=ka*alpha+kb*beta;
geotsam 3:1e0f4cb93eda 153 }
geotsam 3:1e0f4cb93eda 154 else{
AurelienBernier 4:8c56c3ba6e54 155 linear=-kRho*rho;
AurelienBernier 4:8c56c3ba6e54 156 angular=-ka*alpha-kb*beta;
geotsam 3:1e0f4cb93eda 157 }
geotsam 0:8bffb51cc345 158 angular_left=(linear-0.5*b*angular)/r;
geotsam 0:8bffb51cc345 159 angular_right=(linear+0.5*b*angular)/r;
geotsam 0:8bffb51cc345 160
AurelienBernier 2:ea61e801e81f 161 //Slowing down at the end for more precision
AurelienBernier 6:afde4b08166b 162 if (d2<25) {
AurelienBernier 6:afde4b08166b 163 speed = d2*30;
geotsam 0:8bffb51cc345 164 }
AurelienBernier 2:ea61e801e81f 165
AurelienBernier 2:ea61e801e81f 166 //Normalize speed for motors
geotsam 0:8bffb51cc345 167 if(angular_left>angular_right) {
geotsam 0:8bffb51cc345 168 angular_right=speed*angular_right/angular_left;
geotsam 0:8bffb51cc345 169 angular_left=speed;
geotsam 0:8bffb51cc345 170 } else {
geotsam 0:8bffb51cc345 171 angular_left=speed*angular_left/angular_right;
geotsam 0:8bffb51cc345 172 angular_right=speed;
geotsam 0:8bffb51cc345 173 }
geotsam 0:8bffb51cc345 174
geotsam 0:8bffb51cc345 175 pc.printf("\n\r X=%f", X);
geotsam 0:8bffb51cc345 176 pc.printf("\n\r Y=%f", Y);
AurelienBernier 15:44ab4626f1ad 177 pc.printf("\n\r leftMm=%f", leftMm);
AurelienBernier 15:44ab4626f1ad 178 pc.printf("\n\r frontMm=%f", frontMm);
AurelienBernier 15:44ab4626f1ad 179 pc.printf("\n\r rightMm=%f", rightMm);
geotsam 0:8bffb51cc345 180
AurelienBernier 2:ea61e801e81f 181 //Updating motor velocities
AurelienBernier 1:f0807d5c5a4b 182 leftMotor(1,angular_left);
AurelienBernier 1:f0807d5c5a4b 183 rightMotor(1,angular_right);
geotsam 0:8bffb51cc345 184
AurelienBernier 7:c94070f9af78 185 wait(0.2);
AurelienBernier 6:afde4b08166b 186 //Timer stuff
AurelienBernier 6:afde4b08166b 187 t.stop();
AurelienBernier 11:e641aa08c92e 188 } while(d2>1 & !tooClose);
AurelienBernier 8:109314be5b68 189
AurelienBernier 19:dbc5fbad4975 190 if (tooClose) {
AurelienBernier 19:dbc5fbad4975 191 computeObstacle();
AurelienBernier 19:dbc5fbad4975 192 }
AurelienBernier 8:109314be5b68 193 return 0;
AurelienBernier 6:afde4b08166b 194 }