with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
main.cpp
- Committer:
- AurelienBernier
- Date:
- 2017-03-21
- Revision:
- 2:ea61e801e81f
- Parent:
- 1:f0807d5c5a4b
- Child:
- 3:1e0f4cb93eda
File content as of revision 2:ea61e801e81f:
#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(); //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 float d; //distance from target 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; // Max speed at beggining of movement //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; } 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); //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)); }