mix fixes git decided to cancel

include/motors_movement/drivecontroller.h

@@ -20,12 +20,14 @@
 
 //Max possible vel 310
 
-#define MAX_VEL 180
+#define MAX_VEL 125
 #define MAX_VEL_EIGTH ((int)MAX_VEL*0.8)
 #define MAX_VEL_HALF ((int)MAX_VEL*0.5)
 #define MAX_VEL_3QUARTERS ((int)MAX_VEL*0.75)
 #define MAX_VEL_QUARTER ((int)MAX_VEL*0.25)
 
+//#define DRIVE_VECTOR_SUM 
+
 class DriveController{
 
     public:

include/systems/lines/linesys_camera.h

@@ -16,8 +16,8 @@
 #define S4I A0
 #define S4O A1
 
-#define LINE_THRESH_CAM 325
+#define LINE_THRESH_CAM 350
-#define EXIT_TIME 300
+#define EXIT_TIME 400
 
 class LineSysCamera : public LineSystem{
 

src/main.cpp

@@ -17,6 +17,7 @@ bool striker_condition = false;
 bool keeper_condition = false;
 
 void setup() {
+  pinMode(BUZZER, OUTPUT);
   tone(BUZZER, 220, 250);
   delay(1500);
   DEBUG.begin(115200);
@@ -53,9 +54,9 @@ void loop() {
   striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike;
   keeper_condition = role == LOW;
 
-  // striker->play(striker_condition);
+  striker->play(striker_condition);
-  // keeper->play(keeper_condition);
+  keeper->play(keeper_condition);
-  precision_shooter->play(1);
+  // precision_shooter->play(1);
 
   testmenu->testMenu();
 

src/motors_movement/drivecontroller.cpp

@@ -68,11 +68,13 @@ 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
+    #ifdef DRIVE_VECTOR_SUM
     vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx;
     vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
-
+    #else
-    // vx = ((speed * cosins[dir]));
+    vx = ((speed * cosins[dir]));
-    // vy = ((-speed * sins[dir]));
+    vy = ((-speed * sins[dir]));
+    #endif
 
     // 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_ball.cpp

@@ -41,13 +41,13 @@ void DataSourceBall :: test(){
 }
 
 bool DataSourceBall::isInFront(){
-  return (CURRENT_DATA_READ.ballAngle > MOUTH_MIN_ANGLE || CURRENT_DATA_READ.ballAngle < MOUTH_MAX_ANGLE );
+  return CURRENT_DATA_READ.ballSeen && (CURRENT_DATA_READ.ballAngle > MOUTH_MIN_ANGLE || CURRENT_DATA_READ.ballAngle < MOUTH_MAX_ANGLE );
 }
 
 bool DataSourceBall::isInMouth(){
-  return isInFront() && CURRENT_DATA_READ.ballDistance<=MOUTH_DISTANCE;
+  return CURRENT_DATA_READ.ballSeen && (isInFront() && CURRENT_DATA_READ.ballDistance<=MOUTH_DISTANCE);
 }
 
 bool DataSourceBall::isInMouthMaxDistance(){
-  return isInFront() && CURRENT_DATA_READ.ballDistance <= MOUTH_MAX_DISTANCE;
+  return CURRENT_DATA_READ.ballSeen && (isInFront() && CURRENT_DATA_READ.ballDistance <= MOUTH_MAX_DISTANCE);
 }

src/system/positions/positionsys_camera.cpp

@@ -42,26 +42,18 @@ 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;
     }
 
+    //IMPORTANT STEP: or the direction of the plane will be flipped
+    posx *= -1;
+    posy *= -1;
+    
     if(abs(CURRENT_DATA_READ.posx-CURRENT_DATA_WRITE.posx)>MAX_X  || abs(CURRENT_DATA_READ.posy-CURRENT_DATA_WRITE.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;
@@ -154,18 +146,16 @@ void PositionSysCamera::CameraPID(){
         speed = speed > 30 ? speed : 0;
         dir = filterDir->calculate(dir);;
         //speed = filterSpeed->calculate(speed);
-        // drive->prepareDrive(dir, speed, 0);
 
-
+        #ifdef DRIVE_VECTOR_SUM
-        //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]));
         vy = ((-speed * sins[dir]));
-
         CURRENT_DATA_WRITE.addvx = vx;
         CURRENT_DATA_WRITE.addvy = vy;
+        #else
+        drive->prepareDrive(dir, speed, 0);
+        #endif
+
     }
 }
 

utility/OpenMV/conic_eff_h7.py.autosave

@@ -1,218 +0,0 @@
-# color tracking with conic mirror - By: EmaMaker - wed 15 jan 2020
-# Based on:
-# color tracking - By: paolix - ven mag 18 2018
-
-# Automatic RGB565 Color Tracking Example
-#
-
-import sensor, image, time, pyb, math
-
-from pyb import UART
-uart = UART(3,19200, timeout_char = 1000)
-
-START_BYTE = chr(105) #'i'
-END_BYTE = chr(115) #'s'
-BYTE_UNKNOWN = chr(116) #'t'
-
-y_found = False
-b_found = False
-
-#From Arduino Documentation at: https://www.arduino.cc/reference/en/language/functions/math/map/
-def val_map(x, in_min, in_max, out_min, out_max):
-    x = int(x)
-    in_min = int(in_min)
-    in_max = int(in_max)
-    out_min = int(out_min)
-    out_max = int(out_max)
-    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)
-
-# Check side
-def isInLeftSide(img, x):
-    return x < img.width() / 2
-
-def isInRightSide(img, x):
-    return x > img.width() / 2
-
-
-# LED Setup ##################################################################
-red_led = pyb.LED(1)
-green_led = pyb.LED(2)
-blue_led = pyb.LED(3)
-
-red_led.off()
-green_led.off()
-blue_led.on()
-##############################################################################
-
-
-thresholds = [  (66, 88, -12, 26, 50, 79),    # thresholds yellow goalz
-                (40, 61, -9, 19, -61, -21)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
-
-
-roi = (80, 0, 240, 200)
-
-# Camera Setup ###############################################################
-'''sensor.reset()xxxx
-sensor.set_pixformat(sensor.RGB565)
-sensor.set_framesize(sensor.QVGA)
-sensor.skip_frames(time = 2000)
-sensor.set_auto_gain(False)         # must be turned off for color tracking
-sensor.set_auto_whitebal(False)     # must be turned off for color tracking
-++++++++++++++++++++++++++++++  
-sensor.set_auto_exposure(False, 10000) vbc
-#sensor.set_backlight(1)
-#sensor.set_brightness(+2 )
-#sensor.set_windowing(roi)
-clock = time.clock()'''
-
-sensor.reset()
-sensor.set_pixformat(sensor.RGB565)
-sensor.set_framesize(sensor.QVGA)
-sensor.set_windowing(roi)
-sensor.set_contrast(0)
-sensor.set_saturation(2)
-sensor.set_brightness(3)
-sensor.set_auto_whitebal(True, (-6.02073, -4.528669, -1.804))
-sensor.set_auto_exposure(False, 6576)
-#sensor.set_auto_gain(False, gain_db=8.78)
-sensor.skip_frames(time = 300)
-
-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()))
-
-    blue_led.off()
-
-    y_found = False
-    b_found = False
-
-    tt_yellow = [(0,999,0,1)]     ## creo una lista di tuple per il giallo, valore x = 999 : non trovata
-    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=80, area_threshold=100, merge = True):
-        img.draw_rectangle(blob.rect())
-        #img.draw_cross(blob.cx(), blob.cy())
-
-        if (blob.code() == 1):
-            tt_yellow = tt_yellow +  [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
-            y_found = True
-        if (blob.code() == 2):
-            tt_blue = tt_blue +  [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
-            b_found = True
-
-    tt_yellow.sort(key=lambda tup: tup[0])  ## ordino le liste
-    tt_blue.sort(key=lambda tup: tup[0])    ## ordino le liste
-
-    ny = len(tt_yellow)
-    nb = len(tt_blue)
-
-
-    #Formulas to compute position of points, considering that the H7 is rotated by a certain angle
-    #x = y-offset
-    #y = offset - x
-
-    #Compute everything related to Yellow First
-
-    y_area, y1_cx, y1_cy, y_code = tt_yellow[ny-1]
-
-
-    y_cx = int(y1_cy - img.height() / 2)
-    y_cy = int(img.width() / 2 - y1_cx)
-
-
-    #Normalize data between 0 and 100
-    if y_found == True:
-        img.draw_cross(y1_cx, y1_cy)
-
-        y_cx = val_map(y_cx, -img.height() / 2, img.height() / 2, 100, 0)
-        y_cy = val_map(y_cy, -img.width() / 2, img.width() / 2, 0, 100)
-        #Prepare for send as a list of characters
-        s_ycx = chr(y_cx)
-        s_ycy = chr(y_cy)
-    else:
-        y_cx = BYTE_UNKNOWN
-        y_cy = BYTE_UNKNOWN
-        #Prepare for send as a list of characters
-        s_ycx = y_cx
-        s_ycy = y_cy
-
-
-
-    #Compute everything relative to Blue
-    '''Given the light situation in our lab and given that blue is usually harder to spot than yellow, we need to check it we got
-    a blue blob that is in the same side of the ground as the yellow one, if so, discard it and check a new one
-    '''
-
-    b_cx = BYTE_UNKNOWN
-    b_cy = BYTE_UNKNOWN
-    #Prepare for send as a list of characters
-    s_bcx = b_cx
-    s_bcy = b_cy
-
-    if b_found == True:
-        for i in range(nb-1, 0,-1):
-            b_area, b1_cx, b1_cy, b_code = tt_blue[i]
-            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)
-
-    '''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'''
-
-    #print(str(y_cx) + " | " + str(y_cy) + "  ---  " + str(b_cx) + " | " + str(b_cy))
-
-    uart.write(START_BYTE)
-    uart.write(s_bcx)
-    uart.write(s_bcy)
-    uart.write(s_ycx)
-    uart.write(s_ycy)
-    uart.write(END_BYTE)