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@3:37345c109dfc, 2017-07-06 (annotated)

Committer:
Ludwigfr
Date:
Thu Jul 06 11:36:18 2017 +0000
Revision:
3:37345c109dfc
Parent:
2:11cd5173aa36
Child:
5:19f24c363418
test that

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 3:37345c109dfc 6 MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld, float widthRealMap, float heightRealMap):map(widthRealMap,heightRealMap,12,8),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 3:37345c109dfc 27 this->rangeForce=50;
Ludwigfr 3:37345c109dfc 28 this->attractionConstantForce=200;
Ludwigfr 3:37345c109dfc 29 this->repulsionConstantForce=-40000;
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 3:37345c109dfc 123 while(1)
Ludwigfr 3:37345c109dfc 124 this->print_map_with_robot_position();
Ludwigfr 2:11cd5173aa36 125 }
Ludwigfr 2:11cd5173aa36 126
Ludwigfr 0:9f7ee7ed13e4 127 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 0:9f7ee7ed13e4 128 void MiniExplorerCoimbra::randomize_and_map() {
Ludwigfr 0:9f7ee7ed13e4 129 //TODO check that it's aurelien's work
Ludwigfr 2:11cd5173aa36 130 float movementOnX=rand()%(int)(this->map.widthRealMap);
Ludwigfr 2:11cd5173aa36 131 float movementOnY=rand()%(int)(this->map.heightRealMap);
Ludwigfr 3:37345c109dfc 132
Ludwigfr 3:37345c109dfc 133 float targetXWorld = movementOnX;
Ludwigfr 3:37345c109dfc 134 float targetYWorld = movementOnY;
Ludwigfr 3:37345c109dfc 135 float targetAngleWorld = 2*((float)(rand()%31416)-15708)/10000.0;
Ludwigfr 0:9f7ee7ed13e4 136
Ludwigfr 3:37345c109dfc 137 //target between (0,0) and (widthRealMap,heightRealMap)
Ludwigfr 0:9f7ee7ed13e4 138 this->go_to_point_with_angle(targetXWorld, targetYWorld, targetAngleWorld);
Ludwigfr 0:9f7ee7ed13e4 139 }
Ludwigfr 0:9f7ee7ed13e4 140
Ludwigfr 2:11cd5173aa36 141 void MiniExplorerCoimbra::test_sonars_and_map(int nbIteration){
Ludwigfr 2:11cd5173aa36 142 float leftMm;
Ludwigfr 2:11cd5173aa36 143 float frontMm;
Ludwigfr 2:11cd5173aa36 144 float rightMm;
Ludwigfr 2:11cd5173aa36 145 this->myOdometria();
Ludwigfr 2:11cd5173aa36 146 this->print_map_with_robot_position();
Ludwigfr 2:11cd5173aa36 147 for(int i=0;i<nbIteration;i++){
Ludwigfr 2:11cd5173aa36 148 leftMm = get_distance_left_sensor();
Ludwigfr 2:11cd5173aa36 149 frontMm = get_distance_front_sensor();
Ludwigfr 2:11cd5173aa36 150 rightMm = get_distance_right_sensor();
Ludwigfr 3:37345c109dfc 151 pc.printf("\n\r 1 leftMm= %f",leftMm);
Ludwigfr 3:37345c109dfc 152 pc.printf("\n\r 1 frontMm= %f",frontMm);
Ludwigfr 3:37345c109dfc 153 pc.printf("\n\r 1 rightMm= %f",rightMm);
Ludwigfr 2:11cd5173aa36 154 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 2:11cd5173aa36 155 this->print_map_with_robot_position();
Ludwigfr 3:37345c109dfc 156 wait(1);
Ludwigfr 2:11cd5173aa36 157 }
Ludwigfr 2:11cd5173aa36 158 }
Ludwigfr 2:11cd5173aa36 159
Ludwigfr 2:11cd5173aa36 160
Ludwigfr 2:11cd5173aa36 161 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 0:9f7ee7ed13e4 162 //move of targetXWorld and targetYWorld ending in a targetAngleWorld
Ludwigfr 0:9f7ee7ed13e4 163 void MiniExplorerCoimbra::go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld) {
Ludwigfr 0:9f7ee7ed13e4 164 bool keepGoing=true;
Ludwigfr 0:9f7ee7ed13e4 165 float leftMm;
Ludwigfr 0:9f7ee7ed13e4 166 float frontMm;
Ludwigfr 0:9f7ee7ed13e4 167 float rightMm;
Ludwigfr 0:9f7ee7ed13e4 168 float dt;
Ludwigfr 0:9f7ee7ed13e4 169 Timer t;
Ludwigfr 0:9f7ee7ed13e4 170 float distanceToTarget;
Ludwigfr 0:9f7ee7ed13e4 171 do {
Ludwigfr 0:9f7ee7ed13e4 172 //Timer stuff
Ludwigfr 0:9f7ee7ed13e4 173 dt = t.read();
Ludwigfr 0:9f7ee7ed13e4 174 t.reset();
Ludwigfr 0:9f7ee7ed13e4 175 t.start();
Ludwigfr 0:9f7ee7ed13e4 176
Ludwigfr 0:9f7ee7ed13e4 177 //Updating X,Y and theta with the odometry values
Ludwigfr 0:9f7ee7ed13e4 178 this->myOdometria();
Ludwigfr 2:11cd5173aa36 179 leftMm = get_distance_left_sensor();
Ludwigfr 2:11cd5173aa36 180 frontMm = get_distance_front_sensor();
Ludwigfr 2:11cd5173aa36 181 rightMm = get_distance_right_sensor();
Ludwigfr 2:11cd5173aa36 182 //if in dangerzone 150 mm
Ludwigfr 0:9f7ee7ed13e4 183 if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){
Ludwigfr 2:11cd5173aa36 184 //stop motors
Ludwigfr 0:9f7ee7ed13e4 185 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 186 rightMotor(1,0);
Ludwigfr 2:11cd5173aa36 187 //update the map
Ludwigfr 0:9f7ee7ed13e4 188 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 0:9f7ee7ed13e4 189 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 190 keepGoing=false;
Ludwigfr 2:11cd5173aa36 191 this->do_half_flip();
Ludwigfr 0:9f7ee7ed13e4 192 }else{
Ludwigfr 0:9f7ee7ed13e4 193 //if not in danger zone continue as usual
Ludwigfr 0:9f7ee7ed13e4 194 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 2:11cd5173aa36 195 //Updating motor velocities
Ludwigfr 0:9f7ee7ed13e4 196 distanceToTarget=this->update_angular_speed_wheels_go_to_point_with_angle(targetXWorld,targetYWorld,targetAngleWorld,dt);
Ludwigfr 0:9f7ee7ed13e4 197 wait(0.2);
Ludwigfr 0:9f7ee7ed13e4 198 //Timer stuff
Ludwigfr 0:9f7ee7ed13e4 199 t.stop();
Ludwigfr 2:11cd5173aa36 200 pc.printf("\n\rdist to target= %f",distanceToTarget);
Ludwigfr 0:9f7ee7ed13e4 201 }
Ludwigfr 3:37345c109dfc 202 } while((distanceToTarget>2 || (abs(targetAngleWorld-this->thetaWorld)>PI/3)) && keepGoing);
Ludwigfr 0:9f7ee7ed13e4 203
Ludwigfr 0:9f7ee7ed13e4 204 //Stop at the end
Ludwigfr 0:9f7ee7ed13e4 205 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 206 rightMotor(1,0);
Ludwigfr 2:11cd5173aa36 207 pc.printf("\r\nReached Target!");
Ludwigfr 0:9f7ee7ed13e4 208 }
Ludwigfr 0:9f7ee7ed13e4 209
geotsam 1:20f48907c726 210 //move of targetXWorld and targetYWorld ending in a targetAngleWorld
geotsam 1:20f48907c726 211 void MiniExplorerCoimbra::go_to_point_with_angle_first_lab(float targetXWorld, float targetYWorld, float targetAngleWorld) {
geotsam 1:20f48907c726 212 float dt;
geotsam 1:20f48907c726 213 Timer t;
geotsam 1:20f48907c726 214 float distanceToTarget;
geotsam 1:20f48907c726 215 do {
geotsam 1:20f48907c726 216 //Timer stuff
geotsam 1:20f48907c726 217 dt = t.read();
geotsam 1:20f48907c726 218 t.reset();
geotsam 1:20f48907c726 219 t.start();
geotsam 1:20f48907c726 220
geotsam 1:20f48907c726 221 //Updating X,Y and theta with the odometry values
geotsam 1:20f48907c726 222 this->myOdometria();
geotsam 1:20f48907c726 223
geotsam 1:20f48907c726 224 //Updating motor velocities
geotsam 1:20f48907c726 225 distanceToTarget=this->update_angular_speed_wheels_go_to_point_with_angle(targetXWorld,targetYWorld,targetAngleWorld,dt);
geotsam 1:20f48907c726 226
geotsam 1:20f48907c726 227 wait(0.2);
geotsam 1:20f48907c726 228 //Timer stuff
geotsam 1:20f48907c726 229 t.stop();
geotsam 1:20f48907c726 230 pc.printf("\n\rdist to target= %f",distanceToTarget);
geotsam 1:20f48907c726 231
geotsam 1:20f48907c726 232 } while(distanceToTarget>1 || (abs(targetAngleWorld-this->thetaWorld)>0.1));
geotsam 1:20f48907c726 233
geotsam 1:20f48907c726 234 //Stop at the end
geotsam 1:20f48907c726 235 leftMotor(1,0);
geotsam 1:20f48907c726 236 rightMotor(1,0);
geotsam 1:20f48907c726 237 pc.printf("\r\nReached Target!");
geotsam 1:20f48907c726 238 }
geotsam 1:20f48907c726 239
Ludwigfr 0:9f7ee7ed13e4 240 float MiniExplorerCoimbra::update_angular_speed_wheels_go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld, float dt){
Ludwigfr 0:9f7ee7ed13e4 241 //compute_angles_and_distance
Ludwigfr 0:9f7ee7ed13e4 242 //atan2 take the deplacement on x and the deplacement on y as parameters
Ludwigfr 0:9f7ee7ed13e4 243 float angleToPoint = atan2((targetYWorld-this->yWorld),(targetXWorld-this->xWorld))-this->thetaWorld;
geotsam 1:20f48907c726 244
geotsam 1:20f48907c726 245 if(angleToPoint>PI) angleToPoint=-(angleToPoint-PI);
Ludwigfr 3:37345c109dfc 246 else if(angleToPoint<-PI) angleToPoint=-(angleToPoint+PI);
geotsam 1:20f48907c726 247
Ludwigfr 3:37345c109dfc 248 //rho is the distance to the point of arrival
Ludwigfr 0:9f7ee7ed13e4 249 float rho = dist(targetXWorld,targetYWorld,this->xWorld,this->yWorld);
Ludwigfr 0:9f7ee7ed13e4 250 float distanceToTarget = rho;
Ludwigfr 0:9f7ee7ed13e4 251 //TODO check that
Ludwigfr 0:9f7ee7ed13e4 252 float beta = targetAngleWorld-angleToPoint-this->thetaWorld;
Ludwigfr 0:9f7ee7ed13e4 253
Ludwigfr 0:9f7ee7ed13e4 254 //Computing angle error and distance towards the target value
Ludwigfr 0:9f7ee7ed13e4 255 rho += dt*(-this->khro*cos(angleToPoint)*rho);
Ludwigfr 0:9f7ee7ed13e4 256 float temp = angleToPoint;
Ludwigfr 0:9f7ee7ed13e4 257 angleToPoint += dt*(this->khro*sin(angleToPoint)-this->ka*angleToPoint-this->kb*beta);
Ludwigfr 0:9f7ee7ed13e4 258 beta += dt*(-this->khro*sin(temp));
Ludwigfr 0:9f7ee7ed13e4 259
Ludwigfr 0:9f7ee7ed13e4 260 //Computing linear and angular velocities
Ludwigfr 0:9f7ee7ed13e4 261 float linear;
Ludwigfr 0:9f7ee7ed13e4 262 float angular;
Ludwigfr 0:9f7ee7ed13e4 263 if(angleToPoint>=-1.5708 && angleToPoint<=1.5708){
Ludwigfr 0:9f7ee7ed13e4 264 linear=this->khro*rho;
Ludwigfr 0:9f7ee7ed13e4 265 angular=this->ka*angleToPoint+this->kb*beta;
Ludwigfr 0:9f7ee7ed13e4 266 }
Ludwigfr 0:9f7ee7ed13e4 267 else{
Ludwigfr 0:9f7ee7ed13e4 268 linear=-this->khro*rho;
Ludwigfr 0:9f7ee7ed13e4 269 angular=-this->ka*angleToPoint-this->kb*beta;
Ludwigfr 0:9f7ee7ed13e4 270 }
geotsam 1:20f48907c726 271
geotsam 1:20f48907c726 272 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 273 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 274
geotsam 1:20f48907c726 275 //Slowing down at the end for more precision
geotsam 1:20f48907c726 276 if (distanceToTarget<30) {
geotsam 1:20f48907c726 277 this->speed = distanceToTarget*10;
geotsam 1:20f48907c726 278 }
Ludwigfr 0:9f7ee7ed13e4 279
Ludwigfr 0:9f7ee7ed13e4 280 //Normalize speed for motors
geotsam 1:20f48907c726 281 if(angularLeft>angularRight) {
geotsam 1:20f48907c726 282 angularRight=this->speed*angularRight/angularLeft;
geotsam 1:20f48907c726 283 angularLeft=this->speed;
Ludwigfr 0:9f7ee7ed13e4 284 } else {
geotsam 1:20f48907c726 285 angularLeft=this->speed*angularLeft/angularRight;
geotsam 1:20f48907c726 286 angularRight=this->speed;
Ludwigfr 0:9f7ee7ed13e4 287 }
Ludwigfr 0:9f7ee7ed13e4 288
Ludwigfr 0:9f7ee7ed13e4 289 //compute_linear_angular_velocities
geotsam 1:20f48907c726 290 leftMotor(1,angularLeft);
geotsam 1:20f48907c726 291 rightMotor(1,angularRight);
Ludwigfr 0:9f7ee7ed13e4 292
Ludwigfr 0:9f7ee7ed13e4 293 return distanceToTarget;
Ludwigfr 0:9f7ee7ed13e4 294 }
Ludwigfr 0:9f7ee7ed13e4 295
Ludwigfr 0:9f7ee7ed13e4 296 void MiniExplorerCoimbra::update_sonar_values(float leftMm,float frontMm,float rightMm){
Ludwigfr 0:9f7ee7ed13e4 297 float xWorldCell;
Ludwigfr 0:9f7ee7ed13e4 298 float yWorldCell;
Ludwigfr 3:37345c109dfc 299 float probaLeft;
Ludwigfr 3:37345c109dfc 300 float probaFront;
Ludwigfr 3:37345c109dfc 301 float probaRight;
Ludwigfr 3:37345c109dfc 302 float leftCm=leftMm/10;
Ludwigfr 3:37345c109dfc 303 float frontCm=frontMm/10;
Ludwigfr 3:37345c109dfc 304 float rightCm=rightMm/10;
Ludwigfr 0:9f7ee7ed13e4 305 for(int i=0;i<this->map.nbCellWidth;i++){
Ludwigfr 0:9f7ee7ed13e4 306 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 0:9f7ee7ed13e4 307 xWorldCell=this->map.cell_width_coordinate_to_world(i);
Ludwigfr 0:9f7ee7ed13e4 308 yWorldCell=this->map.cell_height_coordinate_to_world(j);
Ludwigfr 3:37345c109dfc 309
Ludwigfr 3:37345c109dfc 310 probaLeft=this->sonarLeft.compute_probability_t(leftCm,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld);
Ludwigfr 3:37345c109dfc 311 probaFront=this->sonarFront.compute_probability_t(frontCm,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld);
Ludwigfr 3:37345c109dfc 312 probaRight=this->sonarRight.compute_probability_t(rightCm,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld);
Ludwigfr 3:37345c109dfc 313
Ludwigfr 3:37345c109dfc 314 /*
Ludwigfr 3:37345c109dfc 315 pc.printf("\n\r leftCm= %f",leftCm);
Ludwigfr 3:37345c109dfc 316 pc.printf("\n\r frontCm= %f",frontCm);
Ludwigfr 3:37345c109dfc 317 pc.printf("\n\r rightCm= %f",rightCm);
Ludwigfr 3:37345c109dfc 318 */
Ludwigfr 3:37345c109dfc 319 /*
Ludwigfr 3:37345c109dfc 320 pc.printf("\n\r probaLeft= %f",probaLeft);
Ludwigfr 3:37345c109dfc 321 pc.printf("\n\r probaFront= %f",probaFront);
Ludwigfr 3:37345c109dfc 322 pc.printf("\n\r probaRight= %f",probaRight);
Ludwigfr 3:37345c109dfc 323 if(probaLeft> 1 || probaLeft < 0 || probaFront> 1 || probaFront < 0 ||probaRight> 1 || probaRight < 0)){
Ludwigfr 3:37345c109dfc 324 pwm_buzzer.pulsewidth_us(250);
Ludwigfr 3:37345c109dfc 325 wait_ms(50);
Ludwigfr 3:37345c109dfc 326 pwm_buzzer.pulsewidth_us(0);
Ludwigfr 3:37345c109dfc 327 wait(20);
Ludwigfr 3:37345c109dfc 328 pwm_buzzer.pulsewidth_us(250);
Ludwigfr 3:37345c109dfc 329 wait_ms(50);
Ludwigfr 3:37345c109dfc 330 pwm_buzzer.pulsewidth_us(0);
Ludwigfr 3:37345c109dfc 331 }
Ludwigfr 3:37345c109dfc 332 */
Ludwigfr 3:37345c109dfc 333 this->map.update_cell_value(i,j,probaLeft);
Ludwigfr 3:37345c109dfc 334 this->map.update_cell_value(i,j,probaFront);
Ludwigfr 3:37345c109dfc 335 this->map.update_cell_value(i,j,probaRight);
Ludwigfr 0:9f7ee7ed13e4 336 }
Ludwigfr 0:9f7ee7ed13e4 337 }
Ludwigfr 0:9f7ee7ed13e4 338 }
Ludwigfr 0:9f7ee7ed13e4 339
Ludwigfr 0:9f7ee7ed13e4 340 void MiniExplorerCoimbra::do_half_flip(){
Ludwigfr 0:9f7ee7ed13e4 341 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 342 float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 0:9f7ee7ed13e4 343 if(theta_plus_h_pi > PI)
Ludwigfr 0:9f7ee7ed13e4 344 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 0:9f7ee7ed13e4 345 leftMotor(0,100);
Ludwigfr 0:9f7ee7ed13e4 346 rightMotor(1,100);
Ludwigfr 0:9f7ee7ed13e4 347 while(abs(theta_plus_h_pi-theta)>0.05){
Ludwigfr 0:9f7ee7ed13e4 348 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 349 // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
Ludwigfr 0:9f7ee7ed13e4 350 }
Ludwigfr 0:9f7ee7ed13e4 351 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 352 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 353 }
Ludwigfr 0:9f7ee7ed13e4 354
Ludwigfr 0:9f7ee7ed13e4 355 //Distance computation function
Ludwigfr 0:9f7ee7ed13e4 356 float MiniExplorerCoimbra::dist(float x1, float y1, float x2, float y2){
Ludwigfr 0:9f7ee7ed13e4 357 return sqrt(pow(y2-y1,2) + pow(x2-x1,2));
Ludwigfr 0:9f7ee7ed13e4 358 }
Ludwigfr 0:9f7ee7ed13e4 359
Ludwigfr 0:9f7ee7ed13e4 360 //use virtual force field
Ludwigfr 0:9f7ee7ed13e4 361 void MiniExplorerCoimbra::try_to_reach_target(float targetXWorld,float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 362 //atan2 gives the angle beetween PI and -PI
Ludwigfr 0:9f7ee7ed13e4 363 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 364 /*
Ludwigfr 0:9f7ee7ed13e4 365 float deplacementOnXWorld=targetXWorld-this->xWorld;
Ludwigfr 0:9f7ee7ed13e4 366 float deplacementOnYWorld=targetYWorld-this->yWorld;
Ludwigfr 0:9f7ee7ed13e4 367 */
Ludwigfr 3:37345c109dfc 368 //float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
Ludwigfr 3:37345c109dfc 369 //pc.printf("\n angleToTarget=%f",angleToTarget);
Ludwigfr 3:37345c109dfc 370 //turn_to_target(angleToTarget);
Ludwigfr 3:37345c109dfc 371 //TODO IDEA check if maybe set a low max range
Ludwigfr 3:37345c109dfc 372 //this->sonarLeft.setMaxRange(30);
Ludwigfr 3:37345c109dfc 373 //this->sonarFront.setMaxRange(30);
Ludwigfr 3:37345c109dfc 374 //this->sonarRight.setMaxRange(30);
Ludwigfr 0:9f7ee7ed13e4 375 bool reached=false;
Ludwigfr 0:9f7ee7ed13e4 376 int print=0;
Ludwigfr 3:37345c109dfc 377 int printLimit=1000;
Ludwigfr 0:9f7ee7ed13e4 378 while (!reached) {
Ludwigfr 0:9f7ee7ed13e4 379 this->vff(&reached,targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 380 //test_got_to_line(&reached);
Ludwigfr 3:37345c109dfc 381 if(print==printLimit){
Ludwigfr 0:9f7ee7ed13e4 382 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 383 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 384 this->print_map_with_robot_position_and_target(targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 385 print=0;
Ludwigfr 0:9f7ee7ed13e4 386 }else
Ludwigfr 0:9f7ee7ed13e4 387 print+=1;
Ludwigfr 0:9f7ee7ed13e4 388 }
Ludwigfr 0:9f7ee7ed13e4 389 //Stop at the end
Ludwigfr 0:9f7ee7ed13e4 390 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 391 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 392 pc.printf("\r\n target reached");
Ludwigfr 0:9f7ee7ed13e4 393 wait(3);//
Ludwigfr 0:9f7ee7ed13e4 394 }
Ludwigfr 0:9f7ee7ed13e4 395
Ludwigfr 0:9f7ee7ed13e4 396 void MiniExplorerCoimbra::vff(bool* reached, float targetXWorld, float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 397 float line_a;
Ludwigfr 0:9f7ee7ed13e4 398 float line_b;
Ludwigfr 0:9f7ee7ed13e4 399 float line_c;
Ludwigfr 0:9f7ee7ed13e4 400 //Updating X,Y and theta with the odometry values
Ludwigfr 0:9f7ee7ed13e4 401 float forceXWorld=0;
Ludwigfr 0:9f7ee7ed13e4 402 float forceYWorld=0;
Ludwigfr 0:9f7ee7ed13e4 403 //we update the odometrie
Ludwigfr 0:9f7ee7ed13e4 404 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 405 //we check the sensors
Ludwigfr 0:9f7ee7ed13e4 406 float leftMm = get_distance_left_sensor();
Ludwigfr 0:9f7ee7ed13e4 407 float frontMm = get_distance_front_sensor();
Ludwigfr 0:9f7ee7ed13e4 408 float rightMm = get_distance_right_sensor();
Ludwigfr 0:9f7ee7ed13e4 409 //update the probabilities values
Ludwigfr 0:9f7ee7ed13e4 410 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 0:9f7ee7ed13e4 411 //we compute the force on X and Y
Ludwigfr 0:9f7ee7ed13e4 412 this->compute_forceX_and_forceY(&forceXWorld, &forceYWorld,targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 413 //we compute a new ine
Ludwigfr 0:9f7ee7ed13e4 414 this->calculate_line(forceXWorld, forceYWorld, &line_a,&line_b,&line_c);
Ludwigfr 0:9f7ee7ed13e4 415 //Updating motor velocities
Ludwigfr 0:9f7ee7ed13e4 416 this->go_to_line(line_a,line_b,line_c,targetXWorld,targetYWorld);
Ludwigfr 3:37345c109dfc 417
Ludwigfr 0:9f7ee7ed13e4 418 //wait(0.1);
Ludwigfr 0:9f7ee7ed13e4 419 this->myOdometria();
Ludwigfr 3:37345c109dfc 420 if(dist(this->xWorld,this->yWorld,targetXWorld,targetYWorld)<3)
Ludwigfr 0:9f7ee7ed13e4 421 *reached=true;
Ludwigfr 0:9f7ee7ed13e4 422 }
Ludwigfr 0:9f7ee7ed13e4 423
Ludwigfr 0:9f7ee7ed13e4 424 /*angleToTarget is obtained through atan2 so it s:
Ludwigfr 0:9f7ee7ed13e4 425 < 0 if the angle is bettween PI and 2pi on a trigo circle
Ludwigfr 0:9f7ee7ed13e4 426 > 0 if it is between 0 and PI
Ludwigfr 0:9f7ee7ed13e4 427 */
Ludwigfr 0:9f7ee7ed13e4 428 void MiniExplorerCoimbra::turn_to_target(float angleToTarget){
Ludwigfr 0:9f7ee7ed13e4 429 this->myOdometria();
Ludwigfr 3:37345c109dfc 430 if(angleToTarget!=0){
Ludwigfr 3:37345c109dfc 431 if(angleToTarget>0){
Ludwigfr 3:37345c109dfc 432 leftMotor(0,1);
Ludwigfr 3:37345c109dfc 433 rightMotor(1,1);
Ludwigfr 3:37345c109dfc 434 }else{
Ludwigfr 3:37345c109dfc 435 leftMotor(1,1);
Ludwigfr 3:37345c109dfc 436 rightMotor(0,1);
Ludwigfr 3:37345c109dfc 437 }
Ludwigfr 3:37345c109dfc 438 while(abs(angleToTarget-this->thetaWorld)>0.05)
Ludwigfr 3:37345c109dfc 439 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 440 }
Ludwigfr 0:9f7ee7ed13e4 441 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 442 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 443 }
Ludwigfr 0:9f7ee7ed13e4 444
Ludwigfr 0:9f7ee7ed13e4 445
Ludwigfr 0:9f7ee7ed13e4 446 void MiniExplorerCoimbra::print_map_with_robot_position_and_target(float targetXWorld, float targetYWorld) {
Ludwigfr 0:9f7ee7ed13e4 447 float currProba;
Ludwigfr 0:9f7ee7ed13e4 448
Ludwigfr 0:9f7ee7ed13e4 449 float heightIndiceInOrthonormal;
Ludwigfr 0:9f7ee7ed13e4 450 float widthIndiceInOrthonormal;
Ludwigfr 0:9f7ee7ed13e4 451
Ludwigfr 0:9f7ee7ed13e4 452 float widthMalus=-(3*this->map.sizeCellWidth/2);
Ludwigfr 0:9f7ee7ed13e4 453 float widthBonus=this->map.sizeCellWidth/2;
Ludwigfr 0:9f7ee7ed13e4 454
Ludwigfr 0:9f7ee7ed13e4 455 float heightMalus=-(3*this->map.sizeCellHeight/2);
Ludwigfr 0:9f7ee7ed13e4 456 float heightBonus=this->map.sizeCellHeight/2;
Ludwigfr 0:9f7ee7ed13e4 457
Ludwigfr 0:9f7ee7ed13e4 458 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 459 for (int y = this->map.nbCellHeight -1; y>-1; y--) {
Ludwigfr 0:9f7ee7ed13e4 460 for (int x= 0; x<this->map.nbCellWidth; x++) {
Ludwigfr 0:9f7ee7ed13e4 461 heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
Ludwigfr 0:9f7ee7ed13e4 462 widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
Ludwigfr 0:9f7ee7ed13e4 463 if(this->yWorld >= (heightIndiceInOrthonormal+heightMalus) && this->yWorld <= (heightIndiceInOrthonormal+heightBonus) && this->xWorld >= (widthIndiceInOrthonormal+widthMalus) && this->xWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 0:9f7ee7ed13e4 464 pc.printf(" R ");
Ludwigfr 0:9f7ee7ed13e4 465 else{
Ludwigfr 0:9f7ee7ed13e4 466 if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 0:9f7ee7ed13e4 467 pc.printf(" T ");
Ludwigfr 0:9f7ee7ed13e4 468 else{
Ludwigfr 0:9f7ee7ed13e4 469 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
Ludwigfr 0:9f7ee7ed13e4 470 if ( currProba < 0.5)
Ludwigfr 0:9f7ee7ed13e4 471 pc.printf(" ");
Ludwigfr 0:9f7ee7ed13e4 472 else{
Ludwigfr 0:9f7ee7ed13e4 473 if(currProba==0.5)
Ludwigfr 0:9f7ee7ed13e4 474 pc.printf(" . ");
Ludwigfr 0:9f7ee7ed13e4 475 else
Ludwigfr 0:9f7ee7ed13e4 476 pc.printf(" X ");
Ludwigfr 0:9f7ee7ed13e4 477 }
Ludwigfr 0:9f7ee7ed13e4 478 }
Ludwigfr 0:9f7ee7ed13e4 479 }
Ludwigfr 0:9f7ee7ed13e4 480 }
Ludwigfr 0:9f7ee7ed13e4 481 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 482 }
Ludwigfr 0:9f7ee7ed13e4 483 }
Ludwigfr 0:9f7ee7ed13e4 484
Ludwigfr 2:11cd5173aa36 485 void MiniExplorerCoimbra::print_map_with_robot_position(){
Ludwigfr 2:11cd5173aa36 486 float currProba;
Ludwigfr 2:11cd5173aa36 487
Ludwigfr 2:11cd5173aa36 488 float heightIndiceInOrthonormal;
Ludwigfr 2:11cd5173aa36 489 float widthIndiceInOrthonormal;
Ludwigfr 2:11cd5173aa36 490
Ludwigfr 2:11cd5173aa36 491 float widthMalus=-(3*this->map.sizeCellWidth/2);
Ludwigfr 2:11cd5173aa36 492 float widthBonus=this->map.sizeCellWidth/2;
Ludwigfr 2:11cd5173aa36 493
Ludwigfr 2:11cd5173aa36 494 float heightMalus=-(3*this->map.sizeCellHeight/2);
Ludwigfr 2:11cd5173aa36 495 float heightBonus=this->map.sizeCellHeight/2;
Ludwigfr 2:11cd5173aa36 496
Ludwigfr 2:11cd5173aa36 497 pc.printf("\n\r");
Ludwigfr 2:11cd5173aa36 498 for (int y = this->map.nbCellHeight -1; y>-1; y--) {
Ludwigfr 2:11cd5173aa36 499 for (int x= 0; x<this->map.nbCellWidth; x++) {
Ludwigfr 2:11cd5173aa36 500 heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
Ludwigfr 2:11cd5173aa36 501 widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
Ludwigfr 2:11cd5173aa36 502 if(this->yWorld >= (heightIndiceInOrthonormal+heightMalus) && this->yWorld <= (heightIndiceInOrthonormal+heightBonus) && this->xWorld >= (widthIndiceInOrthonormal+widthMalus) && this->xWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 2:11cd5173aa36 503 pc.printf(" R ");
Ludwigfr 2:11cd5173aa36 504 else{
Ludwigfr 2:11cd5173aa36 505 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
Ludwigfr 2:11cd5173aa36 506 if ( currProba < 0.5)
Ludwigfr 2:11cd5173aa36 507 pc.printf(" ");
Ludwigfr 2:11cd5173aa36 508 else{
Ludwigfr 2:11cd5173aa36 509 if(currProba==0.5)
Ludwigfr 2:11cd5173aa36 510 pc.printf(" . ");
Ludwigfr 2:11cd5173aa36 511 else
Ludwigfr 2:11cd5173aa36 512 pc.printf(" X ");
Ludwigfr 2:11cd5173aa36 513 }
Ludwigfr 2:11cd5173aa36 514 }
Ludwigfr 2:11cd5173aa36 515 }
Ludwigfr 2:11cd5173aa36 516 pc.printf("\n\r");
Ludwigfr 2:11cd5173aa36 517 }
Ludwigfr 2:11cd5173aa36 518 }
Ludwigfr 0:9f7ee7ed13e4 519
Ludwigfr 0:9f7ee7ed13e4 520 //robotX and robotY are from this->myOdometria(), calculate line_a, line_b and line_c
Ludwigfr 0:9f7ee7ed13e4 521 void MiniExplorerCoimbra::calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c){
Ludwigfr 0:9f7ee7ed13e4 522 /*
Ludwigfr 0:9f7ee7ed13e4 523 in the teachers maths it is
Ludwigfr 0:9f7ee7ed13e4 524
Ludwigfr 0:9f7ee7ed13e4 525 *line_a=forceY;
Ludwigfr 0:9f7ee7ed13e4 526 *line_b=-forceX;
Ludwigfr 0:9f7ee7ed13e4 527
Ludwigfr 0:9f7ee7ed13e4 528 because a*x+b*y+c=0
Ludwigfr 0:9f7ee7ed13e4 529 a impact the vertical and b the horizontal
Ludwigfr 0:9f7ee7ed13e4 530 and he has to put them like this because
Ludwigfr 0:9f7ee7ed13e4 531 Robot space: World space:
Ludwigfr 0:9f7ee7ed13e4 532 ^ ^
Ludwigfr 0:9f7ee7ed13e4 533 |x |y
Ludwigfr 0:9f7ee7ed13e4 534 <- R O ->
Ludwigfr 0:9f7ee7ed13e4 535 y x
Ludwigfr 0:9f7ee7ed13e4 536 but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to
Ludwigfr 0:9f7ee7ed13e4 537 */
Ludwigfr 3:37345c109dfc 538 //*line_a=forceX;
Ludwigfr 3:37345c109dfc 539 //*line_b=forceY;
Ludwigfr 3:37345c109dfc 540
Ludwigfr 3:37345c109dfc 541 *line_a=forceY;
Ludwigfr 3:37345c109dfc 542 *line_b=-forceX;
Ludwigfr 0:9f7ee7ed13e4 543 //because the line computed always pass by the robot center we dont need lince_c
Ludwigfr 0:9f7ee7ed13e4 544 //*line_c=forceX*this->yWorld+forceY*this->xWorld;
Ludwigfr 0:9f7ee7ed13e4 545 *line_c=0;
Ludwigfr 0:9f7ee7ed13e4 546 }
Ludwigfr 0:9f7ee7ed13e4 547 //compute the force on X and Y
Ludwigfr 0:9f7ee7ed13e4 548 void MiniExplorerCoimbra::compute_forceX_and_forceY(float* forceXWorld, float* forceYWorld, float targetXWorld, float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 549 float forceRepulsionComputedX=0;
Ludwigfr 0:9f7ee7ed13e4 550 float forceRepulsionComputedY=0;
Ludwigfr 0:9f7ee7ed13e4 551 for(int i=0;i<this->map.nbCellWidth;i++){ //for each cell of the map we compute a force of repulsion
Ludwigfr 0:9f7ee7ed13e4 552 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 0:9f7ee7ed13e4 553 this->update_force(i,j,&forceRepulsionComputedX,&forceRepulsionComputedY);
Ludwigfr 0:9f7ee7ed13e4 554 }
Ludwigfr 0:9f7ee7ed13e4 555 }
Ludwigfr 0:9f7ee7ed13e4 556 //update with attraction force
Ludwigfr 3:37345c109dfc 557 *forceXWorld=forceRepulsionComputedX;
Ludwigfr 3:37345c109dfc 558 *forceYWorld=forceRepulsionComputedY;
Ludwigfr 3:37345c109dfc 559 this->print_map_with_robot_position();
Ludwigfr 3:37345c109dfc 560 pc.printf("\r\nForce X before:%f",*forceXWorld);
Ludwigfr 3:37345c109dfc 561 pc.printf("\r\nForce Y before:%f",*forceYWorld);
Ludwigfr 0:9f7ee7ed13e4 562 float distanceTargetRobot=sqrt(pow(targetXWorld-this->xWorld,2)+pow(targetYWorld-this->yWorld,2));
Ludwigfr 0:9f7ee7ed13e4 563 if(distanceTargetRobot != 0){
Ludwigfr 3:37345c109dfc 564 *forceXWorld+=-this->attractionConstantForce*(targetXWorld-this->xWorld)/distanceTargetRobot;
Ludwigfr 3:37345c109dfc 565 *forceYWorld+=-this->attractionConstantForce*(targetYWorld-this->yWorld)/distanceTargetRobot;
Ludwigfr 3:37345c109dfc 566 }else{
Ludwigfr 3:37345c109dfc 567 *forceXWorld+=-this->attractionConstantForce*(targetXWorld-this->xWorld)/0.01;
Ludwigfr 3:37345c109dfc 568 *forceYWorld+=-this->attractionConstantForce*(targetYWorld-this->yWorld)/0.01;
Ludwigfr 0:9f7ee7ed13e4 569 }
Ludwigfr 3:37345c109dfc 570 pc.printf("\r\nForce X after:%f",*forceXWorld);
Ludwigfr 3:37345c109dfc 571 pc.printf("\r\nForce Y after:%f",*forceYWorld);
Ludwigfr 3:37345c109dfc 572
Ludwigfr 0:9f7ee7ed13e4 573 float amplitude=sqrt(pow(*forceXWorld,2)+pow(*forceYWorld,2));
Ludwigfr 0:9f7ee7ed13e4 574 if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0
Ludwigfr 0:9f7ee7ed13e4 575 *forceXWorld=*forceXWorld/amplitude;
Ludwigfr 0:9f7ee7ed13e4 576 *forceYWorld=*forceYWorld/amplitude;
Ludwigfr 3:37345c109dfc 577 }else{
Ludwigfr 3:37345c109dfc 578 *forceXWorld=*forceXWorld/0.01;
Ludwigfr 3:37345c109dfc 579 *forceYWorld=*forceYWorld/0.01;
Ludwigfr 0:9f7ee7ed13e4 580 }
Ludwigfr 0:9f7ee7ed13e4 581 }
Ludwigfr 0:9f7ee7ed13e4 582
Ludwigfr 2:11cd5173aa36 583 //for vff
Ludwigfr 0:9f7ee7ed13e4 584 void MiniExplorerCoimbra::go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 585 float lineAngle;
Ludwigfr 0:9f7ee7ed13e4 586 float angleError;
Ludwigfr 0:9f7ee7ed13e4 587 float linear;
Ludwigfr 0:9f7ee7ed13e4 588 float angular;
Ludwigfr 2:11cd5173aa36 589 float d;
Ludwigfr 0:9f7ee7ed13e4 590
Ludwigfr 0:9f7ee7ed13e4 591 //line angle is beetween pi/2 and -pi/2
Ludwigfr 2:11cd5173aa36 592
Ludwigfr 2:11cd5173aa36 593 if(line_b!=0){
Ludwigfr 2:11cd5173aa36 594 lineAngle=atan(line_a/-line_b);
Ludwigfr 0:9f7ee7ed13e4 595 }
Ludwigfr 0:9f7ee7ed13e4 596 else{
Ludwigfr 3:37345c109dfc 597 lineAngle=0;
Ludwigfr 0:9f7ee7ed13e4 598 }
Ludwigfr 0:9f7ee7ed13e4 599
Ludwigfr 2:11cd5173aa36 600 this->myOdometria();
Ludwigfr 2:11cd5173aa36 601 //Computing angle error
Ludwigfr 2:11cd5173aa36 602 angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
Ludwigfr 2:11cd5173aa36 603 if(angleError>PI)
Ludwigfr 2:11cd5173aa36 604 angleError=-(angleError-PI);
Ludwigfr 2:11cd5173aa36 605 else
Ludwigfr 2:11cd5173aa36 606 if(angleError<-PI)
Ludwigfr 2:11cd5173aa36 607 angleError=-(angleError+PI);
Ludwigfr 2:11cd5173aa36 608
Ludwigfr 3:37345c109dfc 609 //d=this->distFromLine(this->xWorld, this->yWorld, line_a, line_b, line_c);//this could be 0
Ludwigfr 3:37345c109dfc 610 d=0;
Ludwigfr 0:9f7ee7ed13e4 611 //Calculating velocities
Ludwigfr 2:11cd5173aa36 612 linear= this->kv*(3.14);
Ludwigfr 2:11cd5173aa36 613 angular=-this->kd*d + this->kh*angleError;
Ludwigfr 0:9f7ee7ed13e4 614
Ludwigfr 0:9f7ee7ed13e4 615 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 2:11cd5173aa36 616 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 0:9f7ee7ed13e4 617
Ludwigfr 2:11cd5173aa36 618 //Normalize speed for motors
Ludwigfr 0:9f7ee7ed13e4 619 if(abs(angularLeft)>abs(angularRight)) {
Ludwigfr 0:9f7ee7ed13e4 620 angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
Ludwigfr 0:9f7ee7ed13e4 621 angularLeft=this->speed*this->sign1(angularLeft);
Ludwigfr 0:9f7ee7ed13e4 622 }
Ludwigfr 0:9f7ee7ed13e4 623 else {
Ludwigfr 0:9f7ee7ed13e4 624 angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
Ludwigfr 0:9f7ee7ed13e4 625 angularRight=this->speed*this->sign1(angularRight);
Ludwigfr 0:9f7ee7ed13e4 626 }
Ludwigfr 2:11cd5173aa36 627
Ludwigfr 2:11cd5173aa36 628 pc.printf("\r\nd = %f", d);
Ludwigfr 3:37345c109dfc 629 pc.printf("\r\nerror = %f, lineAngle=%f, robotAngle=%f\n", angleError,lineAngle,this->thetaWorld);
Ludwigfr 2:11cd5173aa36 630
Ludwigfr 0:9f7ee7ed13e4 631 leftMotor(this->sign2(angularLeft),abs(angularLeft));
Ludwigfr 0:9f7ee7ed13e4 632 rightMotor(this->sign2(angularRight),abs(angularRight));
Ludwigfr 0:9f7ee7ed13e4 633 }
Ludwigfr 0:9f7ee7ed13e4 634
geotsam 1:20f48907c726 635 void MiniExplorerCoimbra::go_to_line_first_lab(float line_a, float line_b, float line_c){
geotsam 1:20f48907c726 636 float lineAngle;
geotsam 1:20f48907c726 637 float angleError;
geotsam 1:20f48907c726 638 float linear;
geotsam 1:20f48907c726 639 float angular;
geotsam 1:20f48907c726 640 float d;
geotsam 1:20f48907c726 641
geotsam 1:20f48907c726 642 //line angle is beetween pi/2 and -pi/2
geotsam 1:20f48907c726 643
geotsam 1:20f48907c726 644 if(line_b!=0){
geotsam 1:20f48907c726 645 lineAngle=atan(line_a/-line_b);
geotsam 1:20f48907c726 646 }
geotsam 1:20f48907c726 647 else{
geotsam 1:20f48907c726 648 lineAngle=1.5708;
geotsam 1:20f48907c726 649 }
geotsam 1:20f48907c726 650
geotsam 1:20f48907c726 651 do{
geotsam 1:20f48907c726 652 this->myOdometria();
geotsam 1:20f48907c726 653 //Computing angle error
Ludwigfr 3:37345c109dfc 654 pc.printf("\r\nline angle = %f", lineAngle);
Ludwigfr 3:37345c109dfc 655 pc.printf("\r\nthetaWorld = %f", thetaWorld);
geotsam 1:20f48907c726 656 angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
geotsam 1:20f48907c726 657
Ludwigfr 3:37345c109dfc 658 if(angleError>PI)
Ludwigfr 3:37345c109dfc 659 angleError=-(angleError-PI);
Ludwigfr 3:37345c109dfc 660 else
Ludwigfr 3:37345c109dfc 661 if(angleError<-PI)
Ludwigfr 3:37345c109dfc 662 angleError=-(angleError+PI);
geotsam 1:20f48907c726 663
Ludwigfr 3:37345c109dfc 664 pc.printf("\r\nangleError = %f\n", angleError);
geotsam 1:20f48907c726 665 d=this->distFromLine(xWorld, yWorld, line_a, line_b, line_c);
Ludwigfr 3:37345c109dfc 666 pc.printf("\r\ndistance to line = %f", d);
geotsam 1:20f48907c726 667
geotsam 1:20f48907c726 668 //Calculating velocities
geotsam 1:20f48907c726 669 linear= this->kv*(3.14);
geotsam 1:20f48907c726 670 angular=-this->kd*d + this->kh*angleError;
geotsam 1:20f48907c726 671
geotsam 1:20f48907c726 672 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 673 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 674
geotsam 1:20f48907c726 675 //Normalize speed for motors
geotsam 1:20f48907c726 676 if(abs(angularLeft)>abs(angularRight)) {
geotsam 1:20f48907c726 677 angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
geotsam 1:20f48907c726 678 angularLeft=this->speed*this->sign1(angularLeft);
geotsam 1:20f48907c726 679 }
geotsam 1:20f48907c726 680 else {
geotsam 1:20f48907c726 681 angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
geotsam 1:20f48907c726 682 angularRight=this->speed*this->sign1(angularRight);
geotsam 1:20f48907c726 683 }
geotsam 1:20f48907c726 684
geotsam 1:20f48907c726 685 leftMotor(this->sign2(angularLeft),abs(angularLeft));
geotsam 1:20f48907c726 686 rightMotor(this->sign2(angularRight),abs(angularRight));
geotsam 1:20f48907c726 687 }while(1);
geotsam 1:20f48907c726 688 }
geotsam 1:20f48907c726 689
Ludwigfr 0:9f7ee7ed13e4 690 void MiniExplorerCoimbra::update_force(int widthIndice, int heightIndice, float* forceRepulsionComputedX, float* forceRepulsionComputedY ){
Ludwigfr 0:9f7ee7ed13e4 691 //get the coordonate of the map and the robot in the ortonormal space
Ludwigfr 0:9f7ee7ed13e4 692 float xWorldCell=this->map.cell_width_coordinate_to_world(widthIndice);
Ludwigfr 0:9f7ee7ed13e4 693 float yWorldCell=this->map.cell_height_coordinate_to_world(heightIndice);
Ludwigfr 0:9f7ee7ed13e4 694 //compute the distance beetween the cell and the robot
Ludwigfr 0:9f7ee7ed13e4 695 float distanceCellToRobot=sqrt(pow(xWorldCell-this->xWorld,2)+pow(yWorldCell-this->yWorld,2));
Ludwigfr 3:37345c109dfc 696 float probaCell;
Ludwigfr 0:9f7ee7ed13e4 697 //check if the cell is in range
Ludwigfr 3:37345c109dfc 698 float anglePointToRobot=atan2(yWorldCell-this->yWorld,xWorldCell-this->xWorld);//like world system
Ludwigfr 3:37345c109dfc 699 float temp1;
Ludwigfr 3:37345c109dfc 700 float temp2;
Ludwigfr 0:9f7ee7ed13e4 701 if(distanceCellToRobot <= this->rangeForce) {
Ludwigfr 3:37345c109dfc 702 probaCell=this->map.get_proba_cell(widthIndice,heightIndice);
Ludwigfr 3:37345c109dfc 703 pc.printf("\r\nupdate force proba:%f",probaCell);
Ludwigfr 3:37345c109dfc 704 temp1=this->repulsionConstantForce*probaCell/pow(distanceCellToRobot,2);
Ludwigfr 3:37345c109dfc 705 temp2=(xWorldCell-this->xWorld)/distanceCellToRobot;
Ludwigfr 3:37345c109dfc 706 *forceRepulsionComputedX+=temp1*temp2;
Ludwigfr 3:37345c109dfc 707 temp2=(yWorldCell-this->yWorld)/distanceCellToRobot;
Ludwigfr 3:37345c109dfc 708 *forceRepulsionComputedY+=temp1*temp2;
Ludwigfr 0:9f7ee7ed13e4 709 }
Ludwigfr 0:9f7ee7ed13e4 710 }
Ludwigfr 0:9f7ee7ed13e4 711
Ludwigfr 0:9f7ee7ed13e4 712 //return 1 if positiv, -1 if negativ
Ludwigfr 0:9f7ee7ed13e4 713 float MiniExplorerCoimbra::sign1(float value){
Ludwigfr 0:9f7ee7ed13e4 714 if(value>=0)
Ludwigfr 0:9f7ee7ed13e4 715 return 1;
Ludwigfr 0:9f7ee7ed13e4 716 else
Ludwigfr 0:9f7ee7ed13e4 717 return -1;
Ludwigfr 0:9f7ee7ed13e4 718 }
Ludwigfr 0:9f7ee7ed13e4 719
Ludwigfr 0:9f7ee7ed13e4 720 //return 1 if positiv, 0 if negativ
Ludwigfr 0:9f7ee7ed13e4 721 int MiniExplorerCoimbra::sign2(float value){
Ludwigfr 0:9f7ee7ed13e4 722 if(value>=0)
Ludwigfr 0:9f7ee7ed13e4 723 return 1;
Ludwigfr 0:9f7ee7ed13e4 724 else
Ludwigfr 0:9f7ee7ed13e4 725 return 0;
Ludwigfr 0:9f7ee7ed13e4 726 }
Ludwigfr 0:9f7ee7ed13e4 727
geotsam 1:20f48907c726 728 float MiniExplorerCoimbra::distFromLine(float robot_x, float robot_y, float line_a, float line_b, float line_c){
geotsam 1:20f48907c726 729 return abs((line_a*robot_x+line_b*robot_y+line_c)/sqrt(line_a*line_a+line_b*line_b));
geotsam 1:20f48907c726 730 }
geotsam 1:20f48907c726 731