...
 
Commits (3)
#ifndef MOTEUR_HPP
#define MOTEUR_HPP
#include <stdint.h>
#include <Servo.h>
#include "configuration.hpp"
extern unsigned long t;
class Moteur
{
public:
void init()
{
setConsigne( moteurNeutre );
}
inline uint16_t getVitesse() const { return mVitesse; }
inline uint16_t getConsigne() const { return mConsigne; }
void setConsigne( uint16_t vitesse )
{
mConsigne = constrain( vitesse, moteurVMaxAv, moteurVMaxAr );
mT0 = t;
mV0 = mVitesse;
}
void stop()
{
}
void MAJ()
{
if( mVitesse < mConsigne )
{
mVitesse = mV0 + moteurAMax * ( t - mT0 ) / 1000;
if( mVitesse > mConsigne ) mVitesse = mConsigne;
}
if( mVitesse > mConsigne )
{
mVitesse = mV0 - moteurAMax * ( t - mT0 ) / 1000;
if( mVitesse < mConsigne ) mVitesse = mConsigne;
}
}
private:
bool freinActif = true;
uint16_t mConsigne = moteurNeutre;
uint16_t mVitesse = moteurNeutre;
uint16_t mV0 = moteurNeutre;
unsigned long mT0 = 0;
};
#endif
#ifndef CONFIGURATION_HPP
#define CONFIGURATION_HPP
/* Moteur */
constexpr uint8_t moteurNeutre = 128;
constexpr uint8_t moteurVMinAv = 120;
constexpr uint8_t moteurVMaxAv = 90;
constexpr uint8_t moteurVMinAr = 140;
constexpr uint8_t moteurVMaxAr = 150;
constexpr float moteurAMax = 50;
#endif
#include "Moteur.hpp"
/* Variables globales
*/
// temps en millisecondes depuis le boot
unsigned long t = 0;
// temps en millisecondes de la derniere itération
unsigned long t0 = 0;
Moteur moteur;
enum Etat
{
arret,
avant,
arriere
};
Etat etat = arret;
void setup()
{
t = millis();
// configuration du port serie
Serial.begin( 57600 );
// configuration des composants
moteur.init();
t0 = t;
}
void loop()
{
// mise à jour du temps global
t = millis();
unsigned long dt = t - t0;
moteur.MAJ();
Serial.print( "0,255," );
Serial.print( moteur.getConsigne() );
Serial.print( ',' );
Serial.print( moteur.getVitesse() );
Serial.print( '\n' );
switch ( etat )
{
case arret:
if ( dt > 2333 )
{
moteur.setConsigne( 90 );
etat = avant;
t0 = t;
}
break;
case avant:
if ( dt > 1333 )
{
moteur.setConsigne( 150 );
etat = arriere;
t0 = t;
}
break;
case arriere:
if ( dt > 1333 )
{
moteur.setConsigne( 128 );
etat = arret;
t0 = t;
}
break;
}
}
#ifndef MOTOR_HPP
#define MOTOR_HPP
#include <stdint.h>
#include "StateMachine.hpp"
#include "RampServo.hpp"
/* time of current loop */
extern unsigned long t;
class Motor
{
public:
static constexpr int8_t forwardMax = 20;
static constexpr int8_t reverseMax = -20;
static constexpr int servoDelay = 4;
static constexpr int8_t kickAngle = 45;
static constexpr unsigned long kickDuration = 200;
static constexpr unsigned long BreakDuration = 1000;
Motor()
: mStateMachine( this )
{
mStateMachine.push( & Motor::stateStop );
}
inline void attach( int servoPin )
{
mRampServo.attach( servoPin );
}
inline void update()
{
mStateMachine.activate();
mRampServo.update();
}
void setSpeed( int8_t speed )
{
if ( speed > forwardMax ) speed = forwardMax;
if ( speed < reverseMax ) speed = reverseMax;
mSpeed = speed;
}
inline void stop()
{
mSpeed = 0;
}
inline void forwardSlow()
{
mSpeed = 10;
}
inline void forwardFast()
{
mSpeed = forwardMax;
}
inline void reverseSlow()
{
mSpeed = -10;
}
inline void reverseFast()
{
mSpeed = reverseMax;
}
protected:
void stateStop( bool entering )
{
if ( entering )
{
mRampServo.write( calibration.servoNeutral );
}
if ( mSpeed > 0 ) mStateMachine.set( & Motor::stateKickForward );
if ( mSpeed < 0 ) mStateMachine.set( & Motor::stateKickReverse );
}
void stateKickForward( bool entering )
{
if ( entering )
{
mT = t + kickDuration;
mRampServo.setAngle( calibration.servoNeutral + kickAngle, servoDelay );
}
if ( t >= mT ) mStateMachine.set( & Motor::stateForward );
}
void stateForward( bool )
{
if ( mSpeed <= 0 )
mStateMachine.set( & Motor::stateBrake );
else
mRampServo.setAngle( calibration.servoNeutral + mSpeed, servoDelay );
}
void stateBrake( bool entering )
{
if ( entering )
{
mT = t + BreakDuration;
mRampServo.setAngle( calibration.servoNeutral - kickAngle, servoDelay );
}
if ( t > mT ) mStateMachine.set( & Motor::stateStop );
}
void stateKickReverse( bool entering )
{
if ( entering )
{
mT = t + kickDuration;
mRampServo.setAngle( calibration.servoNeutral - kickAngle, servoDelay );
}
if ( t >= mT ) mStateMachine.set( & Motor::stateReverse );
}
void stateReverse( bool )
{
if ( mSpeed >= 0 )
mStateMachine.set( & Motor::stateStop );
else
mRampServo.setAngle( calibration.servoNeutral + mSpeed, servoDelay );
}
private:
unsigned long mT = 0;
StateMachine< Motor > mStateMachine;
RampServo mRampServo;
int8_t mSpeed = 0;
struct Calibration {
uint8_t servoNeutral = 90;
} calibration;
};
#endif
#ifndef RAMPSERVO_HPP
#define RAMPSERVO_HPP
#include <stdint.h>
#include <Servo.h>
/* time of current loop */
extern unsigned long t;
class RampServo
{
static constexpr uint8_t defaultAngle = 90;
static constexpr uint8_t maxAxgle = 30;
static constexpr int defaultDelay = 10;
static constexpr uint8_t limit = 180;
public:
inline uint8_t getAngle() const
{
return mAngle;
}
inline setMin( uint8_t angle )
{
if ( angle > limit ) angle = limit;
if ( angle > settings.max ) settings.max = angle;
settings.min = angle;
}
inline setMax( uint8_t angle )
{
if ( angle > limit ) angle = limit;
if ( angle < settings.min ) settings.min = angle;
settings.max = angle;
}
void setAngle( uint8_t angle )
{
mSetpoint = angle;
if ( mSetpoint > settings.max ) mSetpoint = settings.max;
if ( mSetpoint < settings.min ) mSetpoint = settings.min;
}
inline void setAngle( uint8_t angle, unsigned long ms )
{
mDelay = ms;
setAngle( angle );
}
inline unsigned long getDelay() const
{
return mDelay;
}
inline void setDelay( unsigned long ms )
{
mDelay = ms;
}
void update()
{
if ( t < mT ) return;
if ( mAngle < mSetpoint ) {
++mAngle;
mT = t + mDelay;
}
if ( mAngle > mSetpoint ) {
--mAngle;
mT = t + mDelay;
}
mServo.write( mAngle );
}
inline void attach( uint8_t pin ) {
mServo.attach( pin );
}
inline void write( uint8_t angle ) {
setAngle( angle );
}
private:
Servo mServo;
struct Settings {
uint8_t min = defaultAngle - maxAxgle;
uint8_t max = defaultAngle + maxAxgle;
int middle = defaultAngle;
bool swap = false;
} settings;
uint8_t mAngle = settings.middle;
uint8_t mSetpoint = settings.middle;
unsigned long mT = 0;
unsigned long mDelay = defaultDelay;
};
#endif
template< class T >
class StateMachine
{
public:
StateMachine( T * instance )
: mInstance( instance )
{}
typedef void ( T::* StateFunction )( bool entering );
bool push( StateFunction stateFunction )
{
if ( mStackSize >= mStackMax ) return false;
++mStackSize;
mStack[ mStackSize ] = stateFunction;
mChanging = true;
return true;
};
bool pop()
{
if ( mStackSize < 0 ) return false;
--mStackSize;
return true;
};
inline bool set( StateFunction stateFunction )
{
pop();
return push( stateFunction );
}
void activate()
{
if( mStackSize < 0 ) return;
if( mChanging )
{
mChanging = false;
( mInstance->* mStack[ mStackSize ] ) ( true );
}
else
{
( mInstance->* mStack[ mStackSize ] ) ( false );
}
}
private:
T * mInstance;
static constexpr int mStackMax { 8 };
StateFunction mStack[ mStackMax ] {};
int mStackSize { -1 };
bool mChanging = false;
};
#include "Motor.hpp"
unsigned long t = 0;
unsigned long t0 = 0;
Motor motor;
bool timeAt( uint16_t timeStamp ) {
return ( t0 <= timeStamp && t > timeStamp );
}
void reboot()
{
asm volatile (" jmp 0");
}
void setup()
{
motor.attach( 9 );
}
void loop()
{
t0 = t;
t = millis();
motor.update();
if ( timeAt( 1000 ) ) motor.stop();
if ( timeAt( 2000 ) ) motor.forwardSlow();
if ( timeAt( 4000 ) ) motor.stop();
if ( timeAt( 6000 ) ) motor.forwardFast();
if ( timeAt( 8000 ) ) motor.stop();
if ( timeAt( 10000 ) ) motor.reverseSlow();
if ( timeAt( 12000 ) ) motor.stop();
if ( timeAt( 14000 ) ) motor.reverseFast();
if ( timeAt( 16000 ) ) motor.stop();
// if ( timeAt( 18000 ) ) reboot();
}