Yo Fleyt / Mbed 2 deprecated VirtualForces_with_class

with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

main.cpp@21:62154d644531, 2017-04-07 (annotated)

Committer:
AurelienBernier
Date:
Fri Apr 07 15:47:51 2017 +0000
Revision:
21:62154d644531
Parent:
20:6a9062d54eb0
Child:
22:ebb37a249b5f
cleanup;

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