Commit d7352ed2 authored by root's avatar root

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

parents 58165b9d 1832ba31
......@@ -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);
......
......@@ -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;
......
......@@ -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)
......
......@@ -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;
......
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