From 3631fe18ab93d56db13676a06f6409163902f79b Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Wed, 12 May 2021 16:44:17 +0200 Subject: [PATCH] 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 | 2 +- include/motors_movement/roller.h | 2 +- include/sensors/data_source_ball.h | 2 +- .../sensors/data_source_camera_conicmirror.h | 2 +- include/systems/lines/linesys_camera.h | 4 +-- src/main.cpp | 4 +-- src/motors_movement/drivecontroller.cpp | 8 +++--- .../data_source_camera_conicmirror.cpp | 22 ++++++++++++++++ src/strategy_roles/striker.cpp | 12 ++++++--- src/system/positions/positionsys_camera.cpp | 26 ++++++++++++------- utility/OpenMV/conic_eff_h7.py | 20 +++++++------- utility/OpenMV/conic_eff_h7.py.autosave | 23 ++++++++-------- 12 files changed, 81 insertions(+), 46 deletions(-) diff --git a/include/behaviour_control/status_vector.h b/include/behaviour_control/status_vector.h index 4d71821..8c068c0 100644 --- a/include/behaviour_control/status_vector.h +++ b/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, diff --git a/include/motors_movement/roller.h b/include/motors_movement/roller.h index f6c717f..88ee39b 100644 --- a/include/motors_movement/roller.h +++ b/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: diff --git a/include/sensors/data_source_ball.h b/include/sensors/data_source_ball.h index b028a99..5abc015 100644 --- a/include/sensors/data_source_ball.h +++ b/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{ diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h index 87eda64..33428a3 100644 --- a/include/sensors/data_source_camera_conicmirror.h +++ b/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{ diff --git a/include/systems/lines/linesys_camera.h b/include/systems/lines/linesys_camera.h index f679a82..6b36cf4 100644 --- a/include/systems/lines/linesys_camera.h +++ b/include/systems/lines/linesys_camera.h @@ -11,8 +11,8 @@ #define S1O A6 #define S2I A2 #define S2O A3 -#define S3I A1 -#define S3O A0 +#define S3I A0 +#define S3O A1 #define S4I A9 #define S4O A8 diff --git a/src/main.cpp b/src/main.cpp index 9d7453a..4142705 100755 --- a/src/main.cpp +++ b/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); diff --git a/src/motors_movement/drivecontroller.cpp b/src/motors_movement/drivecontroller.cpp index a6b6705..4352e0c 100644 --- a/src/motors_movement/drivecontroller.cpp +++ b/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; - //vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy; + vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx; + vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy; - vx = ((speed * cosins[dir])); - vy = ((-speed * sins[dir])); + // vx = ((speed * cosins[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; diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp index bed89ca..6f5c911 100644 --- a/src/sensors/data_source_camera_conicmirror.cpp +++ b/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; diff --git a/src/strategy_roles/striker.cpp b/src/strategy_roles/striker.cpp index 080a133..6d47b9c 100644 --- a/src/strategy_roles/striker.cpp +++ b/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; } \ No newline at end of file diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index debb0a9..9541574 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/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])); - // vy = ((-speed * sins[dir])); + vx = ((speed * cosins[dir])); + vy = ((-speed * sins[dir])); - // CURRENT_DATA_WRITE.addvx = vx; - // CURRENT_DATA_WRITE.addvy = vy; + CURRENT_DATA_WRITE.addvx = vx; + CURRENT_DATA_WRITE.addvy = vy; } } diff --git a/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py index 8a71e5a..a35bac2 100644 --- a/utility/OpenMV/conic_eff_h7.py +++ b/utility/OpenMV/conic_eff_h7.py @@ -45,11 +45,11 @@ blue_led.on() ############################################################################## -thresholds = [ (51, 74, -18, 12, 25, 70), # thresholds yellow goal - (27, 40, -18, 13, -29, -10)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +thresholds = [ (61, 100, -11, 18, 60, 127), # thresholds yellow goalz + (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_brightness(2) -sensor.set_auto_whitebal(True) -sensor.set_auto_exposure(True) -sensor.set_auto_gain(True) +sensor.set_saturation(2) +sensor.set_brightness(3) +sensor.set_auto_whitebal(False, (-6.02073, -4.528669, -0.707413)) +sensor.set_auto_exposure(False, 6576) +#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()) diff --git a/utility/OpenMV/conic_eff_h7.py.autosave b/utility/OpenMV/conic_eff_h7.py.autosave index e7de787..9003ace 100644 --- a/utility/OpenMV/conic_eff_h7.py.autosave +++ b/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 - (27, 40, -18, 13, -29, -10)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +thresholds = [ (61, 100, -11, 18, 60, 127), # thresholds yellow goalz + (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_brightness(2) -sensor.set_auto_whitebal(True) -sensor.set_auto_exposure(True) -sensor.set_auto_gain(True) +sensor.set_saturation(2) +sensor.set_brightness(3) +sensor.set_auto_whitebal(False, (-6.02073, -4.528669, -0.707413)) +sensor.set_auto_exposure(False, 6576) +#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())