Diff: Odometry/Odometry.cpp

Revision:

0:ad9600df4a70

Child:

2:abdf8c6823a1

diff -r 000000000000 -r ad9600df4a70 Odometry/Odometry.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Odometry/Odometry.cpp	Mon Nov 16 11:32:44 2015 +0000
@@ -0,0 +1,73 @@
+#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)
+{
+    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;
+    
+    m_pulses_right = 0;
+    m_pulses_left = 0;
+}
+
+void Odometry::setPos(double x, double y, double theta)
+{
+    this->x = x;
+    this->y = y;
+    this->theta = theta;
+}
+
+void Odometry::setX(double x)
+{
+    this->x = x;
+}
+
+void Odometry::setY(double y)
+{
+    this->y = y;
+}
+
+void Odometry::setTheta(double theta)
+{
+    this->theta = theta;
+}
+
+void Odometry::update_odo(void)
+{
+    long delta_right = roboclaw.ReadEncM1(ADR) - m_pulses_right;
+    m_pulses_right = roboclaw.ReadEncM1(ADR);
+    long delta_left = roboclaw.ReadEncM2(ADR) - m_pulses_left;
+    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 dx = deltaS*cos(theta);
+    double dy = deltaS*sin(theta);
+    
+    x += dx;
+    y += dy;
+    theta += deltaTheta;
+    
+    while(theta > PI) theta -= 2*PI;
+    while(theta <= -PI) theta += 2*PI;
+}
+
+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);
+}