slight edits to goalie
EmaMaker
parent
cee576e9c8
commit
64db5d2688
|
|
@ -2,12 +2,12 @@
|
|||
|
||||
#include <Arduino.h>
|
||||
#include "motor.h"
|
||||
#include "PID_v1.h"
|
||||
#include "PID_v2.h"
|
||||
|
||||
//PID Constants
|
||||
#define KP 1.2
|
||||
#define KP 1.5
|
||||
#define KI 0.0
|
||||
#define KD 0.0
|
||||
#define KD 0.3
|
||||
|
||||
#define UNLOCK_THRESH 800
|
||||
|
||||
|
|
@ -35,9 +35,9 @@ class DriveController{
|
|||
PID* pid;
|
||||
int pDir, pSpeed, pTilt;
|
||||
int gDir, gSpeed, gTilt;
|
||||
int speed1, speed2, speed3, speed4, errorePre, integral, pidfactor, errorP, errorD, errorI, delta;
|
||||
float x, y, vx, vy, speed1, speed2, speed3, speed4, pidfactor, delta;
|
||||
|
||||
double input, output, setpoint;
|
||||
int vx, vy;
|
||||
|
||||
float sins[360], cosins[360];
|
||||
|
||||
|
|
|
|||
|
|
@ -1 +0,0 @@
|
|||
Subproject commit 9b4ca0e5b6d7bab9c6ac023e249d6af2446d99bb
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
/*
|
||||
https://github.com/sebnil/Moving-Avarage-Filter--Arduino-Library-
|
||||
*/
|
||||
#include "MovingAverageFilter.h"
|
||||
|
||||
MovingAverageFilter::MovingAverageFilter(unsigned int newDataPointsCount)
|
||||
{
|
||||
k = 0; //initialize so that we start to write at index 0
|
||||
if (newDataPointsCount < MAX_DATA_POINTS)
|
||||
dataPointsCount = newDataPointsCount;
|
||||
else
|
||||
dataPointsCount = MAX_DATA_POINTS;
|
||||
|
||||
for (i = 0; i < dataPointsCount; i++)
|
||||
{
|
||||
values[i] = 0; // fill the array with 0's
|
||||
}
|
||||
}
|
||||
|
||||
float MovingAverageFilter::process(float in)
|
||||
{
|
||||
out = 0;
|
||||
|
||||
values[k] = in;
|
||||
k = (k + 1) % dataPointsCount;
|
||||
|
||||
for (i = 0; i < dataPointsCount; i++)
|
||||
{
|
||||
out += values[i];
|
||||
}
|
||||
|
||||
return out / dataPointsCount;
|
||||
}
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
/*
|
||||
https://github.com/sebnil/Moving-Avarage-Filter--Arduino-Library-
|
||||
*/
|
||||
#ifndef MovingAverageFilter_h
|
||||
#define MovingAverageFilter_h
|
||||
|
||||
#define MAX_DATA_POINTS 20
|
||||
|
||||
class MovingAverageFilter
|
||||
{
|
||||
public:
|
||||
//construct without coefs
|
||||
MovingAverageFilter(unsigned int newDataPointsCount);
|
||||
|
||||
float process(float in);
|
||||
|
||||
private:
|
||||
float values[MAX_DATA_POINTS];
|
||||
int k; // k stores the index of the current array read to create a circular memory through the array
|
||||
int dataPointsCount;
|
||||
float out;
|
||||
int i; // just a loop counter
|
||||
};
|
||||
#endif
|
||||
|
|
@ -0,0 +1,231 @@
|
|||
/**********************************************************************************************
|
||||
* Arduino PID Library - Version 1.2.1
|
||||
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
|
||||
*
|
||||
* This Library is licensed under the MIT License
|
||||
**********************************************************************************************/
|
||||
|
||||
#if ARDUINO >= 100
|
||||
#include "Arduino.h"
|
||||
#else
|
||||
#include "WProgram.h"
|
||||
#endif
|
||||
|
||||
#include "PID_v2.h"
|
||||
|
||||
/*Constructor (...)*********************************************************
|
||||
* The parameters specified here are those for for which we can't set up
|
||||
* reliable defaults, so we need to have the user set them.
|
||||
***************************************************************************/
|
||||
PID::PID(double* Input, double* Output, double* Setpoint,
|
||||
double Kp, double Ki, double Kd, int POn, int ControllerDirection)
|
||||
{
|
||||
myOutput = Output;
|
||||
myInput = Input;
|
||||
mySetpoint = Setpoint;
|
||||
inAuto = false;
|
||||
|
||||
PID::SetOutputLimits(0, 255); //default output limit corresponds to
|
||||
//the arduino pwm limits
|
||||
|
||||
SampleTime = 100; //default Controller Sample Time is 0.1 seconds
|
||||
|
||||
PID::SetControllerDirection(ControllerDirection);
|
||||
PID::SetTunings(Kp, Ki, Kd, POn);
|
||||
|
||||
lastTime = millis()-SampleTime;
|
||||
}
|
||||
|
||||
/*Constructor (...)*********************************************************
|
||||
* To allow backwards compatability for v1.1, or for people that just want
|
||||
* to use Proportional on Error without explicitly saying so
|
||||
***************************************************************************/
|
||||
|
||||
PID::PID(double* Input, double* Output, double* Setpoint,
|
||||
double Kp, double Ki, double Kd, int ControllerDirection)
|
||||
:PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* Compute() **********************************************************************
|
||||
* This, as they say, is where the magic happens. this function should be called
|
||||
* every time "void loop()" executes. the function will decide for itself whether a new
|
||||
* pid Output needs to be computed. returns true when the output is computed,
|
||||
* false when nothing has been done.
|
||||
**********************************************************************************/
|
||||
bool PID::Compute()
|
||||
{
|
||||
if(!inAuto) return false;
|
||||
unsigned long now = millis();
|
||||
unsigned long timeChange = (now - lastTime);
|
||||
if(timeChange>=SampleTime)
|
||||
{
|
||||
/*Compute all the working error variables*/
|
||||
double input = *myInput;
|
||||
double error = *mySetpoint - input;
|
||||
double dInput = (input - lastInput);
|
||||
outputSum+= (ki * error);
|
||||
|
||||
/*Add Proportional on Measurement, if P_ON_M is specified*/
|
||||
if(!pOnE) outputSum-= kp * dInput;
|
||||
|
||||
if(outputSum > outMax) outputSum= outMax;
|
||||
else if(outputSum < outMin) outputSum= outMin;
|
||||
|
||||
/*Add Proportional on Error, if P_ON_E is specified*/
|
||||
double output;
|
||||
if(pOnE) output = kp * error;
|
||||
else output = 0;
|
||||
|
||||
if(kd_lagpam <=1){
|
||||
/*Compute Rest of PID Output*/
|
||||
filteredDerivative =(1.0-kd_lagpam)*filteredDerivative
|
||||
+ (kd_lagpam)*dInput;
|
||||
}
|
||||
else{
|
||||
filteredDerivative = maf.process(dInput);
|
||||
}
|
||||
|
||||
output += outputSum - kd * filteredDerivative;
|
||||
|
||||
if(output > outMax) output = outMax;
|
||||
else if(output < outMin) output = outMin;
|
||||
*myOutput = output;
|
||||
|
||||
/*Remember some variables for next time*/
|
||||
lastInput = input;
|
||||
lastTime = now;
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
|
||||
/* SetTunings(...)*************************************************************
|
||||
* This function allows the controller's dynamic performance to be adjusted.
|
||||
* it's called automatically from the constructor, but tunings can also
|
||||
* be adjusted on the fly during normal operation
|
||||
******************************************************************************/
|
||||
void PID::SetTunings(double Kp, double Ki, double Kd, int POn)
|
||||
{
|
||||
if (Kp<0 || Ki<0 || Kd<0) return;
|
||||
|
||||
pOn = POn;
|
||||
pOnE = POn == P_ON_E;
|
||||
|
||||
dispKp = Kp; dispKi = Ki; dispKd = Kd;
|
||||
|
||||
double SampleTimeInSec = ((double)SampleTime)/1000;
|
||||
kp = Kp;
|
||||
ki = Ki * SampleTimeInSec;
|
||||
kd = Kd / SampleTimeInSec;
|
||||
|
||||
if(controllerDirection ==REVERSE)
|
||||
{
|
||||
kp = (0 - kp);
|
||||
ki = (0 - ki);
|
||||
kd = (0 - kd);
|
||||
}
|
||||
}
|
||||
|
||||
/* SetTunings(...)*************************************************************
|
||||
* Set Tunings using the last-rembered POn setting
|
||||
******************************************************************************/
|
||||
void PID::SetTunings(double Kp, double Ki, double Kd){
|
||||
SetTunings(Kp, Ki, Kd, pOn);
|
||||
}
|
||||
|
||||
/* SetSampleTime(...) *********************************************************
|
||||
* sets the period, in Milliseconds, at which the calculation is performed
|
||||
******************************************************************************/
|
||||
void PID::SetSampleTime(int NewSampleTime)
|
||||
{
|
||||
if (NewSampleTime > 0)
|
||||
{
|
||||
double ratio = (double)NewSampleTime
|
||||
/ (double)SampleTime;
|
||||
ki *= ratio;
|
||||
kd /= ratio;
|
||||
SampleTime = (unsigned long)NewSampleTime;
|
||||
}
|
||||
}
|
||||
|
||||
/* SetOutputLimits(...)****************************************************
|
||||
* This function will be used far more often than SetInputLimits. while
|
||||
* the input to the controller will generally be in the 0-1023 range (which is
|
||||
* the default already,) the output will be a little different. maybe they'll
|
||||
* be doing a time window and will need 0-8000 or something. or maybe they'll
|
||||
* want to clamp it from 0-125. who knows. at any rate, that can all be done
|
||||
* here.
|
||||
**************************************************************************/
|
||||
void PID::SetOutputLimits(double Min, double Max)
|
||||
{
|
||||
if(Min >= Max) return;
|
||||
outMin = Min;
|
||||
outMax = Max;
|
||||
|
||||
if(inAuto)
|
||||
{
|
||||
if(*myOutput > outMax) *myOutput = outMax;
|
||||
else if(*myOutput < outMin) *myOutput = outMin;
|
||||
|
||||
if(outputSum > outMax) outputSum= outMax;
|
||||
else if(outputSum < outMin) outputSum= outMin;
|
||||
}
|
||||
}
|
||||
|
||||
/* SetMode(...)****************************************************************
|
||||
* Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
|
||||
* when the transition from manual to auto occurs, the controller is
|
||||
* automatically initialized
|
||||
******************************************************************************/
|
||||
void PID::SetMode(int Mode)
|
||||
{
|
||||
bool newAuto = (Mode == AUTOMATIC);
|
||||
if(newAuto && !inAuto)
|
||||
{ /*we just went from manual to auto*/
|
||||
PID::Initialize();
|
||||
}
|
||||
inAuto = newAuto;
|
||||
}
|
||||
|
||||
/* Initialize()****************************************************************
|
||||
* does all the things that need to happen to ensure a bumpless transfer
|
||||
* from manual to automatic mode.
|
||||
******************************************************************************/
|
||||
void PID::Initialize()
|
||||
{
|
||||
outputSum = *myOutput;
|
||||
lastInput = *myInput;
|
||||
if(outputSum > outMax) outputSum = outMax;
|
||||
else if(outputSum < outMin) outputSum = outMin;
|
||||
}
|
||||
|
||||
/* SetControllerDirection(...)*************************************************
|
||||
* The PID will either be connected to a DIRECT acting process (+Output leads
|
||||
* to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to
|
||||
* know which one, because otherwise we may increase the output when we should
|
||||
* be decreasing. This is called from the constructor.
|
||||
******************************************************************************/
|
||||
void PID::SetControllerDirection(int Direction)
|
||||
{
|
||||
if(inAuto && Direction !=controllerDirection)
|
||||
{
|
||||
kp = (0 - kp);
|
||||
ki = (0 - ki);
|
||||
kd = (0 - kd);
|
||||
}
|
||||
controllerDirection = Direction;
|
||||
}
|
||||
|
||||
/* Status Funcions*************************************************************
|
||||
* Just because you set the Kp=-1 doesn't mean it actually happened. these
|
||||
* functions query the internal state of the PID. they're here for display
|
||||
* purposes. this are the functions the PID Front-end uses for example
|
||||
******************************************************************************/
|
||||
double PID::GetKp(){ return dispKp; }
|
||||
double PID::GetKi(){ return dispKi;}
|
||||
double PID::GetKd(){ return dispKd;}
|
||||
int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;}
|
||||
int PID::GetDirection(){ return controllerDirection;}
|
||||
|
|
@ -0,0 +1,103 @@
|
|||
#ifndef PID_v1_h
|
||||
#define PID_v1_h
|
||||
#define LIBRARY_VERSION 1.2.1
|
||||
|
||||
#include <MovingAverageFilter.h>
|
||||
|
||||
|
||||
|
||||
class PID
|
||||
{
|
||||
|
||||
|
||||
public:
|
||||
|
||||
//Constants used in some of the functions below
|
||||
#define AUTOMATIC 1
|
||||
#define MANUAL 0
|
||||
#define DIRECT 0
|
||||
#define REVERSE 1
|
||||
#define P_ON_M 0
|
||||
#define P_ON_E 1
|
||||
|
||||
//commonly used functions **************************************************************************
|
||||
PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and
|
||||
double, double, double, int, int);// Setpoint. Initial tuning parameters are also set here.
|
||||
// (overload for specifying proportional mode)
|
||||
|
||||
PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and
|
||||
double, double, double, int); // Setpoint. Initial tuning parameters are also set here
|
||||
|
||||
void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0)
|
||||
|
||||
bool Compute(); // * performs the PID calculation. it should be
|
||||
// called every time loop() cycles. ON/OFF and
|
||||
// calculation frequency can be set using SetMode
|
||||
// SetSampleTime respectively
|
||||
|
||||
void SetOutputLimits(double, double); // * clamps the output to a specific range. 0-255 by default, but
|
||||
// it's likely the user will want to change this depending on
|
||||
// the application
|
||||
|
||||
|
||||
|
||||
//available but not commonly used functions ********************************************************
|
||||
void SetTunings(double, double, // * While most users will set the tunings once in the
|
||||
double); // constructor, this function gives the user the option
|
||||
// of changing tunings during runtime for Adaptive control
|
||||
void SetTunings(double, double, // * overload for specifying proportional mode
|
||||
double, int);
|
||||
|
||||
void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT
|
||||
// means the output will increase when error is positive. REVERSE
|
||||
// means the opposite. it's very unlikely that this will be needed
|
||||
// once it is set in the constructor.
|
||||
void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which
|
||||
// the PID calculation is performed. default is 100
|
||||
|
||||
void SetDerivativeLag(double val){
|
||||
kd_lagpam = val;
|
||||
}
|
||||
|
||||
double getDerivative(){
|
||||
return filteredDerivative;
|
||||
}
|
||||
|
||||
|
||||
//Display functions ****************************************************************
|
||||
double GetKp(); // These functions query the pid for interal values.
|
||||
double GetKi(); // they were created mainly for the pid front-end,
|
||||
double GetKd(); // where it's important to know what is actually
|
||||
int GetMode(); // inside the PID.
|
||||
int GetDirection(); //
|
||||
|
||||
private:
|
||||
MovingAverageFilter maf =MovingAverageFilter(20);
|
||||
void Initialize();
|
||||
|
||||
double dispKp; // * we'll hold on to the tuning parameters in user-entered
|
||||
double dispKi; // format for display purposes
|
||||
double dispKd; //
|
||||
|
||||
double kp; // * (P)roportional Tuning Parameter
|
||||
double ki; // * (I)ntegral Tuning Parameter
|
||||
double kd; // * (D)erivative Tuning Parameter
|
||||
double filteredDerivative;
|
||||
double kd_lagpam = 1; //* 0.15 to 0.35
|
||||
|
||||
int controllerDirection;
|
||||
int pOn;
|
||||
|
||||
double *myInput; // * Pointers to the Input, Output, and Setpoint variables
|
||||
double *myOutput; // This creates a hard link between the variables and the
|
||||
double *mySetpoint; // PID, freeing the user from having to constantly tell us
|
||||
// what these values are. with pointers we'll just know.
|
||||
|
||||
unsigned long lastTime;
|
||||
double outputSum, lastInput;
|
||||
|
||||
unsigned long SampleTime;
|
||||
double outMin, outMax;
|
||||
bool inAuto, pOnE;
|
||||
};
|
||||
#endif
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
#include "PID_v2.h"
|
||||
|
||||
double Kp=1, Ki=0, Kd=0;
|
||||
double Input, Output,Setpoint;
|
||||
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
|
||||
|
||||
void setup() {
|
||||
// put your setup code here, to run once:
|
||||
myPID.SetSampleTime(1.5);
|
||||
myPID.SetDerivativeLag(1);
|
||||
myPID.SetOutputLimits(-255,255);
|
||||
myPID.SetControllerDirection(DIRECT);
|
||||
myPID.SetMode(AUTOMATIC);
|
||||
Serial.begin(9600);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
Input = 0; //[-179,180]
|
||||
Setpoint = 0;
|
||||
myPID.Compute();
|
||||
Serial.println(Output);
|
||||
|
||||
}
|
||||
|
|
@ -24,14 +24,17 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
|
|||
speed3 = 0;
|
||||
speed4 = 0;
|
||||
|
||||
pid = new PID(&input, &output, &setpoint, (double)KP, (double)KI, (double)KD, P_ON_M, REVERSE);
|
||||
delta = 0;
|
||||
input = 0;
|
||||
output = 0;
|
||||
setpoint = 0;
|
||||
|
||||
pid = new PID(&input, &output, &setpoint, KP, KI, KD, REVERSE);
|
||||
|
||||
pid->SetSampleTime(1.5);
|
||||
pid->SetDerivativeLag(1);
|
||||
pid->SetOutputLimits(-255,255);
|
||||
pid->SetMode(AUTOMATIC);
|
||||
pid->SetSampleTime(5);
|
||||
|
||||
canUnlock = true;
|
||||
unlockTime = 0;
|
||||
|
|
@ -76,21 +79,15 @@ void DriveController::drive(int dir, int speed, int tilt){
|
|||
speed4 = -(speed2);
|
||||
|
||||
// calcola l'errore di posizione rispetto allo 0
|
||||
// delta = (compass->getValue()-tilt+360)%360;
|
||||
delta = (CURRENT_DATA_READ.IMUAngle-tilt+360)%360;
|
||||
|
||||
setpoint = 0;
|
||||
pid->SetControllerDirection(REVERSE);
|
||||
|
||||
if(delta > 180) {
|
||||
setpoint = 359;//delta = delta-360;
|
||||
pid->SetControllerDirection(DIRECT);
|
||||
}
|
||||
delta = CURRENT_DATA_READ.IMUAngle;
|
||||
if(delta > 180) delta = delta - 360;
|
||||
|
||||
input = delta;
|
||||
pid->Compute();
|
||||
pidfactor = delta > 180 ? output*-1 : output;
|
||||
setpoint = 0;
|
||||
|
||||
pid->Compute();
|
||||
|
||||
pidfactor = output;
|
||||
speed1 += pidfactor;
|
||||
speed2 += pidfactor;
|
||||
speed3 += pidfactor;
|
||||
|
|
|
|||
|
|
@ -18,15 +18,15 @@ void Goalie::init(){
|
|||
}
|
||||
|
||||
void Goalie::realPlay(){
|
||||
if(ball->ballSeen) this->goCenter();
|
||||
else drive->prepareDrive(0,0,0);
|
||||
if(ball->ballSeen) this->goalie(50);
|
||||
else ((PositionSysZone*)ps)->goCenter();
|
||||
}
|
||||
|
||||
int dir, degrees2;
|
||||
void Goalie::goalie(int plusang) {
|
||||
if(ball->distance < 185) drive->prepareDrive(ball->angle, 350, 0);
|
||||
if(ball->distance < 160) drive->prepareDrive(ball->angle, 350, 0);
|
||||
else{
|
||||
if(ball->angle > 340 || ball->angle < 20) plusang -= 20;
|
||||
if(ball->angle > 340 || ball->angle < 20) plusang *= 0.15;
|
||||
if(ball->angle > 180) degrees2 = ball->angle - 360;
|
||||
else degrees2 = ball->angle;
|
||||
|
||||
|
|
@ -37,7 +37,7 @@ void Goalie::goalie(int plusang) {
|
|||
else dir = dir;
|
||||
|
||||
storcimentoPorta();
|
||||
if(ball->distance > 200 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(dir, 350, cstorc);
|
||||
if(ball->distance > 190 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(dir, 350, 0);
|
||||
else {
|
||||
drive->prepareDrive(dir, 350, 0);
|
||||
cstorc = 0;
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@ void setup() {
|
|||
initSensors();
|
||||
initGames();
|
||||
|
||||
delay(1500);
|
||||
delay(500);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -39,8 +39,8 @@ blue_led.on()
|
|||
|
||||
|
||||
|
||||
thresholds = [ (30, 70, -12, 19, 10, 57), # thresholds yellow goal
|
||||
(0, 44, -5, 42, -65, -13)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
|
||||
thresholds = [ (0, 99, -16, 19, 13, 85), # thresholds yellow goal
|
||||
(26, 52, -8, 19, -49, -18)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
|
||||
|
||||
roi = (0, 6, 318, 152)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue