Yo Fleyt / Mbed 2 deprecated VirtualForces_with_class

with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

MiniExplorerCoimbra.cpp@34:c208497dd079, 2017-06-09 (annotated)

Committer:
Ludwigfr
Date:
Fri Jun 09 14:30:21 2017 +0000
Revision:
34:c208497dd079
Parent:
33:814bcd7d3cfe
Child:
35:68f9edbb3cff
okay it compiles

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ludwigfr 33:814bcd7d3cfe 1 #include "MiniExplorerCoimbra.hpp"
Ludwigfr 34:c208497dd079 2 #include "robot.h"
Ludwigfr 33:814bcd7d3cfe 3
Ludwigfr 33:814bcd7d3cfe 4 #define PI 3.14159
Ludwigfr 33:814bcd7d3cfe 5
Ludwigfr 34:c208497dd079 6 MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultX, float defaultY, float defaultTheta):map(200,200,20,20),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){
Ludwigfr 34:c208497dd079 7 i2c1.frequency(100000);
Ludwigfr 33:814bcd7d3cfe 8 initRobot(); //Initializing the robot
Ludwigfr 33:814bcd7d3cfe 9 pc.baud(9600); // baud for the pc communication
Ludwigfr 33:814bcd7d3cfe 10
Ludwigfr 33:814bcd7d3cfe 11 measure_always_on();//TODO check if needed
Ludwigfr 33:814bcd7d3cfe 12
Ludwigfr 34:c208497dd079 13 this->setXYTheta(defaultX,defaultY,defaultTheta);
Ludwigfr 34:c208497dd079 14 this->radiusWheels=3.25;
Ludwigfr 34:c208497dd079 15 this->distanceWheels=7.2;
Ludwigfr 34:c208497dd079 16
Ludwigfr 34:c208497dd079 17 this->khro=12;
Ludwigfr 34:c208497dd079 18 this->ka=30;
Ludwigfr 34:c208497dd079 19 this->kb=-13;
Ludwigfr 34:c208497dd079 20 this->kv=200;
Ludwigfr 34:c208497dd079 21 this->kh=200;
Ludwigfr 33:814bcd7d3cfe 22
Ludwigfr 34:c208497dd079 23 this->rangeForce=30;
Ludwigfr 34:c208497dd079 24 this->attractionConstantForce=10000;
Ludwigfr 34:c208497dd079 25 this->repulsionConstantForce=1;
Ludwigfr 33:814bcd7d3cfe 26 }
Ludwigfr 33:814bcd7d3cfe 27
Ludwigfr 33:814bcd7d3cfe 28 void MiniExplorerCoimbra::setXYTheta(float x, float y, float theta){
Ludwigfr 34:c208497dd079 29 X=x;
Ludwigfr 34:c208497dd079 30 Y=y;
Ludwigfr 34:c208497dd079 31 theta=theta;
Ludwigfr 33:814bcd7d3cfe 32 }
Ludwigfr 33:814bcd7d3cfe 33
Ludwigfr 33:814bcd7d3cfe 34 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 33:814bcd7d3cfe 35 void MiniExplorerCoimbra::randomize_and_map() {
Ludwigfr 33:814bcd7d3cfe 36 //TODO check that it's aurelien's work
Ludwigfr 33:814bcd7d3cfe 37 float movementOnX=rand()%(int)(this->map.widthRealMap/2);
Ludwigfr 33:814bcd7d3cfe 38 float movementOnY=rand()%(int)(this->map.heightRealMap/2);
Ludwigfr 33:814bcd7d3cfe 39
Ludwigfr 33:814bcd7d3cfe 40 float signOfX=rand()%2;
Ludwigfr 33:814bcd7d3cfe 41 if(signOfX < 1)
Ludwigfr 33:814bcd7d3cfe 42 signOfX=-1;
Ludwigfr 33:814bcd7d3cfe 43 float signOfY=rand()%2;
Ludwigfr 33:814bcd7d3cfe 44 if(signOfY < 1)
Ludwigfr 33:814bcd7d3cfe 45 signOfY=-1;
Ludwigfr 33:814bcd7d3cfe 46
Ludwigfr 33:814bcd7d3cfe 47 float targetX = movementOnX*signOfX;
Ludwigfr 33:814bcd7d3cfe 48 float targetY = movementOnY*signOfY;
Ludwigfr 33:814bcd7d3cfe 49 float targetAngle = 2*((float)(rand()%31416)-15708)/10000.0;
Ludwigfr 33:814bcd7d3cfe 50 this->go_to_point_with_angle(targetX, targetY, targetAngle);
Ludwigfr 33:814bcd7d3cfe 51 }
Ludwigfr 33:814bcd7d3cfe 52
Ludwigfr 33:814bcd7d3cfe 53 //go the the given position while updating the map
Ludwigfr 33:814bcd7d3cfe 54 void MiniExplorerCoimbra::go_to_point_with_angle(float targetX, float targetY, float targetAngle) {
Ludwigfr 33:814bcd7d3cfe 55 bool keep_going=true;
Ludwigfr 33:814bcd7d3cfe 56 float leftMm;
Ludwigfr 33:814bcd7d3cfe 57 float frontMm;
Ludwigfr 33:814bcd7d3cfe 58 float rightMm;
Ludwigfr 33:814bcd7d3cfe 59 float dt;
Ludwigfr 33:814bcd7d3cfe 60 Timer t;
Ludwigfr 33:814bcd7d3cfe 61 float d2;
Ludwigfr 33:814bcd7d3cfe 62 do {
Ludwigfr 33:814bcd7d3cfe 63 //Timer stuff
Ludwigfr 33:814bcd7d3cfe 64 dt = t.read();
Ludwigfr 33:814bcd7d3cfe 65 t.reset();
Ludwigfr 33:814bcd7d3cfe 66 t.start();
Ludwigfr 33:814bcd7d3cfe 67
Ludwigfr 33:814bcd7d3cfe 68 //Updating X,Y and theta with the odometry values
Ludwigfr 33:814bcd7d3cfe 69 Odometria();
Ludwigfr 33:814bcd7d3cfe 70 leftMm = get_distance_left_sensor();
Ludwigfr 33:814bcd7d3cfe 71 frontMm = get_distance_front_sensor();
Ludwigfr 33:814bcd7d3cfe 72 rightMm = get_distance_right_sensor();
Ludwigfr 33:814bcd7d3cfe 73
Ludwigfr 33:814bcd7d3cfe 74 //if in dangerzone
Ludwigfr 33:814bcd7d3cfe 75 if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){
Ludwigfr 33:814bcd7d3cfe 76 leftMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 77 rightMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 78 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 33:814bcd7d3cfe 79 //TODO Giorgos maybe you can also test the do_half_flip() function
Ludwigfr 33:814bcd7d3cfe 80 Odometria();
Ludwigfr 33:814bcd7d3cfe 81 //do a flip TODO
Ludwigfr 33:814bcd7d3cfe 82 keep_going=false;
Ludwigfr 33:814bcd7d3cfe 83 this->do_half_flip();
Ludwigfr 33:814bcd7d3cfe 84 }else{
Ludwigfr 33:814bcd7d3cfe 85 //if not in danger zone continue as usual
Ludwigfr 33:814bcd7d3cfe 86 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 33:814bcd7d3cfe 87
Ludwigfr 33:814bcd7d3cfe 88 //Updating motor velocities
Ludwigfr 33:814bcd7d3cfe 89 d2=this->update_angular_speed_wheels_go_to_point_with_angle(targetX,targetY,targetAngle,dt);
Ludwigfr 33:814bcd7d3cfe 90
Ludwigfr 33:814bcd7d3cfe 91 wait(0.2);
Ludwigfr 33:814bcd7d3cfe 92 //Timer stuff
Ludwigfr 33:814bcd7d3cfe 93 t.stop();
Ludwigfr 33:814bcd7d3cfe 94 pc.printf("\n\r dist to target= %f",d2);
Ludwigfr 33:814bcd7d3cfe 95 }
Ludwigfr 33:814bcd7d3cfe 96 } while(d2>1 && (abs(targetAngle-theta)>0.01) && keep_going);
Ludwigfr 33:814bcd7d3cfe 97
Ludwigfr 33:814bcd7d3cfe 98 //Stop at the end
Ludwigfr 33:814bcd7d3cfe 99 leftMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 100 rightMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 101 }
Ludwigfr 33:814bcd7d3cfe 102
Ludwigfr 33:814bcd7d3cfe 103 void MiniExplorerCoimbra::update_sonar_values(float leftMm,float frontMm,float rightMm){
Ludwigfr 33:814bcd7d3cfe 104 float xWorldCell;
Ludwigfr 33:814bcd7d3cfe 105 float yWorldCell;
Ludwigfr 34:c208497dd079 106 float xRobotWorld=this->get_converted_robot_X_into_world();
Ludwigfr 34:c208497dd079 107 float yRobotWorld=this->get_converted_robot_Y_into_world();
Ludwigfr 33:814bcd7d3cfe 108 for(int i=0;i<this->map.nbCellWidth;i++){
Ludwigfr 33:814bcd7d3cfe 109 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 33:814bcd7d3cfe 110 xWorldCell=this->map.cell_width_coordinate_to_world(i);
Ludwigfr 33:814bcd7d3cfe 111 yWorldCell=this->map.cell_height_coordinate_to_world(j);
Ludwigfr 33:814bcd7d3cfe 112 //compute_probability_t(float distanceObstacleDetected, float x, float y, float[2] robotCoordinatesInWorld)
Ludwigfr 34:c208497dd079 113 this->map.update_cell_value(i,j,this->sonarRight.compute_probability_t(rightMm,xWorldCell,yWorldCell,xRobotWorld,yRobotWorld,theta));
Ludwigfr 34:c208497dd079 114 this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm,xWorldCell,yWorldCell,xRobotWorld,yWorldCell,theta));
Ludwigfr 34:c208497dd079 115 this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm,xWorldCell,yWorldCell,xRobotWorld,yWorldCell,theta));
Ludwigfr 33:814bcd7d3cfe 116
Ludwigfr 33:814bcd7d3cfe 117 }
Ludwigfr 33:814bcd7d3cfe 118 }
Ludwigfr 33:814bcd7d3cfe 119 }
Ludwigfr 33:814bcd7d3cfe 120
Ludwigfr 33:814bcd7d3cfe 121 float MiniExplorerCoimbra::get_converted_robot_X_into_world(){
Ludwigfr 33:814bcd7d3cfe 122 //x world coordinate
Ludwigfr 33:814bcd7d3cfe 123 return this->map.nbCellWidth*this->map.sizeCellWidth-Y;
Ludwigfr 33:814bcd7d3cfe 124 }
Ludwigfr 33:814bcd7d3cfe 125
Ludwigfr 33:814bcd7d3cfe 126 float MiniExplorerCoimbra::get_converted_robot_Y_into_world(){
Ludwigfr 33:814bcd7d3cfe 127 //y worldcoordinate
Ludwigfr 33:814bcd7d3cfe 128 return X;
Ludwigfr 33:814bcd7d3cfe 129 }
Ludwigfr 33:814bcd7d3cfe 130
Ludwigfr 33:814bcd7d3cfe 131 void MiniExplorerCoimbra::do_half_flip(){
Ludwigfr 33:814bcd7d3cfe 132 Odometria();
Ludwigfr 33:814bcd7d3cfe 133 float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 33:814bcd7d3cfe 134 if(theta_plus_h_pi > PI)
Ludwigfr 33:814bcd7d3cfe 135 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 33:814bcd7d3cfe 136 leftMotor(0,100);
Ludwigfr 33:814bcd7d3cfe 137 rightMotor(1,100);
Ludwigfr 33:814bcd7d3cfe 138 while(abs(theta_plus_h_pi-theta)>0.05){
Ludwigfr 33:814bcd7d3cfe 139 Odometria();
Ludwigfr 33:814bcd7d3cfe 140 // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
Ludwigfr 33:814bcd7d3cfe 141 }
Ludwigfr 33:814bcd7d3cfe 142 leftMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 143 rightMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 144 }
Ludwigfr 33:814bcd7d3cfe 145
Ludwigfr 33:814bcd7d3cfe 146 //Distance computation function
Ludwigfr 33:814bcd7d3cfe 147 float MiniExplorerCoimbra::dist(float robotX, float robotY, float targetX, float targetY){
Ludwigfr 33:814bcd7d3cfe 148 //pc.printf("%f, %f, %f, %f",robotX,robotY,targetX,targetY);
Ludwigfr 33:814bcd7d3cfe 149 return sqrt(pow(targetY-robotY,2) + pow(targetX-robotX,2));
Ludwigfr 33:814bcd7d3cfe 150 }
Ludwigfr 33:814bcd7d3cfe 151
Ludwigfr 33:814bcd7d3cfe 152 float MiniExplorerCoimbra::update_angular_speed_wheels_go_to_point_with_angle(float targetX, float targetY, float targetAngle, float dt){
Ludwigfr 33:814bcd7d3cfe 153 //compute_angles_and_distance
Ludwigfr 33:814bcd7d3cfe 154 float alpha = atan2((targetY-Y),(targetX-X))-theta;
Ludwigfr 33:814bcd7d3cfe 155 alpha = atan(sin(alpha)/cos(alpha));
Ludwigfr 33:814bcd7d3cfe 156 float rho = this->dist(X, Y, targetX, targetY);
Ludwigfr 33:814bcd7d3cfe 157 float d2 = rho;
Ludwigfr 33:814bcd7d3cfe 158 float beta = -alpha-theta+targetAngle;
Ludwigfr 33:814bcd7d3cfe 159
Ludwigfr 33:814bcd7d3cfe 160 //Computing angle error and distance towards the target value
Ludwigfr 33:814bcd7d3cfe 161 rho += dt*(-this->khro*cos(alpha)*rho);
Ludwigfr 33:814bcd7d3cfe 162 float temp = alpha;
Ludwigfr 33:814bcd7d3cfe 163 alpha += dt*(this->khro*sin(alpha)-this->ka*alpha-this->kb*beta);
Ludwigfr 33:814bcd7d3cfe 164 beta += dt*(-this->khro*sin(temp));
Ludwigfr 33:814bcd7d3cfe 165
Ludwigfr 33:814bcd7d3cfe 166 //Computing linear and angular velocities
Ludwigfr 33:814bcd7d3cfe 167 float linear;
Ludwigfr 33:814bcd7d3cfe 168 float angular;
Ludwigfr 33:814bcd7d3cfe 169 if(alpha>=-1.5708 && alpha<=1.5708){
Ludwigfr 33:814bcd7d3cfe 170 linear=this->khro*rho;
Ludwigfr 33:814bcd7d3cfe 171 angular=this->ka*alpha+this->kb*beta;
Ludwigfr 33:814bcd7d3cfe 172 }
Ludwigfr 33:814bcd7d3cfe 173 else{
Ludwigfr 33:814bcd7d3cfe 174 linear=-this->khro*rho;
Ludwigfr 33:814bcd7d3cfe 175 angular=-this->ka*alpha-this->kb*beta;
Ludwigfr 33:814bcd7d3cfe 176 }
Ludwigfr 33:814bcd7d3cfe 177 float angular_left=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 33:814bcd7d3cfe 178 float angular_right=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 33:814bcd7d3cfe 179
Ludwigfr 33:814bcd7d3cfe 180 //Normalize speed for motors
Ludwigfr 33:814bcd7d3cfe 181 if(angular_left>angular_right) {
Ludwigfr 33:814bcd7d3cfe 182 angular_right=this->speed*angular_right/angular_left;
Ludwigfr 33:814bcd7d3cfe 183 angular_left=this->speed;
Ludwigfr 33:814bcd7d3cfe 184 } else {
Ludwigfr 33:814bcd7d3cfe 185 angular_left=this->speed*angular_left/angular_right;
Ludwigfr 33:814bcd7d3cfe 186 angular_right=this->speed;
Ludwigfr 33:814bcd7d3cfe 187 }
Ludwigfr 33:814bcd7d3cfe 188
Ludwigfr 33:814bcd7d3cfe 189 //compute_linear_angular_velocities
Ludwigfr 33:814bcd7d3cfe 190 leftMotor(1,angular_left);
Ludwigfr 33:814bcd7d3cfe 191 rightMotor(1,angular_right);
Ludwigfr 33:814bcd7d3cfe 192
Ludwigfr 33:814bcd7d3cfe 193 return d2;
Ludwigfr 33:814bcd7d3cfe 194 }
Ludwigfr 33:814bcd7d3cfe 195
Ludwigfr 33:814bcd7d3cfe 196 void MiniExplorerCoimbra::try_to_reach_target(float targetXWorld,float targetYWorld){
Ludwigfr 33:814bcd7d3cfe 197 //atan2 gives the angle beetween PI and -PI
Ludwigfr 33:814bcd7d3cfe 198 float angleToTarget=atan2(targetXWorld,targetYWorld);
Ludwigfr 33:814bcd7d3cfe 199 turn_to_target(angleToTarget);
Ludwigfr 33:814bcd7d3cfe 200 bool reached=false;
Ludwigfr 33:814bcd7d3cfe 201 int print=0;
Ludwigfr 33:814bcd7d3cfe 202 while (!reached) {
Ludwigfr 33:814bcd7d3cfe 203 vff(&reached,targetXWorld,targetYWorld);
Ludwigfr 33:814bcd7d3cfe 204 //test_got_to_line(&reached);
Ludwigfr 33:814bcd7d3cfe 205 if(print==10){
Ludwigfr 33:814bcd7d3cfe 206 leftMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 207 rightMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 208 /*
Ludwigfr 33:814bcd7d3cfe 209 pc.printf("\r\n theta=%f", theta);
Ludwigfr 33:814bcd7d3cfe 210 pc.printf("\r\n IN ORTHO:");
Ludwigfr 33:814bcd7d3cfe 211 pc.printf("\r\n X Robot=%f", this->get_converted_robot_X_into_world());
Ludwigfr 33:814bcd7d3cfe 212 pc.printf("\r\n Y Robot=%f", this->get_converted_robot_Y_into_world());
Ludwigfr 33:814bcd7d3cfe 213 pc.printf("\r\n X Target=%f", targetXWorld);
Ludwigfr 33:814bcd7d3cfe 214 pc.printf("\r\n Y Target=%f", targetYWorld);
Ludwigfr 33:814bcd7d3cfe 215 */
Ludwigfr 34:c208497dd079 216 this->print_final_map_with_robot_position_and_target(X,Y,targetXWorld,targetYWorld);
Ludwigfr 33:814bcd7d3cfe 217 print=0;
Ludwigfr 33:814bcd7d3cfe 218 }else
Ludwigfr 33:814bcd7d3cfe 219 print+=1;
Ludwigfr 33:814bcd7d3cfe 220 }
Ludwigfr 33:814bcd7d3cfe 221 //Stop at the end
Ludwigfr 33:814bcd7d3cfe 222 leftMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 223 rightMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 224 pc.printf("\r\n target reached");
Ludwigfr 33:814bcd7d3cfe 225 wait(3);//
Ludwigfr 33:814bcd7d3cfe 226 }
Ludwigfr 33:814bcd7d3cfe 227
Ludwigfr 33:814bcd7d3cfe 228 void MiniExplorerCoimbra::vff(bool* reached, float targetXWorld, float targetYWorld){
Ludwigfr 33:814bcd7d3cfe 229 float line_a;
Ludwigfr 33:814bcd7d3cfe 230 float line_b;
Ludwigfr 33:814bcd7d3cfe 231 float line_c;
Ludwigfr 33:814bcd7d3cfe 232 //Updating X,Y and theta with the odometry values
Ludwigfr 33:814bcd7d3cfe 233 float forceX=0;
Ludwigfr 33:814bcd7d3cfe 234 float forceY=0;
Ludwigfr 33:814bcd7d3cfe 235 //we update the odometrie
Ludwigfr 33:814bcd7d3cfe 236 Odometria();
Ludwigfr 33:814bcd7d3cfe 237 //we check the sensors
Ludwigfr 33:814bcd7d3cfe 238 float leftMm = get_distance_left_sensor();
Ludwigfr 33:814bcd7d3cfe 239 float frontMm = get_distance_front_sensor();
Ludwigfr 33:814bcd7d3cfe 240 float rightMm = get_distance_right_sensor();
Ludwigfr 33:814bcd7d3cfe 241 //update the probabilities values
Ludwigfr 33:814bcd7d3cfe 242 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 33:814bcd7d3cfe 243 //we compute the force on X and Y
Ludwigfr 33:814bcd7d3cfe 244 this->compute_forceX_and_forceY(&forceX, &forceY,targetXWorld,targetYWorld);
Ludwigfr 33:814bcd7d3cfe 245 //we compute a new ine
Ludwigfr 33:814bcd7d3cfe 246 this->calculate_line(forceX, forceY, &line_a,&line_b,&line_c);
Ludwigfr 33:814bcd7d3cfe 247 //Updating motor velocities
Ludwigfr 33:814bcd7d3cfe 248 this->go_to_line(line_a,line_b,line_c,targetXWorld,targetYWorld);
Ludwigfr 33:814bcd7d3cfe 249
Ludwigfr 33:814bcd7d3cfe 250 //wait(0.1);
Ludwigfr 33:814bcd7d3cfe 251 Odometria();
Ludwigfr 33:814bcd7d3cfe 252 if(dist(this->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world(),targetXWorld,targetYWorld)<10)
Ludwigfr 33:814bcd7d3cfe 253 *reached=true;
Ludwigfr 33:814bcd7d3cfe 254 }
Ludwigfr 33:814bcd7d3cfe 255
Ludwigfr 33:814bcd7d3cfe 256 //compute the force on X and Y
Ludwigfr 33:814bcd7d3cfe 257 void MiniExplorerCoimbra::compute_forceX_and_forceY(float* forceX, float* forceY, float targetXWorld, float targetYWorld){
Ludwigfr 33:814bcd7d3cfe 258 float xRobotWorld=this->get_converted_robot_X_into_world();
Ludwigfr 33:814bcd7d3cfe 259 float yRobotWorld=this->get_converted_robot_Y_into_world();
Ludwigfr 33:814bcd7d3cfe 260
Ludwigfr 33:814bcd7d3cfe 261 float forceRepulsionComputedX=0;
Ludwigfr 33:814bcd7d3cfe 262 float forceRepulsionComputedY=0;
Ludwigfr 33:814bcd7d3cfe 263 //for each cell of the map we compute a force of repulsion
Ludwigfr 33:814bcd7d3cfe 264 for(int i=0;i<this->map.nbCellWidth;i++){
Ludwigfr 33:814bcd7d3cfe 265 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 33:814bcd7d3cfe 266 this->update_force(i,j,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotWorld,yRobotWorld);
Ludwigfr 33:814bcd7d3cfe 267 }
Ludwigfr 33:814bcd7d3cfe 268 }
Ludwigfr 33:814bcd7d3cfe 269 //update with attraction force
Ludwigfr 33:814bcd7d3cfe 270 *forceX=+forceRepulsionComputedX;
Ludwigfr 33:814bcd7d3cfe 271 *forceY=+forceRepulsionComputedY;
Ludwigfr 33:814bcd7d3cfe 272 float distanceTargetRobot=sqrt(pow(targetXWorld-xRobotWorld,2)+pow(targetYWorld-yRobotWorld,2));
Ludwigfr 33:814bcd7d3cfe 273 if(distanceTargetRobot != 0){
Ludwigfr 33:814bcd7d3cfe 274 *forceX-=this->attractionConstantForce*(targetXWorld-xRobotWorld)/distanceTargetRobot;
Ludwigfr 33:814bcd7d3cfe 275 *forceY-=this->attractionConstantForce*(targetYWorld-yRobotWorld)/distanceTargetRobot;
Ludwigfr 33:814bcd7d3cfe 276 }
Ludwigfr 33:814bcd7d3cfe 277 float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2));
Ludwigfr 33:814bcd7d3cfe 278 if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0
Ludwigfr 33:814bcd7d3cfe 279 *forceX=*forceX/amplitude;
Ludwigfr 33:814bcd7d3cfe 280 *forceY=*forceY/amplitude;
Ludwigfr 33:814bcd7d3cfe 281 }
Ludwigfr 33:814bcd7d3cfe 282 }
Ludwigfr 33:814bcd7d3cfe 283
Ludwigfr 33:814bcd7d3cfe 284 void MiniExplorerCoimbra::update_force(int widthIndice, int heightIndice, float* forceX, float* forceY, float xRobotWorld, float yRobotWorld ){
Ludwigfr 33:814bcd7d3cfe 285 //get the coordonate of the map and the robot in the ortonormal space
Ludwigfr 33:814bcd7d3cfe 286 float xWorldCell=this->map.cell_width_coordinate_to_world(widthIndice);
Ludwigfr 33:814bcd7d3cfe 287 float yWorldCell=this->map.cell_height_coordinate_to_world(heightIndice);
Ludwigfr 33:814bcd7d3cfe 288
Ludwigfr 33:814bcd7d3cfe 289 //compute the distance beetween the cell and the robot
Ludwigfr 33:814bcd7d3cfe 290 float distanceCellToRobot=sqrt(pow(xWorldCell-xRobotWorld,2)+pow(yWorldCell-yRobotWorld,2));
Ludwigfr 33:814bcd7d3cfe 291 //check if the cell is in range
Ludwigfr 33:814bcd7d3cfe 292 if(distanceCellToRobot <= this->rangeForce) {
Ludwigfr 33:814bcd7d3cfe 293 float probaCell=this->map.get_proba_cell(widthIndice,heightIndice);
Ludwigfr 33:814bcd7d3cfe 294 float xForceComputed=this->repulsionConstantForce*probaCell*(xWorldCell-xRobotWorld)/pow(distanceCellToRobot,3);
Ludwigfr 33:814bcd7d3cfe 295 float yForceComputed=this->repulsionConstantForce*probaCell*(yWorldCell-yRobotWorld)/pow(distanceCellToRobot,3);
Ludwigfr 33:814bcd7d3cfe 296 *forceX+=xForceComputed;
Ludwigfr 33:814bcd7d3cfe 297 *forceY+=yForceComputed;
Ludwigfr 33:814bcd7d3cfe 298 }
Ludwigfr 33:814bcd7d3cfe 299 }
Ludwigfr 33:814bcd7d3cfe 300
Ludwigfr 33:814bcd7d3cfe 301 //robotX and robotY are from Odometria, calculate line_a, line_b and line_c
Ludwigfr 33:814bcd7d3cfe 302 void MiniExplorerCoimbra::calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c){
Ludwigfr 33:814bcd7d3cfe 303 /*
Ludwigfr 33:814bcd7d3cfe 304 in the teachers maths it is
Ludwigfr 33:814bcd7d3cfe 305
Ludwigfr 33:814bcd7d3cfe 306 *line_a=forceY;
Ludwigfr 33:814bcd7d3cfe 307 *line_b=-forceX;
Ludwigfr 33:814bcd7d3cfe 308
Ludwigfr 33:814bcd7d3cfe 309 because a*x+b*y+c=0
Ludwigfr 33:814bcd7d3cfe 310 a impact the vertical and b the horizontal
Ludwigfr 33:814bcd7d3cfe 311 and he has to put them like this because
Ludwigfr 33:814bcd7d3cfe 312 Robot space: orthonormal space:
Ludwigfr 33:814bcd7d3cfe 313 ^ ^
Ludwigfr 33:814bcd7d3cfe 314 |x |y
Ludwigfr 33:814bcd7d3cfe 315 <- R O ->
Ludwigfr 33:814bcd7d3cfe 316 y x
Ludwigfr 33:814bcd7d3cfe 317 but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to
Ludwigfr 33:814bcd7d3cfe 318 */
Ludwigfr 33:814bcd7d3cfe 319 *line_a=forceX;
Ludwigfr 33:814bcd7d3cfe 320 *line_b=forceY;
Ludwigfr 33:814bcd7d3cfe 321 //because the line computed always pass by the robot center we dont need lince_c
Ludwigfr 33:814bcd7d3cfe 322 //*line_c=forceX*yRobotWorld+forceY*xRobotWorld;
Ludwigfr 33:814bcd7d3cfe 323 *line_c=0;
Ludwigfr 33:814bcd7d3cfe 324 }
Ludwigfr 33:814bcd7d3cfe 325
Ludwigfr 33:814bcd7d3cfe 326 //currently line_c is not used
Ludwigfr 33:814bcd7d3cfe 327 void MiniExplorerCoimbra::go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld){
Ludwigfr 33:814bcd7d3cfe 328 float lineAngle;
Ludwigfr 33:814bcd7d3cfe 329 float angleError;
Ludwigfr 33:814bcd7d3cfe 330 float linear;
Ludwigfr 33:814bcd7d3cfe 331 float angular;
Ludwigfr 33:814bcd7d3cfe 332
Ludwigfr 33:814bcd7d3cfe 333 bool direction=true;
Ludwigfr 33:814bcd7d3cfe 334
Ludwigfr 33:814bcd7d3cfe 335 //take as parameter how much the robot is supposed to move on x and y (world)
Ludwigfr 33:814bcd7d3cfe 336 float angleToTarget=atan2(targetXWorld-this->get_converted_robot_X_into_world(),targetYWorld-this->get_converted_robot_Y_into_world());
Ludwigfr 33:814bcd7d3cfe 337 bool inFront=true;
Ludwigfr 33:814bcd7d3cfe 338 if(angleToTarget < 0)//the target is in front
Ludwigfr 33:814bcd7d3cfe 339 inFront=false;
Ludwigfr 33:814bcd7d3cfe 340
Ludwigfr 33:814bcd7d3cfe 341 if(theta > 0){
Ludwigfr 33:814bcd7d3cfe 342 //the robot is oriented to the left
Ludwigfr 33:814bcd7d3cfe 343 if(theta > PI/2){
Ludwigfr 33:814bcd7d3cfe 344 //the robot is oriented lower left
Ludwigfr 33:814bcd7d3cfe 345 if(inFront)
Ludwigfr 33:814bcd7d3cfe 346 direction=false;//robot and target not oriented the same way
Ludwigfr 33:814bcd7d3cfe 347 }else{
Ludwigfr 33:814bcd7d3cfe 348 //the robot is oriented upper left
Ludwigfr 33:814bcd7d3cfe 349 if(!inFront)
Ludwigfr 33:814bcd7d3cfe 350 direction=false;//robot and target not oriented the same way
Ludwigfr 33:814bcd7d3cfe 351 }
Ludwigfr 33:814bcd7d3cfe 352 }else{
Ludwigfr 33:814bcd7d3cfe 353 //the robot is oriented to the right
Ludwigfr 33:814bcd7d3cfe 354 if(theta < -PI/2){
Ludwigfr 33:814bcd7d3cfe 355 //the robot is oriented lower right
Ludwigfr 33:814bcd7d3cfe 356 if(inFront)
Ludwigfr 33:814bcd7d3cfe 357 direction=false;//robot and target not oriented the same way
Ludwigfr 33:814bcd7d3cfe 358 }else{
Ludwigfr 33:814bcd7d3cfe 359 //the robot is oriented upper right
Ludwigfr 33:814bcd7d3cfe 360 if(!inFront)
Ludwigfr 33:814bcd7d3cfe 361 direction=false;//robot and target not oriented the same way
Ludwigfr 33:814bcd7d3cfe 362 }
Ludwigfr 33:814bcd7d3cfe 363 }
Ludwigfr 33:814bcd7d3cfe 364 //pc.printf("\r\n Target is in front");
Ludwigfr 33:814bcd7d3cfe 365
Ludwigfr 33:814bcd7d3cfe 366 if(line_b!=0){
Ludwigfr 33:814bcd7d3cfe 367 if(!direction)
Ludwigfr 33:814bcd7d3cfe 368 lineAngle=atan(-line_a/line_b);
Ludwigfr 33:814bcd7d3cfe 369 else
Ludwigfr 33:814bcd7d3cfe 370 lineAngle=atan(line_a/-line_b);
Ludwigfr 33:814bcd7d3cfe 371 }
Ludwigfr 33:814bcd7d3cfe 372 else{
Ludwigfr 33:814bcd7d3cfe 373 lineAngle=1.5708;
Ludwigfr 33:814bcd7d3cfe 374 }
Ludwigfr 33:814bcd7d3cfe 375
Ludwigfr 33:814bcd7d3cfe 376 //Computing angle error
Ludwigfr 33:814bcd7d3cfe 377 angleError = lineAngle-theta;
Ludwigfr 33:814bcd7d3cfe 378 angleError = atan(sin(angleError)/cos(angleError));
Ludwigfr 33:814bcd7d3cfe 379
Ludwigfr 33:814bcd7d3cfe 380 //Calculating velocities
Ludwigfr 33:814bcd7d3cfe 381 linear=this->kv*(3.1416);
Ludwigfr 33:814bcd7d3cfe 382 angular=this->kh*angleError;
Ludwigfr 33:814bcd7d3cfe 383
Ludwigfr 33:814bcd7d3cfe 384 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 33:814bcd7d3cfe 385 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 33:814bcd7d3cfe 386
Ludwigfr 33:814bcd7d3cfe 387 //Normalize speed for motors
Ludwigfr 33:814bcd7d3cfe 388 if(abs(angularLeft)>abs(angularRight)) {
Ludwigfr 33:814bcd7d3cfe 389 angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
Ludwigfr 33:814bcd7d3cfe 390 angularLeft=this->speed*this->sign1(angularLeft);
Ludwigfr 33:814bcd7d3cfe 391 }
Ludwigfr 33:814bcd7d3cfe 392 else {
Ludwigfr 33:814bcd7d3cfe 393 angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
Ludwigfr 33:814bcd7d3cfe 394 angularRight=this->speed*this->sign1(angularRight);
Ludwigfr 33:814bcd7d3cfe 395 }
Ludwigfr 33:814bcd7d3cfe 396 leftMotor(this->sign2(angularLeft),abs(angularLeft));
Ludwigfr 33:814bcd7d3cfe 397 rightMotor(this->sign2(angularRight),abs(angularRight));
Ludwigfr 33:814bcd7d3cfe 398 }
Ludwigfr 33:814bcd7d3cfe 399
Ludwigfr 33:814bcd7d3cfe 400 //return 1 if positiv, -1 if negativ
Ludwigfr 33:814bcd7d3cfe 401 float MiniExplorerCoimbra::sign1(float value){
Ludwigfr 33:814bcd7d3cfe 402 if(value>=0)
Ludwigfr 33:814bcd7d3cfe 403 return 1;
Ludwigfr 33:814bcd7d3cfe 404 else
Ludwigfr 33:814bcd7d3cfe 405 return -1;
Ludwigfr 33:814bcd7d3cfe 406 }
Ludwigfr 33:814bcd7d3cfe 407
Ludwigfr 33:814bcd7d3cfe 408 //return 1 if positiv, 0 if negativ
Ludwigfr 33:814bcd7d3cfe 409 int MiniExplorerCoimbra::sign2(float value){
Ludwigfr 33:814bcd7d3cfe 410 if(value>=0)
Ludwigfr 33:814bcd7d3cfe 411 return 1;
Ludwigfr 33:814bcd7d3cfe 412 else
Ludwigfr 33:814bcd7d3cfe 413 return 0;
Ludwigfr 33:814bcd7d3cfe 414 }
Ludwigfr 33:814bcd7d3cfe 415
Ludwigfr 33:814bcd7d3cfe 416 /*angleToTarget is obtained through atan2 so it s:
Ludwigfr 33:814bcd7d3cfe 417 < 0 if the angle is bettween PI and 2pi on a trigo circle
Ludwigfr 33:814bcd7d3cfe 418 > 0 if it is between 0 and PI
Ludwigfr 33:814bcd7d3cfe 419 */
Ludwigfr 33:814bcd7d3cfe 420 void MiniExplorerCoimbra::turn_to_target(float angleToTarget){
Ludwigfr 33:814bcd7d3cfe 421 Odometria();
Ludwigfr 33:814bcd7d3cfe 422 float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 33:814bcd7d3cfe 423 if(theta_plus_h_pi > PI)
Ludwigfr 33:814bcd7d3cfe 424 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 33:814bcd7d3cfe 425 if(angleToTarget>0){
Ludwigfr 33:814bcd7d3cfe 426 leftMotor(0,1);
Ludwigfr 33:814bcd7d3cfe 427 rightMotor(1,1);
Ludwigfr 33:814bcd7d3cfe 428 }else{
Ludwigfr 33:814bcd7d3cfe 429 leftMotor(1,1);
Ludwigfr 33:814bcd7d3cfe 430 rightMotor(0,1);
Ludwigfr 33:814bcd7d3cfe 431 }
Ludwigfr 33:814bcd7d3cfe 432 while(abs(angleToTarget-theta_plus_h_pi)>0.05){
Ludwigfr 33:814bcd7d3cfe 433 Odometria();
Ludwigfr 33:814bcd7d3cfe 434 theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 33:814bcd7d3cfe 435 if(theta_plus_h_pi > PI)
Ludwigfr 33:814bcd7d3cfe 436 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 33:814bcd7d3cfe 437 //pc.printf("\n\r diff=%f", abs(angleToTarget-theta_plus_h_pi));
Ludwigfr 33:814bcd7d3cfe 438 }
Ludwigfr 33:814bcd7d3cfe 439 leftMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 440 rightMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 441 }
Ludwigfr 33:814bcd7d3cfe 442
Ludwigfr 33:814bcd7d3cfe 443
Ludwigfr 33:814bcd7d3cfe 444 /*
Ludwigfr 33:814bcd7d3cfe 445 //x and y passed are TargetNotMap
Ludwigfr 33:814bcd7d3cfe 446 float get_error_angles(float x, float y,float theta){
Ludwigfr 33:814bcd7d3cfe 447 float angleBeetweenRobotAndTarget=atan2(y,x);
Ludwigfr 33:814bcd7d3cfe 448 if(y > 0){
Ludwigfr 33:814bcd7d3cfe 449 if(angleBeetweenRobotAndTarget < PI/2)//up right
Ludwigfr 33:814bcd7d3cfe 450 angleBeetweenRobotAndTarget=-PI/2+angleBeetweenRobotAndTarget;
Ludwigfr 33:814bcd7d3cfe 451 else//up right
Ludwigfr 33:814bcd7d3cfe 452 angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-PI/2;
Ludwigfr 33:814bcd7d3cfe 453 }else{
Ludwigfr 33:814bcd7d3cfe 454 if(angleBeetweenRobotAndTarget > -PI/2)//lower right
Ludwigfr 33:814bcd7d3cfe 455 angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-PI/2;
Ludwigfr 33:814bcd7d3cfe 456 else//lower left
Ludwigfr 33:814bcd7d3cfe 457 angleBeetweenRobotAndTarget=2*PI+angleBeetweenRobotAndTarget-PI/2;
Ludwigfr 33:814bcd7d3cfe 458 }
Ludwigfr 33:814bcd7d3cfe 459 return angleBeetweenRobotAndTarget-theta;
Ludwigfr 33:814bcd7d3cfe 460 }
Ludwigfr 33:814bcd7d3cfe 461 */
Ludwigfr 34:c208497dd079 462
Ludwigfr 34:c208497dd079 463
Ludwigfr 34:c208497dd079 464 void MiniExplorerCoimbra::print_final_map_with_robot_position(float robot_x,float robot_y) {
Ludwigfr 34:c208497dd079 465 float currProba;
Ludwigfr 34:c208497dd079 466 float Xrobot=this->map.robot_x_coordinate_in_world(robot_x,robot_y);
Ludwigfr 34:c208497dd079 467 float Yrobot=this->map.robot_y_coordinate_in_world(robot_x,robot_y);
Ludwigfr 34:c208497dd079 468
Ludwigfr 34:c208497dd079 469 float heightIndiceInOrthonormal;
Ludwigfr 34:c208497dd079 470 float widthIndiceInOrthonormal;
Ludwigfr 34:c208497dd079 471
Ludwigfr 34:c208497dd079 472 float widthMalus=-(3*this->map.sizeCellWidth/2);
Ludwigfr 34:c208497dd079 473 float widthBonus=this->map.sizeCellWidth/2;
Ludwigfr 34:c208497dd079 474
Ludwigfr 34:c208497dd079 475 float heightMalus=-(3*this->map.sizeCellHeight/2);
Ludwigfr 34:c208497dd079 476 float heightBonus=this->map.sizeCellHeight/2;
Ludwigfr 34:c208497dd079 477
Ludwigfr 34:c208497dd079 478 pc.printf("\n\r");
Ludwigfr 34:c208497dd079 479 for (int y = this->map.nbCellHeight -1; y>-1; y--) {
Ludwigfr 34:c208497dd079 480 for (int x= 0; x<this->map.nbCellWidth; x++) {
Ludwigfr 34:c208497dd079 481 heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
Ludwigfr 34:c208497dd079 482 widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
Ludwigfr 34:c208497dd079 483 if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 34:c208497dd079 484 pc.printf(" R ");
Ludwigfr 34:c208497dd079 485 else{
Ludwigfr 34:c208497dd079 486 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
Ludwigfr 34:c208497dd079 487 if ( currProba < 0.5)
Ludwigfr 34:c208497dd079 488 pc.printf(" ");
Ludwigfr 34:c208497dd079 489 else{
Ludwigfr 34:c208497dd079 490 if(currProba==0.5)
Ludwigfr 34:c208497dd079 491 pc.printf(" . ");
Ludwigfr 34:c208497dd079 492 else
Ludwigfr 34:c208497dd079 493 pc.printf(" X ");
Ludwigfr 34:c208497dd079 494 }
Ludwigfr 34:c208497dd079 495 }
Ludwigfr 34:c208497dd079 496 }
Ludwigfr 34:c208497dd079 497 pc.printf("\n\r");
Ludwigfr 34:c208497dd079 498 }
Ludwigfr 34:c208497dd079 499 }
Ludwigfr 34:c208497dd079 500
Ludwigfr 34:c208497dd079 501 void MiniExplorerCoimbra::print_final_map_with_robot_position_and_target(float robot_x,float robot_y,float targetXWorld, float targetYWorld) {
Ludwigfr 34:c208497dd079 502 float currProba;
Ludwigfr 34:c208497dd079 503 float Xrobot=this->map.robot_x_coordinate_in_world(robot_x,robot_y);
Ludwigfr 34:c208497dd079 504 float Yrobot=this->map.robot_y_coordinate_in_world(robot_x,robot_y);
Ludwigfr 34:c208497dd079 505
Ludwigfr 34:c208497dd079 506 float heightIndiceInOrthonormal;
Ludwigfr 34:c208497dd079 507 float widthIndiceInOrthonormal;
Ludwigfr 34:c208497dd079 508
Ludwigfr 34:c208497dd079 509 float widthMalus=-(3*this->map.sizeCellWidth/2);
Ludwigfr 34:c208497dd079 510 float widthBonus=this->map.sizeCellWidth/2;
Ludwigfr 34:c208497dd079 511
Ludwigfr 34:c208497dd079 512 float heightMalus=-(3*this->map.sizeCellHeight/2);
Ludwigfr 34:c208497dd079 513 float heightBonus=this->map.sizeCellHeight/2;
Ludwigfr 34:c208497dd079 514
Ludwigfr 34:c208497dd079 515 pc.printf("\n\r");
Ludwigfr 34:c208497dd079 516 for (int y = this->map.nbCellHeight -1; y>-1; y--) {
Ludwigfr 34:c208497dd079 517 for (int x= 0; x<this->map.nbCellWidth; x++) {
Ludwigfr 34:c208497dd079 518 heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
Ludwigfr 34:c208497dd079 519 widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
Ludwigfr 34:c208497dd079 520 if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 34:c208497dd079 521 pc.printf(" R ");
Ludwigfr 34:c208497dd079 522 else{
Ludwigfr 34:c208497dd079 523 if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 34:c208497dd079 524 pc.printf(" T ");
Ludwigfr 34:c208497dd079 525 else{
Ludwigfr 34:c208497dd079 526 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
Ludwigfr 34:c208497dd079 527 if ( currProba < 0.5)
Ludwigfr 34:c208497dd079 528 pc.printf(" ");
Ludwigfr 34:c208497dd079 529 else{
Ludwigfr 34:c208497dd079 530 if(currProba==0.5)
Ludwigfr 34:c208497dd079 531 pc.printf(" . ");
Ludwigfr 34:c208497dd079 532 else
Ludwigfr 34:c208497dd079 533 pc.printf(" X ");
Ludwigfr 34:c208497dd079 534 }
Ludwigfr 34:c208497dd079 535 }
Ludwigfr 34:c208497dd079 536 }
Ludwigfr 34:c208497dd079 537 }
Ludwigfr 34:c208497dd079 538 pc.printf("\n\r");
Ludwigfr 34:c208497dd079 539 }
Ludwigfr 34:c208497dd079 540 }
Ludwigfr 34:c208497dd079 541
Ludwigfr 34:c208497dd079 542 void MiniExplorerCoimbra::print_final_map() {
Ludwigfr 34:c208497dd079 543 float currProba;
Ludwigfr 34:c208497dd079 544 pc.printf("\n\r");
Ludwigfr 34:c208497dd079 545 for (int y = this->map.nbCellHeight -1; y>-1; y--) {
Ludwigfr 34:c208497dd079 546 for (int x= 0; x<this->map.nbCellWidth; x++) {
Ludwigfr 34:c208497dd079 547 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
Ludwigfr 34:c208497dd079 548 if ( currProba < 0.5) {
Ludwigfr 34:c208497dd079 549 pc.printf(" ");
Ludwigfr 34:c208497dd079 550 } else {
Ludwigfr 34:c208497dd079 551 if(currProba==0.5)
Ludwigfr 34:c208497dd079 552 pc.printf(" . ");
Ludwigfr 34:c208497dd079 553 else
Ludwigfr 34:c208497dd079 554 pc.printf(" X ");
Ludwigfr 34:c208497dd079 555 }
Ludwigfr 34:c208497dd079 556 }
Ludwigfr 34:c208497dd079 557 pc.printf("\n\r");
Ludwigfr 34:c208497dd079 558 }
Ludwigfr 34:c208497dd079 559 }