with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

Committer:
Ludwigfr
Date:
Fri Jun 16 10:40:53 2017 +0000
Revision:
39:890439b495e3
Parent:
20:6a9062d54eb0
last version with the 4th lab;

Who changed what in which revision?

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