Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 2:ea61e801e81f
- Parent:
- 1:f0807d5c5a4b
- Child:
- 3:1e0f4cb93eda
diff -r f0807d5c5a4b -r ea61e801e81f main.cpp --- a/main.cpp Tue Mar 21 15:32:22 2017 +0000 +++ b/main.cpp Tue Mar 21 16:17:56 2017 +0000 @@ -5,9 +5,10 @@ float dist(float robot_x, float robot_y, float target_x, float target_y); int main(){ - initRobot(); + initRobot(); //Initializing the robot pc.baud(9600); // baud for the pc communication + //Target example x,y values float target_x=46.8, target_y=78.6; float angle_error; //angle error @@ -15,32 +16,40 @@ float k_linear=10, k_angular=200; float linear, angular, angular_left, angular_right; + //Diameter of a wheel and distance between the 2 float r=3.25, b=7.2; - int speed=999; + int speed=999; // Max speed at beggining of movement - theta=0; + //Resetting coordinates before moving + theta=0; X=0; Y=0; do { pc.printf("\n\n\r entered while"); - + + //Updating X,Y and theta with the odometry values Odometria(); + //Computing angle error and distance towards the target value angle_error = atan2((target_y-Y),(target_x-X))-theta; angle_error = atan(sin(angle_error)/cos(angle_error)); d=dist(X, Y, target_x, target_y); pc.printf("\n\r d=%f", d); + //Computing linear and angular velocities linear=k_linear*d; angular=k_angular*angle_error; angular_left=(linear-0.5*b*angular)/r; angular_right=(linear+0.5*b*angular)/r; + //Slowing down at the end for more precision if (d<25) { speed = d*30; } + + //Normalize speed for motors if(angular_left>angular_right) { angular_right=speed*angular_right/angular_left; angular_left=speed; @@ -52,18 +61,21 @@ pc.printf("\n\r X=%f", X); pc.printf("\n\r Y=%f", Y); + //Updating motor velocities leftMotor(1,angular_left); rightMotor(1,angular_right); wait(0.5); } while(d>1); + //Stop at the end leftMotor(1,0); rightMotor(1,0); pc.printf("\n\r %f -- arrived!", d); } +//Distance computation function float dist(float robot_x, float robot_y, float target_x, float target_y){ return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2)); } \ No newline at end of file