Diff: main.cpp

Revision:

0:8bffb51cc345

Child:

1:f0807d5c5a4b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 21 15:24:34 2017 +0000
@@ -0,0 +1,69 @@
+#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();
+    pc.baud(9600); // baud for the pc communication
+
+    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;
+
+    float r=3.25, b=7.2;
+
+    int speed=999;
+
+    theta=0;
+    X=0;
+    Y=0;
+
+    do {
+        pc.printf("\n\n\r entered while");
+
+        Odometria();
+
+        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);
+
+        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;
+
+        if (d<25) {
+            speed = d*30;
+        }
+        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);
+
+        leftMotor(1,1*angular_left);
+        rightMotor(1,1*angular_right);
+
+        wait(0.5);
+    } while(d>1);
+
+    leftMotor(1,0);
+    rightMotor(1,0);
+
+    pc.printf("\n\r %f -- arrived!", d);
+}
+
+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