centering with camera only when both goals seen. Using a pid for each axis and then converging the outputs in an angle. This could be possibly done with multiple movements in a single loop and then merged together in a single one.

include/positionsys_camera.h

@@ -1,7 +1,17 @@
+#include "PID_v2.h"
 #include "systems.h"
 
-#define CAMERA_CENTER_X 3
+#define CAMERA_CENTER_X 0
-#define CAMERA_CENTER_Y 6
+#define CAMERA_CENTER_Y_BOTH 3
+#define CAMERA_CENTER_Y_BLUE -60
+#define CAMERA_CENTER_Y_YELLOW -30
+
+#define Kpx 1
+#define Kix 0
+#define Kdx 0
+#define Kpy 1
+#define Kiy 0
+#define Kdy 0
 
 class PositionSysCamera : public PositionSystem{
 
@@ -10,5 +20,12 @@ class PositionSysCamera : public PositionSystem{
         void goCenter();
         void update() override;
         void test() override;
+        void setCameraPID();
+        void CameraPID();
+
+        double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;
+
+        PID* X;
+        PID* Y;
 
 };

lib/Arduino-PID-Library

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

src/drivecontroller.cpp

@@ -50,7 +50,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
     vyn = 0;
 }
 
-void DriveController::prepareDrive(int dir, int speed, int tilt=0){
+void DriveController::prepareDrive(int dir, int speed, int tilt){
     pDir = dir;
     pSpeed = speed;
     pTilt = tilt;

src/main.cpp

@@ -25,6 +25,7 @@ void loop() {
 
   // Last thing to do: movement and update status vector
   // drive->prepareDrive(0,0,  CURRENT_DATA_READ.angleAtkFix);
+
   drive->drivePrepared();  
   updateStatusVector();
 } 

src/positionsys_camera.cpp

@@ -3,7 +3,9 @@
 #include "vars.h"
 #include "sensors.h"
 
-PositionSysCamera::PositionSysCamera() {}
+PositionSysCamera::PositionSysCamera() {
+    setCameraPID();
+}
 
 void PositionSysCamera::update(){
 }
@@ -14,26 +16,83 @@ void PositionSysCamera::test(){
 void PositionSysCamera::goCenter(){
     /*WORKS BUT CAN BE BETTER*/
     //Y
-    if((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0);
+    /* if((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0);
     else if ((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0);
     //X
     else if(CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0);
     else if(CURRENT_DATA_READ.cam_xb > CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0);
-    else drive->prepareDrive(0, 0, 0);
+    else drive->prepareDrive(0, 0, 0); */
 
 
     /*MAKING A SINGLE LINE HERE, DOESN'T WORK FOR NOW*/
-    // int x = 1;
+    /* int x = 1;
-    // int y = 1;
+    int y = 1;
     
-    // //Trying using an angle
+    //Trying using an angle
-    // if((CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) 
+    if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){
-    // y = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy;
+    if((CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) 
-    // if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xb;
+    y = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy;
-    // if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xy;
+    if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xb;
-
+    if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xy;
-    // int dir = -90-(atan2(y*1.5,x)*180/3.14);
-    // dir = (dir+360) % 360;
-    // drive->prepareDrive(dir, 100, 0);
 
+    int dir = -90-(atan2(y*1.5,x)*180/3.14);
+    dir = (dir+360) % 360;
+    drive->prepareDrive(dir, 100, 0);
+    } */
+    CameraPID();
+}
+
+//using a pid controller for the movement, or trying at least
+void PositionSysCamera :: setCameraPID(){
+    Inputx = 0;
+    Outputx = 0;
+    Setpointx = 0;
+    Inputy = 0;
+    Outputy = 0;
+    Setpointy = 0;
+    
+    X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, DIRECT);
+    X->SetOutputLimits(-50,50);
+    X->SetMode(AUTOMATIC);
+    X->SetDerivativeLag(1);
+    X->SetSampleTime(2);
+    Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, DIRECT);
+    Y->SetOutputLimits(-50,50);
+    Y->SetMode(AUTOMATIC);
+    Y->SetDerivativeLag(1);
+    Y->SetSampleTime(2);
+}
+
+void PositionSysCamera :: CameraPID(){   
+    if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){
+        Inputx = (CURRENT_DATA_READ.cam_xy + CURRENT_DATA_READ.cam_xb) / 2;
+        Inputy = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy;
+        Setpointx = CAMERA_CENTER_X;
+        Setpointy = CAMERA_CENTER_Y_BOTH;
+    }
+    if (CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == false){
+        Inputx = CURRENT_DATA_READ.cam_xb;
+        Inputy = CURRENT_DATA_READ.cam_yb;
+        Setpointx = CAMERA_CENTER_X;
+        Setpointy = CAMERA_CENTER_Y_BLUE; 
+    }
+    if (CURRENT_DATA_READ.bSeen == false && CURRENT_DATA_READ.ySeen == true){
+        Inputx = CURRENT_DATA_READ.cam_xy;
+        Inputy = CURRENT_DATA_READ.cam_yy;
+        Setpointx = CAMERA_CENTER_X;
+        Setpointy = CAMERA_CENTER_Y_YELLOW;
+        //Setpointy todo
+    }else{
+
+    }
+    //TODO: no goal seen
+        
+    X->Compute();
+    Y->Compute();
+    
+    // DEBUG.println(Outputx);
+
+    int dir = -90-(atan2(-Outputy,-Outputx)*180/3.14);
+    dir = (dir+360) % 360;
+    drive->prepareDrive(dir, 100, 0);
 }

utility/OpenMV/conic_eff.py

@@ -38,8 +38,8 @@ blue_led.on()
 ##############################################################################
 
 
-thresholds = [  (72, 100, -18, 11, 12, 65)  ,    # thresholds yellow goal
+thresholds = [  (23, 38, -34, -8, -12, 22),    # thresholds yellow goal
-                (39, 61, -18, 11, -47, -16)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (69, 98, -14, 30, 66, 113)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 roi = (0, 6, 318, 152)
 
@@ -60,10 +60,11 @@ sensor.reset()
 sensor.set_pixformat(sensor.RGB565)
 sensor.set_framesize(sensor.QQVGA)
 sensor.set_contrast(+3)
-sensor.set_saturation(0)
+sensor.set_saturation(2)
-sensor.set_brightness(-2)
+sensor.set_brightness(0)
 sensor.set_quality(0)
-sensor.set_auto_exposure(False, 6000)
+sensor.set_auto_whitebal(False)
+sensor.set_auto_exposure(False, 5000)
 sensor.set_auto_gain(True)
 sensor.skip_frames(time = 300)