now the robot can use the camera to recenter even when one the goals can't be seen

include/positionsys_camera.h

@@ -2,9 +2,8 @@
 #include "systems.h"
 
 #define CAMERA_CENTER_X 0
-#define CAMERA_CENTER_Y_BOTH 3
+#define CAMERA_CENTER_Y -13
-#define CAMERA_CENTER_Y_BLUE -60
+#define CAMERA_CENTER_Y_ABS_SUM 72
-#define CAMERA_CENTER_Y_YELLOW -30
 
 #define Kpx 1
 #define Kix 0
@@ -22,6 +21,7 @@ class PositionSysCamera : public PositionSystem{
         void test() override;
         void setCameraPID();
         void CameraPID();
+        int calcOtherGoalY(int goalY);
 
         double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;
 

src/data_source_camera_conicmirror.cpp

@@ -52,8 +52,8 @@ void DataSourceCameraConic ::readSensor() {
         yAngleFix = ((int)((yAngle + angleFix * 0.8)) + 360) % 360;
         bAngleFix = ((int)((bAngle + angleFix * 0.8)) + 360) % 360;
 
-        yDist = sqrt((true_yy - 50) * (true_yy - 50) + (50 - true_xy) * (50 - true_xy));
+        yDist = sqrt((true_yy) * (true_yy) + (true_xy) * (true_xy));
-        bDist = sqrt((true_yb - 50) * (true_yb - 50) + (50 - true_xb) * (50 - true_xb));
+        bDist = sqrt((true_yb) * (true_yb) + (true_xb) * (true_xb));
 
         //Important: update status vector
         CURRENT_INPUT_WRITE.cameraByte = value;
@@ -68,14 +68,10 @@ void DataSourceCameraConic ::readSensor() {
         CURRENT_DATA_WRITE.yDist = yDist;
         CURRENT_DATA_WRITE.bDist = bDist;
 
-        if (xb == unkn || yb == unkn)
+        if (xb == unkn || yb == unkn) CURRENT_DATA_WRITE.bSeen = false;
-          CURRENT_DATA_WRITE.bSeen = false;
+        else CURRENT_DATA_WRITE.bSeen = true;
-        else
+        if (xy == unkn || yy == unkn) CURRENT_DATA_WRITE.ySeen = false;
-          CURRENT_DATA_WRITE.bSeen = true;
+        else CURRENT_DATA_WRITE.ySeen = true;
-        if (xy == unkn || yy == unkn)
-          CURRENT_DATA_WRITE.ySeen = false;
-        else
-          CURRENT_DATA_WRITE.ySeen = true;
 
         if (goalOrientation == HIGH) {
           CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.yAngle;
@@ -132,7 +128,9 @@ void DataSourceCameraConic::test()
   DEBUG.print(" | ");
   DEBUG.print(bAngleFix);
   DEBUG.print(" | ");
-  DEBUG.println(bDist);
+  DEBUG.print(bDist);
+  DEBUG.print(" | ");
+  DEBUG.println(CURRENT_DATA_READ.bSeen);
   DEBUG.println(" --- ");
 
   DEBUG.print("Yellow: ");
@@ -140,7 +138,9 @@ void DataSourceCameraConic::test()
   DEBUG.print(" | ");
   DEBUG.print(yAngleFix);
   DEBUG.print(" | ");
-  DEBUG.println(yDist);
+  DEBUG.print(yDist);
+  DEBUG.print(" | ");
+  DEBUG.println(CURRENT_DATA_READ.ySeen);
   DEBUG.println("---------------");
   DEBUG.print("Data: ");
   DEBUG.print(true_xb);

src/main.cpp

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

src/positionsys_camera.cpp

@@ -63,36 +63,47 @@ void PositionSysCamera :: setCameraPID(){
     Y->SetSampleTime(2);
 }
 
+/*Knowing the sum of the absolute values of the y position of the goals, it calculates the missing goal y knowing the other one
+We know the sum of the absolute values is a fixed number.
+By subtracting the absolute value of the goal y we know to the sum of the absolute values, we get the absolute value of the missing goal y
+The sign of the goal y we found is simply the reverse of the one we got
+*/
+int PositionSysCamera::calcOtherGoalY(int goalY){
+    int otherGoalY = CAMERA_CENTER_Y_ABS_SUM - abs(goalY);
+    otherGoalY = goalY < 0 ? otherGoalY : -otherGoalY;
+    return otherGoalY;
+}
+
 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;
+    }else if (CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == false){
-        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;
+        Inputy = CURRENT_DATA_READ.cam_yb + calcOtherGoalY(CURRENT_DATA_READ.cam_yb);
-        Setpointx = CAMERA_CENTER_X;
+    }else if (CURRENT_DATA_READ.bSeen == false && CURRENT_DATA_READ.ySeen == true){
-        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;
+        Inputy = CURRENT_DATA_READ.cam_yy + calcOtherGoalY(CURRENT_DATA_READ.cam_yy);
-        Setpointx = CAMERA_CENTER_X;
-        Setpointy = CAMERA_CENTER_Y_YELLOW;
         //Setpointy todo
     }else{
 
     }
+    Setpointx = CAMERA_CENTER_X;
+    Setpointy = CAMERA_CENTER_Y;
     //TODO: no goal seen
         
     X->Compute();
     Y->Compute();
     
-    // DEBUG.println(Outputx);
+    // DEBUG.print(CURRENT_DATA_READ.cam_yb);
+    // DEBUG.print(" ");
+    // DEBUG.println(calcOtherGoalY(CURRENT_DATA_READ.cam_yb));
 
+    if(abs(Outputx) <= 1 && abs(Outputy) <= 1){
+        drive->prepareDrive(0,0,0);
+    }else{
         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 = [  (23, 38, -34, -8, -12, 22),    # thresholds yellow goal
+thresholds = [  (50, 98, -2, 30, 57, 113),    # thresholds yellow goal
-                (69, 98, -14, 30, 66, 113)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (17, 30, -28, -8, -8, 12)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 roi = (0, 6, 318, 152)
 
@@ -59,12 +59,12 @@ clock = time.clock()'''
 sensor.reset()
 sensor.set_pixformat(sensor.RGB565)
 sensor.set_framesize(sensor.QQVGA)
-sensor.set_contrast(+3)
+sensor.set_contrast(1)
-sensor.set_saturation(2)
+sensor.set_saturation(1)
 sensor.set_brightness(0)
 sensor.set_quality(0)
 sensor.set_auto_whitebal(False)
-sensor.set_auto_exposure(False, 5000)
+sensor.set_auto_exposure(False, 4500)
 sensor.set_auto_gain(True)
 sensor.skip_frames(time = 300)
 
@@ -84,7 +84,7 @@ while(True):
     tt_blue = [(0,999,0,2)]       ## creo una lista di tuple per il blue, valore x = 999 : non trovata
 
     img = sensor.snapshot()
-    for blob in img.find_blobs(thresholds, pixels_threshold=50, area_threshold=80, merge = True):
+    for blob in img.find_blobs(thresholds, pixels_threshold=60, area_threshold=90, merge = True):
         img.draw_rectangle(blob.rect())
         img.draw_cross(blob.cx(), blob.cy())