with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
main.cpp@1:f0807d5c5a4b, 2017-03-21 (annotated)
- Committer:
- AurelienBernier
- Date:
- Tue Mar 21 15:32:22 2017 +0000
- Revision:
- 1:f0807d5c5a4b
- Parent:
- 0:8bffb51cc345
- Child:
- 2:ea61e801e81f
test Commit
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(){ |
geotsam | 0:8bffb51cc345 | 8 | initRobot(); |
geotsam | 0:8bffb51cc345 | 9 | pc.baud(9600); // baud for the pc communication |
geotsam | 0:8bffb51cc345 | 10 | |
geotsam | 0:8bffb51cc345 | 11 | float target_x=46.8, target_y=78.6; |
geotsam | 0:8bffb51cc345 | 12 | |
geotsam | 0:8bffb51cc345 | 13 | float angle_error; //angle error |
geotsam | 0:8bffb51cc345 | 14 | float d; //distance from target |
geotsam | 0:8bffb51cc345 | 15 | float k_linear=10, k_angular=200; |
geotsam | 0:8bffb51cc345 | 16 | float linear, angular, angular_left, angular_right; |
geotsam | 0:8bffb51cc345 | 17 | |
geotsam | 0:8bffb51cc345 | 18 | float r=3.25, b=7.2; |
geotsam | 0:8bffb51cc345 | 19 | |
geotsam | 0:8bffb51cc345 | 20 | int speed=999; |
geotsam | 0:8bffb51cc345 | 21 | |
geotsam | 0:8bffb51cc345 | 22 | theta=0; |
geotsam | 0:8bffb51cc345 | 23 | X=0; |
geotsam | 0:8bffb51cc345 | 24 | Y=0; |
geotsam | 0:8bffb51cc345 | 25 | |
geotsam | 0:8bffb51cc345 | 26 | do { |
geotsam | 0:8bffb51cc345 | 27 | pc.printf("\n\n\r entered while"); |
geotsam | 0:8bffb51cc345 | 28 | |
geotsam | 0:8bffb51cc345 | 29 | Odometria(); |
geotsam | 0:8bffb51cc345 | 30 | |
geotsam | 0:8bffb51cc345 | 31 | angle_error = atan2((target_y-Y),(target_x-X))-theta; |
geotsam | 0:8bffb51cc345 | 32 | angle_error = atan(sin(angle_error)/cos(angle_error)); |
geotsam | 0:8bffb51cc345 | 33 | d=dist(X, Y, target_x, target_y); |
geotsam | 0:8bffb51cc345 | 34 | pc.printf("\n\r d=%f", d); |
geotsam | 0:8bffb51cc345 | 35 | |
geotsam | 0:8bffb51cc345 | 36 | linear=k_linear*d; |
geotsam | 0:8bffb51cc345 | 37 | angular=k_angular*angle_error; |
geotsam | 0:8bffb51cc345 | 38 | angular_left=(linear-0.5*b*angular)/r; |
geotsam | 0:8bffb51cc345 | 39 | angular_right=(linear+0.5*b*angular)/r; |
geotsam | 0:8bffb51cc345 | 40 | |
geotsam | 0:8bffb51cc345 | 41 | if (d<25) { |
geotsam | 0:8bffb51cc345 | 42 | speed = d*30; |
geotsam | 0:8bffb51cc345 | 43 | } |
geotsam | 0:8bffb51cc345 | 44 | if(angular_left>angular_right) { |
geotsam | 0:8bffb51cc345 | 45 | angular_right=speed*angular_right/angular_left; |
geotsam | 0:8bffb51cc345 | 46 | angular_left=speed; |
geotsam | 0:8bffb51cc345 | 47 | } else { |
geotsam | 0:8bffb51cc345 | 48 | angular_left=speed*angular_left/angular_right; |
geotsam | 0:8bffb51cc345 | 49 | angular_right=speed; |
geotsam | 0:8bffb51cc345 | 50 | } |
geotsam | 0:8bffb51cc345 | 51 | |
geotsam | 0:8bffb51cc345 | 52 | pc.printf("\n\r X=%f", X); |
geotsam | 0:8bffb51cc345 | 53 | pc.printf("\n\r Y=%f", Y); |
geotsam | 0:8bffb51cc345 | 54 | |
AurelienBernier | 1:f0807d5c5a4b | 55 | leftMotor(1,angular_left); |
AurelienBernier | 1:f0807d5c5a4b | 56 | rightMotor(1,angular_right); |
geotsam | 0:8bffb51cc345 | 57 | |
geotsam | 0:8bffb51cc345 | 58 | wait(0.5); |
geotsam | 0:8bffb51cc345 | 59 | } while(d>1); |
geotsam | 0:8bffb51cc345 | 60 | |
geotsam | 0:8bffb51cc345 | 61 | leftMotor(1,0); |
geotsam | 0:8bffb51cc345 | 62 | rightMotor(1,0); |
geotsam | 0:8bffb51cc345 | 63 | |
geotsam | 0:8bffb51cc345 | 64 | pc.printf("\n\r %f -- arrived!", d); |
geotsam | 0:8bffb51cc345 | 65 | } |
geotsam | 0:8bffb51cc345 | 66 | |
geotsam | 0:8bffb51cc345 | 67 | float dist(float robot_x, float robot_y, float target_x, float target_y){ |
geotsam | 0:8bffb51cc345 | 68 | return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2)); |
geotsam | 0:8bffb51cc345 | 69 | } |