ARES
Time is good
Fork of
Robot2016_2-0
by 
ARES
Revision 0:ad9600df4a70, committed 2015-11-16
- Comitter:
- sype
- Date:
- Mon Nov 16 11:32:44 2015 +0000
- Child:
- 1:cb8c2b2fc37d
- Commit message:
- oklm;
Changed in this revision
--- /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);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Odometry/Odometry.h Mon Nov 16 11:32:44 2015 +0000
@@ -0,0 +1,57 @@
+#ifndef ODOMETRY_H
+#define ODOMETRY_H
+
+#include "mbed.h"
+#include "RoboClaw.h"
+
+#define PI 3.1415926535897932384626433832795
+
+/*
+* Author : Benjamin Bertelone, reworked by Simon Emarre
+*/
+
+class Odometry
+{
+ public:
+ Odometry(double diameter_right, double diameter_left, double v);
+
+ 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);
+
+ double getX() {return x;}
+ double getY() {return y;}
+ double getTheta() {return theta;} // ]-PI;PI]
+
+ double getVitLeft() {return m_vitLeft;}
+ double getVitRight() {return m_vitRight;}
+
+ double getDistLeft() {return m_distLeft;}
+ double getDistRight() {return m_distRight;}
+
+ void setDistLeft(double dist) {m_distLeft = dist;}
+ void setDistRight(double dist) {m_distRight = dist;}
+
+ void update_odo(void);
+ double calcul_distance(double x, double y, double theta_goal);
+
+ long getPulsesLeft(void) {return m_pulses_left;}
+ long getPulsesRight(void) {return m_pulses_right;}
+
+ private:
+
+ long m_pulses_left;
+ long m_pulses_right;
+
+ double x, y, theta;
+ double m_vitLeft, m_vitRight;
+ double m_distLeft, m_distRight;
+
+ double m_distPerTick_left, m_distPerTick_right, m_v;
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RoboClaw.lib Mon Nov 16 11:32:44 2015 +0000 @@ -0,0 +1,1 @@ +RoboClaw#af5cf35e1a25
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Nov 16 11:32:44 2015 +0000
@@ -0,0 +1,36 @@
+#include "Odometry.h"
+
+#define dt 10000
+
+Ticker ticker;
+//Timer t;
+
+Odometry odo(47.4, 47.2, 231.0);
+
+//Serial pc(PA_9, PA_10);
+//Serial pc(USBTX, USBRX);
+
+void update_main(void);
+
+int main(void)
+{
+ odo.setPos(0, 0, 0);
+ //pc.baud(115200);
+ //pc.printf("Hello world !\n\r");
+ 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);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Nov 16 11:32:44 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/ba1f97679dad \ No newline at end of file