Commit 9c649c74 authored by Arnaud Cadot's avatar Arnaud Cadot

Updated KJ tempo

parent 52ce2907
......@@ -8,8 +8,9 @@
*/
class KJ2016Tempo
{
unsigned int LEFT_SERVO_ID;
unsigned int RIGHT_SERVO_ID;
static const float KJ_INTERAXIS = 100.f; // To update
static const float KJ_WHEEL_DIAMETER = 60.f;
static const float SERVO_MAX_RPM = 59.f;
public:
/**
......@@ -20,6 +21,9 @@ class KJ2016Tempo
~KJ2016Tempo();
private:
const unsigned int LEFT_SERVO_ID;
const unsigned int RIGHT_SERVO_ID;
const float SERVO_SPEED_FACTOR;
void enginesStart();
void enginesStop();
......
......@@ -5,7 +5,8 @@
#include "clock.h"
KJ2016Tempo::KJ2016Tempo(unsigned int leftServoID, unsigned int rightServoID): LEFT_SERVO_ID(leftServoID), RIGHT_SERVO_ID(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))
{}
KJ2016Tempo::~KJ2016Tempo()
......@@ -60,36 +61,35 @@ void KJ2016Tempo::enginesStop()
void KJ2016Tempo::turn90(bool toLeft)
{
const float correctionFactor = 1.0f;
static const float alpha = (3.1415 * KJ2016Tempo::KJ_INTERAXIS * KJ2016Tempo::SERVO_SPEED_FACTOR) / (2 * KJ2016Tempo::KJ_WHEEL_DIAMETER);
const unsigned int servoOne = toLeft?LEFT_SERVO_ID:RIGHT_SERVO_ID;
const unsigned int servoTwo = toLeft?RIGHT_SERVO_ID:LEFT_SERVO_ID;
unsigned int angularSpeed = 0x0100;
const unsigned int speed = 0x0100;
angularSpeed = (toLeft)?angularSpeed:-angularSpeed;
enginesStart();
ServosNumeriques::moveAtSpeed(0x0400 - speed, servoOne);
ServosNumeriques::moveAtSpeed(0x0400 + speed, servoTwo);
ServosNumeriques::moveAtSpeed(0x0400 + angularSpeed, LEFT_SERVO_ID);
ServosNumeriques::moveAtSpeed(0x0400 + angularSpeed, RIGHT_SERVO_ID);
Clock::delay(static_cast<unsigned int>( 1/((float)speed) * correctionFactor));
Clock::delay(static_cast<unsigned int>( alpha/(float)angularSpeed ));
enginesStop();
}
void KJ2016Tempo::move(int distance)
{
const float correctionFactor = 1.f;
static const float beta = KJ2016Tempo::SERVO_SPEED_FACTOR * 2.f / KJ2016Tempo::KJ_WHEEL_DIAMETER;
unsigned int speed = 0x0100;
unsigned int angularSpeed = 0x0100;
speed = (distance>0)?speed:-speed;
angularSpeed = (distance>0)?angularSpeed:-angularSpeed;
enginesStart();
ServosNumeriques::moveAtSpeed(0x0400 + speed, LEFT_SERVO_ID);
ServosNumeriques::moveAtSpeed(0x0400 + speed, RIGHT_SERVO_ID);
ServosNumeriques::moveAtSpeed(0x0400 - angularSpeed, LEFT_SERVO_ID);
ServosNumeriques::moveAtSpeed(0x0400 + angularSpeed, RIGHT_SERVO_ID);
Clock::delay(static_cast<unsigned int>((float)distance / (float)speed * correctionFactor));
Clock::delay(static_cast<unsigned int>( beta * (float)distance / (float)angularSpeed ));
enginesStop();
}
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