Revision 2:abdf8c6823a1, committed 2015-11-24

Comitter:

sype

Date:

Tue Nov 24 15:02:01 2015 +0000

Parent:

1:cb8c2b2fc37d

Child:

3:62e9d715de65

Commit message:

gotothet

Changed in this revision

--- 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;
+}

--- a/Odometry/Odometry.h	Mon Nov 16 11:34:08 2015 +0000
+++ b/Odometry/Odometry.h	Tue Nov 24 15:02:01 2015 +0000
@@ -5,27 +5,37 @@
 #include "RoboClaw.h"
 
 #define PI 3.1415926535897932384626433832795
+#define C 1.1538461538461538461538461538462
 
 /*
 *   Author : Benjamin Bertelone, reworked by Simon Emarre
 */
 
+extern Serial pc;
+
 class Odometry
 {
     public:
-        Odometry(double diameter_right, double diameter_left, double v);
+        Odometry(double diameter_right, double diameter_left, double v, RoboClaw &rc);
         
         void setPos(double x, double y, double theta);
         void setX(double x);
         void setY(double y);
         void setTheta(double theta);
         
-        void GotoXYT(double x, double y, double theta_goal);
-        void GotoThet(double theta_goal);
+        void GotoXYT(double x_goal, double y_goal, double theta_goal);
+        void GotoThet(double theta_);
+        void GotoB(double distance);
         
         double getX() {return x;}
         double getY() {return y;}
         double getTheta() {return theta;} // ]-PI;PI]
+        double getTheta_(double x, double y);
+        
+        double abs_d(double x){
+            if(x<0) return -x;
+            else return x;
+        }
         
         double getVitLeft() {return m_vitLeft;}
         double getVitRight() {return m_vitRight;}
@@ -41,9 +51,12 @@
         
         long getPulsesLeft(void) {return m_pulses_left;}
         long getPulsesRight(void) {return m_pulses_right;}
+        double carre(double a) {return a*a;}
+        
+        bool isArrivedRot(double theta_);
     
     private:
-    
+        RoboClaw &roboclaw;
         long m_pulses_left;
         long m_pulses_right;
         
@@ -52,6 +65,8 @@
         double m_distLeft, m_distRight;
         
         double m_distPerTick_left, m_distPerTick_right, m_v;
+        
+        double erreur_ang;
 };
 
 #endif

--- a/RoboClaw.lib	Mon Nov 16 11:34:08 2015 +0000
+++ b/RoboClaw.lib	Tue Nov 24 15:02:01 2015 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/ARES/code/RoboClaw/#af5cf35e1a25
+https://developer.mbed.org/teams/ARES/code/RoboClaw/#f76058f9f548

--- a/main.cpp	Mon Nov 16 11:34:08 2015 +0000
+++ b/main.cpp	Tue Nov 24 15:02:01 2015 +0000
@@ -2,35 +2,39 @@
 
 #define dt 10000
 
+InterruptIn button(USER_BUTTON);
+DigitalOut led(LED1);
 Ticker ticker;
-//Timer t;
-
-Odometry odo(47.4, 47.2, 231.0);
-
+Serial pc(USBTX, USBRX);
 //Serial pc(PA_9, PA_10);
-//Serial pc(USBTX, USBRX);
+RoboClaw roboclaw(115200, PA_11, PA_12);
+Odometry odo(47.4, 47.1, 242.0, roboclaw);
 
 void update_main(void);
+void init(void);
 
 int main(void)
+{   
+    init();
+    odo.GotoXYT(1000,300,0);
+    while(1);
+}
+
+void init(void)
 {
-    odo.setPos(0, 0, 0);
-    //pc.baud(115200);
-    //pc.printf("Hello world !\n\r");
+    pc.baud(115200);
+    pc.printf("Hello from main !\n\r");
+    roboclaw.ForwardM1(ADR, 0);
+    roboclaw.ForwardM2(ADR, 0);
+    wait_ms(500);
+    roboclaw.ResetEnc(ADR);
+    odo.setPos(100, 1000, -PI/2);
     ticker.attach_us(&update_main,dt); // 100 Hz
-    wait_ms(1000);
-    
-    while(1)
-    {
-        //pc.printf("Enc1 : %6.2f\tSpeed1 :%6.2f\tEnc2 : %6.2f\tSpeed2 : %6.2f\n\r",enc1,speed1,enc2,speed2);
-        //pc.printf("Theta : %5d\n\r", _roboclaw.ReadEncM1(ADR));
-        wait_ms(10);
-    }
 }
 
 void update_main(void)
 {
     odo.update_odo();
-    odo.GotoXYT(100,100,0);
-    //pc.printf("Theta : %3.3f\n\r", odo.getTheta()*180/PI);
+    //pc.printf("EncM1 : %6d\tEncM2 : %6d\tTheta : %3.2f\n\r", roboclaw.ReadEncM1(ADR), roboclaw.ReadEncM2(ADR), odo.getTheta()*180/PI);
+    //if(pc.readable()) if(pc.getc()=='l') led = !led;
 }