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