Diff: robot.h

Revision:

2:92ca9a6a1d4e

Parent:

1:b81277897a5b

Child:

3:b996cbe5dabd

--- a/robot.h	Fri Jul 07 11:49:22 2017 +0000
+++ b/robot.h	Tue Jul 11 16:54:58 2017 +0000
@@ -51,6 +51,9 @@
 int IRobot=0;
 int JRobot=0;
 
+float D_cm=0;
+float L_cm=0;
+
 //SAIDAS DIGITAIS (normal)
 DigitalOut  q_pha_mot_rig       (PTE4,0);     //Phase Motor Right
 DigitalOut  q_sleep_mot_rig     (PTE3,0);     //Nano Sleep Motor Right
@@ -1004,24 +1007,25 @@
 {
     long int ticks1d=R_encoder();
     long int ticks1e=L_encoder();
-
+    //pc.printf("\r\nticks1d:%f",ticks1d);
+    //pc.printf("\r\nticks1e:%f",ticks1d);
     long int D_ticks=ticks1d - ticks2d;
     long int E_ticks=ticks1e - ticks2e;
-
+    //pc.printf("\r\nD_ticks:%f",D_ticks);
+    //pc.printf("\r\nE_ticks:%f",E_ticks);
     ticks2d=ticks1d;
     ticks2e=ticks1e;
 
-    float D_cm= (float)D_ticks*((3.25*3.1415)/4096);
-    float L_cm= (float)E_ticks*((3.25*3.1415)/4096);
-
+    D_cm= (float)D_ticks*((3.25*3.1415)/4096);
+    L_cm= (float)E_ticks*((3.25*3.1415)/4096);
+    //pc.printf("\r\nD_cm:%f",D_cm);
+    //pc.printf("\r\nL_cm:%f",L_cm);
     float CM=(D_cm + L_cm)/2;
-
+    //pc.printf("\r\nCM:%f",CM);
     theta +=(D_cm - L_cm)/7.18;
-
     theta = atan2(sin(theta), cos(theta));
 
     // meter entre 0
-
     X += CM*cos(theta);
     Y += CM*sin(theta);
 
@@ -1038,7 +1042,20 @@
     ticks2d=ticks1d;
     ticks2e=ticks1e;
 
-    *R_cm= (float)D_ticks*((3.25*3.1415)/4096);
-    *L_cm= (float)E_ticks*((3.25*3.1415)/4096);
+    float D_cm= (float)D_ticks*((3.25*3.1415)/4096);
+    float E_cm= (float)E_ticks*((3.25*3.1415)/4096);
+
+    float CM=(D_cm + E_cm)/2;
+
+    theta +=(D_cm - E_cm)/7.18;
+
+    theta = atan2(sin(theta), cos(theta));
 
+    // meter entre 0
+
+    X += CM*cos(theta);
+    Y += CM*sin(theta);
+
+    *R_cm= D_cm;
+    *L_cm= E_cm;
 }
\ No newline at end of file