Voili voilou
Dependencies: RoboClaw StepperMotor mbed
Fork of Robot2016_2-0 by
Diff: Odometry/Odometry.cpp
- Revision:
- 35:4e3d9ab1b94b
- Parent:
- 34:7f8c29ddee61
- Child:
- 36:2d7357a385bc
diff -r 7f8c29ddee61 -r 4e3d9ab1b94b Odometry/Odometry.cpp --- a/Odometry/Odometry.cpp Tue Jan 26 17:14:54 2016 +0000 +++ b/Odometry/Odometry.cpp Tue Jan 26 17:21:11 2016 +0000 @@ -172,3 +172,24 @@ //pc.printf("arrivey %d\n\r",pos_prog); //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI); } + +void Odometry::TestEntraxe(int i) { + int32_t distance_ticks_left; + int32_t distance_ticks_right; + + int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left; + + double erreur_theta = 2*PI*i - getTheta(); + if(erreur_theta < 0) { + distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left; + distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right; + } else { + distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left; + distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right; + } + + roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1); + + while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); + wait(0.4); +}