Yo Fleyt / Mbed 2 deprecated VirtualForces_with_class

with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

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?

UserRevisionLine numberNew 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 }