Commit d200f99d authored by root's avatar root

Merge branch 'master' of /srv/git/hook_robotique/../repositories/gitRobotique

parents 2ec2a0ae 0d58475f
...@@ -8,7 +8,7 @@ ...@@ -8,7 +8,7 @@
*/ */
class KJ2016Tempo class KJ2016Tempo
{ {
static const float KJ_INTERAXIS = 132.f; // To update static const float KJ_INTERAXIS = 142.56f; // To update
static const float KJ_WHEEL_DIAMETER = 70.f; static const float KJ_WHEEL_DIAMETER = 70.f;
static const float SERVO_MAX_RPM = 52.f; static const float SERVO_MAX_RPM = 52.f;
...@@ -23,7 +23,12 @@ class KJ2016Tempo ...@@ -23,7 +23,12 @@ class KJ2016Tempo
const unsigned int RIGHT_SERVO_ID; const unsigned int RIGHT_SERVO_ID;
const float SERVO_SPEED_FACTOR; const float SERVO_SPEED_FACTOR;
unsigned int m_leftServoSpeed;
unsigned int m_rightServoSpeed;
void stop(); void stop();
void pause();
void resume();
void turn90(bool toLeft); void turn90(bool toLeft);
......
...@@ -48,19 +48,36 @@ void KJ2016Tempo::run(bool isYellow) ...@@ -48,19 +48,36 @@ void KJ2016Tempo::run(bool isYellow)
void KJ2016Tempo::stop() void KJ2016Tempo::stop()
{
m_leftServoSpeed = 0;
m_rightServoSpeed = 0;
ServosNumeriques::moveAtSpeed(0, LEFT_SERVO_ID);
ServosNumeriques::moveAtSpeed(0, RIGHT_SERVO_ID);
}
void KJ2016Tempo::pause()
{ {
ServosNumeriques::moveAtSpeed(0, LEFT_SERVO_ID); ServosNumeriques::moveAtSpeed(0, LEFT_SERVO_ID);
ServosNumeriques::moveAtSpeed(0, RIGHT_SERVO_ID); ServosNumeriques::moveAtSpeed(0, RIGHT_SERVO_ID);
} }
void KJ2016Tempo::resume()
{
ServosNumeriques::moveAtSpeed(m_leftServoSpeed, LEFT_SERVO_ID);
ServosNumeriques::moveAtSpeed(m_rightServoSpeed, RIGHT_SERVO_ID);
}
void KJ2016Tempo::turn90(bool toLeft) void KJ2016Tempo::turn90(bool toLeft)
{ {
static const float alpha = 1.08f*(1000.f * KJ2016Tempo::KJ_INTERAXIS * 3.1415)/(KJ2016Tempo::SERVO_SPEED_FACTOR * KJ2016Tempo::KJ_WHEEL_DIAMETER * 2.f); static const float alpha = (1000.f * KJ2016Tempo::KJ_INTERAXIS * 3.1415)/(KJ2016Tempo::SERVO_SPEED_FACTOR * KJ2016Tempo::KJ_WHEEL_DIAMETER * 2.f);
unsigned int angularSpeed = 1023; unsigned int angularSpeed = 1023;
ServosNumeriques::moveAtSpeed((toLeft?0:1023) + angularSpeed, RIGHT_SERVO_ID); m_leftServoSpeed = (toLeft?0:1023) + angularSpeed;
ServosNumeriques::moveAtSpeed((toLeft?0:1023) + angularSpeed, LEFT_SERVO_ID); m_rightServoSpeed = (toLeft?0:1023) + angularSpeed;
resume();
waitForArrival(static_cast<unsigned int>( alpha/(float)angularSpeed )); waitForArrival(static_cast<unsigned int>( alpha/(float)angularSpeed ));
...@@ -73,8 +90,10 @@ void KJ2016Tempo::move(int distance) ...@@ -73,8 +90,10 @@ void KJ2016Tempo::move(int distance)
unsigned int angularSpeed = 1023; unsigned int angularSpeed = 1023;
ServosNumeriques::moveAtSpeed((distance<0?1023:0) + angularSpeed, LEFT_SERVO_ID); m_leftServoSpeed = (distance<0?1023:0) + angularSpeed;
ServosNumeriques::moveAtSpeed((distance>0?1023:0) + angularSpeed, RIGHT_SERVO_ID); m_rightServoSpeed = (distance>0?1023:0) + angularSpeed;
resume();
waitForArrival(static_cast<unsigned int>( beta * (float)((distance<0)?-distance:distance) / (float)angularSpeed )); waitForArrival(static_cast<unsigned int>( beta * (float)((distance<0)?-distance:distance) / (float)angularSpeed ));
stop(); stop();
...@@ -96,6 +115,9 @@ void KJ2016Tempo::waitForArrival(unsigned int duration) ...@@ -96,6 +115,9 @@ void KJ2016Tempo::waitForArrival(unsigned int duration)
duration -= t; duration -= t;
while(Sensors::getSingleton()->sharpDetect()); // We wait for all sharps to be clear while(Sensors::getSingleton()->sharpDetect()) // We wait for all sharps to be clear
pause();
resume();
} }
} }
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment