Commit 61839773 authored by Guillaume Picquet's avatar Guillaume Picquet

Version du jour de la course qui a échoué avec panache !

parent c7e3249b
......@@ -12,7 +12,7 @@ class Direction
{
public:
static constexpr uint8_t steerMax = 30;
static constexpr int steerDelay = 10;
static constexpr int steerDelay = 5;
static constexpr int steerQuickDelay = 2;
inline void attach( int servoPin )
......
......@@ -33,7 +33,7 @@ class GPS
mTargetLng = lng;
}
void setTarget( const Location & location )
void setTarget( Location & location )
{
mTargetLat = location.lat();
mTargetLng = location.lng();
......@@ -53,7 +53,7 @@ class GPS
mTargetLng );
}
int16_t getHeading( const Location & location )
int16_t getHeading( Location & location )
{
return TinyGPSPlus::courseTo(
mLocation.lat(),
......@@ -71,7 +71,7 @@ class GPS
mTargetLng );
}
int16_t getDistance( const Location & location )
int16_t getDistance( Location & location )
{
return TinyGPSPlus::distanceBetween(
mLocation.lat(),
......@@ -80,6 +80,16 @@ class GPS
location.lng() );
}
int16_t getDistance( double lat, double lng )
{
return TinyGPSPlus::distanceBetween(
mLocation.lat(),
mLocation.lng(),
lat,
lng );
}
inline double getLat() const
{
return mPositionLat;
......
......@@ -13,7 +13,7 @@ class Motor
{
public:
static constexpr int8_t forwardMax = 16;
static constexpr int8_t forwardMin = 13;
static constexpr int8_t forwardMin = 12;
static constexpr int8_t reverseMax = -forwardMax;
static constexpr int8_t reverseMin = -forwardMin;
static constexpr int servoDelay = 4;
......
......@@ -9,6 +9,32 @@
#include "Motor.hpp"
#include "GPS.hpp"
struct Poi
{
Poi() = default;
Poi( double lat, double lng, double distance )
: mLat( lat )
, mLng( lng )
, mDistance( distance )
{}
Poi( const Poi & rhs )
: mLat( rhs.mLat )
, mLng( rhs.mLng )
, mDistance( rhs.mDistance )
{}
double mLat = 0.0;
double mLng = 0.0;
double mDistance = 0.0;
};
Poi poiArray[ 2 ] = {
Poi( 43.988674, 1.328633, 27.0 ),
Poi( 43.988696, 1.328986, 27.0 )
};
class Pilot
{
public:
......@@ -44,13 +70,7 @@ class Pilot
{
if ( mBoussole.update() )
{
/* mAverageHeading.update( mBoussole.getHeadingDegree() );
Serial.print( "cap " );
Serial.println( mAverageHeading.get() );
*/
}
mBoussole.update();
mGps.update();
mDirection.update();
mMotor.update();
......@@ -73,21 +93,19 @@ class Pilot
mDirection.goStraight();
}
//mInitialHeading = mBoussole.getHeadingDegree();//mAverageHeading.get();
if ( ! mGps.isValid() )
{
Serial.println( "cap invalide" );
return;
}
mGps.setTarget( poiArray[ 1 ].mLat, poiArray[ 1 ].mLng );
mInitialHeading = mGps.getHeading();
mInitialLocation = mGps.getLocation();
// println( mGps.getLat() );
// println( mGps.getLng() );
mStateMachine.set( & Pilot::stateGoStrait );
mStartPoi = poiArray[ 0 ];
mStateMachine.set( & Pilot::stateGoStrait );
Serial.print( "cap initial " );
Serial.println( mInitialHeading );
......@@ -97,7 +115,7 @@ class Pilot
{
if ( entry )
{
mT = t + 20000;
mT = t + 30000;
Serial.print( "Tout droit au " );
Serial.print( mInitialHeading );
......@@ -111,77 +129,75 @@ class Pilot
}
// corriger cap
int16_t cap = mBoussole.getHeadingDegree();
int16_t diff = mInitialHeading - cap;
if ( diff < -180 ) diff += 360;
if ( diff > 180 ) diff -= 360;
int16_t currentHeading = mBoussole.getHeadingDegree();
int16_t targetHeading = mInitialHeading;
while ( currentHeading > 180 ) currentHeading -= 360;
while ( currentHeading < -180 ) currentHeading += 360;
while ( targetHeading > 180 ) targetHeading -= 360;
while ( targetHeading < -180 ) targetHeading += 360;
int16_t diff = currentHeading - targetHeading;
Serial.print( "Tout droit au " );
Serial.print( mInitialHeading );
Serial.print( " cap " );
Serial.print( cap );
Serial.print( " diff " );
Serial.println( diff );
while ( diff > 180 ) diff -= 360;
while ( diff < -180 ) diff += 360;
mDirection.turn( diff );
mDirection.turn( -diff / 2 );
if ( mGps.getDistance( mInitialLocation ) >= 5 )
if ( ( -6 < diff ) && ( diff < 6 ) )
{
mMotor.forwardSlow();
if ( mGps.getDistance( mStartPoi.mLat, mStartPoi.mLng ) < poiArray[ mPoiIndex ].mDistance / 2 )
{
mMotor.forwardFast();
}
}
if ( mGps.getDistance( mStartPoi.mLat, mStartPoi.mLng ) < poiArray[ mPoiIndex ].mDistance / 2 )
{
mInitialHeading = mGps.getHeading();
}
else
{
mMotor.forwardSlow();
}
if ( mGps.getDistance( mInitialLocation ) >= 10 )
if ( mGps.getDistance( mStartPoi.mLat, mStartPoi.mLng ) >= poiArray[ mPoiIndex ].mDistance )
{
mStateMachine.set( & Pilot::stateUTurn );
}
}
void stateUTurn( bool )
{
GPS::Location tmp = mGps.getLocation();
mStartPoi = poiArray[ mPoiIndex ];
mGps.setTarget( mInitialLocation );
mInitialLocation = tmp;
mPoiIndex = 1 - mPoiIndex;
mGps.setTarget( poiArray[ mPoiIndex ].mLat, poiArray[ mPoiIndex ].mLng );
mInitialHeading = mGps.getHeading();
mMotor.forwardSlow();
++count;
if ( count > 3 ) mStateMachine.set( & Pilot::stateDone );
if ( count >= 6 )
{
mStateMachine.set( & Pilot::stateDone );
return;
}
++count;
mStateMachine.set( & Pilot::stateGoStrait );
}
void stateDone( bool entry )
void stateDone( bool )
{
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;
......@@ -194,11 +210,13 @@ class Pilot
int16_t mInitialHeading = 0.0f;
float mTargetHeading = 0.0f;
GPS::Location mInitialLocation;
Poi mStartPoi;
unsigned long mT = 0;
int count = 0;;
int count = 0;
int mPoiIndex = 0;
};
#endif
#ifndef AVERAGEFILTER_HPP
#define AVERAGEFILTER_HPP
#include <stdint.h>
template< typename T, unsigned int N >
class AverageFilter
{
public:
void update( const T & newValue )
{
if( this->mValid < N ) ++this->mValid;
this->mValues[ this->mIndex ] = newValue;
++this->mIndex;
if( this->mIndex >= N ) this->mIndex = 0;
this->mAverage = 0;
for( uint8_t i = 0; i < this->mValid; ++i )
{
this->mAverage += this->mValues[ i ];
}
this->mAverage /= static_cast< T >( this->mValid );
}
inline const T & get() const
{
return this->mAverage;
}
inline bool isFilled() const
{
return ( mValid >= N );
}
private:
uint8_t mIndex = 0;
uint8_t mValid = 0;
T mValues[ N ];
T mAverage = 0;
};
#endif
#ifndef BOUSSOLE_HPP
#define BOUSSOLE_HPP
#include <stdint.h>
#include <Wire.h>
#include <EEPROM.h>
/* time of current loop */
extern unsigned long t;
/*
http://www.magnetic-declination.com/
Magnetic Declination: +0° 44'
Declination is POSITIVE (EAST)
*/
class Boussole
{
static constexpr byte address = 0x1E;
static constexpr byte configurationRegistersIndex = 0;
static constexpr byte dataRegistersIndex = 3;
static constexpr byte identificationRegistersIndex = 10;
static constexpr unsigned long timeout = 500;
static constexpr int16_t offsetLimit = 810;
static constexpr float deg2rad = PI / 180.0;
static constexpr float rad2deg = 180.0 / PI;
static constexpr float declinaison = 44.0 / 60.0 * deg2rad;
// struct matching configuration registers A & B
union Configuration {
// Values for number of samples averaged
enum Average {
avg_1 = 0b00,
avg_2 = 0b01,
avg_4 = 0b10,
avg_8 = 0b11,
};
// Values for output rate
enum OutputRate {
f_0_75Hz = 0b000,
f_1_5Hz = 0b001,
f_3Hz = 0b010,
f_7_5Hz = 0b011,
f_15Hz = 0b100,
f_30Hz = 0b101,
f_75Hz = 0b110,
};
enum MeasurmentMode {
normal = 0b00,
positiveBias = 0b01,
negativeBias = 0b10,
};
enum Gain {
gain_0_88 = 0b000,
gain_1_3 = 0b001,
gain_1_9 = 0b010,
gain_2_5 = 0b011,
gain_4_0 = 0b100,
gain_4_7 = 0b101,
gain_5_6 = 0b110,
gain_8_1 = 0b111,
};
enum Mode {
continuous = 0b00,
single = 0b01,
idle = 0b10,
};
struct {
// Configuration register A
uint8_t measurementMode : 2;
uint8_t dataOutputRate : 3;
uint8_t measureAverage : 2;
uint8_t zeroA : 1; // Must be 0
// Configuration register B
uint8_t zeroB : 5; // Must be 0
uint8_t gain : 3;
// Mode register
uint8_t operatingMode : 2;
uint8_t signleMeasureDone : 6; // These bits must be cleared for correct operation.
};
uint8_t raw[ 3 ];
};
public:
union Data {
struct {
int16_t x;
int16_t y;
int16_t z;
};
uint8_t raw[ 6 ];
};
enum Status {
ok,
undefined,
noResponse,
badSignature,
};
void init() {
checkDevice();
readConfiguration();
mConfiguration.measurementMode = Configuration::normal;
mConfiguration.operatingMode = Configuration::continuous;
mConfiguration.measureAverage = Configuration::avg_8;
mConfiguration.dataOutputRate = Configuration::f_15Hz;
writeConfiguration();
}
inline Status getStatus() {
return mStatus;
}
const char * getStatusString() const {
switch ( mStatus )
{
case ok : return "OK";
case noResponse : return "error device do not respond";
case badSignature : return "error device have wrong signature";
case undefined:
default : return "undefined";
}
}
inline bool update() {
return readData();
}
inline Data & getData() {
return mData;
}
inline float getHeading() {
return mHeading;
}
inline int16_t getHeadingDegree() {
return mHeading * rad2deg + 180;
}
bool isReady() const {
return ok == mStatus;
}
void loadAt( unsigned address ) {
EEPROM.get( address, calibration );
}
void saveAt( unsigned address ) {
EEPROM.put( address, calibration );
}
protected:
bool checkDevice() {
// check i2c link
Wire.beginTransmission( address );
if ( Wire.endTransmission() ) {
mStatus = noResponse;
return false;
}
// check signature
Wire.beginTransmission( address );
Wire.write( identificationRegistersIndex );
Wire.endTransmission();
Wire.requestFrom( (int)address, 3 );
if ( ! waitBytes( 3 ) ) return false;
mStatus = ok;
byte x = 0;
x = Wire.read();
if ( x != 0b01001000 ) mStatus = badSignature;
x = Wire.read();
if ( x != 0b00110100 ) mStatus = badSignature;
x = Wire.read();
if ( x != 0b00110011 ) mStatus = badSignature;
return isReady();
}
void readConfiguration() {
Wire.beginTransmission( address );
Wire.write( configurationRegistersIndex );
Wire.endTransmission();
Wire.requestFrom( (int)address, 3 );
if ( ! waitBytes( 3 ) ) return;
mConfiguration.raw[ 0 ] = Wire.read();
mConfiguration.raw[ 1 ] = Wire.read();
mConfiguration.raw[ 2 ] = Wire.read();
mStatus = ok;
}
void writeConfiguration() {
mConfiguration.zeroA = 0;
mConfiguration.zeroB = 0;
mConfiguration.signleMeasureDone = 0;
Wire.beginTransmission( address );
Wire.write( configurationRegistersIndex );
Wire.write( mConfiguration.raw[ 0 ] );
Wire.write( mConfiguration.raw[ 1 ] );
Wire.write( mConfiguration.raw[ 2 ] );
Wire.endTransmission();
switch ( mConfiguration.dataOutputRate ) {
case Configuration::f_0_75Hz : mDelay = 1337; break;
case Configuration::f_1_5Hz : mDelay = 667; break;
case Configuration::f_3Hz : mDelay = 334; break;
case Configuration::f_7_5Hz : mDelay = 134; break;
case Configuration::f_15Hz : mDelay = 67; break;
case Configuration::f_30Hz : mDelay = 34; break;
case Configuration::f_75Hz : mDelay = 14; break;
}
}
bool waitBytes( uint8_t count ) {
unsigned long limit = millis() + timeout;
while ( Wire.available() < count ) {
if ( millis() > limit ) {
mStatus = noResponse;
return false;
}
}
return true;
}
bool readData() {
switch ( mReadState ) {
case request:
Wire.beginTransmission( address );
Wire.write( dataRegistersIndex );
Wire.endTransmission();
Wire.requestFrom( (int)address, 6 );
mReadState = waitData;
return false;
case waitData:
if ( Wire.available() < 6 ) return false;
mData.raw[ 1 ] = Wire.read();
mData.raw[ 0 ] = Wire.read();
mData.raw[ 5 ] = Wire.read();
mData.raw[ 4 ] = Wire.read();
mData.raw[ 3 ] = Wire.read();
mData.raw[ 2 ] = Wire.read();
computeHeading();
mT = t + mDelay;
mReadState = waitDelay;
return true;
case waitDelay:
if ( t > mT ) mReadState = request;
return false;
default :
return false;
}
}
void computeHeading() {
if ( mData.x < calibration.xMin && mData.x > -offsetLimit ) {
calibration.xMin = mData.x;
}
if ( mData.x > calibration.xMax && mData.x < offsetLimit ) {
calibration.xMax = mData.x;
}
xOffset = ( calibration.xMin + calibration.xMax ) >> 1;
if ( mData.y < calibration.yMin && mData.y > -offsetLimit ) {
calibration.yMin = mData.y;
}
if ( mData.y > calibration.yMax && mData.y < offsetLimit ) {
calibration.yMax = mData.y;
}
yOffset = ( calibration.yMin + calibration.yMax ) >> 1;
mHeading = atan2f( mData.x - xOffset, mData.y - yOffset ) + declinaison;
}
private:
Status mStatus = undefined;
enum ReadState {
request,
waitData,
waitDelay,
};
ReadState mReadState = request;
Configuration mConfiguration = { .raw = {} };
Data mData = { .raw = {} };
struct Calibration {
int16_t xMin = offsetLimit;
int16_t xMax = -offsetLimit;
int16_t yMin = offsetLimit;
int16_t yMax = -offsetLimit;
} calibration;
int16_t xOffset = 0;
int16_t yOffset = 0;
unsigned long mDelay = 0;
// temps de la dernière mesure
unsigned long mT = 0;
// dernier cap mesuré
float mHeading = 0;
};
#endif
#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 = 5;
static constexpr int steerQuickDelay = 2;
inline void attach( int servoPin )
{
mRampServo.attach( servoPin );
}
void init()
{