From 1832ba313998ab8e5febb93e245c14a715e086f6 Mon Sep 17 00:00:00 2001 From: Arnaud Cadot Date: Tue, 22 Mar 2016 18:50:19 +0100 Subject: [PATCH] Updated KJ tempo, fixed bug in clock --- stm32/include/KJ2016Tempo.h | 7 ++---- stm32/include/clock.h | 8 ++++++ stm32/src/KJ2016Tempo.cpp | 49 +++++++++++++------------------------ stm32/src/clock.cpp | 23 +++++++++++++---- 4 files changed, 45 insertions(+), 42 deletions(-) mode change 100644 => 100755 stm32/include/KJ2016Tempo.h mode change 100644 => 100755 stm32/include/clock.h mode change 100644 => 100755 stm32/src/KJ2016Tempo.cpp mode change 100644 => 100755 stm32/src/clock.cpp diff --git a/stm32/include/KJ2016Tempo.h b/stm32/include/KJ2016Tempo.h old mode 100644 new mode 100755 index 9e19a41f..63ef6e76 --- a/stm32/include/KJ2016Tempo.h +++ b/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); diff --git a/stm32/include/clock.h b/stm32/include/clock.h old mode 100644 new mode 100755 index 776318cb..7f1df6ed --- a/stm32/include/clock.h +++ b/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; diff --git a/stm32/src/KJ2016Tempo.cpp b/stm32/src/KJ2016Tempo.cpp old mode 100644 new mode 100755 index 4439dc1d..465aa1e5 --- a/stm32/src/KJ2016Tempo.cpp +++ b/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( 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( beta * (float)distance / (float)angularSpeed )); - enginesStop(); + waitForArrival(static_cast( beta * (float)((distance<0)?-distance:distance) / (float)angularSpeed )); + stop(); } void KJ2016Tempo::waitForArrival(unsigned int duration) diff --git a/stm32/src/clock.cpp b/stm32/src/clock.cpp old mode 100644 new mode 100755 index 56331c93..8b3d48ee --- a/stm32/src/clock.cpp +++ b/stm32/src/clock.cpp @@ -9,7 +9,7 @@ #include #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; -- GitLab