ARES / Mbed 2 deprecated TestMyPathFind

Test du path finding

Dependencies:   RoboClaw mbed

Fork of TestMyPathFind by Romain Ame

Odometry/Odometry.cpp@0:ad9600df4a70, 2015-11-16 (annotated)

Committer:
sype
Date:
Mon Nov 16 11:32:44 2015 +0000
Revision:
0:ad9600df4a70
Child:
2:abdf8c6823a1
oklm;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sype 0:ad9600df4a70 1 #include "Odometry.h"
sype 0:ad9600df4a70 2
sype 0:ad9600df4a70 3 Serial pc(USBTX, USBRX);
sype 0:ad9600df4a70 4
sype 0:ad9600df4a70 5 // M1 = Moteur droit, M2 = Moteur gauche
sype 0:ad9600df4a70 6
sype 0:ad9600df4a70 7 RoboClaw roboclaw(115200, PA_11, PA_12);
sype 0:ad9600df4a70 8
sype 0:ad9600df4a70 9 Odometry::Odometry(double diameter_right, double diameter_left, double v)
sype 0:ad9600df4a70 10 {
sype 0:ad9600df4a70 11 pc.baud(115200);
sype 0:ad9600df4a70 12 pc.printf("Hello world\n\r");
sype 0:ad9600df4a70 13 roboclaw.ResetEnc(ADR);
sype 0:ad9600df4a70 14 m_v = v;
sype 0:ad9600df4a70 15 m_distPerTick_left = diameter_left*PI/37400;
sype 0:ad9600df4a70 16 m_distPerTick_right = diameter_right*PI/37400;
sype 0:ad9600df4a70 17
sype 0:ad9600df4a70 18 m_pulses_right = 0;
sype 0:ad9600df4a70 19 m_pulses_left = 0;
sype 0:ad9600df4a70 20 }
sype 0:ad9600df4a70 21
sype 0:ad9600df4a70 22 void Odometry::setPos(double x, double y, double theta)
sype 0:ad9600df4a70 23 {
sype 0:ad9600df4a70 24 this->x = x;
sype 0:ad9600df4a70 25 this->y = y;
sype 0:ad9600df4a70 26 this->theta = theta;
sype 0:ad9600df4a70 27 }
sype 0:ad9600df4a70 28
sype 0:ad9600df4a70 29 void Odometry::setX(double x)
sype 0:ad9600df4a70 30 {
sype 0:ad9600df4a70 31 this->x = x;
sype 0:ad9600df4a70 32 }
sype 0:ad9600df4a70 33
sype 0:ad9600df4a70 34 void Odometry::setY(double y)
sype 0:ad9600df4a70 35 {
sype 0:ad9600df4a70 36 this->y = y;
sype 0:ad9600df4a70 37 }
sype 0:ad9600df4a70 38
sype 0:ad9600df4a70 39 void Odometry::setTheta(double theta)
sype 0:ad9600df4a70 40 {
sype 0:ad9600df4a70 41 this->theta = theta;
sype 0:ad9600df4a70 42 }
sype 0:ad9600df4a70 43
sype 0:ad9600df4a70 44 void Odometry::update_odo(void)
sype 0:ad9600df4a70 45 {
sype 0:ad9600df4a70 46 long delta_right = roboclaw.ReadEncM1(ADR) - m_pulses_right;
sype 0:ad9600df4a70 47 m_pulses_right = roboclaw.ReadEncM1(ADR);
sype 0:ad9600df4a70 48 long delta_left = roboclaw.ReadEncM2(ADR) - m_pulses_left;
sype 0:ad9600df4a70 49 m_pulses_left = roboclaw.ReadEncM2(ADR);
sype 0:ad9600df4a70 50
sype 0:ad9600df4a70 51 double deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0f;
sype 0:ad9600df4a70 52 double deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left) / m_v;
sype 0:ad9600df4a70 53
sype 0:ad9600df4a70 54 double dx = deltaS*cos(theta);
sype 0:ad9600df4a70 55 double dy = deltaS*sin(theta);
sype 0:ad9600df4a70 56
sype 0:ad9600df4a70 57 x += dx;
sype 0:ad9600df4a70 58 y += dy;
sype 0:ad9600df4a70 59 theta += deltaTheta;
sype 0:ad9600df4a70 60
sype 0:ad9600df4a70 61 while(theta > PI) theta -= 2*PI;
sype 0:ad9600df4a70 62 while(theta <= -PI) theta += 2*PI;
sype 0:ad9600df4a70 63 }
sype 0:ad9600df4a70 64
sype 0:ad9600df4a70 65 void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal)
sype 0:ad9600df4a70 66 {
sype 0:ad9600df4a70 67 double theta_ = atan2(y_goal-y, x_goal-x);
sype 0:ad9600df4a70 68 double distance_ticks_left = (theta_*m_v/2)/m_distPerTick_left;
sype 0:ad9600df4a70 69 double distance_ticks_right = (theta_*m_v/2)/m_distPerTick_right;
sype 0:ad9600df4a70 70 pc.printf("Theta : %3.2f\tDL : %6.2f\tDR : %6.2f\n\r",theta_*180/PI,distance_ticks_left, distance_ticks_right);
sype 0:ad9600df4a70 71 //roboclaw.SpeedAccelDeccelPositionM1(ADR, 300000, 150000, 300000, distance_ticks_right, 1);
sype 0:ad9600df4a70 72 //roboclaw.SpeedAccelDeccelPositionM2(ADR, 300000, 150000, 300000, distance_ticks_left, 1);
sype 0:ad9600df4a70 73 }