Mapping for TP2

Dependencies:   ISR_Mini-explorer mbed

Fork of GoToPointWithAngle by Georgios Tsamis

Committer:
Ludwigfr
Date:
Sat Apr 29 07:29:30 2017 +0000
Revision:
31:352be78e1aad
I guess this code is fine for putting in the report (I tweaked the variables for a 250x250cm arena and a 20x20cells map

Who changed what in which revision?

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