diff --git a/include/drivecontroller.h b/include/drivecontroller.h index 6b7b5e2..16810fa 100755 --- a/include/drivecontroller.h +++ b/include/drivecontroller.h @@ -2,12 +2,12 @@ #include #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]; diff --git a/lib/Arduino-PID-Library b/lib/Arduino-PID-Library deleted file mode 160000 index 9b4ca0e..0000000 --- a/lib/Arduino-PID-Library +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 9b4ca0e5b6d7bab9c6ac023e249d6af2446d99bb diff --git a/lib/ArduinoPIDLibraryFlavio/MovingAverageFilter.cpp b/lib/ArduinoPIDLibraryFlavio/MovingAverageFilter.cpp new file mode 100644 index 0000000..cb8b509 --- /dev/null +++ b/lib/ArduinoPIDLibraryFlavio/MovingAverageFilter.cpp @@ -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; +} diff --git a/lib/ArduinoPIDLibraryFlavio/MovingAverageFilter.h b/lib/ArduinoPIDLibraryFlavio/MovingAverageFilter.h new file mode 100644 index 0000000..15b0859 --- /dev/null +++ b/lib/ArduinoPIDLibraryFlavio/MovingAverageFilter.h @@ -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 diff --git a/lib/ArduinoPIDLibraryFlavio/PID_v2.cpp b/lib/ArduinoPIDLibraryFlavio/PID_v2.cpp new file mode 100644 index 0000000..2ba9c4c --- /dev/null +++ b/lib/ArduinoPIDLibraryFlavio/PID_v2.cpp @@ -0,0 +1,231 @@ +/********************************************************************************************** + * Arduino PID Library - Version 1.2.1 + * by Brett Beauregard 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;} diff --git a/lib/ArduinoPIDLibraryFlavio/PID_v2.h b/lib/ArduinoPIDLibraryFlavio/PID_v2.h new file mode 100644 index 0000000..6c7dd44 --- /dev/null +++ b/lib/ArduinoPIDLibraryFlavio/PID_v2.h @@ -0,0 +1,103 @@ +#ifndef PID_v1_h +#define PID_v1_h +#define LIBRARY_VERSION 1.2.1 + +#include + + + +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 diff --git a/lib/ArduinoPIDLibraryFlavio/sketch_feb12a.ino b/lib/ArduinoPIDLibraryFlavio/sketch_feb12a.ino new file mode 100644 index 0000000..2a3b18d --- /dev/null +++ b/lib/ArduinoPIDLibraryFlavio/sketch_feb12a.ino @@ -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); + +} diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp index c06b124..534b6b9 100755 --- a/src/drivecontroller.cpp +++ b/src/drivecontroller.cpp @@ -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; @@ -75,22 +78,16 @@ void DriveController::drive(int dir, int speed, int tilt){ speed3 = -(speed1); 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); - } + // calcola l'errore di posizione rispetto allo 0 + 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; diff --git a/src/goalie.cpp b/src/goalie.cpp index 275c9a0..0352e8d 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -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; diff --git a/src/main.cpp b/src/main.cpp index 0459db1..b1d2742 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -12,7 +12,7 @@ void setup() { initSensors(); initGames(); - delay(1500); + delay(500); } diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index 4a16bd5..05cb677 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -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)