Diff: Odometry/Odometry.cpp

Revision:

2:abdf8c6823a1

Parent:

0:ad9600df4a70

Child:

3:62e9d715de65

--- a/Odometry/Odometry.cpp	Mon Nov 16 11:34:08 2015 +0000
+++ b/Odometry/Odometry.cpp	Tue Nov 24 15:02:01 2015 +0000
@@ -1,22 +1,17 @@
 #include "Odometry.h"
 
-Serial pc(USBTX, USBRX);
-
 // M1 = Moteur droit, M2 = Moteur gauche
 
-RoboClaw roboclaw(115200, PA_11, PA_12);
-
-Odometry::Odometry(double diameter_right, double diameter_left, double v)
+Odometry::Odometry(double diameter_right, double diameter_left, double v, RoboClaw &rc) : roboclaw(rc)
 {
-    pc.baud(115200);
-    pc.printf("Hello world\n\r");
-    roboclaw.ResetEnc(ADR);
     m_v = v;
     m_distPerTick_left = diameter_left*PI/37400;
     m_distPerTick_right = diameter_right*PI/37400;
     
+    erreur_ang = 0.1;
     m_pulses_right = 0;
     m_pulses_left = 0;
+    wait_ms(100);
 }
 
 void Odometry::setPos(double x, double y, double theta)
@@ -49,15 +44,17 @@
     m_pulses_left = roboclaw.ReadEncM2(ADR);
     
     double deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0f;
-    double deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left) / m_v;
+    double deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left)*C / m_v;
     
-    double dx = deltaS*cos(theta);
-    double dy = deltaS*sin(theta);
+    double radius = deltaS/deltaTheta;
+    double xO = x - radius*sin(theta);
+    double yO = y + radius*cos(theta);
     
-    x += dx;
-    y += dy;
     theta += deltaTheta;
     
+    x = xO + radius*sin(theta);
+    y = yO - radius*cos(theta);
+    
     while(theta > PI) theta -= 2*PI;
     while(theta <= -PI) theta += 2*PI;
 }
@@ -65,9 +62,45 @@
 void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal)
 {
     double theta_ = atan2(y_goal-y, x_goal-x);
-    double distance_ticks_left = (theta_*m_v/2)/m_distPerTick_left;
-    double distance_ticks_right = (theta_*m_v/2)/m_distPerTick_right;
-    pc.printf("Theta : %3.2f\tDL : %6.2f\tDR : %6.2f\n\r",theta_*180/PI,distance_ticks_left, distance_ticks_right);
-    //roboclaw.SpeedAccelDeccelPositionM1(ADR, 300000, 150000, 300000, distance_ticks_right, 1);
-    //roboclaw.SpeedAccelDeccelPositionM2(ADR, 300000, 150000, 300000, distance_ticks_left, 1);
+    float distance = sqrt(carre(x_goal-x)+carre(y_goal-y));
+    GotoThet(theta_);
 }
+
+void Odometry::GotoThet(double theta_)
+{
+    double distance_ticks_left;
+    double distance_ticks_right;
+    double erreur_theta = theta_-getTheta();
+    
+    while(erreur_theta > PI) erreur_theta -= 2*PI;
+    while(erreur_theta <= -PI) erreur_theta += 2*PI;
+    
+    if(erreur_theta >= 0)
+    {
+        distance_ticks_left = -(erreur_theta*m_v/2)/m_distPerTick_left;
+        distance_ticks_right = (erreur_theta*m_v/2)/m_distPerTick_right;
+    }
+    else
+    {
+        distance_ticks_left = (erreur_theta*m_v/2)/m_distPerTick_left;
+        distance_ticks_right = -(erreur_theta*m_v/2)/m_distPerTick_right;
+    }
+    
+    pc.printf("T_%3.2f\t T%3.2f\t ET%3.2f\n\r",theta_*180/PI, getTheta()*180/PI, erreur_theta*180/PI);
+    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 150000, 150000, (long)distance_ticks_right, 150000, 150000, 150000, (long)distance_ticks_left, 1);
+    while(isArrivedRot(erreur_theta))pc.printf("Theta : %3.2f\n\r",getTheta()*180/PI);;
+    pc.printf("Arrived");
+}
+
+void Odometry::GotoB(double distance)
+{
+    double distance_ticks_left = distance/m_distPerTick_left;
+    double distance_ticks_right =  distance/m_distPerTick_right;
+    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 200000, 150000, (long)distance_ticks_right, 150000, 200000, 150000, (long)distance_ticks_left, 1);
+}
+
+bool Odometry::isArrivedRot(double theta_)
+{
+    if((abs_d(getTheta())<=abs_d(theta_)+erreur_ang)|(abs_d(getTheta())>=abs_d(theta_)-erreur_ang)) return true;
+    else return false;
+}