<commit_before>#include "Servo.h"
#include "SPI.h"
#include "Wire.h"
#include "MINDSi.h"
#include "Encoder.h"
#include "DroneLibs.h"
#include "util/callbackTemplate.h"

//Constants that should never change during driving and never/rarely tuned
#define useEncoder true
const uint8_t VoltagePin  = 67;
const uint8_t LEDpin[]    = {25, 26, 27}; //blue, yellow, red
const uint8_t PingPin[]	  = {A0, A1, A2, A3, A4}; //left to right
const uint8_t ServoPin[]  = {12, 11,  8};//drive, steer, backS; APM 1,2,3 resp.
const uint8_t RadioPin[]  = {0, 7, 6}; //auto switch, drive, steer
const uint8_t EncoderPin[]= {2, 3};
const double  pAngle[5]   = { 79.27, 36.83, 0.0, -36.83, -79.27};
const double  dplsb       = 4.f/float(0xffff); //dps Per leastSigBit for gyro
const int ScheduleDelay   = 22;
const uint16_t warn[]     = {1000, 1600, 3000, 1600, 1000};
const double PointRadius  = .001; //in miles, margin for error in rover location
							//tire circ in miles per inch diameter * diff ratio
const float MilesPerRev   = (((PI)/12.f)/5280.f) * (13.f/37.f);
							//hours per min      rev per mile
const float MPHvRPM       = (1.f/60.f)        * (1.f/MilesPerRev);

//Global variables used throught the program
HardwareSerial *commSerial	= &Serial;
Storage<float> *storage	= eeStorage::getInstance();
CommManager		manager(commSerial, storage);
Settings		settings(storage);
LEA6H			gps;
MPU6000			mpu;
Waypoint		location(0,0);
Waypoint		backWaypoint(0,0);
HLA				lowFilter (600000, 0);//10 minutes
HLA				highFilter(    10, 0);//10 milliseconds
HLA 			pitch( 100, 0);
HLA 			roll ( 100, 0);
Servo			servo[3]; //drive, steer, backSteer
PIDcontroller   cruise;
		//scheduler, navigation, obstacle, stop times
uint32_t uTime = 0, nTime = 0, oTime = 0, sTime = 0;
uint32_t gpsHalfTime = 0, gpsTime = 0;
 int32_t Ax,Ay,Az; //used in accelerometer calculations
uint16_t ping[5] = {20000,20000,20000,20000,20000};
uint8_t  sIter,pIter; //iterators for scheduler and ping
double   pathHeading; //All Headings are Clockwise+, -179 to 180, 0=north
double   trueHeading;
double   gyroHalf; //Store Gyro Heading halfway between gps points
double   distance;
boolean  stop = true;
boolean  backDir;

void (*schedule[])(void) = {	extrapPosition,
								checkPing,
								readAccelerometer,
								reportLocation,
								};

//These parameters are loaded from eeprom if the code has not been reuploaded
//since when they were last set.
//the dashboard can update records in the storage instance passed into manager
//The callbacks keep them up to date while the code is running
float	lineGravity;
int		steerThrow, steerStyle;
float	steerFactor;
float	minFwd, maxFwd;
int		revThrow;
float	revSpeed;
float	pingWeight;
int		coastTime, dangerTime; //danger should include coastTime
float	tireDiameter;
int		steerCenter;
void dangerTimeCallback(float in){ dangerTime = coastTime+in; }
void newPIDparam(float x){
	using namespace groundSettings;
	PIDparameters newPID = PIDparameters(settings.get(CRUISE_P),
										 settings.get(CRUISE_I),
										 settings.get(CRUISE_D) );
	cruise.tune(newPID);
}

void setupSettings(){
	using namespace groundSettings;
	settings.attach( LINE_GRAV	, .50  , callback<float, &lineGravity>	);
	settings.attach( STEER_THROW, 45   , callback<int  , &steerThrow>	);
	settings.attach( STEER_STYLE, 1    , callback<int  , &steerStyle>	);
	settings.attach( STEER_FAC	, 1.0  , callback<float, &steerFactor>	);
	settings.attach( MIN_FWD_SPD, 1.5  , callback<float, &minFwd>		);
	settings.attach( MAX_FWD_SPD, 6.0  , callback<float, &maxFwd>		);
	settings.attach( REV_STR_THR, 20   , callback<int  , &revThrow>		);<commit_msg>Updated RM6 with new PID api<commit_after>#include "Servo.h"
#include "SPI.h"
#include "Wire.h"
#include "MINDSi.h"
#include "Encoder.h"
#include "DroneLibs.h"
#include "util/callbackTemplate.h"

//Constants that should never change during driving and never/rarely tuned
#define useEncoder true
const uint8_t VoltagePin  = 67;
const uint8_t LEDpin[]    = {25, 26, 27}; //blue, yellow, red
const uint8_t PingPin[]	  = {A0, A1, A2, A3, A4}; //left to right
const uint8_t ServoPin[]  = {12, 11,  8};//drive, steer, backS; APM 1,2,3 resp.
const uint8_t RadioPin[]  = {0, 7, 6}; //auto switch, drive, steer
const uint8_t EncoderPin[]= {2, 3};
const double  pAngle[5]   = { 79.27, 36.83, 0.0, -36.83, -79.27};
const double  dplsb       = 4.f/float(0xffff); //dps Per leastSigBit for gyro
const int ScheduleDelay   = 22;
const uint16_t warn[]     = {1000, 1600, 3000, 1600, 1000};
const double PointRadius  = .001; //in miles, margin for error in rover location
							//tire circ in miles per inch diameter * diff ratio
const float MilesPerRev   = (((PI)/12.f)/5280.f) * (13.f/37.f);
							//hours per min      rev per mile
const float MPHvRPM       = (1.f/60.f)        * (1.f/MilesPerRev);

//Global variables used throught the program
HardwareSerial *commSerial	= &Serial;
Storage<float> *storage	= eeStorage::getInstance();
CommManager		manager(commSerial, storage);
Settings		settings(storage);
LEA6H			gps;
MPU6000			mpu;
Waypoint		location(0,0);
Waypoint		backWaypoint(0,0);
HLA				lowFilter (600000, 0);//10 minutes
HLA				highFilter(    10, 0);//10 milliseconds
HLA 			pitch( 100, 0);
HLA 			roll ( 100, 0);
Servo			servo[3]; //drive, steer, backSteer
PIDparameters   cruisePID(0,0,0,-90,90);
PIDcontroller   cruise(&cruisePID);
		//scheduler, navigation, obstacle, stop times
uint32_t uTime = 0, nTime = 0, oTime = 0, sTime = 0;
uint32_t gpsHalfTime = 0, gpsTime = 0;
 int32_t Ax,Ay,Az; //used in accelerometer calculations
uint16_t ping[5] = {20000,20000,20000,20000,20000};
uint8_t  sIter,pIter; //iterators for scheduler and ping
double   pathHeading; //All Headings are Clockwise+, -179 to 180, 0=north
double   trueHeading;
double   gyroHalf; //Store Gyro Heading halfway between gps points
double   distance;
boolean  stop = true;
boolean  backDir;

void (*schedule[])(void) = {	extrapPosition,
								checkPing,
								readAccelerometer,
								reportLocation,
								};

//These parameters are loaded from eeprom if the code has not been reuploaded
//since when they were last set.
//the dashboard can update records in the storage instance passed into manager
//The callbacks keep them up to date while the code is running
float	lineGravity;
int		steerThrow, steerStyle;
float	steerFactor;
float	minFwd, maxFwd;
int		revThrow;
float	revSpeed;
float	pingWeight;
int		coastTime, dangerTime; //danger should include coastTime
float	tireDiameter;
int		steerCenter;
void dangerTimeCallback(float in){ dangerTime = coastTime+in; }
void newPIDparam(float x){
	using namespace groundSettings;
	cruisePID = PIDparameters(settings.get(CRUISE_P),
                              settings.get(CRUISE_I),
                              settings.get(CRUISE_D) );
}

void setupSettings(){
	using namespace groundSettings;
	settings.attach( LINE_GRAV	, .50  , callback<float, &lineGravity>	);
	settings.attach( STEER_THROW, 45   , callback<int  , &steerThrow>	);
	settings.attach( STEER_STYLE, 1    , callback<int  , &steerStyle>	);
	settings.attach( STEER_FAC	, 1.0  , callback<float, &steerFactor>	);
	settings.attach( MIN_FWD_SPD, 1.5  , callback<float, &minFwd>		);
	settings.attach( MAX_FWD_SPD, 6.0  , callback<float, &maxFwd>		);
	settings.attach( REV_STR_THR, 20   , callback<int  , &revThrow>		);
{'fraction_non_alphanumeric': 0.1195963452884222, 'fraction_numerical': 0.05209327696713487, 'mean_word_length': 3.8795741849634067, 'pattern_counts': {'":': 0, '<': 19, '<?xml version=': 0, '>': 19, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3096816', 'n_tokens_mistral': 3030, 'n_tokens_neox': 2613, 'n_words': 783}