Yo Fleyt / Mbed 2 deprecated VirtualForces_with_class

with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

main.cpp@3:1e0f4cb93eda, 2017-03-24 (annotated)

Committer:
geotsam
Date:
Fri Mar 24 15:46:46 2017 +0000
Revision:
3:1e0f4cb93eda
Parent:
2:ea61e801e81f
Child:
4:8c56c3ba6e54
it should work :P

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(){
AurelienBernier 2:ea61e801e81f 8 initRobot(); //Initializing the robot
geotsam 0:8bffb51cc345 9 pc.baud(9600); // baud for the pc communication
geotsam 0:8bffb51cc345 10
AurelienBernier 2:ea61e801e81f 11 //Target example x,y values
geotsam 3:1e0f4cb93eda 12 float target_x=46.8, target_y=78.6, target_angle=0;
geotsam 0:8bffb51cc345 13
geotsam 3:1e0f4cb93eda 14 float a; //angle error
geotsam 0:8bffb51cc345 15 float d; //distance from target
geotsam 3:1e0f4cb93eda 16 float beta;
geotsam 3:1e0f4cb93eda 17 //float k_linear=10, k_angular=200;
geotsam 3:1e0f4cb93eda 18 float kr=3, ka=8, kb=-2;
geotsam 0:8bffb51cc345 19 float linear, angular, angular_left, angular_right;
geotsam 3:1e0f4cb93eda 20 float dt=0.5;
geotsam 3:1e0f4cb93eda 21 float temp;
geotsam 0:8bffb51cc345 22
AurelienBernier 2:ea61e801e81f 23 //Diameter of a wheel and distance between the 2
geotsam 0:8bffb51cc345 24 float r=3.25, b=7.2;
geotsam 0:8bffb51cc345 25
AurelienBernier 2:ea61e801e81f 26 int speed=999; // Max speed at beggining of movement
geotsam 0:8bffb51cc345 27
AurelienBernier 2:ea61e801e81f 28 //Resetting coordinates before moving
AurelienBernier 2:ea61e801e81f 29 theta=0;
geotsam 0:8bffb51cc345 30 X=0;
geotsam 0:8bffb51cc345 31 Y=0;
geotsam 0:8bffb51cc345 32
geotsam 3:1e0f4cb93eda 33 a = atan2((target_y-Y),(target_x-X))-theta;
geotsam 3:1e0f4cb93eda 34 a = atan(sin(a)/cos(a));
geotsam 3:1e0f4cb93eda 35 d = dist(X, Y, target_x, target_y);
geotsam 3:1e0f4cb93eda 36 beta = -a-theta+target_angle;
geotsam 3:1e0f4cb93eda 37
geotsam 0:8bffb51cc345 38 do {
geotsam 0:8bffb51cc345 39 pc.printf("\n\n\r entered while");
AurelienBernier 2:ea61e801e81f 40
AurelienBernier 2:ea61e801e81f 41 //Updating X,Y and theta with the odometry values
geotsam 0:8bffb51cc345 42 Odometria();
geotsam 3:1e0f4cb93eda 43
geotsam 3:1e0f4cb93eda 44 a = atan2((target_y-Y),(target_x-X))-theta;
geotsam 3:1e0f4cb93eda 45 a = atan(sin(a)/cos(a));
geotsam 3:1e0f4cb93eda 46 d = dist(X, Y, target_x, target_y);
geotsam 3:1e0f4cb93eda 47 beta = -a-theta+target_angle;
geotsam 0:8bffb51cc345 48
AurelienBernier 2:ea61e801e81f 49 //Computing angle error and distance towards the target value
geotsam 3:1e0f4cb93eda 50 d += dt*(-kr*cos(a)*d);
geotsam 3:1e0f4cb93eda 51 a = temp;
geotsam 3:1e0f4cb93eda 52 a += dt*(kr*sin(a)-ka*a-kb*b);
geotsam 3:1e0f4cb93eda 53 b += dt*(-kr*sin(temp));
geotsam 0:8bffb51cc345 54 pc.printf("\n\r d=%f", d);
geotsam 0:8bffb51cc345 55
AurelienBernier 2:ea61e801e81f 56 //Computing linear and angular velocities
geotsam 3:1e0f4cb93eda 57 if(a>=-1.5708 && a<=1.5708){
geotsam 3:1e0f4cb93eda 58 linear=kr*d;
geotsam 3:1e0f4cb93eda 59 angular=ka*a+kb*beta;
geotsam 3:1e0f4cb93eda 60 }
geotsam 3:1e0f4cb93eda 61 else{
geotsam 3:1e0f4cb93eda 62 linear=-kr*d;
geotsam 3:1e0f4cb93eda 63 angular=-ka*a-kb*beta;
geotsam 3:1e0f4cb93eda 64 }
geotsam 0:8bffb51cc345 65 angular_left=(linear-0.5*b*angular)/r;
geotsam 0:8bffb51cc345 66 angular_right=(linear+0.5*b*angular)/r;
geotsam 0:8bffb51cc345 67
AurelienBernier 2:ea61e801e81f 68 //Slowing down at the end for more precision
geotsam 0:8bffb51cc345 69 if (d<25) {
geotsam 0:8bffb51cc345 70 speed = d*30;
geotsam 0:8bffb51cc345 71 }
AurelienBernier 2:ea61e801e81f 72
AurelienBernier 2:ea61e801e81f 73 //Normalize speed for motors
geotsam 0:8bffb51cc345 74 if(angular_left>angular_right) {
geotsam 0:8bffb51cc345 75 angular_right=speed*angular_right/angular_left;
geotsam 0:8bffb51cc345 76 angular_left=speed;
geotsam 0:8bffb51cc345 77 } else {
geotsam 0:8bffb51cc345 78 angular_left=speed*angular_left/angular_right;
geotsam 0:8bffb51cc345 79 angular_right=speed;
geotsam 0:8bffb51cc345 80 }
geotsam 0:8bffb51cc345 81
geotsam 0:8bffb51cc345 82 pc.printf("\n\r X=%f", X);
geotsam 0:8bffb51cc345 83 pc.printf("\n\r Y=%f", Y);
geotsam 0:8bffb51cc345 84
AurelienBernier 2:ea61e801e81f 85 //Updating motor velocities
AurelienBernier 1:f0807d5c5a4b 86 leftMotor(1,angular_left);
AurelienBernier 1:f0807d5c5a4b 87 rightMotor(1,angular_right);
geotsam 0:8bffb51cc345 88
geotsam 3:1e0f4cb93eda 89 wait(dt);
geotsam 0:8bffb51cc345 90 } while(d>1);
geotsam 0:8bffb51cc345 91
AurelienBernier 2:ea61e801e81f 92 //Stop at the end
geotsam 0:8bffb51cc345 93 leftMotor(1,0);
geotsam 0:8bffb51cc345 94 rightMotor(1,0);
geotsam 0:8bffb51cc345 95
geotsam 0:8bffb51cc345 96 pc.printf("\n\r %f -- arrived!", d);
geotsam 0:8bffb51cc345 97 }
geotsam 0:8bffb51cc345 98
AurelienBernier 2:ea61e801e81f 99 //Distance computation function
geotsam 0:8bffb51cc345 100 float dist(float robot_x, float robot_y, float target_x, float target_y){
geotsam 0:8bffb51cc345 101 return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
geotsam 0:8bffb51cc345 102 }