test morning
Dependencies: ISR_Mini-explorer mbed
Fork of roboticLab_withclass_3_July by
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?
User | Revision | Line number | New 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 |