robocup preparation: correct pos-sys-camera bugs

better use of roller
nice startup/debugging sounds
disable auto gain in H7

include/behaviour_control/status_vector.h

@@ -38,7 +38,7 @@ typedef struct data{
     int IMUAngle, ballAngle, ballAngleFix, ballDistance, 
         yAngle, bAngle, yAngleFix, bAngleFix, 
         yDist, bDist, 
-        angleAtk, angleAtkFix, angleDef, angleDefFix,
+        angleAtk, angleAtkFix, angleDef, angleDefFix, yAtk, yAtkFix, xAtk, xAtkFix, yDef, yDefFix, xDef, xDefFix,
         cam_xb, cam_yb, cam_xy, cam_yy,
         speed, tilt, dir, axisBlock[4],
         USfr, USsx, USdx, USrr, 

include/motors_movement/roller.h

@@ -2,7 +2,7 @@
 
 #include "ESC.h"
 
-#define ROLLER_DEFAULT_SPEED 1100
+#define ROLLER_DEFAULT_SPEED 1200
 
 class Roller{
     public:

include/sensors/data_source_ball.h

@@ -4,7 +4,7 @@
 
 #define MOUTH_MIN_ANGLE 340
 #define MOUTH_MAX_ANGLE 20
-#define MOUTH_DISTANCE 110
+#define MOUTH_DISTANCE 115
 #define MOUTH_MAX_DISTANCE 140
 
 class DataSourceBall : public DataSource{

include/sensors/data_source_camera_conicmirror.h

@@ -22,7 +22,7 @@
 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.
 These values need to be subtracted from the coords used in setMoveSetpoints*/
 #define CAMERA_TRANSLATION_X 1
-#define CAMERA_TRANSLATION_Y 4
+#define CAMERA_TRANSLATION_Y -4
 
 class DataSourceCameraConic : public DataSource{
 

include/systems/lines/linesys_camera.h

@@ -11,8 +11,8 @@
 #define S1O A6
 #define S2I A2
 #define S2O A3
-#define S3I A1
+#define S3I A0
-#define S3O A0
+#define S3O A1
 #define S4I A9
 #define S4O A8
 

src/main.cpp

@@ -29,7 +29,7 @@ void setup() {
   testmenu = new TestMenu();
   tone(BUZZER, 240, 250);
   initStatusVector();
-  delay(100);
+  delay(250);
 
   tone(BUZZER, 260, 250);
   initSensors();
@@ -37,7 +37,7 @@ void setup() {
   
   tone(BUZZER, 320, 250);
   initGames();
-  delay(200);
+  delay(250);
 
   //Startup sound
   tone(BUZZER, 350.00, 250);

src/motors_movement/drivecontroller.cpp

@@ -68,11 +68,11 @@ void DriveController::drive(int dir, int speed, int tilt){
     //TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE?
     // Disable vector sum because calculations are a bitty crappy imho. Will have to test if it's what makes the robot act strange with lines
     // Re enabling the below lines requires to comment out drive->prepareDrive and uncommenting the lines relative to vector sum inside positionsys_camera and comment out the other lines here
-    //vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx;
+    vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx;
-    //vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
+    vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
 
-    vx = ((speed * cosins[dir]));
+    // vx = ((speed * cosins[dir]));
-    vy = ((-speed * sins[dir]));
+    // vy = ((-speed * sins[dir]));
 
     // if((((vy < 0 && vxn == 1) || (vy > 0 && vxp == 1) || (vx < 0 && vyp == 1) || (vx > 0 && vyn == 1)) && canUnlock) || (millis() > this->unlockTime+UNLOCK_THRESH)) {
     //     vxn = 0;

src/sensors/data_source_camera_conicmirror.cpp

@@ -160,16 +160,38 @@ void DataSourceCameraConic ::computeCoordsAngles() {
     CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.yAngle;
     CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix;
     CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.ySeen;
+    
+    CURRENT_DATA_WRITE.yAtk = CURRENT_DATA_WRITE.cam_yy;
+    CURRENT_DATA_WRITE.yAtkFix = CURRENT_DATA_WRITE.cam_yy_fixed;
+    CURRENT_DATA_WRITE.xAtk = CURRENT_DATA_WRITE.cam_xy;
+    CURRENT_DATA_WRITE.xAtkFix = CURRENT_DATA_WRITE.cam_xy_fixed;
+
     CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.bAngle;
     CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.bAngleFix;
     CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.bSeen;
+
+    CURRENT_DATA_WRITE.yDef = CURRENT_DATA_WRITE.cam_yb;
+    CURRENT_DATA_WRITE.yDefFix = CURRENT_DATA_WRITE.cam_yb_fixed;
+    CURRENT_DATA_WRITE.xDef = CURRENT_DATA_WRITE.cam_xb;
+    CURRENT_DATA_WRITE.xDefFix = CURRENT_DATA_WRITE.cam_xb_fixed;
   } else {
     CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.bAngle;
     CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.bAngleFix;
     CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.bSeen;
+
+    CURRENT_DATA_WRITE.yAtk = CURRENT_DATA_WRITE.cam_yb;
+    CURRENT_DATA_WRITE.yAtkFix = CURRENT_DATA_WRITE.cam_yb_fixed;
+    CURRENT_DATA_WRITE.xAtk = CURRENT_DATA_WRITE.cam_xb;
+    CURRENT_DATA_WRITE.xAtkFix = CURRENT_DATA_WRITE.cam_xb_fixed;
+
     CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.yAngle;
     CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.yAngleFix;
     CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.ySeen;
+
+    CURRENT_DATA_WRITE.yDef = CURRENT_DATA_WRITE.cam_yy;
+    CURRENT_DATA_WRITE.yDefFix = CURRENT_DATA_WRITE.cam_yy_fixed;
+    CURRENT_DATA_WRITE.xDef = CURRENT_DATA_WRITE.cam_xy;
+    CURRENT_DATA_WRITE.xDefFix = CURRENT_DATA_WRITE.cam_xy_fixed;
   }
 
   byte to_32u4 = 0;

src/strategy_roles/striker.cpp

@@ -47,8 +47,15 @@ void Striker::striker(){
   else dir = ball_deg - plusang;                                //se sto nel negativo sottraggo
 
   dir = (dir + 360) % 360;
-
   drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
+
+  if(ball->isInFront()) roller->speed(ROLLER_DEFAULT_SPEED);
+  else roller->speed(roller->MIN);
+
+  // if(ball->isInMouth() && ( (CURRENT_DATA_READ.posx <= -30 && CURRENT_DATA_READ.posy >= 35) || (CURRENT_DATA_READ.posx >= 30 && CURRENT_DATA_READ.posy >= 35))){
+  //   ps->goCenter();
+  // }
+  
 }
 
 int Striker::tilt() {
@@ -62,8 +69,5 @@ int Striker::tilt() {
     atk_tilt = roller->roller_armed ? CURRENT_DATA_READ.angleAtkFix : constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45);
   }
 
-  if(ball->isInFront()) roller->speed(ROLLER_DEFAULT_SPEED);
-  else roller->speed(roller->MIN);
-
   return atk_tilt;
 }

src/system/positions/positionsys_camera.cpp

@@ -42,12 +42,24 @@ void PositionSysCamera::update(){
     if(CURRENT_DATA_WRITE.bSeen == true && CURRENT_DATA_WRITE.ySeen == true){
         posx = (CURRENT_DATA_WRITE.cam_xy + CURRENT_DATA_WRITE.cam_xb) / 2;
         posy = CURRENT_DATA_WRITE.cam_yb + CURRENT_DATA_WRITE.cam_yy;
+
+        //IMPORTANT STEP: or the direction of the plane will be flipped
+        posx *= -1;
+        posy *= -1;
     }else if (CURRENT_DATA_WRITE.bSeen == true && CURRENT_DATA_WRITE.ySeen == false){
         posx = CURRENT_DATA_WRITE.cam_xb;
         posy = CURRENT_DATA_WRITE.cam_yb + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yb);
+
+        //IMPORTANT STEP: or the direction of the plane will be flipped
+        posx *= -1;
+        posy *= -1;
     }else if (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == true){
         posx = CURRENT_DATA_WRITE.cam_xy;
         posy = CURRENT_DATA_WRITE.cam_yy + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yy);
+
+        //IMPORTANT STEP: or the direction of the plane will be flipped
+        posx *= -1;
+        posy *= -1;
     }else{
         // Go back in time until we found a valid status, when we saw at least one goal
         int i = 1;
@@ -66,10 +78,6 @@ void PositionSysCamera::update(){
 
         }
     }
-    
-    //IMPORTANT STEP: or the direction of the plane will be flipped
-    posx *= -1;
-    posy *= -1;
 
     CURRENT_DATA_WRITE.posx = posx;
     CURRENT_DATA_WRITE.posy = posy;
@@ -144,18 +152,18 @@ void PositionSysCamera::CameraPID(){
         speed = speed > 30 ? speed : 0;
         dir = filterDir->calculate(dir);;
         //speed = filterSpeed->calculate(speed);
-        drive->prepareDrive(dir, speed, 0);
+        // drive->prepareDrive(dir, speed, 0);
 
 
         //Disable below lines for now because they probably result in unexpected behaviour on lines. Re enabling them requires to comment out the drive->prepareDrive above
         //and check the notes in drivecontroller for the other stuff to comment and uncomment
 
         //TODO: add complementary filter on this speed if we keep using it
-        // vx = ((speed * cosins[dir]));
+        vx = ((speed * cosins[dir]));
-        // vy = ((-speed * sins[dir]));
+        vy = ((-speed * sins[dir]));
 
-        // CURRENT_DATA_WRITE.addvx = vx;
+        CURRENT_DATA_WRITE.addvx = vx;
-        // CURRENT_DATA_WRITE.addvy = vy;
+        CURRENT_DATA_WRITE.addvy = vy;
     }
 }
 

utility/OpenMV/conic_eff_h7.py

@@ -45,11 +45,11 @@ blue_led.on()
 ##############################################################################
 
 
-thresholds = [  (51, 74, -18, 12, 25, 70),    # thresholds yellow goal
+thresholds = [  (61, 100, -11, 18, 60, 127),    # thresholds yellow goalz
-                (27, 40, -18, 13, -29, -10)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (34, 52, -14, 21, -67, -20)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 
-roi = (80, 0, 240, 220)
+roi = (80, 0, 220, 200)
 
 # Camera Setup ###############################################################
 '''sensor.reset()xxxx
@@ -69,11 +69,11 @@ sensor.set_pixformat(sensor.RGB565)
 sensor.set_framesize(sensor.QVGA)
 sensor.set_windowing(roi)
 sensor.set_contrast(0)
-sensor.set_saturation(1)
+sensor.set_saturation(2)
-sensor.set_brightness(2)
+sensor.set_brightness(3)
-sensor.set_auto_whitebal(True)
+sensor.set_auto_whitebal(False, (-6.02073, -4.528669, -0.707413))
-sensor.set_auto_exposure(True)
+sensor.set_auto_exposure(False, 6576)
-sensor.set_auto_gain(True)
+#sensor.set_auto_gain(False, gain_db=8.78)
 sensor.skip_frames(time = 300)
 
 clock = time.clock()
@@ -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()
 
@@ -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=40, area_threshold=50, merge = True):
+    for blob in img.find_blobs(thresholds, pixels_threshold=60, area_threshold=80, merge = True):
         img.draw_rectangle(blob.rect())
         #img.draw_cross(blob.cx(), blob.cy())
 

utility/OpenMV/conic_eff_h7.py.autosave

@@ -36,7 +36,8 @@ def isInRightSide(img, x):
 
 # LED Setup ##################################################################
 red_led = pyb.LED(1)
-green_led = pyb.LED(2)
+green_led
+= pyb.LED(2)
 blue_led = pyb.LED(3)
 
 red_led.off()
@@ -45,11 +46,11 @@ blue_led.on()
 ##############################################################################
 
 
-thresholds = [  (51, 74, -18, 12, 25, 70),    # thresholds yellow goalz
+thresholds = [  (61, 100, -11, 18, 60, 127),    # thresholds yellow goalz
-                (27, 40, -18, 13, -29, -10)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (34, 52, -14, 21, -67, -20)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 
-roi = (80, 0, 240, 220)
+roi = (80, 0, 220, 200)
 
 # Camera Setup ###############################################################
 '''sensor.reset()xxxx
@@ -69,11 +70,11 @@ sensor.set_pixformat(sensor.RGB565)
 sensor.set_framesize(sensor.QVGA)
 sensor.set_windowing(roi)
 sensor.set_contrast(0)
-sensor.set_saturation(1)
+sensor.set_saturation(2)
-sensor.set_brightness(2)
+sensor.set_brightness(3)
-sensor.set_auto_whitebal(True)
+sensor.set_auto_whitebal(False, (-6.02073, -4.528669, -0.707413))
-sensor.set_auto_exposure(True)
+sensor.set_auto_exposure(False, 6576)
-sensor.set_auto_gain(True)
+#sensor.set_auto_gain(False, gain_db=8.78)
 sensor.skip_frames(time = 300)
 
 clock = time.clock()
@@ -83,7 +84,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()
 
@@ -94,7 +95,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=40, area_threshold=50, merge = True):
+    for blob in img.find_blobs(thresholds, pixels_threshold=60, area_threshold=80, merge = True):
         img.draw_rectangle(blob.rect())
         #img.draw_cross(blob.cx(), blob.cy())