...
 
Commits (2)
......@@ -13,7 +13,7 @@ class Direction
public:
static constexpr uint8_t steerMax = 30;
static constexpr int steerDelay = 10;
static constexpr int steerQuickDelay = 5;
static constexpr int steerQuickDelay = 2;
inline void attach( int servoPin )
{
......
#ifndef GPS_HPP
#define GPS_HPP
#include <stdint.h>
#include <TinyGPS++.h>
/* time of current loop */
extern unsigned long t;
class GPS
{
public:
using Location = TinyGPSLocation;
void init() {
Serial1.begin( 9600 );
}
void update() {
while ( Serial1.available() )
gps.encode(Serial1.read());
/*
mPositionLat = gps.location.lat();
mPositionLng = gps.location.lng();
*/
mLocation = gps.location;
}
void setTarget( double lat, double lng )
{
mTargetLat = lat;
mTargetLng = lng;
}
void setTarget( const Location & location )
{
mTargetLat = location.lat();
mTargetLng = location.lng();
}
bool isValid()
{
return mLocation.isValid();
}
int16_t getHeading()
{
return TinyGPSPlus::courseTo(
mLocation.lat(),
mLocation.lng(),
mTargetLat,
mTargetLng );
}
int16_t getHeading( const Location & location )
{
return TinyGPSPlus::courseTo(
mLocation.lat(),
mLocation.lng(),
location.lat(),
location.lng() );
}
int16_t getDistance()
{
return TinyGPSPlus::distanceBetween(
mLocation.lat(),
mLocation.lng(),
mTargetLat,
mTargetLng );
}
int16_t getDistance( const Location & location )
{
return TinyGPSPlus::distanceBetween(
mLocation.lat(),
mLocation.lng(),
location.lat(),
location.lng() );
}
inline double getLat() const
{
return mPositionLat;
}
inline double getLng() const
{
return mPositionLng;
}
inline Location getLocation() const
{
return mLocation;
}
private:
TinyGPSPlus gps;
double mPositionLat;
double mPositionLng;
double mTargetLat;
double mTargetLng;
TinyGPSLocation mLocation;
};
#endif
......@@ -12,8 +12,8 @@ extern unsigned long t;
class Motor
{
public:
static constexpr int8_t forwardMax = 20;
static constexpr int8_t forwardMin = 12;
static constexpr int8_t forwardMax = 16;
static constexpr int8_t forwardMin = 13;
static constexpr int8_t reverseMax = -forwardMax;
static constexpr int8_t reverseMin = -forwardMin;
static constexpr int servoDelay = 4;
......
......@@ -7,14 +7,16 @@
#include "AverageFilter.hpp"
#include "Direction.hpp"
#include "Motor.hpp"
#include "GPS.hpp"
class Pilot
{
public:
Pilot( Direction & d, Motor & m, Boussole & b )
Pilot( Direction & d, Motor & m, Boussole & b, GPS & g )
: mDirection( d ),
mMotor( m ),
mBoussole( b ),
mGps( g ),
mStateMachine( this )
{
mStateMachine.push( & Pilot::stateInitial );
......@@ -49,6 +51,7 @@ class Pilot
Serial.println( mAverageHeading.get() );
*/
}
mGps.update();
mDirection.update();
mMotor.update();
......@@ -63,51 +66,48 @@ class Pilot
protected:
void stateInitial( bool )
void stateInitial( bool entry )
{
if ( entry )
{
mDirection.goStraight();
}
// if ( ! mAverageHeading.isFilled() ) return;
//mInitialHeading = mBoussole.getHeadingDegree();//mAverageHeading.get();
mInitialHeading = mBoussole.getHeadingDegree();//mAverageHeading.get();
if ( ! mGps.isValid() )
{
Serial.println( "cap invalide" );
return;
}
mStateMachine.set( & Pilot::stateGoStraitBlind );
mInitialHeading = mGps.getHeading();
mInitialLocation = mGps.getLocation();
// println( mGps.getLat() );
// println( mGps.getLng() );
mStateMachine.set( & Pilot::stateGoStrait );
//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" );
}
if ( t > mT )
{
mStateMachine.set( & Pilot::stateGoStrait );
}
}
void stateGoStrait( bool entry )
{
if ( entry )
{
mT = t + 10000;
mT = t + 20000;
Serial.print( "Tout droit au " );
Serial.println( mInitialHeading );
Serial.print( mInitialHeading );
mMotor.forwardSlow();
//mMotor.forwardFast();
}
if ( t > mT )
{
mStateMachine.set( & Pilot::stateUTurnLeft );
mStateMachine.set( & Pilot::stateDone );
}
// corriger cap
......@@ -115,55 +115,78 @@ class Pilot
int16_t diff = mInitialHeading - cap;
if ( diff < -180 ) diff += 360;
if ( diff > 180 ) diff -= 360;
/*
Serial.print( "diff=" );
Serial.println( diff );
*/
Serial.print( "Tout droit au " );
Serial.print( mInitialHeading );
Serial.print( " cap " );
Serial.print( cap );
Serial.print( " diff " );
Serial.println( diff );
mDirection.turn( diff );
}
if ( mGps.getDistance( mInitialLocation ) >= 5 )
{
mMotor.forwardSlow();
}
void stateUTurnLeft( bool entry )
{
if ( entry )
if ( mGps.getDistance( mInitialLocation ) >= 10 )
{
mInitialHeading -= 180;
if ( mInitialHeading < 0 ) mInitialHeading += 360;
Serial.print( "Demi tour au " );
Serial.println( mInitialHeading );
mMotor.stop();
mDirection.turnLeftQuick();
mStateMachine.set( & Pilot::stateUTurn );
}
int16_t cap = mBoussole.getHeadingDegree();
}
void stateUTurn( bool )
{
GPS::Location tmp = mGps.getLocation();
mGps.setTarget( mInitialLocation );
mInitialLocation = tmp;
mInitialHeading = mGps.getHeading();
/* if ( entry )
{
mTargetHeading = mTargetHeading - 180.0;
if ( mTargetHeading < 0 ) mTargetHeading += 360;
}
mMotor.forwardSlow();
float target = mTargetHeading - 180.0;
float current = mAverageHeading.get() - 180.0;
++count;
if ( current < target )
{
mDirection.turnRight( target - current );
}
if ( count > 3 ) mStateMachine.set( & Pilot::stateDone );
mStateMachine.set( & Pilot::stateGoStrait );
if ( current > target )
{
mDirection.turnLeft( current - target );
}
*/
}
void stateDone( bool entry )
{
mMotor.stop();
}
/* 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:
Direction & mDirection;
Motor & mMotor;
Boussole & mBoussole;
GPS & mGps;
AverageFilter< int16_t, 4 > mAverageHeading;
StateMachine< Pilot > mStateMachine;
......@@ -171,7 +194,11 @@ class Pilot
int16_t mInitialHeading = 0.0f;
float mTargetHeading = 0.0f;
GPS::Location mInitialLocation;
unsigned long mT = 0;
int count = 0;;
};
#endif
......@@ -4,6 +4,7 @@
#include "Motor.hpp"
#include "Boussole.hpp"
#include "Pilot.hpp"
#include "GPS.hpp"
unsigned long t = 0;
unsigned long t0 = 0;
......@@ -11,7 +12,9 @@ unsigned long t0 = 0;
Direction direction;
Motor motor;
Boussole boussole;
Pilot pilot( direction, motor, boussole );
GPS gps;
Pilot pilot( direction, motor, boussole, gps );
/*
bool timeAt( uint16_t timeStamp )
......@@ -21,6 +24,9 @@ Pilot pilot( direction, motor, boussole );
*/
void setup() {
Serial.begin( 57600 );
while ( ! Serial );
Serial.println( "setup()" );
direction.attach( 8 );
......@@ -31,16 +37,17 @@ void setup() {
motor.update();
direction.init();
Serial.begin( 57600 );
while ( ! Serial );
Wire.begin();
boussole.init();
boussole.loadAt( 1 );
gps.init();
pilot.init();
gps.setTarget( 43.988903, 1.331675);
Serial.println( "loop()" );
}
......