slight edits to goalie

include/drivecontroller.h

@@ -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];
 

lib/Arduino-PID-Library

@@ -1 +0,0 @@
-Subproject commit 9b4ca0e5b6d7bab9c6ac023e249d6af2446d99bb

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;
+}

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

lib/ArduinoPIDLibraryFlavio/PID_v2.cpp

@@ -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;}

lib/ArduinoPIDLibraryFlavio/PID_v2.h

@@ -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

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);
+  
+}

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;
@@ -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;
-   delta = (CURRENT_DATA_READ.IMUAngle-tilt+360)%360;
+    if(delta > 180) delta = delta - 360;
-
-    setpoint = 0;
-    pid->SetControllerDirection(REVERSE);
-
-    if(delta > 180) {
-        setpoint = 359;//delta = delta-360;
-        pid->SetControllerDirection(DIRECT);
-    }
 
     input = delta;
-    pid->Compute();             
+    setpoint = 0;
-    pidfactor = delta > 180 ? output*-1 : output;
     
+    pid->Compute();
+
+    pidfactor = output;
     speed1 += pidfactor;
     speed2 += pidfactor;
     speed3 += pidfactor;

src/goalie.cpp

@@ -18,15 +18,15 @@ void Goalie::init(){
 }
 
 void Goalie::realPlay(){
-  if(ball->ballSeen) this->goCenter();
+  if(ball->ballSeen) this->goalie(50);
-  else drive->prepareDrive(0,0,0);
+  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;

src/main.cpp

@@ -12,7 +12,7 @@ void setup() {
   initSensors();
   initGames();
   
-  delay(1500);
+  delay(500);
 }
 
 

utility/OpenMV/conic_eff.py

@@ -39,8 +39,8 @@ blue_led.on()
 
 
 
-thresholds = [  (30, 70, -12, 19, 10, 57),    # thresholds yellow goal
+thresholds = [  (0, 99, -16, 19, 13, 85),    # thresholds yellow goal
-                (0, 44, -5, 42, -65, -13)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (26, 52, -8, 19, -49, -18)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 roi = (0, 6, 318, 152)