Commit d29f8456 authored by Guillaume Picquet's avatar Guillaume Picquet

Maj du travail effectué le 19/09/2019

parent 87bbc9d8
......@@ -147,6 +147,10 @@ class Boussole
return mHeading;
}
inline int16_t getHeadingDegree() {
return mHeading * rad2deg + 180;
}
bool isReady() const {
return ok == mStatus;
}
......
#ifndef DIRECTION_HPP
#define DIRECTION_HPP
#include <stdint.h>
#include "RampServo.hpp"
/* time of current loop */
extern unsigned long t;
class Direction
{
public:
static constexpr uint8_t steerMax = 30;
static constexpr int steerDelay = 10;
static constexpr int steerQuickDelay = 5;
inline void attach( int servoPin )
{
mRampServo.attach( servoPin );
}
void init()
{
mRampServo.write( 45 );
}
inline void update()
{
mRampServo.update();
}
inline void turn( int8_t steer )
{
mRampServo.setAngle( calibration.servoNeutral - steer, steerDelay );
}
inline void turnQuick( int8_t steer )
{
mRampServo.setAngle( calibration.servoNeutral - steer, steerQuickDelay );
}
inline void turnRight( int8_t steer = steerMax ) {
turn( steer );
}
inline void turnLeft( int8_t steer = steerMax ) {
turn( -steer );
}
inline void turnRightQuick( int8_t steer = steerMax ) {
turnQuick( steer );
}
inline void turnLeftQuick( int8_t steer = steerMax ) {
turnQuick( -steer );
}
inline void goStraight(){
turn( 0 );
}
inline void goStraightQuick(){
turnQuick( 0 );
}
private:
RampServo mRampServo;
struct Calibration {
uint8_t servoNeutral = 90;
} calibration;
};
#endif
#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 forwardMin = 12;
static constexpr int8_t reverseMax = -forwardMax;
static constexpr int8_t reverseMin = -forwardMin;
static constexpr int servoDelay = 4;
static constexpr int8_t kickAngle = 30;
static constexpr unsigned long kickDuration = 100;
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 = forwardMin;
}
inline void forwardFast()
{
mSpeed = forwardMax;
}
inline void reverseSlow()
{
mSpeed = -reverseMin;
}
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 PILOT_HPP
#define PILOT_HPP
#include "StateMachine.hpp"
#include "Boussole.hpp"
#include "AverageFilter.hpp"
#include "Direction.hpp"
#include "Motor.hpp"
class Pilot
{
public:
Pilot( boussole & boussole ):
mBoussole( boussole )
Pilot( Direction & d, Motor & m, Boussole & b )
: mDirection( d ),
mMotor( m ),
mBoussole( b ),
mStateMachine( this )
{
mStateMachine.push( & Dummy::stateInitial );
mStateMachine.push( & Pilot::stateInitial );
}
void init()
{
mMotor.update();
for ( int i = 0; i < 10; ++i )
{
while ( ! mBoussole.update() )
{
t = millis();
}
Serial.print( "lecture " );
Serial.println( mBoussole.getHeadingDegree() );
}
}
void update()
{
if ( mBoussole.update() )
{
mAverageHeading.update( boussole.getHeading() );
/* mAverageHeading.update( mBoussole.getHeadingDegree() );
Serial.print( "cap " );
Serial.println( mAverageHeading.get() );
*/
}
mDirection.update();
mMotor.update();
mStateMachine.activate();
}
......@@ -35,51 +65,113 @@ class Pilot
void stateInitial( bool )
{
if ( ! mAverageHeading.isFilled() ) return;
InitialHeading = mAverageHeading.get();
// if ( ! mAverageHeading.isFilled() ) return;
mInitialHeading = mBoussole.getHeadingDegree();//mAverageHeading.get();
mStateMachine.set( & Pilot::stateGoStraitBlind );
//mDirection.goStraight();
Serial.print( "cap initial " );
Serial.println( mInitialHeading );
}
void stateGoStraitBlind( bool entry )
{
if ( entry )
{
mT = t + 2000;
mDirection.turnLeft();
mMotor.forwardSlow();
Serial.println( "Tout droit" );
}
mStateMachine.set( stateGoStrait );
if ( t > mT )
{
mStateMachine.set( & Pilot::stateGoStrait );
}
}
void stateGoStrait( bool entry )
{
if ( entry )
{
mBegin = t;
mT = t + 10000;
Serial.print( "Tout droit au " );
Serial.println( mInitialHeading );
mMotor.forwardSlow();
}
if ( t - mBegin ) > 5000
if ( t > mT )
{
mStateMachine.set( stateUTurnLeft );
mStateMachine.set( & Pilot::stateUTurnLeft );
}
// corriger cap
int16_t cap = mBoussole.getHeadingDegree();
int16_t diff = mInitialHeading - cap;
if ( diff < -180 ) diff += 360;
if ( diff > 180 ) diff -= 360;
/*
Serial.print( "diff=" );
Serial.println( diff );
*/
mDirection.turn( diff );
}
void stateUTurnLeft( bool entry )
{
if( entry )
if ( entry )
{
targetHeading = targetHeading - 180.0;
if( targetHeading < 0 ) targetHeading += 360;
mInitialHeading -= 180;
if ( mInitialHeading < 0 ) mInitialHeading += 360;
Serial.print( "Demi tour au " );
Serial.println( mInitialHeading );
mMotor.stop();
mDirection.turnLeftQuick();
}
int16_t cap = mBoussole.getHeadingDegree();
/* if ( entry )
{
mTargetHeading = mTargetHeading - 180.0;
if ( mTargetHeading < 0 ) mTargetHeading += 360;
}
float target = mTargetHeading - 180.0;
float current = mAverageHeading.get() - 180.0;
if ( current < target )
{
mDirection.turnRight( target - current );
}
if ( current > target )
{
mDirection.turnLeft( current - target );
}
*/
}
private:
Boussole & mBoussole;
AverageFilter< float, 4 > mAverageHeading;
Direction & mDirection;
Motor & mMotor;
Boussole & mBoussole;
AverageFilter< int16_t, 4 > mAverageHeading;
StateMachine< Pilot > mStateMachine;
float InitialHeading = 0.0f;
float targetHeading = 0.0f;
unsigned long mBegin = 0;
int16_t mInitialHeading = 0.0f;
float mTargetHeading = 0.0f;
unsigned long mT = 0;
};
#endif
......@@ -21,14 +21,14 @@ class RampServo
return mAngle;
}
inline setMin( uint8_t angle )
inline void setMin( uint8_t angle )
{
if ( angle > limit ) angle = limit;
if ( angle > settings.max ) settings.max = angle;
settings.min = angle;
}
inline setMax( uint8_t angle )
inline void setMax( uint8_t angle )
{
if ( angle > limit ) angle = limit;
if ( angle < settings.min ) settings.min = angle;
......@@ -79,7 +79,8 @@ class RampServo
}
inline void write( uint8_t angle ) {
setAngle( angle );
mAngle = angle;
mServo.write( mAngle );
}
private:
......
#ifndef STATEMACHINE_HPP
#define STATEMACHINE_HPP
template< class T >
class StateMachine
{
......@@ -53,3 +56,5 @@ class StateMachine
int mStackSize { -1 };
bool mChanging = false;
};
#endif
/*
Controlling a servo position using a potentiometer (variable resistor)
Speed
Point mort 87.
Arriere 84 à 75.
Avant 90 à 100.
penser a faire des rampes acceleration et freinage.
direction
point mort 95.
droite 50
gauche 170
*/
#include <stdint.h>
#include "RampServo.hpp"
#include "Direction.hpp"
#include "Motor.hpp"
#include "Boussole.hpp"
#include "Pilot.hpp"
unsigned long t = 0;
unsigned long t0 = 0;
RampServo direction;
RampServo moteur;
Direction direction;
Motor motor;
Boussole boussole;
Pilot pilot( direction, motor, boussole );
bool timeAt( uint16_t timeStamp ) {
/*
bool timeAt( uint16_t timeStamp )
{
return ( t0 <= timeStamp && t > timeStamp );
}
}
*/
void setup() {
direction.attach( 8 );
moteur.attach( 9 );
}
Serial.println( "setup()" );
void loop() {
t0 = t;
t = millis();
direction.attach( 8 );
motor.attach( 9 );
direction.update();
moteur.update();
delay( 3000 );
if ( timeAt( 1000 ) ) direction.setAngle( 0, 10 );
if ( timeAt( 1000 ) ) moteur.setAngle( 90 );
motor.update();
direction.init();
if ( timeAt( 2000 ) ) direction.setAngle( 180 );
if ( timeAt( 3000 ) ) direction.setAngle( 90 );
Serial.begin( 57600 );
while ( ! Serial );
Wire.begin();
if ( timeAt( 4000 ) ) moteur.setAngle( 110, 5 );
if ( timeAt( 4500 ) ) moteur.setAngle( 100, 20 );
boussole.init();
boussole.loadAt( 1 );
if ( timeAt( 6000 ) ) direction.setAngle( 0 );
if ( timeAt( 7000 ) ) direction.setAngle( 180 );
if ( timeAt( 8000 ) ) direction.setAngle( 90 );
pilot.init();
if ( timeAt( 10000 ) ) moteur.setAngle( 90 );
if ( timeAt( 11000 ) ) moteur.setAngle( 80 );
if ( timeAt( 12000 ) ) moteur.setAngle( 90 );
if ( timeAt( 13000 ) ) moteur.setAngle( 80 );
Serial.println( "loop()" );
}
if ( timeAt( 20000 ) ) moteur.setAngle( 90 );
void loop() {
t0 = t;
t = millis();
pilot.update();
}
......@@ -16,34 +16,30 @@
#include <Servo.h>
#include "Direction.hpp"
#include "RampServo.hpp"
unsigned long t = 0;
unsigned long t0 = 0;
Direction direction;
RampServo moteur;
bool timeAt( uint16_t timeStamp ) {
return ( t0 <= timeStamp && t > timeStamp );
}
void setup() {
direction.attach( 8 );
moteur.attach( 9 );
}
void reboot()
{
asm volatile (" jmp 0");
}
void setup() {
direction.attach( 8 );
}
void loop() {
t0 = t;
t = millis();
direction.update();
moteur.update();
if ( timeAt( 1000 ) ) direction.goStraight();
......@@ -59,8 +55,5 @@ void loop() {
if ( timeAt( 8000 ) ) direction.turnLeftQuick( Direction::steerMax );
if ( timeAt( 9000 ) ) direction.goStraight();
if ( timeAt( 11000 ) ) reboot();
}
#ifndef BOUSSOLE_HPP
#define BOUSSOLE_HPP
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Fonts/FreeMono9pt7b.h>
class Display
{
void init()
{
display.begin( SSD1306_SWITCHCAPVCC, 0x3C );
display.clearDisplay();
display.setTextColor( WHITE );
}
private:
Adafruit_SSD1306 mDisplay( 128, 64 );
};
#endif
void setup() {
// put your setup code here, to run once:
}
void loop() {
// put your main code here, to run repeatedly:
}
#include <SoftwareSerial.h>
#include "GPS.hpp"
//#include "GPS.hpp"
void setup()
{
Serial.begin(57600);
while (!Serial) {}
Serial1.begin(9600);
while (!Serial1) {}
}
void loop()
{
while( Serial1.available() )
{
Serial.write( Serial1.read() );
}
/*
// This sketch displays information every time a new sentence is correctly encoded.
while (ss.available() > 0)
// while (ss.available() > 0)
if (gps.encode(ss.read()))
displayInfo();
......@@ -21,7 +25,7 @@ void loop()
Serial.println(F("No GPS detected: check wiring."));
while(true);
}
*/
*/
}
/*
...