Yo Fleyt / Mbed 2 deprecated roboticLab_withclass_3_July

test morning

Dependencies:   ISR_Mini-explorer mbed

Fork of roboticLab_withclass_3_July by Georgios Tsamis

MiniExplorerCoimbra.cpp@2:11cd5173aa36, 2017-07-05 (annotated)

Committer:
Ludwigfr
Date:
Wed Jul 05 08:56:12 2017 +0000
Revision:
2:11cd5173aa36
Parent:
1:20f48907c726
Child:
3:37345c109dfc
test 05 07 9:56

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ludwigfr 0:9f7ee7ed13e4 1 #include "MiniExplorerCoimbra.hpp"
Ludwigfr 0:9f7ee7ed13e4 2 #include "robot.h"
Ludwigfr 0:9f7ee7ed13e4 3
Ludwigfr 0:9f7ee7ed13e4 4 #define PI 3.14159
Ludwigfr 0:9f7ee7ed13e4 5
Ludwigfr 2:11cd5173aa36 6 MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld, float widthRealMap, float heightRealMap):map(widthRealMap,heightRealMap,20,20),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){
Ludwigfr 0:9f7ee7ed13e4 7 i2c1.frequency(100000);
Ludwigfr 0:9f7ee7ed13e4 8 initRobot(); //Initializing the robot
Ludwigfr 0:9f7ee7ed13e4 9 pc.baud(9600); // baud for the pc communication
Ludwigfr 0:9f7ee7ed13e4 10
Ludwigfr 0:9f7ee7ed13e4 11 measure_always_on();//TODO check if needed
Ludwigfr 0:9f7ee7ed13e4 12
Ludwigfr 0:9f7ee7ed13e4 13 this->setXYThetaAndXYThetaWorld(defaultXWorld,defaultYWorld,defaultThetaWorld);
Ludwigfr 0:9f7ee7ed13e4 14 this->radiusWheels=3.25;
Ludwigfr 0:9f7ee7ed13e4 15 this->distanceWheels=7.2;
Ludwigfr 0:9f7ee7ed13e4 16
geotsam 1:20f48907c726 17 this->k_linear=10;
geotsam 1:20f48907c726 18 this->k_angular=200;
Ludwigfr 0:9f7ee7ed13e4 19 this->khro=12;
Ludwigfr 0:9f7ee7ed13e4 20 this->ka=30;
Ludwigfr 0:9f7ee7ed13e4 21 this->kb=-13;
Ludwigfr 0:9f7ee7ed13e4 22 this->kv=200;
Ludwigfr 0:9f7ee7ed13e4 23 this->kh=200;
geotsam 1:20f48907c726 24 this->kd=5;
geotsam 1:20f48907c726 25 this->speed=300;
Ludwigfr 0:9f7ee7ed13e4 26
Ludwigfr 0:9f7ee7ed13e4 27 this->rangeForce=30;
Ludwigfr 0:9f7ee7ed13e4 28 this->attractionConstantForce=10000;
Ludwigfr 0:9f7ee7ed13e4 29 this->repulsionConstantForce=1;
Ludwigfr 0:9f7ee7ed13e4 30 }
Ludwigfr 0:9f7ee7ed13e4 31
Ludwigfr 0:9f7ee7ed13e4 32 void MiniExplorerCoimbra::setXYThetaAndXYThetaWorld(float defaultXWorld, float defaultYWorld, float defaultThetaWorld){
Ludwigfr 0:9f7ee7ed13e4 33 this->xWorld=defaultXWorld;
Ludwigfr 0:9f7ee7ed13e4 34 this->yWorld=defaultYWorld;
Ludwigfr 0:9f7ee7ed13e4 35 this->thetaWorld=defaultThetaWorld;
Ludwigfr 0:9f7ee7ed13e4 36 X=defaultYWorld;
Ludwigfr 0:9f7ee7ed13e4 37 Y=-defaultXWorld;
Ludwigfr 0:9f7ee7ed13e4 38 if(defaultThetaWorld < -PI/2)
Ludwigfr 0:9f7ee7ed13e4 39 theta=PI/2+PI-defaultThetaWorld;
Ludwigfr 0:9f7ee7ed13e4 40 else
Ludwigfr 0:9f7ee7ed13e4 41 theta=defaultThetaWorld-PI/2;
Ludwigfr 0:9f7ee7ed13e4 42 }
Ludwigfr 0:9f7ee7ed13e4 43
Ludwigfr 0:9f7ee7ed13e4 44 void MiniExplorerCoimbra::myOdometria(){
Ludwigfr 0:9f7ee7ed13e4 45 Odometria();
Ludwigfr 0:9f7ee7ed13e4 46 this->xWorld=-Y;
Ludwigfr 0:9f7ee7ed13e4 47 this->yWorld=X;
Ludwigfr 0:9f7ee7ed13e4 48 if(theta >PI/2)
Ludwigfr 0:9f7ee7ed13e4 49 this->thetaWorld=-PI+(theta-PI/2);
Ludwigfr 0:9f7ee7ed13e4 50 else
Ludwigfr 0:9f7ee7ed13e4 51 this->thetaWorld=theta+PI/2;
Ludwigfr 0:9f7ee7ed13e4 52 }
Ludwigfr 0:9f7ee7ed13e4 53
geotsam 1:20f48907c726 54 void MiniExplorerCoimbra::go_to_point(float targetXWorld, float targetYWorld) {
geotsam 1:20f48907c726 55
geotsam 1:20f48907c726 56 float angleError; //angle error
geotsam 1:20f48907c726 57 float d; //distance from target
geotsam 1:20f48907c726 58 float k_linear=10, k_angular=200;
geotsam 1:20f48907c726 59 float angularLeft, angularRight, linear, angular;
geotsam 1:20f48907c726 60 int speed=300;
geotsam 1:20f48907c726 61
geotsam 1:20f48907c726 62 do {
geotsam 1:20f48907c726 63 //Updating X,Y and theta with the odometry values
geotsam 1:20f48907c726 64 this->myOdometria();
geotsam 1:20f48907c726 65
geotsam 1:20f48907c726 66 //Computing angle error and distance towards the target value
geotsam 1:20f48907c726 67 angleError = atan2((targetYWorld-this->yWorld),(targetXWorld-this->xWorld))-this->thetaWorld;
geotsam 1:20f48907c726 68 if(angleError>PI) angleError=-(angleError-PI);
geotsam 1:20f48907c726 69 else if(angleError<-PI) angleError=-(angleError+PI);
geotsam 1:20f48907c726 70 pc.printf("\n\r error=%f",angleError);
geotsam 1:20f48907c726 71
geotsam 1:20f48907c726 72 d=this->dist(this->xWorld, this->yWorld, targetXWorld, targetYWorld);
geotsam 1:20f48907c726 73 pc.printf("\n\r dist=%f/n", d);
geotsam 1:20f48907c726 74
geotsam 1:20f48907c726 75 //Computing linear and angular velocities
geotsam 1:20f48907c726 76 linear=k_linear*d;
geotsam 1:20f48907c726 77 angular=k_angular*angleError;
geotsam 1:20f48907c726 78 angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 79 angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 80
geotsam 1:20f48907c726 81
geotsam 1:20f48907c726 82 //Normalize speed for motors
geotsam 1:20f48907c726 83 if(angularLeft>angularRight) {
geotsam 1:20f48907c726 84 angularRight=speed*angularRight/angularLeft;
geotsam 1:20f48907c726 85 angularLeft=speed;
geotsam 1:20f48907c726 86 } else {
geotsam 1:20f48907c726 87 angularLeft=speed*angularLeft/angularRight;
geotsam 1:20f48907c726 88 angularRight=speed;
geotsam 1:20f48907c726 89 }
geotsam 1:20f48907c726 90
geotsam 1:20f48907c726 91 pc.printf("\n\r X=%f", this->xWorld);
geotsam 1:20f48907c726 92 pc.printf("\n\r Y=%f", this->yWorld);
geotsam 1:20f48907c726 93 pc.printf("\n\r theta=%f", this->thetaWorld);
geotsam 1:20f48907c726 94
geotsam 1:20f48907c726 95 //Updating motor velocities
geotsam 1:20f48907c726 96 if(angularLeft>0){
geotsam 1:20f48907c726 97 leftMotor(1,angularLeft);
geotsam 1:20f48907c726 98 }
geotsam 1:20f48907c726 99 else{
geotsam 1:20f48907c726 100 leftMotor(0,-angularLeft);
geotsam 1:20f48907c726 101 }
geotsam 1:20f48907c726 102
geotsam 1:20f48907c726 103 if(angularRight>0){
geotsam 1:20f48907c726 104 rightMotor(1,angularRight);
geotsam 1:20f48907c726 105 }
geotsam 1:20f48907c726 106 else{
geotsam 1:20f48907c726 107 rightMotor(0,-angularRight);
geotsam 1:20f48907c726 108 }
geotsam 1:20f48907c726 109
geotsam 1:20f48907c726 110 wait(0.5);
geotsam 1:20f48907c726 111 } while(d>1);
geotsam 1:20f48907c726 112
geotsam 1:20f48907c726 113 //Stop at the end
geotsam 1:20f48907c726 114 leftMotor(1,0);
geotsam 1:20f48907c726 115 rightMotor(1,0);
geotsam 1:20f48907c726 116 }
geotsam 1:20f48907c726 117
Ludwigfr 2:11cd5173aa36 118 void MiniExplorerCoimbra::test_procedure_lab2(int nbIteration){
Ludwigfr 2:11cd5173aa36 119 for(int i=0;i<nbIteration;i++){
Ludwigfr 2:11cd5173aa36 120 this->randomize_and_map();
Ludwigfr 2:11cd5173aa36 121 this->print_map_with_robot_position();
Ludwigfr 2:11cd5173aa36 122 }
Ludwigfr 2:11cd5173aa36 123 }
Ludwigfr 2:11cd5173aa36 124
Ludwigfr 0:9f7ee7ed13e4 125 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 0:9f7ee7ed13e4 126 void MiniExplorerCoimbra::randomize_and_map() {
Ludwigfr 0:9f7ee7ed13e4 127 //TODO check that it's aurelien's work
Ludwigfr 2:11cd5173aa36 128 float movementOnX=rand()%(int)(this->map.widthRealMap);
Ludwigfr 2:11cd5173aa36 129 float movementOnY=rand()%(int)(this->map.heightRealMap);
Ludwigfr 0:9f7ee7ed13e4 130
Ludwigfr 0:9f7ee7ed13e4 131 float signOfX=rand()%2;
Ludwigfr 0:9f7ee7ed13e4 132 if(signOfX < 1)
Ludwigfr 0:9f7ee7ed13e4 133 signOfX=-1;
Ludwigfr 0:9f7ee7ed13e4 134 float signOfY=rand()%2;
Ludwigfr 0:9f7ee7ed13e4 135 if(signOfY < 1)
Ludwigfr 0:9f7ee7ed13e4 136 signOfY=-1;
Ludwigfr 2:11cd5173aa36 137
Ludwigfr 0:9f7ee7ed13e4 138 float targetXWorld = movementOnX*signOfX;
Ludwigfr 0:9f7ee7ed13e4 139 float targetYWorld = movementOnY*signOfY;
Ludwigfr 0:9f7ee7ed13e4 140 float targetAngleWorld = 2*((float)(rand()%31416)-15708)/10000.0;
Ludwigfr 0:9f7ee7ed13e4 141 this->go_to_point_with_angle(targetXWorld, targetYWorld, targetAngleWorld);
Ludwigfr 0:9f7ee7ed13e4 142 }
Ludwigfr 0:9f7ee7ed13e4 143
Ludwigfr 2:11cd5173aa36 144 void MiniExplorerCoimbra::test_sonars_and_map(int nbIteration){
Ludwigfr 2:11cd5173aa36 145 float leftMm;
Ludwigfr 2:11cd5173aa36 146 float frontMm;
Ludwigfr 2:11cd5173aa36 147 float rightMm;
Ludwigfr 2:11cd5173aa36 148 this->myOdometria();
Ludwigfr 2:11cd5173aa36 149 this->print_map_with_robot_position();
Ludwigfr 2:11cd5173aa36 150 for(int i=0;i<nbIteration;i++){
Ludwigfr 2:11cd5173aa36 151 leftMm = get_distance_left_sensor();
Ludwigfr 2:11cd5173aa36 152 frontMm = get_distance_front_sensor();
Ludwigfr 2:11cd5173aa36 153 rightMm = get_distance_right_sensor();
Ludwigfr 2:11cd5173aa36 154 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 2:11cd5173aa36 155 this->print_map_with_robot_position();
Ludwigfr 2:11cd5173aa36 156 }
Ludwigfr 2:11cd5173aa36 157 }
Ludwigfr 2:11cd5173aa36 158
Ludwigfr 2:11cd5173aa36 159
Ludwigfr 2:11cd5173aa36 160 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 0:9f7ee7ed13e4 161 //move of targetXWorld and targetYWorld ending in a targetAngleWorld
Ludwigfr 0:9f7ee7ed13e4 162 void MiniExplorerCoimbra::go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld) {
Ludwigfr 0:9f7ee7ed13e4 163 bool keepGoing=true;
Ludwigfr 0:9f7ee7ed13e4 164 float leftMm;
Ludwigfr 0:9f7ee7ed13e4 165 float frontMm;
Ludwigfr 0:9f7ee7ed13e4 166 float rightMm;
Ludwigfr 0:9f7ee7ed13e4 167 float dt;
Ludwigfr 0:9f7ee7ed13e4 168 Timer t;
Ludwigfr 0:9f7ee7ed13e4 169 float distanceToTarget;
Ludwigfr 0:9f7ee7ed13e4 170 do {
Ludwigfr 0:9f7ee7ed13e4 171 //Timer stuff
Ludwigfr 0:9f7ee7ed13e4 172 dt = t.read();
Ludwigfr 0:9f7ee7ed13e4 173 t.reset();
Ludwigfr 0:9f7ee7ed13e4 174 t.start();
Ludwigfr 0:9f7ee7ed13e4 175
Ludwigfr 0:9f7ee7ed13e4 176 //Updating X,Y and theta with the odometry values
Ludwigfr 0:9f7ee7ed13e4 177 this->myOdometria();
Ludwigfr 2:11cd5173aa36 178 leftMm = get_distance_left_sensor();
Ludwigfr 2:11cd5173aa36 179 frontMm = get_distance_front_sensor();
Ludwigfr 2:11cd5173aa36 180 rightMm = get_distance_right_sensor();
Ludwigfr 2:11cd5173aa36 181 //if in dangerzone 150 mm
Ludwigfr 0:9f7ee7ed13e4 182 if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){
Ludwigfr 2:11cd5173aa36 183 //stop motors
Ludwigfr 0:9f7ee7ed13e4 184 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 185 rightMotor(1,0);
Ludwigfr 2:11cd5173aa36 186 //update the map
Ludwigfr 0:9f7ee7ed13e4 187 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 0:9f7ee7ed13e4 188 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 189 keepGoing=false;
Ludwigfr 2:11cd5173aa36 190 this->do_half_flip();
Ludwigfr 0:9f7ee7ed13e4 191 }else{
Ludwigfr 0:9f7ee7ed13e4 192 //if not in danger zone continue as usual
Ludwigfr 0:9f7ee7ed13e4 193 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 2:11cd5173aa36 194 //Updating motor velocities
Ludwigfr 0:9f7ee7ed13e4 195 distanceToTarget=this->update_angular_speed_wheels_go_to_point_with_angle(targetXWorld,targetYWorld,targetAngleWorld,dt);
Ludwigfr 0:9f7ee7ed13e4 196 wait(0.2);
Ludwigfr 0:9f7ee7ed13e4 197 //Timer stuff
Ludwigfr 0:9f7ee7ed13e4 198 t.stop();
Ludwigfr 2:11cd5173aa36 199 pc.printf("\n\rdist to target= %f",distanceToTarget);
Ludwigfr 0:9f7ee7ed13e4 200 }
Ludwigfr 2:11cd5173aa36 201 } while((distanceToTarget>1 || (abs(targetAngleWorld-this->thetaWorld)>0.1)) && keepGoing);
Ludwigfr 0:9f7ee7ed13e4 202
Ludwigfr 0:9f7ee7ed13e4 203 //Stop at the end
Ludwigfr 0:9f7ee7ed13e4 204 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 205 rightMotor(1,0);
Ludwigfr 2:11cd5173aa36 206 pc.printf("\r\nReached Target!");
Ludwigfr 0:9f7ee7ed13e4 207 }
Ludwigfr 0:9f7ee7ed13e4 208
geotsam 1:20f48907c726 209 //move of targetXWorld and targetYWorld ending in a targetAngleWorld
geotsam 1:20f48907c726 210 void MiniExplorerCoimbra::go_to_point_with_angle_first_lab(float targetXWorld, float targetYWorld, float targetAngleWorld) {
geotsam 1:20f48907c726 211 float dt;
geotsam 1:20f48907c726 212 Timer t;
geotsam 1:20f48907c726 213 float distanceToTarget;
geotsam 1:20f48907c726 214 do {
geotsam 1:20f48907c726 215 //Timer stuff
geotsam 1:20f48907c726 216 dt = t.read();
geotsam 1:20f48907c726 217 t.reset();
geotsam 1:20f48907c726 218 t.start();
geotsam 1:20f48907c726 219
geotsam 1:20f48907c726 220 //Updating X,Y and theta with the odometry values
geotsam 1:20f48907c726 221 this->myOdometria();
geotsam 1:20f48907c726 222
geotsam 1:20f48907c726 223 //Updating motor velocities
geotsam 1:20f48907c726 224 distanceToTarget=this->update_angular_speed_wheels_go_to_point_with_angle(targetXWorld,targetYWorld,targetAngleWorld,dt);
geotsam 1:20f48907c726 225
geotsam 1:20f48907c726 226 wait(0.2);
geotsam 1:20f48907c726 227 //Timer stuff
geotsam 1:20f48907c726 228 t.stop();
geotsam 1:20f48907c726 229 pc.printf("\n\rdist to target= %f",distanceToTarget);
geotsam 1:20f48907c726 230
geotsam 1:20f48907c726 231 } while(distanceToTarget>1 || (abs(targetAngleWorld-this->thetaWorld)>0.1));
geotsam 1:20f48907c726 232
geotsam 1:20f48907c726 233 //Stop at the end
geotsam 1:20f48907c726 234 leftMotor(1,0);
geotsam 1:20f48907c726 235 rightMotor(1,0);
geotsam 1:20f48907c726 236 pc.printf("\r\nReached Target!");
geotsam 1:20f48907c726 237 }
geotsam 1:20f48907c726 238
Ludwigfr 0:9f7ee7ed13e4 239 float MiniExplorerCoimbra::update_angular_speed_wheels_go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld, float dt){
Ludwigfr 0:9f7ee7ed13e4 240 //compute_angles_and_distance
Ludwigfr 0:9f7ee7ed13e4 241 //atan2 take the deplacement on x and the deplacement on y as parameters
Ludwigfr 0:9f7ee7ed13e4 242 float angleToPoint = atan2((targetYWorld-this->yWorld),(targetXWorld-this->xWorld))-this->thetaWorld;
geotsam 1:20f48907c726 243
geotsam 1:20f48907c726 244 if(angleToPoint>PI) angleToPoint=-(angleToPoint-PI);
geotsam 1:20f48907c726 245 else if(angleToPoint<-PI) angleToPoint=-(angleToPoint+PI); //rho is the distance to the point of arrival
geotsam 1:20f48907c726 246
Ludwigfr 0:9f7ee7ed13e4 247 float rho = dist(targetXWorld,targetYWorld,this->xWorld,this->yWorld);
Ludwigfr 0:9f7ee7ed13e4 248 float distanceToTarget = rho;
Ludwigfr 0:9f7ee7ed13e4 249 //TODO check that
Ludwigfr 0:9f7ee7ed13e4 250 float beta = targetAngleWorld-angleToPoint-this->thetaWorld;
Ludwigfr 0:9f7ee7ed13e4 251
Ludwigfr 0:9f7ee7ed13e4 252 //Computing angle error and distance towards the target value
Ludwigfr 0:9f7ee7ed13e4 253 rho += dt*(-this->khro*cos(angleToPoint)*rho);
Ludwigfr 0:9f7ee7ed13e4 254 float temp = angleToPoint;
Ludwigfr 0:9f7ee7ed13e4 255 angleToPoint += dt*(this->khro*sin(angleToPoint)-this->ka*angleToPoint-this->kb*beta);
Ludwigfr 0:9f7ee7ed13e4 256 beta += dt*(-this->khro*sin(temp));
Ludwigfr 0:9f7ee7ed13e4 257
Ludwigfr 0:9f7ee7ed13e4 258 //Computing linear and angular velocities
Ludwigfr 0:9f7ee7ed13e4 259 float linear;
Ludwigfr 0:9f7ee7ed13e4 260 float angular;
Ludwigfr 0:9f7ee7ed13e4 261 if(angleToPoint>=-1.5708 && angleToPoint<=1.5708){
Ludwigfr 0:9f7ee7ed13e4 262 linear=this->khro*rho;
Ludwigfr 0:9f7ee7ed13e4 263 angular=this->ka*angleToPoint+this->kb*beta;
Ludwigfr 0:9f7ee7ed13e4 264 }
Ludwigfr 0:9f7ee7ed13e4 265 else{
Ludwigfr 0:9f7ee7ed13e4 266 linear=-this->khro*rho;
Ludwigfr 0:9f7ee7ed13e4 267 angular=-this->ka*angleToPoint-this->kb*beta;
Ludwigfr 0:9f7ee7ed13e4 268 }
geotsam 1:20f48907c726 269
geotsam 1:20f48907c726 270 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 271 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 272
geotsam 1:20f48907c726 273 //Slowing down at the end for more precision
geotsam 1:20f48907c726 274 if (distanceToTarget<30) {
geotsam 1:20f48907c726 275 this->speed = distanceToTarget*10;
geotsam 1:20f48907c726 276 }
Ludwigfr 0:9f7ee7ed13e4 277
Ludwigfr 0:9f7ee7ed13e4 278 //Normalize speed for motors
geotsam 1:20f48907c726 279 if(angularLeft>angularRight) {
geotsam 1:20f48907c726 280 angularRight=this->speed*angularRight/angularLeft;
geotsam 1:20f48907c726 281 angularLeft=this->speed;
Ludwigfr 0:9f7ee7ed13e4 282 } else {
geotsam 1:20f48907c726 283 angularLeft=this->speed*angularLeft/angularRight;
geotsam 1:20f48907c726 284 angularRight=this->speed;
Ludwigfr 0:9f7ee7ed13e4 285 }
Ludwigfr 0:9f7ee7ed13e4 286
Ludwigfr 0:9f7ee7ed13e4 287 //compute_linear_angular_velocities
geotsam 1:20f48907c726 288 leftMotor(1,angularLeft);
geotsam 1:20f48907c726 289 rightMotor(1,angularRight);
Ludwigfr 0:9f7ee7ed13e4 290
Ludwigfr 0:9f7ee7ed13e4 291 return distanceToTarget;
Ludwigfr 0:9f7ee7ed13e4 292 }
Ludwigfr 0:9f7ee7ed13e4 293
Ludwigfr 0:9f7ee7ed13e4 294 void MiniExplorerCoimbra::update_sonar_values(float leftMm,float frontMm,float rightMm){
Ludwigfr 0:9f7ee7ed13e4 295 float xWorldCell;
Ludwigfr 0:9f7ee7ed13e4 296 float yWorldCell;
Ludwigfr 0:9f7ee7ed13e4 297 for(int i=0;i<this->map.nbCellWidth;i++){
Ludwigfr 0:9f7ee7ed13e4 298 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 0:9f7ee7ed13e4 299 xWorldCell=this->map.cell_width_coordinate_to_world(i);
Ludwigfr 0:9f7ee7ed13e4 300 yWorldCell=this->map.cell_height_coordinate_to_world(j);
Ludwigfr 0:9f7ee7ed13e4 301 this->map.update_cell_value(i,j,this->sonarRight.compute_probability_t(rightMm/10,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
Ludwigfr 0:9f7ee7ed13e4 302 this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm/10,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
Ludwigfr 0:9f7ee7ed13e4 303 this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm/10,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
Ludwigfr 0:9f7ee7ed13e4 304
Ludwigfr 0:9f7ee7ed13e4 305 }
Ludwigfr 0:9f7ee7ed13e4 306 }
Ludwigfr 0:9f7ee7ed13e4 307 }
Ludwigfr 0:9f7ee7ed13e4 308
Ludwigfr 0:9f7ee7ed13e4 309 void MiniExplorerCoimbra::do_half_flip(){
Ludwigfr 0:9f7ee7ed13e4 310 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 311 float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 0:9f7ee7ed13e4 312 if(theta_plus_h_pi > PI)
Ludwigfr 0:9f7ee7ed13e4 313 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 0:9f7ee7ed13e4 314 leftMotor(0,100);
Ludwigfr 0:9f7ee7ed13e4 315 rightMotor(1,100);
Ludwigfr 0:9f7ee7ed13e4 316 while(abs(theta_plus_h_pi-theta)>0.05){
Ludwigfr 0:9f7ee7ed13e4 317 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 318 // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
Ludwigfr 0:9f7ee7ed13e4 319 }
Ludwigfr 0:9f7ee7ed13e4 320 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 321 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 322 }
Ludwigfr 0:9f7ee7ed13e4 323
Ludwigfr 0:9f7ee7ed13e4 324 //Distance computation function
Ludwigfr 0:9f7ee7ed13e4 325 float MiniExplorerCoimbra::dist(float x1, float y1, float x2, float y2){
Ludwigfr 0:9f7ee7ed13e4 326 return sqrt(pow(y2-y1,2) + pow(x2-x1,2));
Ludwigfr 0:9f7ee7ed13e4 327 }
Ludwigfr 0:9f7ee7ed13e4 328
Ludwigfr 0:9f7ee7ed13e4 329 //use virtual force field
Ludwigfr 0:9f7ee7ed13e4 330 void MiniExplorerCoimbra::try_to_reach_target(float targetXWorld,float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 331 //atan2 gives the angle beetween PI and -PI
Ludwigfr 0:9f7ee7ed13e4 332 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 333 /*
Ludwigfr 0:9f7ee7ed13e4 334 float deplacementOnXWorld=targetXWorld-this->xWorld;
Ludwigfr 0:9f7ee7ed13e4 335 float deplacementOnYWorld=targetYWorld-this->yWorld;
Ludwigfr 0:9f7ee7ed13e4 336 */
Ludwigfr 0:9f7ee7ed13e4 337 float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
Ludwigfr 0:9f7ee7ed13e4 338 turn_to_target(angleToTarget);
Ludwigfr 0:9f7ee7ed13e4 339 bool reached=false;
Ludwigfr 0:9f7ee7ed13e4 340 int print=0;
Ludwigfr 0:9f7ee7ed13e4 341 while (!reached) {
Ludwigfr 0:9f7ee7ed13e4 342 this->vff(&reached,targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 343 //test_got_to_line(&reached);
Ludwigfr 0:9f7ee7ed13e4 344 if(print==10){
Ludwigfr 0:9f7ee7ed13e4 345 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 346 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 347 this->print_map_with_robot_position_and_target(targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 348 print=0;
Ludwigfr 0:9f7ee7ed13e4 349 }else
Ludwigfr 0:9f7ee7ed13e4 350 print+=1;
Ludwigfr 0:9f7ee7ed13e4 351 }
Ludwigfr 0:9f7ee7ed13e4 352 //Stop at the end
Ludwigfr 0:9f7ee7ed13e4 353 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 354 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 355 pc.printf("\r\n target reached");
Ludwigfr 0:9f7ee7ed13e4 356 wait(3);//
Ludwigfr 0:9f7ee7ed13e4 357 }
Ludwigfr 0:9f7ee7ed13e4 358
Ludwigfr 0:9f7ee7ed13e4 359 void MiniExplorerCoimbra::vff(bool* reached, float targetXWorld, float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 360 float line_a;
Ludwigfr 0:9f7ee7ed13e4 361 float line_b;
Ludwigfr 0:9f7ee7ed13e4 362 float line_c;
Ludwigfr 0:9f7ee7ed13e4 363 //Updating X,Y and theta with the odometry values
Ludwigfr 0:9f7ee7ed13e4 364 float forceXWorld=0;
Ludwigfr 0:9f7ee7ed13e4 365 float forceYWorld=0;
Ludwigfr 0:9f7ee7ed13e4 366 //we update the odometrie
Ludwigfr 0:9f7ee7ed13e4 367 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 368 //we check the sensors
Ludwigfr 0:9f7ee7ed13e4 369 float leftMm = get_distance_left_sensor();
Ludwigfr 0:9f7ee7ed13e4 370 float frontMm = get_distance_front_sensor();
Ludwigfr 0:9f7ee7ed13e4 371 float rightMm = get_distance_right_sensor();
Ludwigfr 0:9f7ee7ed13e4 372 //update the probabilities values
Ludwigfr 0:9f7ee7ed13e4 373 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 0:9f7ee7ed13e4 374 //we compute the force on X and Y
Ludwigfr 0:9f7ee7ed13e4 375 this->compute_forceX_and_forceY(&forceXWorld, &forceYWorld,targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 376 //we compute a new ine
Ludwigfr 0:9f7ee7ed13e4 377 this->calculate_line(forceXWorld, forceYWorld, &line_a,&line_b,&line_c);
Ludwigfr 0:9f7ee7ed13e4 378 //Updating motor velocities
Ludwigfr 0:9f7ee7ed13e4 379 this->go_to_line(line_a,line_b,line_c,targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 380
Ludwigfr 0:9f7ee7ed13e4 381 //wait(0.1);
Ludwigfr 0:9f7ee7ed13e4 382 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 383 if(dist(this->xWorld,this->yWorld,targetXWorld,targetYWorld)<10)
Ludwigfr 0:9f7ee7ed13e4 384 *reached=true;
Ludwigfr 0:9f7ee7ed13e4 385 }
Ludwigfr 0:9f7ee7ed13e4 386
Ludwigfr 0:9f7ee7ed13e4 387 /*angleToTarget is obtained through atan2 so it s:
Ludwigfr 0:9f7ee7ed13e4 388 < 0 if the angle is bettween PI and 2pi on a trigo circle
Ludwigfr 0:9f7ee7ed13e4 389 > 0 if it is between 0 and PI
Ludwigfr 0:9f7ee7ed13e4 390 */
Ludwigfr 0:9f7ee7ed13e4 391 void MiniExplorerCoimbra::turn_to_target(float angleToTarget){
Ludwigfr 0:9f7ee7ed13e4 392 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 393 float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 0:9f7ee7ed13e4 394 if(theta_plus_h_pi > PI)
Ludwigfr 0:9f7ee7ed13e4 395 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 0:9f7ee7ed13e4 396 if(angleToTarget>0){
Ludwigfr 0:9f7ee7ed13e4 397 leftMotor(0,1);
Ludwigfr 0:9f7ee7ed13e4 398 rightMotor(1,1);
Ludwigfr 0:9f7ee7ed13e4 399 }else{
Ludwigfr 0:9f7ee7ed13e4 400 leftMotor(1,1);
Ludwigfr 0:9f7ee7ed13e4 401 rightMotor(0,1);
Ludwigfr 0:9f7ee7ed13e4 402 }
Ludwigfr 0:9f7ee7ed13e4 403 while(abs(angleToTarget-theta_plus_h_pi)>0.05){
Ludwigfr 0:9f7ee7ed13e4 404 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 405 theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 0:9f7ee7ed13e4 406 if(theta_plus_h_pi > PI)
Ludwigfr 0:9f7ee7ed13e4 407 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 0:9f7ee7ed13e4 408 //pc.printf("\n\r diff=%f", abs(angleToTarget-theta_plus_h_pi));
Ludwigfr 0:9f7ee7ed13e4 409 }
Ludwigfr 0:9f7ee7ed13e4 410 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 411 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 412 }
Ludwigfr 0:9f7ee7ed13e4 413
Ludwigfr 0:9f7ee7ed13e4 414
Ludwigfr 0:9f7ee7ed13e4 415 void MiniExplorerCoimbra::print_map_with_robot_position_and_target(float targetXWorld, float targetYWorld) {
Ludwigfr 0:9f7ee7ed13e4 416 float currProba;
Ludwigfr 0:9f7ee7ed13e4 417
Ludwigfr 0:9f7ee7ed13e4 418 float heightIndiceInOrthonormal;
Ludwigfr 0:9f7ee7ed13e4 419 float widthIndiceInOrthonormal;
Ludwigfr 0:9f7ee7ed13e4 420
Ludwigfr 0:9f7ee7ed13e4 421 float widthMalus=-(3*this->map.sizeCellWidth/2);
Ludwigfr 0:9f7ee7ed13e4 422 float widthBonus=this->map.sizeCellWidth/2;
Ludwigfr 0:9f7ee7ed13e4 423
Ludwigfr 0:9f7ee7ed13e4 424 float heightMalus=-(3*this->map.sizeCellHeight/2);
Ludwigfr 0:9f7ee7ed13e4 425 float heightBonus=this->map.sizeCellHeight/2;
Ludwigfr 0:9f7ee7ed13e4 426
Ludwigfr 0:9f7ee7ed13e4 427 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 428 for (int y = this->map.nbCellHeight -1; y>-1; y--) {
Ludwigfr 0:9f7ee7ed13e4 429 for (int x= 0; x<this->map.nbCellWidth; x++) {
Ludwigfr 0:9f7ee7ed13e4 430 heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
Ludwigfr 0:9f7ee7ed13e4 431 widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
Ludwigfr 0:9f7ee7ed13e4 432 if(this->yWorld >= (heightIndiceInOrthonormal+heightMalus) && this->yWorld <= (heightIndiceInOrthonormal+heightBonus) && this->xWorld >= (widthIndiceInOrthonormal+widthMalus) && this->xWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 0:9f7ee7ed13e4 433 pc.printf(" R ");
Ludwigfr 0:9f7ee7ed13e4 434 else{
Ludwigfr 0:9f7ee7ed13e4 435 if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 0:9f7ee7ed13e4 436 pc.printf(" T ");
Ludwigfr 0:9f7ee7ed13e4 437 else{
Ludwigfr 0:9f7ee7ed13e4 438 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
Ludwigfr 0:9f7ee7ed13e4 439 if ( currProba < 0.5)
Ludwigfr 0:9f7ee7ed13e4 440 pc.printf(" ");
Ludwigfr 0:9f7ee7ed13e4 441 else{
Ludwigfr 0:9f7ee7ed13e4 442 if(currProba==0.5)
Ludwigfr 0:9f7ee7ed13e4 443 pc.printf(" . ");
Ludwigfr 0:9f7ee7ed13e4 444 else
Ludwigfr 0:9f7ee7ed13e4 445 pc.printf(" X ");
Ludwigfr 0:9f7ee7ed13e4 446 }
Ludwigfr 0:9f7ee7ed13e4 447 }
Ludwigfr 0:9f7ee7ed13e4 448 }
Ludwigfr 0:9f7ee7ed13e4 449 }
Ludwigfr 0:9f7ee7ed13e4 450 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 451 }
Ludwigfr 0:9f7ee7ed13e4 452 }
Ludwigfr 0:9f7ee7ed13e4 453
Ludwigfr 2:11cd5173aa36 454 void MiniExplorerCoimbra::print_map_with_robot_position(){
Ludwigfr 2:11cd5173aa36 455 float currProba;
Ludwigfr 2:11cd5173aa36 456
Ludwigfr 2:11cd5173aa36 457 float heightIndiceInOrthonormal;
Ludwigfr 2:11cd5173aa36 458 float widthIndiceInOrthonormal;
Ludwigfr 2:11cd5173aa36 459
Ludwigfr 2:11cd5173aa36 460 float widthMalus=-(3*this->map.sizeCellWidth/2);
Ludwigfr 2:11cd5173aa36 461 float widthBonus=this->map.sizeCellWidth/2;
Ludwigfr 2:11cd5173aa36 462
Ludwigfr 2:11cd5173aa36 463 float heightMalus=-(3*this->map.sizeCellHeight/2);
Ludwigfr 2:11cd5173aa36 464 float heightBonus=this->map.sizeCellHeight/2;
Ludwigfr 2:11cd5173aa36 465
Ludwigfr 2:11cd5173aa36 466 pc.printf("\n\r");
Ludwigfr 2:11cd5173aa36 467 for (int y = this->map.nbCellHeight -1; y>-1; y--) {
Ludwigfr 2:11cd5173aa36 468 for (int x= 0; x<this->map.nbCellWidth; x++) {
Ludwigfr 2:11cd5173aa36 469 heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
Ludwigfr 2:11cd5173aa36 470 widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
Ludwigfr 2:11cd5173aa36 471 if(this->yWorld >= (heightIndiceInOrthonormal+heightMalus) && this->yWorld <= (heightIndiceInOrthonormal+heightBonus) && this->xWorld >= (widthIndiceInOrthonormal+widthMalus) && this->xWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 2:11cd5173aa36 472 pc.printf(" R ");
Ludwigfr 2:11cd5173aa36 473 else{
Ludwigfr 2:11cd5173aa36 474 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
Ludwigfr 2:11cd5173aa36 475 if ( currProba < 0.5)
Ludwigfr 2:11cd5173aa36 476 pc.printf(" ");
Ludwigfr 2:11cd5173aa36 477 else{
Ludwigfr 2:11cd5173aa36 478 if(currProba==0.5)
Ludwigfr 2:11cd5173aa36 479 pc.printf(" . ");
Ludwigfr 2:11cd5173aa36 480 else
Ludwigfr 2:11cd5173aa36 481 pc.printf(" X ");
Ludwigfr 2:11cd5173aa36 482 }
Ludwigfr 2:11cd5173aa36 483 }
Ludwigfr 2:11cd5173aa36 484 }
Ludwigfr 2:11cd5173aa36 485 pc.printf("\n\r");
Ludwigfr 2:11cd5173aa36 486 }
Ludwigfr 2:11cd5173aa36 487 }
Ludwigfr 0:9f7ee7ed13e4 488
Ludwigfr 0:9f7ee7ed13e4 489 //robotX and robotY are from this->myOdometria(), calculate line_a, line_b and line_c
Ludwigfr 0:9f7ee7ed13e4 490 void MiniExplorerCoimbra::calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c){
Ludwigfr 0:9f7ee7ed13e4 491 /*
Ludwigfr 0:9f7ee7ed13e4 492 in the teachers maths it is
Ludwigfr 0:9f7ee7ed13e4 493
Ludwigfr 0:9f7ee7ed13e4 494 *line_a=forceY;
Ludwigfr 0:9f7ee7ed13e4 495 *line_b=-forceX;
Ludwigfr 0:9f7ee7ed13e4 496
Ludwigfr 0:9f7ee7ed13e4 497 because a*x+b*y+c=0
Ludwigfr 0:9f7ee7ed13e4 498 a impact the vertical and b the horizontal
Ludwigfr 0:9f7ee7ed13e4 499 and he has to put them like this because
Ludwigfr 0:9f7ee7ed13e4 500 Robot space: World space:
Ludwigfr 0:9f7ee7ed13e4 501 ^ ^
Ludwigfr 0:9f7ee7ed13e4 502 |x |y
Ludwigfr 0:9f7ee7ed13e4 503 <- R O ->
Ludwigfr 0:9f7ee7ed13e4 504 y x
Ludwigfr 0:9f7ee7ed13e4 505 but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to
Ludwigfr 0:9f7ee7ed13e4 506 */
Ludwigfr 0:9f7ee7ed13e4 507 *line_a=forceX;
Ludwigfr 0:9f7ee7ed13e4 508 *line_b=forceY;
Ludwigfr 0:9f7ee7ed13e4 509 //because the line computed always pass by the robot center we dont need lince_c
Ludwigfr 0:9f7ee7ed13e4 510 //*line_c=forceX*this->yWorld+forceY*this->xWorld;
Ludwigfr 0:9f7ee7ed13e4 511 *line_c=0;
Ludwigfr 0:9f7ee7ed13e4 512 }
Ludwigfr 0:9f7ee7ed13e4 513 //compute the force on X and Y
Ludwigfr 0:9f7ee7ed13e4 514 void MiniExplorerCoimbra::compute_forceX_and_forceY(float* forceXWorld, float* forceYWorld, float targetXWorld, float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 515 float forceRepulsionComputedX=0;
Ludwigfr 0:9f7ee7ed13e4 516 float forceRepulsionComputedY=0;
Ludwigfr 0:9f7ee7ed13e4 517 for(int i=0;i<this->map.nbCellWidth;i++){ //for each cell of the map we compute a force of repulsion
Ludwigfr 0:9f7ee7ed13e4 518 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 0:9f7ee7ed13e4 519 this->update_force(i,j,&forceRepulsionComputedX,&forceRepulsionComputedY);
Ludwigfr 0:9f7ee7ed13e4 520 }
Ludwigfr 0:9f7ee7ed13e4 521 }
Ludwigfr 0:9f7ee7ed13e4 522 //update with attraction force
Ludwigfr 0:9f7ee7ed13e4 523 *forceXWorld=+forceRepulsionComputedX;
Ludwigfr 0:9f7ee7ed13e4 524 *forceYWorld=+forceRepulsionComputedY;
Ludwigfr 0:9f7ee7ed13e4 525 float distanceTargetRobot=sqrt(pow(targetXWorld-this->xWorld,2)+pow(targetYWorld-this->yWorld,2));
Ludwigfr 0:9f7ee7ed13e4 526 if(distanceTargetRobot != 0){
Ludwigfr 0:9f7ee7ed13e4 527 *forceXWorld-=this->attractionConstantForce*(targetXWorld-this->xWorld)/distanceTargetRobot;
Ludwigfr 0:9f7ee7ed13e4 528 *forceYWorld-=this->attractionConstantForce*(targetYWorld-this->yWorld)/distanceTargetRobot;
Ludwigfr 0:9f7ee7ed13e4 529 }
Ludwigfr 0:9f7ee7ed13e4 530 float amplitude=sqrt(pow(*forceXWorld,2)+pow(*forceYWorld,2));
Ludwigfr 0:9f7ee7ed13e4 531 if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0
Ludwigfr 0:9f7ee7ed13e4 532 *forceXWorld=*forceXWorld/amplitude;
Ludwigfr 0:9f7ee7ed13e4 533 *forceYWorld=*forceYWorld/amplitude;
Ludwigfr 0:9f7ee7ed13e4 534 }
Ludwigfr 0:9f7ee7ed13e4 535 }
Ludwigfr 0:9f7ee7ed13e4 536
Ludwigfr 2:11cd5173aa36 537 //for vff
Ludwigfr 0:9f7ee7ed13e4 538 void MiniExplorerCoimbra::go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 539 float lineAngle;
Ludwigfr 0:9f7ee7ed13e4 540 float angleError;
Ludwigfr 0:9f7ee7ed13e4 541 float linear;
Ludwigfr 0:9f7ee7ed13e4 542 float angular;
Ludwigfr 2:11cd5173aa36 543 float d;
Ludwigfr 0:9f7ee7ed13e4 544
Ludwigfr 0:9f7ee7ed13e4 545 //line angle is beetween pi/2 and -pi/2
Ludwigfr 2:11cd5173aa36 546
Ludwigfr 2:11cd5173aa36 547 if(line_b!=0){
Ludwigfr 2:11cd5173aa36 548 lineAngle=atan(line_a/-line_b);
Ludwigfr 0:9f7ee7ed13e4 549 }
Ludwigfr 0:9f7ee7ed13e4 550 else{
Ludwigfr 0:9f7ee7ed13e4 551 lineAngle=1.5708;
Ludwigfr 0:9f7ee7ed13e4 552 }
Ludwigfr 0:9f7ee7ed13e4 553
Ludwigfr 2:11cd5173aa36 554 this->myOdometria();
Ludwigfr 2:11cd5173aa36 555 //Computing angle error
Ludwigfr 2:11cd5173aa36 556 angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
Ludwigfr 2:11cd5173aa36 557 if(angleError>PI)
Ludwigfr 2:11cd5173aa36 558 angleError=-(angleError-PI);
Ludwigfr 2:11cd5173aa36 559 else
Ludwigfr 2:11cd5173aa36 560 if(angleError<-PI)
Ludwigfr 2:11cd5173aa36 561 angleError=-(angleError+PI);
Ludwigfr 2:11cd5173aa36 562
Ludwigfr 2:11cd5173aa36 563 d=this->distFromLine(this->xWorld, this->yWorld, line_a, line_b, line_c);//this could be 0
Ludwigfr 0:9f7ee7ed13e4 564
Ludwigfr 0:9f7ee7ed13e4 565 //Calculating velocities
Ludwigfr 2:11cd5173aa36 566 linear= this->kv*(3.14);
Ludwigfr 2:11cd5173aa36 567 angular=-this->kd*d + this->kh*angleError;
Ludwigfr 0:9f7ee7ed13e4 568
Ludwigfr 0:9f7ee7ed13e4 569 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 2:11cd5173aa36 570 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 0:9f7ee7ed13e4 571
Ludwigfr 2:11cd5173aa36 572 //Normalize speed for motors
Ludwigfr 0:9f7ee7ed13e4 573 if(abs(angularLeft)>abs(angularRight)) {
Ludwigfr 0:9f7ee7ed13e4 574 angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
Ludwigfr 0:9f7ee7ed13e4 575 angularLeft=this->speed*this->sign1(angularLeft);
Ludwigfr 0:9f7ee7ed13e4 576 }
Ludwigfr 0:9f7ee7ed13e4 577 else {
Ludwigfr 0:9f7ee7ed13e4 578 angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
Ludwigfr 0:9f7ee7ed13e4 579 angularRight=this->speed*this->sign1(angularRight);
Ludwigfr 0:9f7ee7ed13e4 580 }
Ludwigfr 2:11cd5173aa36 581
Ludwigfr 2:11cd5173aa36 582 pc.printf("\r\nd = %f", d);
Ludwigfr 2:11cd5173aa36 583 pc.printf("\r\nerror = %f\n", angleError);
Ludwigfr 2:11cd5173aa36 584
Ludwigfr 0:9f7ee7ed13e4 585 leftMotor(this->sign2(angularLeft),abs(angularLeft));
Ludwigfr 0:9f7ee7ed13e4 586 rightMotor(this->sign2(angularRight),abs(angularRight));
Ludwigfr 0:9f7ee7ed13e4 587 }
Ludwigfr 0:9f7ee7ed13e4 588
geotsam 1:20f48907c726 589 void MiniExplorerCoimbra::go_to_line_first_lab(float line_a, float line_b, float line_c){
geotsam 1:20f48907c726 590 float lineAngle;
geotsam 1:20f48907c726 591 float angleError;
geotsam 1:20f48907c726 592 float linear;
geotsam 1:20f48907c726 593 float angular;
geotsam 1:20f48907c726 594 float d;
geotsam 1:20f48907c726 595
geotsam 1:20f48907c726 596 //line angle is beetween pi/2 and -pi/2
geotsam 1:20f48907c726 597
geotsam 1:20f48907c726 598 if(line_b!=0){
geotsam 1:20f48907c726 599 lineAngle=atan(line_a/-line_b);
geotsam 1:20f48907c726 600 }
geotsam 1:20f48907c726 601 else{
geotsam 1:20f48907c726 602 lineAngle=1.5708;
geotsam 1:20f48907c726 603 }
geotsam 1:20f48907c726 604
geotsam 1:20f48907c726 605 do{
geotsam 1:20f48907c726 606 this->myOdometria();
geotsam 1:20f48907c726 607 //Computing angle error
geotsam 1:20f48907c726 608 angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
geotsam 1:20f48907c726 609
geotsam 1:20f48907c726 610 if(angleError>PI) angleError=-(angleError-PI);
geotsam 1:20f48907c726 611 else if(angleError<-PI) angleError=-(angleError+PI);
geotsam 1:20f48907c726 612
geotsam 1:20f48907c726 613 d=this->distFromLine(xWorld, yWorld, line_a, line_b, line_c);
geotsam 1:20f48907c726 614
geotsam 1:20f48907c726 615 //Calculating velocities
geotsam 1:20f48907c726 616 linear= this->kv*(3.14);
geotsam 1:20f48907c726 617 angular=-this->kd*d + this->kh*angleError;
geotsam 1:20f48907c726 618
geotsam 1:20f48907c726 619 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 620 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 621
geotsam 1:20f48907c726 622 //Normalize speed for motors
geotsam 1:20f48907c726 623 if(abs(angularLeft)>abs(angularRight)) {
geotsam 1:20f48907c726 624 angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
geotsam 1:20f48907c726 625 angularLeft=this->speed*this->sign1(angularLeft);
geotsam 1:20f48907c726 626 }
geotsam 1:20f48907c726 627 else {
geotsam 1:20f48907c726 628 angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
geotsam 1:20f48907c726 629 angularRight=this->speed*this->sign1(angularRight);
geotsam 1:20f48907c726 630 }
geotsam 1:20f48907c726 631
geotsam 1:20f48907c726 632 pc.printf("\r\nd = %f", d);
geotsam 1:20f48907c726 633 pc.printf("\r\nerror = %f\n", angleError);
geotsam 1:20f48907c726 634
geotsam 1:20f48907c726 635 leftMotor(this->sign2(angularLeft),abs(angularLeft));
geotsam 1:20f48907c726 636 rightMotor(this->sign2(angularRight),abs(angularRight));
geotsam 1:20f48907c726 637 }while(1);
geotsam 1:20f48907c726 638 }
geotsam 1:20f48907c726 639
Ludwigfr 0:9f7ee7ed13e4 640 void MiniExplorerCoimbra::update_force(int widthIndice, int heightIndice, float* forceRepulsionComputedX, float* forceRepulsionComputedY ){
Ludwigfr 0:9f7ee7ed13e4 641 //get the coordonate of the map and the robot in the ortonormal space
Ludwigfr 0:9f7ee7ed13e4 642 float xWorldCell=this->map.cell_width_coordinate_to_world(widthIndice);
Ludwigfr 0:9f7ee7ed13e4 643 float yWorldCell=this->map.cell_height_coordinate_to_world(heightIndice);
Ludwigfr 0:9f7ee7ed13e4 644 //compute the distance beetween the cell and the robot
Ludwigfr 0:9f7ee7ed13e4 645 float distanceCellToRobot=sqrt(pow(xWorldCell-this->xWorld,2)+pow(yWorldCell-this->yWorld,2));
Ludwigfr 0:9f7ee7ed13e4 646 //check if the cell is in range
Ludwigfr 0:9f7ee7ed13e4 647 if(distanceCellToRobot <= this->rangeForce) {
Ludwigfr 0:9f7ee7ed13e4 648 float probaCell=this->map.get_proba_cell(widthIndice,heightIndice);
Ludwigfr 0:9f7ee7ed13e4 649 *forceRepulsionComputedX+=this->repulsionConstantForce*probaCell*(xWorldCell-this->xWorld)/pow(distanceCellToRobot,3);
Ludwigfr 0:9f7ee7ed13e4 650 *forceRepulsionComputedY+=this->repulsionConstantForce*probaCell*(yWorldCell-this->yWorld)/pow(distanceCellToRobot,3);
Ludwigfr 0:9f7ee7ed13e4 651 }
Ludwigfr 0:9f7ee7ed13e4 652 }
Ludwigfr 0:9f7ee7ed13e4 653
Ludwigfr 0:9f7ee7ed13e4 654 //return 1 if positiv, -1 if negativ
Ludwigfr 0:9f7ee7ed13e4 655 float MiniExplorerCoimbra::sign1(float value){
Ludwigfr 0:9f7ee7ed13e4 656 if(value>=0)
Ludwigfr 0:9f7ee7ed13e4 657 return 1;
Ludwigfr 0:9f7ee7ed13e4 658 else
Ludwigfr 0:9f7ee7ed13e4 659 return -1;
Ludwigfr 0:9f7ee7ed13e4 660 }
Ludwigfr 0:9f7ee7ed13e4 661
Ludwigfr 0:9f7ee7ed13e4 662 //return 1 if positiv, 0 if negativ
Ludwigfr 0:9f7ee7ed13e4 663 int MiniExplorerCoimbra::sign2(float value){
Ludwigfr 0:9f7ee7ed13e4 664 if(value>=0)
Ludwigfr 0:9f7ee7ed13e4 665 return 1;
Ludwigfr 0:9f7ee7ed13e4 666 else
Ludwigfr 0:9f7ee7ed13e4 667 return 0;
Ludwigfr 0:9f7ee7ed13e4 668 }
Ludwigfr 0:9f7ee7ed13e4 669
geotsam 1:20f48907c726 670 float MiniExplorerCoimbra::distFromLine(float robot_x, float robot_y, float line_a, float line_b, float line_c){
geotsam 1:20f48907c726 671 return abs((line_a*robot_x+line_b*robot_y+line_c)/sqrt(line_a*line_a+line_b*line_b));
geotsam 1:20f48907c726 672 }
geotsam 1:20f48907c726 673