diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h index 11bd567..ad47a5f 100644 --- a/include/motors_movement/drivecontroller.h +++ b/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: diff --git a/include/systems/lines/linesys_camera.h b/include/systems/lines/linesys_camera.h index 7ad9996..4227b58 100644 --- a/include/systems/lines/linesys_camera.h +++ b/include/systems/lines/linesys_camera.h @@ -16,8 +16,8 @@ #define S4I A0 #define S4O A1 -#define LINE_THRESH_CAM 325 -#define EXIT_TIME 300 +#define LINE_THRESH_CAM 350 +#define EXIT_TIME 400 class LineSysCamera : public LineSystem{ diff --git a/src/main.cpp b/src/main.cpp index 999f8a5..380eae6 100755 --- a/src/main.cpp +++ b/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); - // keeper->play(keeper_condition); - precision_shooter->play(1); + striker->play(striker_condition); + keeper->play(keeper_condition); + // precision_shooter->play(1); testmenu->testMenu(); diff --git a/src/motors_movement/drivecontroller.cpp b/src/motors_movement/drivecontroller.cpp index 4352e0c..930d057 100644 --- a/src/motors_movement/drivecontroller.cpp +++ b/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; - - // vx = ((speed * cosins[dir])); - // vy = ((-speed * sins[dir])); + #else + vx = ((speed * cosins[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; diff --git a/src/sensors/data_source_ball.cpp b/src/sensors/data_source_ball.cpp index 806075b..c95b498 100644 --- a/src/sensors/data_source_ball.cpp +++ b/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); } \ No newline at end of file diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index 03e4222..e8cc2fd 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -42,25 +42,17 @@ 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 @@ -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); - - //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 + #ifdef DRIVE_VECTOR_SUM 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 + } } diff --git a/utility/OpenMV/conic_eff_h7.py.autosave b/utility/OpenMV/conic_eff_h7.py.autosave deleted file mode 100644 index 3b88cb2..0000000 --- a/utility/OpenMV/conic_eff_h7.py.autosave +++ /dev/null @@ -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)