best pid YET

include/drivecontroller.h

@@ -6,8 +6,8 @@
 
 //PID Constants
 #define KP 1.5
-#define KI 0.0
-#define KD 0.3
+#define KI 0
+#define KD 0.1
 
 #define UNLOCK_THRESH 800
 

include/sensors.h

@@ -31,7 +31,7 @@ s_extr LineSys2019* linesCtrl;
 
 s_extr DataSourceBNO055* compass;
 s_extr DataSourceBall* ball;
-s_extr DataSourceCameraConic* camera;
+s_extr DataSourceCameraVShaped* camera;
 s_extr DriveController* drive;
 s_extr DataSourceBT* bt;
 

lib/ArduinoPIDLibraryFlavio/PID_v2.cpp

@@ -65,6 +65,12 @@ bool PID::Compute()
       /*Compute all the working error variables*/
       double input = *myInput;
       double error = *mySetpoint - input;
+
+      if(angleWrap){
+         if(error < -179) error += 360;
+         if(error > 180) error -= 360;
+      }
+      
       double dInput = (input - lastInput);
       outputSum+= (ki * error);
 

lib/ArduinoPIDLibraryFlavio/PID_v2.h

@@ -59,6 +59,10 @@ class PID
       kd_lagpam = val;
     }
 
+    void setAngleWrap(bool a){
+      angleWrap = a;
+    }
+
     double getDerivative(){
       return filteredDerivative;
     }
@@ -98,6 +102,6 @@ class PID
 
 	unsigned long SampleTime;
 	double outMin, outMax;
-	bool inAuto, pOnE;
+	bool inAuto, pOnE, angleWrap;
 };
 #endif

src/data_source_camera_vshapedmirror.cpp

@@ -88,9 +88,6 @@ void DataSourceCameraVShaped :: postProcess(){
       pDef = valY * -1;
     }
 
-    datavalid = 0;
-    cameraReady = 1;  //attivo flag di ricezione pacchetto
-  }
 
     //attacco gialla
     if(goalOrientation == HIGH){
@@ -104,6 +101,11 @@ void DataSourceCameraVShaped :: postProcess(){
       CURRENT_DATA_WRITE.angleDef = fixCamIMU(valY);
       CURRENT_DATA_WRITE.angleDefFix = valY;
     }
+
+    datavalid = 0;
+    cameraReady = 1;  //attivo flag di ricezione pacchetto
+  }
+
 }
 
 // int DataSourceCameraVShaped::getValueAtk(bool fixed){

src/drivecontroller.cpp

@@ -30,10 +30,11 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
     output = 0;
     setpoint = 0;
 
-    pid = new PID(&input, &output, &setpoint, KP, KI, KD, REVERSE);
+    pid = new PID(&input, &output, &setpoint, KP, KI, KD, 1,DIRECT);
 
     pid->SetSampleTime(1.5);
-    pid->SetDerivativeLag(1);
+    pid->setAngleWrap(true);
+    pid->SetDerivativeLag(2);
     pid->SetOutputLimits(-255,255);
     pid->SetMode(AUTOMATIC);
 
@@ -46,7 +47,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
     vyn = 0;
 }
 
-void DriveController::prepareDrive(int dir, int speed, int tilt){
+void DriveController::prepareDrive(int dir, int speed, int tilt=0){
     pDir = dir;
     pSpeed = speed;
     pTilt = tilt;
@@ -81,7 +82,7 @@ void DriveController::drive(int dir, int speed, int tilt){
     speed4 = -(speed2);
 
     //  calcola l'errore di posizione rispetto allo 0
-    delta = CURRENT_DATA_READ.IMUAngle;
+    delta = compass->getValue();
     if(delta > 180) delta = delta - 360;
 
     input = delta;
@@ -89,7 +90,7 @@ void DriveController::drive(int dir, int speed, int tilt){
     
     pid->Compute();
 
-    pidfactor = output;
+    pidfactor = -output;
     speed1 += pidfactor;
     speed2 += pidfactor;
     speed3 += pidfactor;

src/goalie.cpp

@@ -2,6 +2,7 @@
 #include "sensors.h"
 #include "vars.h"
 #include "status_vector.h"
+#include "math.h"
 #include "positionsys_zone.h"
 
 Goalie::Goalie() : Game() {
@@ -38,7 +39,7 @@ void Goalie::goalie(int plusang) {
     else dir = dir;
 
     storcimentoPorta();
-    if(ball->distance > 190 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(dir, 350, 0);
+    if(ball->distance > 185 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(dir, 350, cstorc);
     else {
       drive->prepareDrive(dir, 350, 0);
       cstorc = 0;
@@ -48,7 +49,7 @@ void Goalie::goalie(int plusang) {
 
 void Goalie::storcimentoPorta() {
   if (CURRENT_DATA_READ.angleAtkFix >= 10 && CURRENT_DATA_READ.angleAtkFix <= 90) cstorc+=9;
-  else if (CURRENT_DATA_READ.angleAtkFix  <= 350 && CURRENT_DATA_READ.angleAtkFix >= 270) cstorc-=9;
+  else if (CURRENT_DATA_READ.angleAtkFix  <= -10 && CURRENT_DATA_READ.angleAtkFix >= -90) cstorc-=9;
   // else cstorc *= 0.7;
   cstorc = constrain(cstorc, -45, 45);
 }

src/main.cpp

@@ -24,6 +24,7 @@ void loop() {
   keeper->play(role==0);  
 
   // Last thing to do: movement and update status vector
+  drive->prepareDrive(0,0,0);
   drive->drivePrepared();  
   updateStatusVector();
 } 

src/positionsys_zone.cpp

@@ -374,9 +374,9 @@ void PositionSysZone::testLogicZone(){
 
 
 void PositionSysZone::goCenter() {
-  if((camera->true_yb + camera->true_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0);
-  else if ((camera->true_yb + camera->true_yy) < CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0); 
-  else drive->prepareDrive(0, 0, 0);
+  // if((camera->true_yb + camera->true_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0);
+  // else if ((camera->true_yb + camera->true_yy) < CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0); 
+  // else drive->prepareDrive(0, 0, 0);
 /*   if(camera->true_xb < -CAMERA_CENTER_X || camera->true_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0);
   else if(camera->true_xb > CAMERA_CENTER_X || camera->true_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0);
   else drive->prepareDrive(0, 0, 0); */

src/sensors.cpp

@@ -15,7 +15,7 @@ void initSensors(){
     drive = new DriveController(new Motor(12, 11, 2, 45),new  Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
     compass = new DataSourceBNO055();
     ball = new DataSourceBall(&Serial4, 57600);
-    camera = new DataSourceCameraConic(&Serial2, 19200);
+    camera = new DataSourceCameraVShaped(&Serial2, 19200);
     usCtrl = new DataSourceCtrl(dUs);
     bt = new DataSourceBT(&Serial3, 115200);
  }
@@ -26,6 +26,6 @@ void updateSensors(){
     
     compass->update();
     ball->update();
-    camera->update();
+    // camera->update();
     usCtrl->update();
 }

utility/OpenMV/conic_eff.py

@@ -39,8 +39,8 @@ blue_led.on()
 
 
 
-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)
+thresholds = [  (26, 74, -11, 6, 17, 50),    # thresholds yellow goal
+                (12, 44, -34, 42, -105, -25)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 roi = (0, 6, 318, 152)