Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
main.cpp
- Committer:
- geotsam
- Date:
- 2017-03-21
- Revision:
- 0:8bffb51cc345
- Child:
- 1:f0807d5c5a4b
File content as of revision 0:8bffb51cc345:
#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)); }