robocup preparation: fix lines

add a workaround for when we don't see either of the goals
add nice startup sounds

include/behaviour_control/status_vector.h

@@ -22,7 +22,7 @@
 #define sv_extr extern
 #endif
 
-#define dim 20
+#define dim 50
 #define CURRENT_DATA_READ ( datas[((currentSVIndex-1+dim) % dim)]  )
 #define CURRENT_DATA_WRITE ( datas[((currentSVIndex))]  )
 #define CURRENT_INPUT_READ ( inputs[((currentSVIndex-1+dim) % dim)] )
@@ -60,4 +60,6 @@ sv_extr data datas[dim];
 sv_extr int currentSVIndex;
 
 void initStatusVector();
-void updateStatusVector();
+void updateStatusVector();
+data getDataAtIndex(int index);
+data getDataAtIndex_backwardsFromCurrent(int steps);

include/motors_movement/roller.h

@@ -16,5 +16,5 @@ class Roller{
         int pinPwm, pinSense, MIN, MAX, ARM;
         bool roller_arm_setup, roller_armed;
 
-        int roller_setup_phase, roller_counter;
+        int roller_setup_phase, roller_counter, roller_speed;
 };

include/systems/lines/linesys_camera.h

@@ -17,8 +17,7 @@
 #define S4O A8
 
 #define LINE_THRESH_CAM 325
-#define EXIT_TIME 250
-#define LINES_EXIT_SPD 350
+#define EXIT_TIME 300
 
 class LineSysCamera : public LineSystem{
 

include/systems/position/positionsys_camera.h

@@ -3,6 +3,7 @@
 #include "PID_v2.h"
 #include "systems/systems.h"
 #include "behaviour_control/complementary_filter.h"
+#include "behaviour_control/status_vector.h"
 
 /*Camera translation: because of mechanical imprecision, the center of the camera and the center of the cone mirror may not coincide
 To overcome this, each coordinate needs to be shifted by some amount, defined on a per-robot basis that needs to be recalibrated each time.
@@ -55,4 +56,6 @@ class PositionSysCamera : public PositionSystem{
         ComplementaryFilter* filterDir;
         ComplementaryFilter* filterSpeed;
 
+        data valid_data;
+
 };

src/behaviour_control/status_vector.cpp

@@ -10,8 +10,19 @@ void initStatusVector(){
     }
 }
 
+data getDataAtIndex(int index){
+    index = constrain(index, 0, dim-1);
+    return datas[index];
+}
+
+data getDataAtIndex_backwardsFromCurrent(int steps){
+    steps = constrain(steps, 0, dim-1);
+    return getDataAtIndex((currentSVIndex-steps+dim) % dim);
+}
+
 void updateStatusVector(){
     currentSVIndex = (currentSVIndex+1) % dim;
     CURRENT_DATA_WRITE = CURRENT_DATA_READ;
     CURRENT_INPUT_WRITE = CURRENT_INPUT_READ;
-}
+}
+

src/main.cpp

@@ -17,6 +17,7 @@ bool striker_condition = false;
 bool keeper_condition = false;
 
 void setup() {
+  tone(BUZZER, 220, 250);
   delay(1500);
   DEBUG.begin(115200);
     
@@ -26,8 +27,11 @@ void setup() {
   }
 
   testmenu = new TestMenu();
+  tone(BUZZER, 240, 250);
   initStatusVector();
+  tone(BUZZER, 260, 250);
   initSensors();
+  tone(BUZZER, 320, 250);
   initGames();
 
   delay(500);
@@ -35,7 +39,7 @@ void setup() {
   drive->prepareDrive(0,0,0);
 
   //Startup sound
-  tone(BUZZER, 220.00, 250);
+  tone(BUZZER, 350.00, 250);
 }
 
 void loop() {
@@ -48,7 +52,7 @@ void loop() {
 
   striker->play(striker_condition);
   keeper->play(keeper_condition);
-  testmenu->testMenu();
+  // testmenu->testMenu();
 
   // Last thing to do: movement and update status vector
   drive->drivePrepared();  

src/motors_movement/drivecontroller.cpp

@@ -121,15 +121,15 @@ void DriveController::drive(int dir, int speed, int tilt){
         speed3 /= ratio;
         speed4 /= ratio;
 
-        DEBUG.print(speed1);
-        DEBUG.print(" | ");
-        DEBUG.print(speed2);
-        DEBUG.print(" | ");
-        DEBUG.print(speed3);
-        DEBUG.print(" | ");
-        DEBUG.print(speed4);
-        DEBUG.print(" | ");
-        DEBUG.println(maxVel);  
+        // DEBUG.print(speed1);
+        // DEBUG.print(" | ");
+        // DEBUG.print(speed2);
+        // DEBUG.print(" | ");
+        // DEBUG.print(speed3);
+        // DEBUG.print(" | ");
+        // DEBUG.print(speed4);
+        // DEBUG.print(" | ");
+        // DEBUG.println(maxVel);  
     }
 
     speed1 = constrain(speed1, -255, 255);

src/motors_movement/roller.cpp

@@ -22,7 +22,8 @@ Roller::Roller(int pinPwm_, int pinSense_, int MIN_, int MAX_, int ARM_){
     roller_setup_phase = 99;
 
     roller_counter = 0;
-
+    roller_speed = 0;
+    
     pinMode(pinSense, INPUT_PULLUP);
     roller = new ESC(pinPwm, MIN, MAX, ARM);
 }
@@ -85,12 +86,12 @@ void Roller::update(){
     this->setup();
 
   if(millis() - t > 10 && roller_armed){
-    roller->speed(1300);
+    roller->speed(roller_speed);
     t = millis();
   }
 }
 
-void Roller::speed(int speed){
+void Roller::speed(int speed_){
     if(roller_armed)
-        roller->speed(speed);
+        roller_speed = speed_;
 }

src/sensors/sensors.cpp

@@ -7,10 +7,18 @@ void initSensors(){
     pinMode(SWITCH_ID, INPUT);
     
     drive = new DriveController(new Motor(12, 11, 4, 55), new  Motor(25, 24, 5, 135), new Motor(27, 26, 2, 225), new Motor(29, 28, 3, 305));
+    // tone(BUZZER, 270, 250);
+    // delay(350);
     compass = new DataSourceBNO055();
+    // tone(BUZZER, 275, 250);
+    // delay(350);
     ball = new DataSourceBall(&BALL_32U4, 57600);
+    // tone(BUZZER, 280, 250);
+    // delay(350);
     camera = new DataSourceCameraConic(&Serial3, 19200);
-    bt = new DataSourceBT(&Serial1, 115200);
+    // tone(BUZZER, 285, 250);
+    // delay(350);
+    // bt = new DataSourceBT(&Serial1, 115200);
     roller = new Roller(30, 31, 1000, 2000, 500);
  }
 

src/system/lines/linesys_camera.cpp

@@ -78,7 +78,7 @@ void LineSysCamera::outOfBounds(){
   }
 
   if (millis() - exitTimer < EXIT_TIME){
-    CURRENT_DATA_WRITE.game->ps->goCenter();
+    CURRENT_DATA_READ.game->ps->goCenter();
     tookLine = true;
     tone(BUZZER, 220.00, 250);
   }else{

src/system/positions/positionsys_camera.cpp

@@ -30,7 +30,7 @@ PositionSysCamera::PositionSysCamera() {
     Y->SetDerivativeLag(1);
     Y->SetSampleTime(2);
 
-    filterDir = new ComplementaryFilter(0.65);
+    filterDir = new ComplementaryFilter(0.35);
     filterSpeed = new ComplementaryFilter(0.65);
 }
 
@@ -49,8 +49,24 @@ void PositionSysCamera::update(){
         posx = CURRENT_DATA_WRITE.cam_xy;
         posy = CURRENT_DATA_WRITE.cam_yy + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yy);
     }else{
-        //TODO: no goal seen ?
+        // Go back in time until we found a valid status, when we saw at least one goal
+        int i = 1;
+        do{
+            valid_data = getDataAtIndex_backwardsFromCurrent(i);
+            i++;
+        }while(!valid_data.ySeen && !valid_data.bSeen);
+
+        if(valid_data.ySeen || valid_data.bSeen){
+            posx = valid_data.posx;
+            posy = valid_data.posy;
+
+            // 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;
+
+        }
     }
+    
     //IMPORTANT STEP: or the direction of the plane will be flipped
     posx *= -1;
     posy *= -1;
@@ -123,9 +139,9 @@ void PositionSysCamera::CameraPID(){
         int dist = sqrt( ( (CURRENT_DATA_WRITE.posx-Setpointx)*(CURRENT_DATA_WRITE.posx-Setpointx) ) + (CURRENT_DATA_WRITE.posy-Setpointy)*(CURRENT_DATA_WRITE.posy-Setpointy) );
         // int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
         int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0,  MAX_VEL);
-        speed = filterSpeed->calculate(speed);
-        speed = speed > 40 ? speed : 0;
-        dir = filterDir->calculate(dir);
+        speed = speed > 30 ? speed : 0;
+        dir = filterDir->calculate(dir);;
+        //speed = filterSpeed->calculate(speed);
         drive->prepareDrive(dir, speed, 0);
 
 

src/test_menu.cpp

@@ -60,6 +60,7 @@ void TestMenu::testMenu()
             drive->m2->test();
             drive->m3->test();
             drive->m4->test();
+            flagtest = true;
             break;
         case '4':
             DEBUG.println("Pid recenter test");

utility/OpenMV/conic_eff_h7.py

@@ -46,7 +46,7 @@ blue_led.on()
 
 
 thresholds = [  (55, 92, -3, 24, 60, 90),    # thresholds yellow goal
-                (33, 49, -9, 12, -52, -12)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (45, 61, -18, 12, -55, -21)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 
 roi = (30, 0, 290, 240)
@@ -94,7 +94,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=60, area_threshold=90, merge = True):
+    for blob in img.find_blobs(thresholds, pixels_threshold=40, area_threshold=50, merge = True):
         img.draw_rectangle(blob.rect())
         #img.draw_cross(blob.cx(), blob.cy())