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();
@@ -29,3 +30,4 @@ g_extr Game* tc1;
 g_extr Game* tc2;
 g_extr Game* st_tc1;
 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;
@@ -151,11 +154,6 @@ void DataSourceCameraConic ::computeCoordsAngles() {
   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,7 +174,12 @@ while(True):
                 #Prepare for send as a list of characters
                 s_bcx = chr(b_cx)
                 s_bcy = chr(b_cy)
-
+    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
         '''index = 1
         if b_found == True:
             while nb-index >= 0:
@@ -199,15 +204,9 @@ while(True):
                     s_bcx = chr(b_cx)
                     s_bcy = chr(b_cy)
 
-                break
+                    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'''
 
-    #print(str(y_cx) + " | " + str(y_cy) + "  ---  " + str(b_cx) + " | " + str(b_cy))
+    print(str(s_ycx) + " | " + str(s_ycy) + "  ---  " + str(s_bcx) + " | " + str(s_bcy))
 
     uart.write(START_BYTE)
     uart.write(s_bcx)