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

stm32/include/KJ2016Tempo.h

@@ -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);
 

stm32/src/KJ2016Tempo.cpp

@@ -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();
     }
 }