![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
All the lab works are here!
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
main.cpp@18:6a3b14e284ee, 2017-03-27 (annotated)
- Committer:
- geotsam
- Date:
- Mon Mar 27 17:07:10 2017 +0000
- Revision:
- 18:6a3b14e284ee
- Parent:
- 17:caf393b63e27
added a small delay between the new positions - but it doesn't work, it's just spinning
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
geotsam | 0:8bffb51cc345 | 1 | #include "mbed.h" |
geotsam | 0:8bffb51cc345 | 2 | #include "robot.h" // Initializes the robot. This include should be used in all main.cpp! |
geotsam | 0:8bffb51cc345 | 3 | #include "math.h" |
AurelienBernier | 6:afde4b08166b | 4 | |
AurelienBernier | 6:afde4b08166b | 5 | Timer t; |
AurelienBernier | 4:8c56c3ba6e54 | 6 | |
geotsam | 0:8bffb51cc345 | 7 | float dist(float robot_x, float robot_y, float target_x, float target_y); |
geotsam | 0:8bffb51cc345 | 8 | |
geotsam | 12:3c0ca2350624 | 9 | int goToPointWithAngle(float target_x, float target_y, float target_angle); |
AurelienBernier | 8:109314be5b68 | 10 | |
AurelienBernier | 11:e641aa08c92e | 11 | int updateSonarValues(); |
AurelienBernier | 11:e641aa08c92e | 12 | |
AurelienBernier | 8:109314be5b68 | 13 | float alpha; //angle error |
AurelienBernier | 8:109314be5b68 | 14 | float rho; //distance from target |
AurelienBernier | 8:109314be5b68 | 15 | float beta; |
AurelienBernier | 11:e641aa08c92e | 16 | float kRho=12, ka=30, kb=-13; //Kappa values |
AurelienBernier | 8:109314be5b68 | 17 | float linear, angular, angular_left, angular_right; |
AurelienBernier | 8:109314be5b68 | 18 | float dt=0.5; |
AurelienBernier | 8:109314be5b68 | 19 | float temp; |
AurelienBernier | 8:109314be5b68 | 20 | float d2; |
AurelienBernier | 8:109314be5b68 | 21 | |
geotsam | 14:d58f2bdbf42e | 22 | //bool tooClose = false; |
AurelienBernier | 11:e641aa08c92e | 23 | |
AurelienBernier | 11:e641aa08c92e | 24 | int leftMm; |
AurelienBernier | 11:e641aa08c92e | 25 | int frontMm; |
AurelienBernier | 11:e641aa08c92e | 26 | int rightMm; |
AurelienBernier | 11:e641aa08c92e | 27 | |
AurelienBernier | 8:109314be5b68 | 28 | //Diameter of a wheel and distance between the 2 |
AurelienBernier | 8:109314be5b68 | 29 | float r=3.25, b=7.2; |
AurelienBernier | 8:109314be5b68 | 30 | |
AurelienBernier | 8:109314be5b68 | 31 | int speed=999; // Max speed at beggining of movement |
AurelienBernier | 8:109314be5b68 | 32 | |
AurelienBernier | 8:109314be5b68 | 33 | //Target example x,y values |
geotsam | 17:caf393b63e27 | 34 | float target_x, target_y, target_angle; |
AurelienBernier | 8:109314be5b68 | 35 | |
AurelienBernier | 4:8c56c3ba6e54 | 36 | //Timeout time; |
geotsam | 0:8bffb51cc345 | 37 | int main(){ |
geotsam | 17:caf393b63e27 | 38 | target_x=200*rand(); |
geotsam | 17:caf393b63e27 | 39 | target_y=200*rand(); |
geotsam | 17:caf393b63e27 | 40 | target_angle=3.1416*2*rand()-3.1416; |
geotsam | 17:caf393b63e27 | 41 | |
geotsam | 13:41f75c132135 | 42 | i2c1.frequency(100000); |
AurelienBernier | 2:ea61e801e81f | 43 | initRobot(); //Initializing the robot |
geotsam | 0:8bffb51cc345 | 44 | pc.baud(9600); // baud for the pc communication |
geotsam | 0:8bffb51cc345 | 45 | |
geotsam | 13:41f75c132135 | 46 | measure_always_on(); |
geotsam | 13:41f75c132135 | 47 | wait_ms(50); |
geotsam | 13:41f75c132135 | 48 | |
AurelienBernier | 2:ea61e801e81f | 49 | //Resetting coordinates before moving |
AurelienBernier | 2:ea61e801e81f | 50 | theta=0; |
geotsam | 0:8bffb51cc345 | 51 | X=0; |
geotsam | 0:8bffb51cc345 | 52 | Y=0; |
geotsam | 0:8bffb51cc345 | 53 | |
AurelienBernier | 4:8c56c3ba6e54 | 54 | alpha = atan2((target_y-Y),(target_x-X))-theta; |
AurelienBernier | 4:8c56c3ba6e54 | 55 | alpha = atan(sin(alpha)/cos(alpha)); |
AurelienBernier | 4:8c56c3ba6e54 | 56 | rho = dist(X, Y, target_x, target_y); |
AurelienBernier | 6:afde4b08166b | 57 | |
AurelienBernier | 4:8c56c3ba6e54 | 58 | beta = -alpha-theta+target_angle; |
AurelienBernier | 6:afde4b08166b | 59 | //beta = atan(sin(beta)/cos(beta)); |
AurelienBernier | 8:109314be5b68 | 60 | |
geotsam | 17:caf393b63e27 | 61 | while(1){ |
geotsam | 17:caf393b63e27 | 62 | goToPointWithAngle(target_x, target_y, target_angle); |
geotsam | 18:6a3b14e284ee | 63 | wait(1); |
geotsam | 17:caf393b63e27 | 64 | target_x=200*rand(); |
geotsam | 17:caf393b63e27 | 65 | target_y=200*rand(); |
geotsam | 17:caf393b63e27 | 66 | target_angle=3.1416*2*rand()-3.1416; |
geotsam | 17:caf393b63e27 | 67 | } |
geotsam | 17:caf393b63e27 | 68 | |
AurelienBernier | 8:109314be5b68 | 69 | //Stop at the end |
AurelienBernier | 8:109314be5b68 | 70 | leftMotor(1,0); |
AurelienBernier | 8:109314be5b68 | 71 | rightMotor(1,0); |
AurelienBernier | 8:109314be5b68 | 72 | |
AurelienBernier | 8:109314be5b68 | 73 | pc.printf("\n\r %f -- arrived!", rho); |
AurelienBernier | 8:109314be5b68 | 74 | } |
AurelienBernier | 8:109314be5b68 | 75 | |
AurelienBernier | 8:109314be5b68 | 76 | //Distance computation function |
AurelienBernier | 8:109314be5b68 | 77 | float dist(float robot_x, float robot_y, float target_x, float target_y){ |
AurelienBernier | 8:109314be5b68 | 78 | return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2)); |
AurelienBernier | 8:109314be5b68 | 79 | } |
AurelienBernier | 8:109314be5b68 | 80 | |
AurelienBernier | 11:e641aa08c92e | 81 | //Updates sonar values |
AurelienBernier | 11:e641aa08c92e | 82 | int updateSonarValues() { |
AurelienBernier | 11:e641aa08c92e | 83 | leftMm = get_distance_left_sensor(); |
AurelienBernier | 11:e641aa08c92e | 84 | frontMm = get_distance_front_sensor(); |
AurelienBernier | 11:e641aa08c92e | 85 | rightMm = get_distance_right_sensor(); |
AurelienBernier | 11:e641aa08c92e | 86 | return 0; |
AurelienBernier | 11:e641aa08c92e | 87 | } |
AurelienBernier | 11:e641aa08c92e | 88 | |
geotsam | 12:3c0ca2350624 | 89 | int goToPointWithAngle(float target_x, float target_y, float target_angle) { |
AurelienBernier | 8:109314be5b68 | 90 | do { |
geotsam | 0:8bffb51cc345 | 91 | pc.printf("\n\n\r entered while"); |
AurelienBernier | 2:ea61e801e81f | 92 | |
AurelienBernier | 6:afde4b08166b | 93 | //Timer stuff |
AurelienBernier | 6:afde4b08166b | 94 | dt = t.read(); |
AurelienBernier | 6:afde4b08166b | 95 | t.reset(); |
AurelienBernier | 6:afde4b08166b | 96 | t.start(); |
AurelienBernier | 6:afde4b08166b | 97 | |
geotsam | 14:d58f2bdbf42e | 98 | //Updating X,Y and theta with the odometry values |
geotsam | 14:d58f2bdbf42e | 99 | Odometria(); |
geotsam | 14:d58f2bdbf42e | 100 | |
AurelienBernier | 11:e641aa08c92e | 101 | updateSonarValues(); |
AurelienBernier | 11:e641aa08c92e | 102 | if (leftMm < 100 || frontMm < 100 || rightMm < 100) { |
geotsam | 14:d58f2bdbf42e | 103 | break; |
AurelienBernier | 11:e641aa08c92e | 104 | } |
AurelienBernier | 11:e641aa08c92e | 105 | |
AurelienBernier | 4:8c56c3ba6e54 | 106 | alpha = atan2((target_y-Y),(target_x-X))-theta; |
AurelienBernier | 4:8c56c3ba6e54 | 107 | alpha = atan(sin(alpha)/cos(alpha)); |
AurelienBernier | 4:8c56c3ba6e54 | 108 | rho = dist(X, Y, target_x, target_y); |
AurelienBernier | 6:afde4b08166b | 109 | d2 = rho; |
AurelienBernier | 5:dea05b8f30d0 | 110 | beta = -alpha-theta+target_angle; |
AurelienBernier | 6:afde4b08166b | 111 | //beta = atan(sin(beta)/cos(beta)); |
AurelienBernier | 6:afde4b08166b | 112 | |
AurelienBernier | 6:afde4b08166b | 113 | |
AurelienBernier | 2:ea61e801e81f | 114 | //Computing angle error and distance towards the target value |
AurelienBernier | 4:8c56c3ba6e54 | 115 | rho += dt*(-kRho*cos(alpha)*rho); |
AurelienBernier | 4:8c56c3ba6e54 | 116 | temp = alpha; |
AurelienBernier | 6:afde4b08166b | 117 | alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta); |
AurelienBernier | 6:afde4b08166b | 118 | beta += dt*(-kRho*sin(temp)); |
AurelienBernier | 6:afde4b08166b | 119 | pc.printf("\n\r d2=%f", d2); |
AurelienBernier | 6:afde4b08166b | 120 | pc.printf("\n\r dt=%f", dt); |
geotsam | 0:8bffb51cc345 | 121 | |
AurelienBernier | 2:ea61e801e81f | 122 | //Computing linear and angular velocities |
AurelienBernier | 4:8c56c3ba6e54 | 123 | if(alpha>=-1.5708 && alpha<=1.5708){ |
AurelienBernier | 4:8c56c3ba6e54 | 124 | linear=kRho*rho; |
AurelienBernier | 4:8c56c3ba6e54 | 125 | angular=ka*alpha+kb*beta; |
geotsam | 3:1e0f4cb93eda | 126 | } |
geotsam | 3:1e0f4cb93eda | 127 | else{ |
AurelienBernier | 4:8c56c3ba6e54 | 128 | linear=-kRho*rho; |
AurelienBernier | 4:8c56c3ba6e54 | 129 | angular=-ka*alpha-kb*beta; |
geotsam | 3:1e0f4cb93eda | 130 | } |
geotsam | 0:8bffb51cc345 | 131 | angular_left=(linear-0.5*b*angular)/r; |
geotsam | 0:8bffb51cc345 | 132 | angular_right=(linear+0.5*b*angular)/r; |
geotsam | 0:8bffb51cc345 | 133 | |
AurelienBernier | 2:ea61e801e81f | 134 | //Slowing down at the end for more precision |
AurelienBernier | 6:afde4b08166b | 135 | if (d2<25) { |
AurelienBernier | 6:afde4b08166b | 136 | speed = d2*30; |
geotsam | 0:8bffb51cc345 | 137 | } |
AurelienBernier | 2:ea61e801e81f | 138 | |
AurelienBernier | 2:ea61e801e81f | 139 | //Normalize speed for motors |
geotsam | 0:8bffb51cc345 | 140 | if(angular_left>angular_right) { |
geotsam | 0:8bffb51cc345 | 141 | angular_right=speed*angular_right/angular_left; |
geotsam | 0:8bffb51cc345 | 142 | angular_left=speed; |
geotsam | 0:8bffb51cc345 | 143 | } else { |
geotsam | 0:8bffb51cc345 | 144 | angular_left=speed*angular_left/angular_right; |
geotsam | 0:8bffb51cc345 | 145 | angular_right=speed; |
geotsam | 0:8bffb51cc345 | 146 | } |
geotsam | 0:8bffb51cc345 | 147 | |
geotsam | 0:8bffb51cc345 | 148 | pc.printf("\n\r X=%f", X); |
geotsam | 0:8bffb51cc345 | 149 | pc.printf("\n\r Y=%f", Y); |
AurelienBernier | 15:44ab4626f1ad | 150 | pc.printf("\n\r leftMm=%f", leftMm); |
AurelienBernier | 15:44ab4626f1ad | 151 | pc.printf("\n\r frontMm=%f", frontMm); |
AurelienBernier | 15:44ab4626f1ad | 152 | pc.printf("\n\r rightMm=%f", rightMm); |
geotsam | 0:8bffb51cc345 | 153 | |
AurelienBernier | 2:ea61e801e81f | 154 | //Updating motor velocities |
AurelienBernier | 1:f0807d5c5a4b | 155 | leftMotor(1,angular_left); |
AurelienBernier | 1:f0807d5c5a4b | 156 | rightMotor(1,angular_right); |
geotsam | 0:8bffb51cc345 | 157 | |
AurelienBernier | 7:c94070f9af78 | 158 | wait(0.2); |
AurelienBernier | 6:afde4b08166b | 159 | //Timer stuff |
AurelienBernier | 6:afde4b08166b | 160 | t.stop(); |
geotsam | 14:d58f2bdbf42e | 161 | } while(d2>1); |
AurelienBernier | 8:109314be5b68 | 162 | |
AurelienBernier | 8:109314be5b68 | 163 | return 0; |
AurelienBernier | 6:afde4b08166b | 164 | } |