Commit 0d58475f authored by Arnaud Cadot's avatar Arnaud Cadot

Updated sharp checks for KJ (servos weren't stopping)

parent 00b93aa7
......@@ -8,7 +8,7 @@
*/
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 SERVO_MAX_RPM = 52.f;
......@@ -23,7 +23,12 @@ class KJ2016Tempo
const unsigned int RIGHT_SERVO_ID;
const float SERVO_SPEED_FACTOR;
unsigned int m_leftServoSpeed;
unsigned int m_rightServoSpeed;
void stop();
void pause();
void resume();
void turn90(bool toLeft);
......
......@@ -48,19 +48,36 @@ void KJ2016Tempo::run(bool isYellow)
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, 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)
{
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;
ServosNumeriques::moveAtSpeed((toLeft?0:1023) + angularSpeed, RIGHT_SERVO_ID);
ServosNumeriques::moveAtSpeed((toLeft?0:1023) + angularSpeed, LEFT_SERVO_ID);
m_leftServoSpeed = (toLeft?0:1023) + angularSpeed;
m_rightServoSpeed = (toLeft?0:1023) + angularSpeed;
resume();
waitForArrival(static_cast<unsigned int>( alpha/(float)angularSpeed ));
......@@ -73,8 +90,10 @@ void KJ2016Tempo::move(int distance)
unsigned int angularSpeed = 1023;
ServosNumeriques::moveAtSpeed((distance<0?1023:0) + angularSpeed, LEFT_SERVO_ID);
ServosNumeriques::moveAtSpeed((distance>0?1023:0) + angularSpeed, RIGHT_SERVO_ID);
m_leftServoSpeed = (distance<0?1023:0) + angularSpeed;
m_rightServoSpeed = (distance>0?1023:0) + angularSpeed;
resume();
waitForArrival(static_cast<unsigned int>( beta * (float)((distance<0)?-distance:distance) / (float)angularSpeed ));
stop();
......@@ -96,6 +115,9 @@ void KJ2016Tempo::waitForArrival(unsigned int duration)
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