camera: correct a critical bug in goal seen logic

include/behaviour_control/status_vector.h

@@ -40,11 +40,11 @@ typedef struct data{
         yAngle, bAngle, yAngleFix, bAngleFix, 
         yDist, bDist, 
         angleAtk, angleAtkFix, angleDef, angleDefFix, distAtk, distDef, yAtk, yAtkFix, xAtk, xAtkFix, yDef, yDefFix, xDef, xDefFix,
-        cam_xb, cam_yb, cam_xy, cam_yy,
+        cam_xb = 0, cam_yb = 0, cam_xy = 0, cam_yy = 0,
         speed, tilt, dir, axisBlock[4],
         USfr, USsx, USdx, USrr, 
-        lineOutDir, matePos, role, cam_xb_fixed,
+        lineOutDir, matePos, role, cam_xb_fixed = 0,
-        cam_xy_fixed, cam_yb_fixed, cam_yy_fixed,
+        cam_xy_fixed = 0, cam_yb_fixed = 0, cam_yy_fixed = 0,
         posx, posy, addvx, addvy;
     Game* game;
     LineSystem* lineSystem;
@@ -52,7 +52,7 @@ typedef struct data{
     byte lineSeen, lineActive;
     bool mate, 
         ATKgoal, DEFgoal, 
-        atkSeen, defSeen, bSeen, ySeen,
+        atkSeen, defSeen, bSeen = true, ySeen = false,
         ballSeen, ballPresent;
 }data;
 

include/sensors/data_source_camera_conicmirror.h

@@ -28,7 +28,7 @@ These values need to be subtracted from the coords used in setMoveSetpoints*/
 
 //Robot with roller
 #define CAMERA_TRANSLATION_X 0
-#define CAMERA_TRANSLATION_Y -3
+#define CAMERA_TRANSLATION_Y 0
 
 class DataSourceCameraConic : public DataSource{
 

include/strategy_roles/games.h

@@ -15,6 +15,7 @@
 #include "strategy_roles/spot_finder.h"
 #include "strategy_roles/the_spinner.h"
 #include "strategy_roles/round_robin.h"
+#include "strategy_roles/corner_kick_2.h"
 // #include "strategy_roles/keeper.h"
 
 void initGames();
@@ -28,4 +29,5 @@ g_extr Game* pass_and_shoot;
 g_extr Game* tc1;
 g_extr Game* tc2;
 g_extr Game* st_tc1;
-g_extr Game* st_tc3;
+g_extr Game* st_tc3;
+g_extr Game* tc3_2;

src/sensors/data_source_camera_conicmirror.cpp

@@ -85,6 +85,9 @@ void DataSourceCameraConic ::readSensor() {
 
 
 void DataSourceCameraConic ::computeCoordsAngles() {
+  CURRENT_DATA_WRITE.bSeen = true_xb != unkn && true_yb != unkn;
+  CURRENT_DATA_WRITE.ySeen = true_xy != unkn && true_yy != unkn;
+
   //Where are the goals relative to the robot?
   //Remap from [0,100] to [-50, +50] to correctly compute angles and distances and calculate them, getting the original coords calculated by the camera
   true_xb = 50 - true_xb + CAMERA_TRANSLATION_X*0.5;
@@ -150,12 +153,7 @@ void DataSourceCameraConic ::computeCoordsAngles() {
   CURRENT_DATA_WRITE.bAngleFix = bAngleFix;
   CURRENT_DATA_WRITE.yDist = yDist;
   CURRENT_DATA_WRITE.bDist = bDist;
-
+  
-  if (xb == unkn || yb == unkn) CURRENT_DATA_WRITE.bSeen = false;
-  else CURRENT_DATA_WRITE.bSeen = 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;
     CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix;
@@ -201,7 +199,7 @@ void DataSourceCameraConic ::computeCoordsAngles() {
   byte to_32u4 = 0;
   to_32u4 |= (CURRENT_DATA_READ.ySeen);
   to_32u4 |= (CURRENT_DATA_READ.bSeen) << 1;
-  // BALL_32U4.write(to_32u4);
+  BALL_32U4.write(to_32u4);
 }
 
 void DataSourceCameraConic::test(){

src/system/positions/positionsys_camera.cpp

@@ -54,8 +54,7 @@ void PositionSysCamera::update(){
     posx *= -1;
     posy *= -1;
 
-    //Filtering error in calculation like this is a dirty hack, I know
+    if(CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == false ) {
-    if(posx < -MAX_X || posx > MAX_X || posy <  -MAX_Y || posy > MAX_Y || (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == false) ) {
         // Go back in time until we found a valid status, when we saw at least one goal
         int i = 1;
         do{
@@ -70,7 +69,6 @@ void PositionSysCamera::update(){
             // Trick the status vector into thinking this was a valid status
             CURRENT_DATA_WRITE.ySeen = valid_data.ySeen;
             CURRENT_DATA_WRITE.bSeen = valid_data.bSeen;
-
         }
     }
 

utility/OpenMV/conic_eff_h7.py

@@ -83,7 +83,7 @@ clock = time.clock()
 while(True):
     clock.tick()
 
-    print("Exposure: " + str(sensor.get_exposure_us()) + " Gain: " + str(sensor.get_gain_db()) + " White Bal: " + str(sensor.get_rgb_gain_db()))
+    #print("Exposure: " + str(sensor.get_exposure_us()) + " Gain: " + str(sensor.get_gain_db()) + " White Bal: " + str(sensor.get_rgb_gain_db()))
 
     blue_led.off()
 
@@ -174,40 +174,39 @@ while(True):
                 #Prepare for send as a list of characters
                 s_bcx = chr(b_cx)
                 s_bcy = chr(b_cy)
-
-    '''index = 1
-    if b_found == True:
-        while nb-index >= 0:
-            b_area, b1_cx, b1_cy, b_code = tt_blue[nb-index]
-
-            index += 1
-            # If the two blobs are on opposide side of the field, everything is good
-            if (not y_found) or ((isInRightSide(img, b1_cx) and isInLeftSide(img, y1_cx)) or (isInRightSide(img, y1_cx) and isInLeftSide(img, b1_cx))):
-
-                img.draw_cross(b1_cx, b1_cy)
-
-                b_cx = int(b1_cy - img.height() / 2)
-                b_cy = int(img.width() / 2 - b1_cx)
-
-                print("before :" + str(b_cx) + " " + str(b_cy))
-                b_cx = val_map(b_cx, -img.height() / 2, img.height() / 2, 100, 0)
-                b_cy = val_map(b_cy, -img.width() / 2, img.width() / 2, 0, 100)
-
-                print("after :" + str(b_cx) + " " + str(b_cy))
-
-                #Prepare for send as a list of characters
-                s_bcx = chr(b_cx)
-                s_bcy = chr(b_cy)
-
-                break
     else:
         b_cx = BYTE_UNKNOWN
         b_cy = BYTE_UNKNOWN
         #Prepare for send as a list of characters
         s_bcx = b_cx
-        s_bcy = b_cy'''
+        s_bcy = b_cy
+        '''index = 1
+        if b_found == True:
+            while nb-index >= 0:
+                b_area, b1_cx, b1_cy, b_code = tt_blue[nb-index]
 
-    #print(str(y_cx) + " | " + str(y_cy) + "  ---  " + str(b_cx) + " | " + str(b_cy))
+                index += 1
+                # If the two blobs are on opposide side of the field, everything is good
+                if (not y_found) or ((isInRightSide(img, b1_cx) and isInLeftSide(img, y1_cx)) or (isInRightSide(img, y1_cx) and isInLeftSide(img, b1_cx))):
+
+                    img.draw_cross(b1_cx, b1_cy)
+
+                    b_cx = int(b1_cy - img.height() / 2)
+                    b_cy = int(img.width() / 2 - b1_cx)
+
+                    print("before :" + str(b_cx) + " " + str(b_cy))
+                    b_cx = val_map(b_cx, -img.height() / 2, img.height() / 2, 100, 0)
+                    b_cy = val_map(b_cy, -img.width() / 2, img.width() / 2, 0, 100)
+
+                    print("after :" + str(b_cx) + " " + str(b_cy))
+
+                    #Prepare for send as a list of characters
+                    s_bcx = chr(b_cx)
+                    s_bcy = chr(b_cy)
+
+                    break'''
+
+    print(str(s_ycx) + " | " + str(s_ycy) + "  ---  " + str(s_bcx) + " | " + str(s_bcy))
 
     uart.write(START_BYTE)
     uart.write(s_bcx)