![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
main.cpp@3:1e0f4cb93eda, 2017-03-24 (annotated)
- Committer:
- geotsam
- Date:
- Fri Mar 24 15:46:46 2017 +0000
- Revision:
- 3:1e0f4cb93eda
- Parent:
- 2:ea61e801e81f
- Child:
- 4:8c56c3ba6e54
it should work :P
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" |
geotsam | 0:8bffb51cc345 | 4 | |
geotsam | 0:8bffb51cc345 | 5 | float dist(float robot_x, float robot_y, float target_x, float target_y); |
geotsam | 0:8bffb51cc345 | 6 | |
geotsam | 0:8bffb51cc345 | 7 | int main(){ |
AurelienBernier | 2:ea61e801e81f | 8 | initRobot(); //Initializing the robot |
geotsam | 0:8bffb51cc345 | 9 | pc.baud(9600); // baud for the pc communication |
geotsam | 0:8bffb51cc345 | 10 | |
AurelienBernier | 2:ea61e801e81f | 11 | //Target example x,y values |
geotsam | 3:1e0f4cb93eda | 12 | float target_x=46.8, target_y=78.6, target_angle=0; |
geotsam | 0:8bffb51cc345 | 13 | |
geotsam | 3:1e0f4cb93eda | 14 | float a; //angle error |
geotsam | 0:8bffb51cc345 | 15 | float d; //distance from target |
geotsam | 3:1e0f4cb93eda | 16 | float beta; |
geotsam | 3:1e0f4cb93eda | 17 | //float k_linear=10, k_angular=200; |
geotsam | 3:1e0f4cb93eda | 18 | float kr=3, ka=8, kb=-2; |
geotsam | 0:8bffb51cc345 | 19 | float linear, angular, angular_left, angular_right; |
geotsam | 3:1e0f4cb93eda | 20 | float dt=0.5; |
geotsam | 3:1e0f4cb93eda | 21 | float temp; |
geotsam | 0:8bffb51cc345 | 22 | |
AurelienBernier | 2:ea61e801e81f | 23 | //Diameter of a wheel and distance between the 2 |
geotsam | 0:8bffb51cc345 | 24 | float r=3.25, b=7.2; |
geotsam | 0:8bffb51cc345 | 25 | |
AurelienBernier | 2:ea61e801e81f | 26 | int speed=999; // Max speed at beggining of movement |
geotsam | 0:8bffb51cc345 | 27 | |
AurelienBernier | 2:ea61e801e81f | 28 | //Resetting coordinates before moving |
AurelienBernier | 2:ea61e801e81f | 29 | theta=0; |
geotsam | 0:8bffb51cc345 | 30 | X=0; |
geotsam | 0:8bffb51cc345 | 31 | Y=0; |
geotsam | 0:8bffb51cc345 | 32 | |
geotsam | 3:1e0f4cb93eda | 33 | a = atan2((target_y-Y),(target_x-X))-theta; |
geotsam | 3:1e0f4cb93eda | 34 | a = atan(sin(a)/cos(a)); |
geotsam | 3:1e0f4cb93eda | 35 | d = dist(X, Y, target_x, target_y); |
geotsam | 3:1e0f4cb93eda | 36 | beta = -a-theta+target_angle; |
geotsam | 3:1e0f4cb93eda | 37 | |
geotsam | 0:8bffb51cc345 | 38 | do { |
geotsam | 0:8bffb51cc345 | 39 | pc.printf("\n\n\r entered while"); |
AurelienBernier | 2:ea61e801e81f | 40 | |
AurelienBernier | 2:ea61e801e81f | 41 | //Updating X,Y and theta with the odometry values |
geotsam | 0:8bffb51cc345 | 42 | Odometria(); |
geotsam | 3:1e0f4cb93eda | 43 | |
geotsam | 3:1e0f4cb93eda | 44 | a = atan2((target_y-Y),(target_x-X))-theta; |
geotsam | 3:1e0f4cb93eda | 45 | a = atan(sin(a)/cos(a)); |
geotsam | 3:1e0f4cb93eda | 46 | d = dist(X, Y, target_x, target_y); |
geotsam | 3:1e0f4cb93eda | 47 | beta = -a-theta+target_angle; |
geotsam | 0:8bffb51cc345 | 48 | |
AurelienBernier | 2:ea61e801e81f | 49 | //Computing angle error and distance towards the target value |
geotsam | 3:1e0f4cb93eda | 50 | d += dt*(-kr*cos(a)*d); |
geotsam | 3:1e0f4cb93eda | 51 | a = temp; |
geotsam | 3:1e0f4cb93eda | 52 | a += dt*(kr*sin(a)-ka*a-kb*b); |
geotsam | 3:1e0f4cb93eda | 53 | b += dt*(-kr*sin(temp)); |
geotsam | 0:8bffb51cc345 | 54 | pc.printf("\n\r d=%f", d); |
geotsam | 0:8bffb51cc345 | 55 | |
AurelienBernier | 2:ea61e801e81f | 56 | //Computing linear and angular velocities |
geotsam | 3:1e0f4cb93eda | 57 | if(a>=-1.5708 && a<=1.5708){ |
geotsam | 3:1e0f4cb93eda | 58 | linear=kr*d; |
geotsam | 3:1e0f4cb93eda | 59 | angular=ka*a+kb*beta; |
geotsam | 3:1e0f4cb93eda | 60 | } |
geotsam | 3:1e0f4cb93eda | 61 | else{ |
geotsam | 3:1e0f4cb93eda | 62 | linear=-kr*d; |
geotsam | 3:1e0f4cb93eda | 63 | angular=-ka*a-kb*beta; |
geotsam | 3:1e0f4cb93eda | 64 | } |
geotsam | 0:8bffb51cc345 | 65 | angular_left=(linear-0.5*b*angular)/r; |
geotsam | 0:8bffb51cc345 | 66 | angular_right=(linear+0.5*b*angular)/r; |
geotsam | 0:8bffb51cc345 | 67 | |
AurelienBernier | 2:ea61e801e81f | 68 | //Slowing down at the end for more precision |
geotsam | 0:8bffb51cc345 | 69 | if (d<25) { |
geotsam | 0:8bffb51cc345 | 70 | speed = d*30; |
geotsam | 0:8bffb51cc345 | 71 | } |
AurelienBernier | 2:ea61e801e81f | 72 | |
AurelienBernier | 2:ea61e801e81f | 73 | //Normalize speed for motors |
geotsam | 0:8bffb51cc345 | 74 | if(angular_left>angular_right) { |
geotsam | 0:8bffb51cc345 | 75 | angular_right=speed*angular_right/angular_left; |
geotsam | 0:8bffb51cc345 | 76 | angular_left=speed; |
geotsam | 0:8bffb51cc345 | 77 | } else { |
geotsam | 0:8bffb51cc345 | 78 | angular_left=speed*angular_left/angular_right; |
geotsam | 0:8bffb51cc345 | 79 | angular_right=speed; |
geotsam | 0:8bffb51cc345 | 80 | } |
geotsam | 0:8bffb51cc345 | 81 | |
geotsam | 0:8bffb51cc345 | 82 | pc.printf("\n\r X=%f", X); |
geotsam | 0:8bffb51cc345 | 83 | pc.printf("\n\r Y=%f", Y); |
geotsam | 0:8bffb51cc345 | 84 | |
AurelienBernier | 2:ea61e801e81f | 85 | //Updating motor velocities |
AurelienBernier | 1:f0807d5c5a4b | 86 | leftMotor(1,angular_left); |
AurelienBernier | 1:f0807d5c5a4b | 87 | rightMotor(1,angular_right); |
geotsam | 0:8bffb51cc345 | 88 | |
geotsam | 3:1e0f4cb93eda | 89 | wait(dt); |
geotsam | 0:8bffb51cc345 | 90 | } while(d>1); |
geotsam | 0:8bffb51cc345 | 91 | |
AurelienBernier | 2:ea61e801e81f | 92 | //Stop at the end |
geotsam | 0:8bffb51cc345 | 93 | leftMotor(1,0); |
geotsam | 0:8bffb51cc345 | 94 | rightMotor(1,0); |
geotsam | 0:8bffb51cc345 | 95 | |
geotsam | 0:8bffb51cc345 | 96 | pc.printf("\n\r %f -- arrived!", d); |
geotsam | 0:8bffb51cc345 | 97 | } |
geotsam | 0:8bffb51cc345 | 98 | |
AurelienBernier | 2:ea61e801e81f | 99 | //Distance computation function |
geotsam | 0:8bffb51cc345 | 100 | float dist(float robot_x, float robot_y, float target_x, float target_y){ |
geotsam | 0:8bffb51cc345 | 101 | return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2)); |
geotsam | 0:8bffb51cc345 | 102 | } |