Commit d7352ed2 authored by root's avatar root
Browse files

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

parents 58165b9d 1832ba31
...@@ -10,7 +10,7 @@ class KJ2016Tempo ...@@ -10,7 +10,7 @@ class KJ2016Tempo
{ {
static const float KJ_INTERAXIS = 132.f; // To update static const float KJ_INTERAXIS = 132.f; // To update
static const float KJ_WHEEL_DIAMETER = 70.f; 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: public:
/** /**
...@@ -18,15 +18,12 @@ class KJ2016Tempo ...@@ -18,15 +18,12 @@ class KJ2016Tempo
*/ */
static void run(bool isYellow); static void run(bool isYellow);
~KJ2016Tempo();
private: private:
const unsigned int LEFT_SERVO_ID; const unsigned int LEFT_SERVO_ID;
const unsigned int RIGHT_SERVO_ID; const unsigned int RIGHT_SERVO_ID;
const float SERVO_SPEED_FACTOR; const float SERVO_SPEED_FACTOR;
void enginesStart(); void stop();
void enginesStop();
void turn90(bool toLeft); void turn90(bool toLeft);
......
...@@ -34,6 +34,13 @@ class Clock ...@@ -34,6 +34,13 @@ class Clock
* @see every5ms everyTick MS_PER_TICK * @see every5ms everyTick MS_PER_TICK
*/ */
virtual void everySecond(); 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: public:
/** /**
...@@ -103,6 +110,7 @@ class Clock ...@@ -103,6 +110,7 @@ class Clock
unsigned long m_tickCount; unsigned long m_tickCount;
unsigned long m_last5msTick; unsigned long m_last5msTick;
unsigned long m_last100msTick;
unsigned long m_last1sTick; unsigned long m_last1sTick;
unsigned long m_matchStartTime; unsigned long m_matchStartTime;
......
...@@ -7,12 +7,10 @@ ...@@ -7,12 +7,10 @@
#include "actionneurs/sensors.h" #include "actionneurs/sensors.h"
KJ2016Tempo::KJ2016Tempo(unsigned int leftServoID, unsigned int rightServoID): 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)) LEFT_SERVO_ID(leftServoID), RIGHT_SERVO_ID(rightServoID), SERVO_SPEED_FACTOR((KJ2016Tempo::SERVO_MAX_RPM * 2*3.1415)/(1024*60))
{}
KJ2016Tempo::~KJ2016Tempo()
{ {
enginesStop(); ServosNumeriques::changeContinuousRotationMode(LEFT_SERVO_ID, true);
ServosNumeriques::changeContinuousRotationMode(RIGHT_SERVO_ID, true);
} }
...@@ -48,51 +46,38 @@ void KJ2016Tempo::run(bool isYellow) ...@@ -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::moveAtSpeed(0, LEFT_SERVO_ID);
ServosNumeriques::changeContinuousRotationMode(RIGHT_SERVO_ID, false); ServosNumeriques::moveAtSpeed(0, RIGHT_SERVO_ID);
} }
void KJ2016Tempo::turn90(bool toLeft) void KJ2016Tempo::turn90(bool toLeft)
{ {
static const float alpha = (3.1415 * KJ2016Tempo::KJ_INTERAXIS * KJ2016Tempo::SERVO_SPEED_FACTOR) / (2 * KJ2016Tempo::KJ_WHEEL_DIAMETER); static const float alpha = 1.08f*(1000.f * KJ2016Tempo::KJ_INTERAXIS * 3.1415)/(KJ2016Tempo::SERVO_SPEED_FACTOR * KJ2016Tempo::KJ_WHEEL_DIAMETER * 2.f);
unsigned int angularSpeed = 0x0100;
angularSpeed = (toLeft)?angularSpeed:-angularSpeed;
enginesStart(); unsigned int angularSpeed = 1023;
ServosNumeriques::moveAtSpeed(0x0400 + angularSpeed, LEFT_SERVO_ID); ServosNumeriques::moveAtSpeed((toLeft?0:1023) + angularSpeed, RIGHT_SERVO_ID);
ServosNumeriques::moveAtSpeed(0x0400 + angularSpeed, RIGHT_SERVO_ID); ServosNumeriques::moveAtSpeed((toLeft?0:1023) + angularSpeed, LEFT_SERVO_ID);
waitForArrival(static_cast<unsigned int>( alpha/(float)angularSpeed )); waitForArrival(static_cast<unsigned int>( alpha/(float)angularSpeed ));
enginesStop(); stop();
} }
void KJ2016Tempo::move(int distance) void KJ2016Tempo::move(int distance)
{ {
static const float beta = KJ2016Tempo::SERVO_SPEED_FACTOR * 2.f / KJ2016Tempo::KJ_WHEEL_DIAMETER; static const float beta = (1000.f * 2.f)/(KJ2016Tempo::SERVO_SPEED_FACTOR * KJ2016Tempo::KJ_WHEEL_DIAMETER);
unsigned int angularSpeed = 0x0100;
angularSpeed = (distance>0)?angularSpeed:-angularSpeed;
enginesStart(); unsigned int angularSpeed = 1023;
ServosNumeriques::moveAtSpeed(0x0400 - angularSpeed, LEFT_SERVO_ID); ServosNumeriques::moveAtSpeed((distance<0?1023:0) + angularSpeed, LEFT_SERVO_ID);
ServosNumeriques::moveAtSpeed(0x0400 + angularSpeed, RIGHT_SERVO_ID); ServosNumeriques::moveAtSpeed((distance>0?1023:0) + angularSpeed, RIGHT_SERVO_ID);
waitForArrival(static_cast<unsigned int>( beta * (float)distance / (float)angularSpeed )); waitForArrival(static_cast<unsigned int>( beta * (float)((distance<0)?-distance:distance) / (float)angularSpeed ));
enginesStop(); stop();
} }
void KJ2016Tempo::waitForArrival(unsigned int duration) void KJ2016Tempo::waitForArrival(unsigned int duration)
......
...@@ -9,7 +9,7 @@ ...@@ -9,7 +9,7 @@
#include <QTimer> #include <QTimer>
#endif #endif
#ifdef STM32F40_41xxx #ifndef STM32F40_41xxx
#define HW_CLOCK_SPEED 72000000 // 72Mhz #define HW_CLOCK_SPEED 72000000 // 72Mhz
#else #else
#define HW_CLOCK_SPEED 168000000 // 168Mhz #define HW_CLOCK_SPEED 168000000 // 168Mhz
...@@ -24,15 +24,22 @@ void Clock::every5ms() ...@@ -24,15 +24,22 @@ void Clock::every5ms()
#ifndef STM32F40_41xxx #ifndef STM32F40_41xxx
Odometrie::odometrie->update(); Odometrie::odometrie->update();
Asservissement::asservissement->update(); Asservissement::asservissement->update();
Tourelle::getSingleton()->update(); Tourelle::getSingleton()->update();
//StrategieV2::update(); //StrategieV2::update();
#endif #endif
} }
void Clock::every100ms()
{
#ifndef STM32F40_41xxx
//Tourelle::getSingleton()->update();
//StrategieV2::update();
#endif
}
void Clock::everySecond() void Clock::everySecond()
{ {
Led::toggle(0); //Led::toggle(0);
} }
Clock* Clock::getInstance() Clock* Clock::getInstance()
...@@ -74,13 +81,19 @@ void Clock::tick() ...@@ -74,13 +81,19 @@ void Clock::tick()
everyTick(); everyTick();
if((m_tickCount - m_last5msTick) * Clock::MS_PER_TICK > 5) if((m_tickCount - m_last5msTick) * Clock::MS_PER_TICK >= 5)
{ {
every5ms(); every5ms();
m_last5msTick = m_tickCount; 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(); everySecond();
m_last1sTick = m_tickCount; m_last1sTick = m_tickCount;
......
Supports Markdown
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