Georgios Tsamis / Mbed 2 deprecated OneFileToRuleThemAll

All the lab works are here!

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

main.cpp@2:ea61e801e81f, 2017-03-21 (annotated)

Committer:
AurelienBernier
Date:
Tue Mar 21 16:17:56 2017 +0000
Revision:
2:ea61e801e81f
Parent:
1:f0807d5c5a4b
Child:
3:1e0f4cb93eda
added clear comments

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