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

stm32/include/KJ2016Tempo.h

@@ -10,7 +10,7 @@ class KJ2016Tempo
 {
     static const float KJ_INTERAXIS        = 132.f; // To update
     static const float KJ_WHEEL_DIAMETER   = 70.f;
-    static const float SERVO_MAX_RPM       = 59.f;
+    static const float SERVO_MAX_RPM       = 52.f;
 
     public:
         /**
@@ -18,15 +18,12 @@ class KJ2016Tempo
          */
         static void run(bool isYellow);
 
-        ~KJ2016Tempo();
-
     private:
         const unsigned int LEFT_SERVO_ID;
         const unsigned int RIGHT_SERVO_ID;
         const float SERVO_SPEED_FACTOR;
 
-        void enginesStart();
-        void enginesStop();
+        void stop();
 
         void turn90(bool toLeft);
 

stm32/include/clock.h

@@ -34,6 +34,13 @@ class Clock
          * @see every5ms everyTick MS_PER_TICK
          */
         virtual void everySecond();
+        /**
+         * @brief This function is called approximatively 100ms.
+         *
+         * Important note: the same restrictions than every5ms apply here (but should be less noticeable).
+         * @see every5ms everyTick MS_PER_TICK
+         */
+        virtual void every100ms();
 
     public:
         /**
@@ -103,6 +110,7 @@ class Clock
 
         unsigned long m_tickCount;
         unsigned long m_last5msTick;
+        unsigned long m_last100msTick;
         unsigned long m_last1sTick;
 
         unsigned long m_matchStartTime;

stm32/src/KJ2016Tempo.cpp

@@ -7,12 +7,10 @@
 #include "actionneurs/sensors.h"
 
 KJ2016Tempo::KJ2016Tempo(unsigned int leftServoID, unsigned int rightServoID):
-    LEFT_SERVO_ID(leftServoID), RIGHT_SERVO_ID(rightServoID), SERVO_SPEED_FACTOR((KJ2016Tempo::SERVO_MAX_RPM / 1024.f) * 60.f / (2.f*3.1415))
-{}
-
-KJ2016Tempo::~KJ2016Tempo()
+    LEFT_SERVO_ID(leftServoID), RIGHT_SERVO_ID(rightServoID), SERVO_SPEED_FACTOR((KJ2016Tempo::SERVO_MAX_RPM * 2*3.1415)/(1024*60))
 {
-    enginesStop();
+    ServosNumeriques::changeContinuousRotationMode(LEFT_SERVO_ID,   true);
+    ServosNumeriques::changeContinuousRotationMode(RIGHT_SERVO_ID,  true);
 }
 
 
@@ -48,51 +46,38 @@ void KJ2016Tempo::run(bool isYellow)
 
 }
 
-void KJ2016Tempo::enginesStart()
-{
-    ServosNumeriques::changeContinuousRotationMode(LEFT_SERVO_ID,   true);
-    ServosNumeriques::changeContinuousRotationMode(RIGHT_SERVO_ID,  true);
-}
 
-void KJ2016Tempo::enginesStop()
+void KJ2016Tempo::stop()
 {
-    ServosNumeriques::changeContinuousRotationMode(LEFT_SERVO_ID,   false);
-    ServosNumeriques::changeContinuousRotationMode(RIGHT_SERVO_ID,  false);
+    ServosNumeriques::moveAtSpeed(0, LEFT_SERVO_ID);
+    ServosNumeriques::moveAtSpeed(0, RIGHT_SERVO_ID);
 }
 
 void KJ2016Tempo::turn90(bool toLeft)
 {
-    static const float alpha = (3.1415 * KJ2016Tempo::KJ_INTERAXIS * KJ2016Tempo::SERVO_SPEED_FACTOR) / (2 * KJ2016Tempo::KJ_WHEEL_DIAMETER);
-
-    unsigned int angularSpeed = 0x0100;
-
-    angularSpeed = (toLeft)?angularSpeed:-angularSpeed;
+    static const float alpha = 1.08f*(1000.f * KJ2016Tempo::KJ_INTERAXIS * 3.1415)/(KJ2016Tempo::SERVO_SPEED_FACTOR * KJ2016Tempo::KJ_WHEEL_DIAMETER * 2.f);
 
-    enginesStart();
+    unsigned int angularSpeed = 1023;
 
-    ServosNumeriques::moveAtSpeed(0x0400 + angularSpeed, LEFT_SERVO_ID);
-    ServosNumeriques::moveAtSpeed(0x0400 + angularSpeed, RIGHT_SERVO_ID);
+    ServosNumeriques::moveAtSpeed((toLeft?0:1023) + angularSpeed, RIGHT_SERVO_ID);
+    ServosNumeriques::moveAtSpeed((toLeft?0:1023) + angularSpeed, LEFT_SERVO_ID);
 
     waitForArrival(static_cast<unsigned int>( alpha/(float)angularSpeed ));
 
-    enginesStop();
+    stop();
 }
 
 void KJ2016Tempo::move(int distance)
 {
-    static const float beta = KJ2016Tempo::SERVO_SPEED_FACTOR * 2.f / KJ2016Tempo::KJ_WHEEL_DIAMETER;
-
-    unsigned int angularSpeed = 0x0100;
-
-    angularSpeed = (distance>0)?angularSpeed:-angularSpeed;
+    static const float beta = (1000.f * 2.f)/(KJ2016Tempo::SERVO_SPEED_FACTOR * KJ2016Tempo::KJ_WHEEL_DIAMETER);
 
-    enginesStart();
+    unsigned int angularSpeed = 1023;
 
-    ServosNumeriques::moveAtSpeed(0x0400 - angularSpeed, LEFT_SERVO_ID);
-    ServosNumeriques::moveAtSpeed(0x0400 + angularSpeed, RIGHT_SERVO_ID);
+    ServosNumeriques::moveAtSpeed((distance<0?1023:0) + angularSpeed, LEFT_SERVO_ID);
+    ServosNumeriques::moveAtSpeed((distance>0?1023:0) + angularSpeed, RIGHT_SERVO_ID);
 
-    waitForArrival(static_cast<unsigned int>( beta * (float)distance / (float)angularSpeed ));
-    enginesStop();
+    waitForArrival(static_cast<unsigned int>( beta * (float)((distance<0)?-distance:distance) / (float)angularSpeed ));
+    stop();
 }
 
 void KJ2016Tempo::waitForArrival(unsigned int duration)

stm32/src/clock.cpp

@@ -9,7 +9,7 @@
     #include <QTimer>
 #endif
 
-#ifdef STM32F40_41xxx
+#ifndef STM32F40_41xxx
     #define HW_CLOCK_SPEED 72000000   // 72Mhz
 #else
     #define HW_CLOCK_SPEED 168000000  // 168Mhz
@@ -24,15 +24,22 @@ void Clock::every5ms()
     #ifndef STM32F40_41xxx
         Odometrie::odometrie->update();
         Asservissement::asservissement->update();
-
         Tourelle::getSingleton()->update();
         //StrategieV2::update();
     #endif
 }
 
+void Clock::every100ms()
+{
+    #ifndef STM32F40_41xxx
+        //Tourelle::getSingleton()->update();
+        //StrategieV2::update();
+    #endif
+}
+
 void Clock::everySecond()
 {
-    Led::toggle(0);
+    //Led::toggle(0);
 }
 
 Clock* Clock::getInstance()
@@ -74,13 +81,19 @@ void Clock::tick()
 
     everyTick();
 
-    if((m_tickCount - m_last5msTick) * Clock::MS_PER_TICK > 5)
+    if((m_tickCount - m_last5msTick) * Clock::MS_PER_TICK >= 5)
     {
         every5ms();
         m_last5msTick = m_tickCount;
     }
 
-    if((m_tickCount - m_last5msTick) * Clock::MS_PER_TICK > 1000)
+    if((m_tickCount - m_last100msTick) * Clock::MS_PER_TICK >= 100)
+    {
+        every100ms();
+        m_last100msTick = m_tickCount;
+    }
+
+    if((m_tickCount - m_last1sTick) * Clock::MS_PER_TICK >= 1000)
     {
         everySecond();
         m_last1sTick = m_tickCount;