Diff: main.cpp

Revision:

2:ea61e801e81f

Parent:

1:f0807d5c5a4b

Child:

3:1e0f4cb93eda

--- a/main.cpp	Tue Mar 21 15:32:22 2017 +0000
+++ b/main.cpp	Tue Mar 21 16:17:56 2017 +0000
@@ -5,9 +5,10 @@
 float dist(float robot_x, float robot_y, float target_x, float target_y);
 
 int main(){
-    initRobot();
+    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
@@ -15,32 +16,40 @@
     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;
+    int speed=999; // Max speed at beggining of movement
 
-    theta=0;
+    //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;
@@ -52,18 +61,21 @@
         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));
 }
\ No newline at end of file