with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
main.cpp.orig@39:890439b495e3, 2017-06-16 (annotated)
- Committer:
- Ludwigfr
- Date:
- Fri Jun 16 10:40:53 2017 +0000
- Revision:
- 39:890439b495e3
- Parent:
- 20:6a9062d54eb0
last version with the 4th lab;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
AurelienBernier | 20:6a9062d54eb0 | 1 | #include "mbed.h" |
AurelienBernier | 20:6a9062d54eb0 | 2 | #include "robot.h" // Initializes the robot. This include should be used in all main.cpp! |
AurelienBernier | 20:6a9062d54eb0 | 3 | #include "math.h" |
AurelienBernier | 20:6a9062d54eb0 | 4 | |
AurelienBernier | 20:6a9062d54eb0 | 5 | Timer t; |
AurelienBernier | 20:6a9062d54eb0 | 6 | |
AurelienBernier | 20:6a9062d54eb0 | 7 | float dist(float robot_x, float robot_y, float target_x, float target_y); |
AurelienBernier | 20:6a9062d54eb0 | 8 | |
AurelienBernier | 20:6a9062d54eb0 | 9 | int goToPointWithAngle(float target_x, float target_y, float target_angle); |
AurelienBernier | 20:6a9062d54eb0 | 10 | |
AurelienBernier | 20:6a9062d54eb0 | 11 | int randomizeAndMap(); |
AurelienBernier | 20:6a9062d54eb0 | 12 | int updateSonarValues(); |
AurelienBernier | 20:6a9062d54eb0 | 13 | int computeObstacle(); |
AurelienBernier | 20:6a9062d54eb0 | 14 | |
AurelienBernier | 20:6a9062d54eb0 | 15 | bool map[25][25]; |
AurelienBernier | 20:6a9062d54eb0 | 16 | float alpha; //angle error |
AurelienBernier | 20:6a9062d54eb0 | 17 | float rho; //distance from target |
AurelienBernier | 20:6a9062d54eb0 | 18 | float beta; |
AurelienBernier | 20:6a9062d54eb0 | 19 | float kRho=12, ka=30, kb=-13; //Kappa values |
AurelienBernier | 20:6a9062d54eb0 | 20 | float linear, angular, angular_left, angular_right; |
AurelienBernier | 20:6a9062d54eb0 | 21 | float dt=0.5; |
AurelienBernier | 20:6a9062d54eb0 | 22 | float temp; |
AurelienBernier | 20:6a9062d54eb0 | 23 | float d2; |
AurelienBernier | 20:6a9062d54eb0 | 24 | |
AurelienBernier | 20:6a9062d54eb0 | 25 | bool tooClose = false; |
AurelienBernier | 20:6a9062d54eb0 | 26 | |
AurelienBernier | 20:6a9062d54eb0 | 27 | int leftMm; |
AurelienBernier | 20:6a9062d54eb0 | 28 | int frontMm; |
AurelienBernier | 20:6a9062d54eb0 | 29 | int rightMm; |
AurelienBernier | 20:6a9062d54eb0 | 30 | |
AurelienBernier | 20:6a9062d54eb0 | 31 | |
AurelienBernier | 20:6a9062d54eb0 | 32 | //Diameter of a wheel and distance between the 2 |
AurelienBernier | 20:6a9062d54eb0 | 33 | float r=3.25, b=7.2; |
AurelienBernier | 20:6a9062d54eb0 | 34 | |
AurelienBernier | 20:6a9062d54eb0 | 35 | int speed=999; // Max speed at beggining of movement |
AurelienBernier | 20:6a9062d54eb0 | 36 | |
AurelienBernier | 20:6a9062d54eb0 | 37 | //Target example x,y values |
AurelienBernier | 20:6a9062d54eb0 | 38 | float target_x=46.8, target_y=78.6, target_angle=1.57; |
AurelienBernier | 20:6a9062d54eb0 | 39 | |
AurelienBernier | 20:6a9062d54eb0 | 40 | //Timeout time; |
AurelienBernier | 20:6a9062d54eb0 | 41 | int main(){ |
AurelienBernier | 20:6a9062d54eb0 | 42 | i2c1.frequency(100000); |
AurelienBernier | 20:6a9062d54eb0 | 43 | initRobot(); //Initializing the robot |
AurelienBernier | 20:6a9062d54eb0 | 44 | pc.baud(9600); // baud for the pc communication |
AurelienBernier | 20:6a9062d54eb0 | 45 | |
AurelienBernier | 20:6a9062d54eb0 | 46 | measure_always_on(); |
AurelienBernier | 20:6a9062d54eb0 | 47 | wait_ms(50); |
AurelienBernier | 20:6a9062d54eb0 | 48 | |
AurelienBernier | 20:6a9062d54eb0 | 49 | //Fill map |
AurelienBernier | 20:6a9062d54eb0 | 50 | for (int i = 0; i<25; i++) { |
AurelienBernier | 20:6a9062d54eb0 | 51 | for (int j = 0; j<25; j++) { |
AurelienBernier | 20:6a9062d54eb0 | 52 | map[i][j] = false; |
AurelienBernier | 20:6a9062d54eb0 | 53 | } |
AurelienBernier | 20:6a9062d54eb0 | 54 | } |
AurelienBernier | 20:6a9062d54eb0 | 55 | |
AurelienBernier | 20:6a9062d54eb0 | 56 | //Resetting coordinates before moving |
AurelienBernier | 20:6a9062d54eb0 | 57 | theta=0; |
AurelienBernier | 20:6a9062d54eb0 | 58 | X=0; |
AurelienBernier | 20:6a9062d54eb0 | 59 | Y=0; |
AurelienBernier | 20:6a9062d54eb0 | 60 | |
AurelienBernier | 20:6a9062d54eb0 | 61 | /*alpha = atan2((target_y-Y),(target_x-X))-theta; |
AurelienBernier | 20:6a9062d54eb0 | 62 | alpha = atan(sin(alpha)/cos(alpha)); |
AurelienBernier | 20:6a9062d54eb0 | 63 | rho = dist(X, Y, target_x, target_y); |
AurelienBernier | 20:6a9062d54eb0 | 64 | |
AurelienBernier | 20:6a9062d54eb0 | 65 | beta = -alpha-theta+target_angle;*/ |
AurelienBernier | 20:6a9062d54eb0 | 66 | for (int i = 0; i<10; i++) { |
AurelienBernier | 20:6a9062d54eb0 | 67 | randomizeAndMap(); |
AurelienBernier | 20:6a9062d54eb0 | 68 | } |
AurelienBernier | 20:6a9062d54eb0 | 69 | |
AurelienBernier | 20:6a9062d54eb0 | 70 | //Stop at the end |
AurelienBernier | 20:6a9062d54eb0 | 71 | leftMotor(1,0); |
AurelienBernier | 20:6a9062d54eb0 | 72 | rightMotor(1,0); |
AurelienBernier | 20:6a9062d54eb0 | 73 | |
AurelienBernier | 20:6a9062d54eb0 | 74 | pc.printf("\n\r %f -- arrived!", rho); |
AurelienBernier | 20:6a9062d54eb0 | 75 | } |
AurelienBernier | 20:6a9062d54eb0 | 76 | |
AurelienBernier | 20:6a9062d54eb0 | 77 | //Distance computation function |
AurelienBernier | 20:6a9062d54eb0 | 78 | float dist(float robot_x, float robot_y, float target_x, float target_y){ |
AurelienBernier | 20:6a9062d54eb0 | 79 | return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2)); |
AurelienBernier | 20:6a9062d54eb0 | 80 | } |
AurelienBernier | 20:6a9062d54eb0 | 81 | |
AurelienBernier | 20:6a9062d54eb0 | 82 | //Updates sonar values |
AurelienBernier | 20:6a9062d54eb0 | 83 | int updateSonarValues() { |
AurelienBernier | 20:6a9062d54eb0 | 84 | leftMm = get_distance_left_sensor(); |
AurelienBernier | 20:6a9062d54eb0 | 85 | frontMm = get_distance_front_sensor(); |
AurelienBernier | 20:6a9062d54eb0 | 86 | rightMm = get_distance_right_sensor(); |
AurelienBernier | 20:6a9062d54eb0 | 87 | return 0; |
AurelienBernier | 20:6a9062d54eb0 | 88 | } |
AurelienBernier | 20:6a9062d54eb0 | 89 | |
AurelienBernier | 20:6a9062d54eb0 | 90 | int randomizeAndMap() { |
AurelienBernier | 20:6a9062d54eb0 | 91 | target_x = (rand()%2500)/10;//for decimal precision |
AurelienBernier | 20:6a9062d54eb0 | 92 | target_y = (rand()%2500)/10; |
AurelienBernier | 20:6a9062d54eb0 | 93 | target_angle = ((rand()%31416)-15708)/1000; |
AurelienBernier | 20:6a9062d54eb0 | 94 | pc.printf("\n\r targ_X=%f", target_x); |
AurelienBernier | 20:6a9062d54eb0 | 95 | pc.printf("\n\r targ_Y=%f", target_y); |
AurelienBernier | 20:6a9062d54eb0 | 96 | pc.printf("\n\r targ_Angle=%f", target_angle); |
AurelienBernier | 20:6a9062d54eb0 | 97 | goToPointWithAngle(target_x, target_y, target_angle); |
AurelienBernier | 20:6a9062d54eb0 | 98 | return 0; |
AurelienBernier | 20:6a9062d54eb0 | 99 | } |
AurelienBernier | 20:6a9062d54eb0 | 100 | |
AurelienBernier | 20:6a9062d54eb0 | 101 | int computeObstacle() { |
AurelienBernier | 20:6a9062d54eb0 | 102 | //get the sensor values |
AurelienBernier | 20:6a9062d54eb0 | 103 | //compute the probabilistic shit of empty/non-empty in the direction of the sensor below 10 cm |
AurelienBernier | 20:6a9062d54eb0 | 104 | //update the map object with true where it's likely to be obstacled |
AurelienBernier | 20:6a9062d54eb0 | 105 | int xObstacle, yObstacle; |
AurelienBernier | 20:6a9062d54eb0 | 106 | if (leftMm < 10) { |
AurelienBernier | 20:6a9062d54eb0 | 107 | |
AurelienBernier | 20:6a9062d54eb0 | 108 | } else if (frontMm < 10) { |
AurelienBernier | 20:6a9062d54eb0 | 109 | |
AurelienBernier | 20:6a9062d54eb0 | 110 | } else if (rightMm < 10) { |
AurelienBernier | 20:6a9062d54eb0 | 111 | |
AurelienBernier | 20:6a9062d54eb0 | 112 | } |
AurelienBernier | 20:6a9062d54eb0 | 113 | |
AurelienBernier | 20:6a9062d54eb0 | 114 | map[xObstacle][yObstacle] = true; |
AurelienBernier | 20:6a9062d54eb0 | 115 | return 0; |
AurelienBernier | 20:6a9062d54eb0 | 116 | } |
AurelienBernier | 20:6a9062d54eb0 | 117 | |
AurelienBernier | 20:6a9062d54eb0 | 118 | int goToPointWithAngle(float target_x, float target_y, float target_angle) { |
AurelienBernier | 20:6a9062d54eb0 | 119 | do { |
AurelienBernier | 20:6a9062d54eb0 | 120 | pc.printf("\n\n\r entered while"); |
AurelienBernier | 20:6a9062d54eb0 | 121 | |
AurelienBernier | 20:6a9062d54eb0 | 122 | //Timer stuff |
AurelienBernier | 20:6a9062d54eb0 | 123 | dt = t.read(); |
AurelienBernier | 20:6a9062d54eb0 | 124 | t.reset(); |
AurelienBernier | 20:6a9062d54eb0 | 125 | t.start(); |
AurelienBernier | 20:6a9062d54eb0 | 126 | |
AurelienBernier | 20:6a9062d54eb0 | 127 | updateSonarValues(); |
AurelienBernier | 20:6a9062d54eb0 | 128 | if (leftMm < 100 || frontMm < 100 || rightMm < 100) { |
AurelienBernier | 20:6a9062d54eb0 | 129 | tooClose = true; |
AurelienBernier | 20:6a9062d54eb0 | 130 | } |
AurelienBernier | 20:6a9062d54eb0 | 131 | |
AurelienBernier | 20:6a9062d54eb0 | 132 | //Updating X,Y and theta with the odometry values |
AurelienBernier | 20:6a9062d54eb0 | 133 | Odometria(); |
AurelienBernier | 20:6a9062d54eb0 | 134 | |
AurelienBernier | 20:6a9062d54eb0 | 135 | alpha = atan2((target_y-Y),(target_x-X))-theta; |
AurelienBernier | 20:6a9062d54eb0 | 136 | alpha = atan(sin(alpha)/cos(alpha)); |
AurelienBernier | 20:6a9062d54eb0 | 137 | rho = dist(X, Y, target_x, target_y); |
AurelienBernier | 20:6a9062d54eb0 | 138 | d2 = rho; |
AurelienBernier | 20:6a9062d54eb0 | 139 | beta = -alpha-theta+target_angle; |
AurelienBernier | 20:6a9062d54eb0 | 140 | |
AurelienBernier | 20:6a9062d54eb0 | 141 | //Computing angle error and distance towards the target value |
AurelienBernier | 20:6a9062d54eb0 | 142 | rho += dt*(-kRho*cos(alpha)*rho); |
AurelienBernier | 20:6a9062d54eb0 | 143 | temp = alpha; |
AurelienBernier | 20:6a9062d54eb0 | 144 | alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta); |
AurelienBernier | 20:6a9062d54eb0 | 145 | beta += dt*(-kRho*sin(temp)); |
AurelienBernier | 20:6a9062d54eb0 | 146 | pc.printf("\n\r d2=%f", d2); |
AurelienBernier | 20:6a9062d54eb0 | 147 | pc.printf("\n\r dt=%f", dt); |
AurelienBernier | 20:6a9062d54eb0 | 148 | |
AurelienBernier | 20:6a9062d54eb0 | 149 | //Computing linear and angular velocities |
AurelienBernier | 20:6a9062d54eb0 | 150 | if(alpha>=-1.5708 && alpha<=1.5708){ |
AurelienBernier | 20:6a9062d54eb0 | 151 | linear=kRho*rho; |
AurelienBernier | 20:6a9062d54eb0 | 152 | angular=ka*alpha+kb*beta; |
AurelienBernier | 20:6a9062d54eb0 | 153 | } |
AurelienBernier | 20:6a9062d54eb0 | 154 | else{ |
AurelienBernier | 20:6a9062d54eb0 | 155 | linear=-kRho*rho; |
AurelienBernier | 20:6a9062d54eb0 | 156 | angular=-ka*alpha-kb*beta; |
AurelienBernier | 20:6a9062d54eb0 | 157 | } |
AurelienBernier | 20:6a9062d54eb0 | 158 | angular_left=(linear-0.5*b*angular)/r; |
AurelienBernier | 20:6a9062d54eb0 | 159 | angular_right=(linear+0.5*b*angular)/r; |
AurelienBernier | 20:6a9062d54eb0 | 160 | |
AurelienBernier | 20:6a9062d54eb0 | 161 | //Slowing down at the end for more precision |
AurelienBernier | 20:6a9062d54eb0 | 162 | if (d2<25) { |
AurelienBernier | 20:6a9062d54eb0 | 163 | speed = d2*30; |
AurelienBernier | 20:6a9062d54eb0 | 164 | } |
AurelienBernier | 20:6a9062d54eb0 | 165 | |
AurelienBernier | 20:6a9062d54eb0 | 166 | //Normalize speed for motors |
AurelienBernier | 20:6a9062d54eb0 | 167 | if(angular_left>angular_right) { |
AurelienBernier | 20:6a9062d54eb0 | 168 | angular_right=speed*angular_right/angular_left; |
AurelienBernier | 20:6a9062d54eb0 | 169 | angular_left=speed; |
AurelienBernier | 20:6a9062d54eb0 | 170 | } else { |
AurelienBernier | 20:6a9062d54eb0 | 171 | angular_left=speed*angular_left/angular_right; |
AurelienBernier | 20:6a9062d54eb0 | 172 | angular_right=speed; |
AurelienBernier | 20:6a9062d54eb0 | 173 | } |
AurelienBernier | 20:6a9062d54eb0 | 174 | |
AurelienBernier | 20:6a9062d54eb0 | 175 | pc.printf("\n\r X=%f", X); |
AurelienBernier | 20:6a9062d54eb0 | 176 | pc.printf("\n\r Y=%f", Y); |
AurelienBernier | 20:6a9062d54eb0 | 177 | pc.printf("\n\r leftMm=%f", leftMm); |
AurelienBernier | 20:6a9062d54eb0 | 178 | pc.printf("\n\r frontMm=%f", frontMm); |
AurelienBernier | 20:6a9062d54eb0 | 179 | pc.printf("\n\r rightMm=%f", rightMm); |
AurelienBernier | 20:6a9062d54eb0 | 180 | |
AurelienBernier | 20:6a9062d54eb0 | 181 | //Updating motor velocities |
AurelienBernier | 20:6a9062d54eb0 | 182 | leftMotor(1,angular_left); |
AurelienBernier | 20:6a9062d54eb0 | 183 | rightMotor(1,angular_right); |
AurelienBernier | 20:6a9062d54eb0 | 184 | |
AurelienBernier | 20:6a9062d54eb0 | 185 | wait(0.2); |
AurelienBernier | 20:6a9062d54eb0 | 186 | //Timer stuff |
AurelienBernier | 20:6a9062d54eb0 | 187 | t.stop(); |
AurelienBernier | 20:6a9062d54eb0 | 188 | } while(d2>1 & !tooClose); |
AurelienBernier | 20:6a9062d54eb0 | 189 | |
AurelienBernier | 20:6a9062d54eb0 | 190 | if (tooClose) { |
AurelienBernier | 20:6a9062d54eb0 | 191 | computeObstacle(); |
AurelienBernier | 20:6a9062d54eb0 | 192 | } |
AurelienBernier | 20:6a9062d54eb0 | 193 | return 0; |
AurelienBernier | 20:6a9062d54eb0 | 194 | } |