Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 0:8bffb51cc345
- Child:
- 1:f0807d5c5a4b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Mar 21 15:24:34 2017 +0000 @@ -0,0 +1,69 @@ +#include "mbed.h" +#include "robot.h" // Initializes the robot. This include should be used in all main.cpp! +#include "math.h" + +float dist(float robot_x, float robot_y, float target_x, float target_y); + +int main(){ + initRobot(); + pc.baud(9600); // baud for the pc communication + + float target_x=46.8, target_y=78.6; + + float angle_error; //angle error + float d; //distance from target + float k_linear=10, k_angular=200; + float linear, angular, angular_left, angular_right; + + float r=3.25, b=7.2; + + int speed=999; + + theta=0; + X=0; + Y=0; + + do { + pc.printf("\n\n\r entered while"); + + Odometria(); + + 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); + + 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; + + if (d<25) { + speed = d*30; + } + if(angular_left>angular_right) { + angular_right=speed*angular_right/angular_left; + angular_left=speed; + } else { + angular_left=speed*angular_left/angular_right; + angular_right=speed; + } + + pc.printf("\n\r X=%f", X); + pc.printf("\n\r Y=%f", Y); + + leftMotor(1,1*angular_left); + rightMotor(1,1*angular_right); + + wait(0.5); + } while(d>1); + + leftMotor(1,0); + rightMotor(1,0); + + pc.printf("\n\r %f -- arrived!", d); +} + +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