From 16a781df34e3b13b817c918437dc19be376bbfae Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Thu, 13 May 2021 08:38:20 +0200 Subject: [PATCH] romecup: precision shooter --- include/motors_movement/drivecontroller.h | 2 +- .../sensors/data_source_camera_conicmirror.h | 4 +- include/strategy_roles/games.h | 4 +- include/strategy_roles/precision_shooter.h | 28 +++++ include/strategy_roles/striker.h | 2 +- include/strategy_roles/striker_test.h | 26 ----- include/systems/lines/linesys_camera.h | 16 +-- src/main.cpp | 6 +- src/strategy_roles/games.cpp | 1 + src/strategy_roles/precision_shooter.cpp | 88 +++++++++++++++ src/strategy_roles/striker.cpp | 2 +- src/strategy_roles/striker_test.cpp | 100 ------------------ src/system/positions/positionsys_camera.cpp | 4 +- src/system/positions/positionsys_empty.cpp | 4 +- utility/OpenMV/conic_eff_h7.py | 10 +- utility/OpenMV/conic_eff_h7.py.autosave | 14 +-- 16 files changed, 153 insertions(+), 158 deletions(-) create mode 100644 include/strategy_roles/precision_shooter.h delete mode 100644 include/strategy_roles/striker_test.h create mode 100644 src/strategy_roles/precision_shooter.cpp delete mode 100644 src/strategy_roles/striker_test.cpp diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h index 784213b..11bd567 100644 --- a/include/motors_movement/drivecontroller.h +++ b/include/motors_movement/drivecontroller.h @@ -20,7 +20,7 @@ //Max possible vel 310 -#define MAX_VEL 150 +#define MAX_VEL 180 #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) diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h index 33428a3..7ad04d8 100644 --- a/include/sensors/data_source_camera_conicmirror.h +++ b/include/sensors/data_source_camera_conicmirror.h @@ -21,8 +21,8 @@ /*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. 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_X 0 +#define CAMERA_TRANSLATION_Y 0 class DataSourceCameraConic : public DataSource{ diff --git a/include/strategy_roles/games.h b/include/strategy_roles/games.h index f3ba188..1298d02 100644 --- a/include/strategy_roles/games.h +++ b/include/strategy_roles/games.h @@ -9,11 +9,11 @@ #include #include "strategy_roles/game.h" #include "strategy_roles/striker.h" -#include "strategy_roles/striker_test.h" +#include "strategy_roles/precision_shooter.h" #include "strategy_roles/keeper.h" void initGames(); g_extr Game* striker; -g_extr Game* striker_test; +g_extr Game* precision_shooter; g_extr Game* keeper; \ No newline at end of file diff --git a/include/strategy_roles/precision_shooter.h b/include/strategy_roles/precision_shooter.h new file mode 100644 index 0000000..eadf914 --- /dev/null +++ b/include/strategy_roles/precision_shooter.h @@ -0,0 +1,28 @@ +#pragma once + +#include "sensors/data_source_camera_vshapedmirror.h" +#include "sensors/sensors.h" +#include "strategy_roles/game.h" + +#define PS_ATTACK_DISTANCE 110 +#define PS_TILT_STOP_DISTANCE 140 +#define PS_PLUSANG 55 +#define PS_PLUSANG_VISIONCONE 7 + +class PrecisionShooter : public Game{ + + public: + PrecisionShooter(); + PrecisionShooter(LineSystem* ls, PositionSystem* ps); + + private: + void realPlay() override; + void init() override; + void striker(); + int tilt(); + + int atk_speed, atk_direction, atk_tilt; + float cstorc; + + bool gotta_tilt; +}; diff --git a/include/strategy_roles/striker.h b/include/strategy_roles/striker.h index 2f76213..0853fc0 100644 --- a/include/strategy_roles/striker.h +++ b/include/strategy_roles/striker.h @@ -6,7 +6,7 @@ #define STRIKER_ATTACK_DISTANCE 110 #define STRIKER_TILT_STOP_DISTANCE 140 -#define STRIKER_PLUSANG 55 +#define STRIKER_PLUSANG 57 #define STRIKER_PLUSANG_VISIONCONE 7 class Striker : public Game{ diff --git a/include/strategy_roles/striker_test.h b/include/strategy_roles/striker_test.h deleted file mode 100644 index 78eff8c..0000000 --- a/include/strategy_roles/striker_test.h +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once - -#include "sensors/data_source_camera_vshapedmirror.h" -#include "sensors/sensors.h" -#include "strategy_roles/game.h" - -#define TARGET_DIST 120 -#define ATTACK_DIST 150 -#define ANGLE_SHIFT_STEP 5 -#define STRIKER_SPD 80 - -class StrikerTest : public Game{ - - public: - StrikerTest(); - StrikerTest(LineSystem* ls, PositionSystem* ps); - - private: - void realPlay() override; - void init() override; - void striker(); - void ballBack(); - - int atk_speed, atk_direction; - -}; diff --git a/include/systems/lines/linesys_camera.h b/include/systems/lines/linesys_camera.h index 6b36cf4..7ad9996 100644 --- a/include/systems/lines/linesys_camera.h +++ b/include/systems/lines/linesys_camera.h @@ -7,14 +7,14 @@ #include "vars.h" -#define S1I A7 -#define S1O A6 -#define S2I A2 -#define S2O A3 -#define S3I A0 -#define S3O A1 -#define S4I A9 -#define S4O A8 +#define S1O A7 +#define S1I A6 +#define S2O A2 +#define S2I A3 +#define S3I A9 +#define S3O A8 +#define S4I A0 +#define S4O A1 #define LINE_THRESH_CAM 325 #define EXIT_TIME 300 diff --git a/src/main.cpp b/src/main.cpp index 4142705..999f8a5 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -53,8 +53,10 @@ void loop() { striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike; keeper_condition = role == LOW; - striker->play(striker_condition); - keeper->play(keeper_condition); + // striker->play(striker_condition); + // keeper->play(keeper_condition); + precision_shooter->play(1); + testmenu->testMenu(); // Last thing to do: movement and update status vector diff --git a/src/strategy_roles/games.cpp b/src/strategy_roles/games.cpp index 77e9cb4..283d5e7 100644 --- a/src/strategy_roles/games.cpp +++ b/src/strategy_roles/games.cpp @@ -13,5 +13,6 @@ void initGames(){ // striker_test = new StrikerTest(new LineSysCamera(lIn, lOut), new PositionSysCamera()); striker = new Striker(new LineSysCamera(lIn, lOut), new PositionSysCamera()); + precision_shooter = new PrecisionShooter(new LineSystemEmpty(), new PositionSysCamera()); keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera()); } \ No newline at end of file diff --git a/src/strategy_roles/precision_shooter.cpp b/src/strategy_roles/precision_shooter.cpp new file mode 100644 index 0000000..d4928a5 --- /dev/null +++ b/src/strategy_roles/precision_shooter.cpp @@ -0,0 +1,88 @@ +#include "behaviour_control/status_vector.h" +#include "systems/position/positionsys_camera.h" +#include "sensors/sensors.h" +#include "sensors/data_source_ball.h" +#include "strategy_roles/precision_shooter.h" +#include "vars.h" +#include "math.h" + + +PrecisionShooter::PrecisionShooter() : Game() +{ + init(); +} + +PrecisionShooter::PrecisionShooter(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) +{ + init(); +} + +void PrecisionShooter::init() +{ + atk_speed = 0; + atk_direction = 0; + atk_tilt = 0; + cstorc = 0; + + gotta_tilt = false; +} + +void PrecisionShooter::realPlay() +{ + if (CURRENT_DATA_READ.ballSeen) + this->striker(); + else + ps->goCenter(); +} + +unsigned long t3 = 0; + +void PrecisionShooter::striker(){ + + #ifdef MAX_VEL + #undef MAX_VEL + #define MAX_VEL 100 + #endif + + //seguo palla + int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = PS_PLUSANG; + + if(ball_deg >= 346 || ball_deg <= 16) plusang = PS_PLUSANG_VISIONCONE; //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus + if(ball_deg > 180) ball_degrees2 = ball_deg - 360; //ragiono in +180 -180 + else ball_degrees2 = ball_deg; + + if(ball_degrees2 > 0) dir = ball_deg + plusang; //se sto nel quadrante positivo aggiungo + 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->isInFront() && CURRENT_DATA_READ.ballDistance <= 85 && CURRENT_DATA_READ.posy >= 32 && (CURRENT_DATA_READ.posx >= 15 || CURRENT_DATA_READ.posx <= -15) ) { + t3 = millis(); + } + + if(millis() - t3 < 1000){ + roller->speed(1800); + ps->goCenter(); + } + + + +} + +int PrecisionShooter::tilt() { + if (ball->isInMouth() /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true; + else gotta_tilt = false; + + if(!gotta_tilt || !CURRENT_DATA_READ.atkSeen) { + atk_tilt *= 0.8; + if(atk_tilt <= 10) atk_tilt = 0; + }else{ + atk_tilt = roller->roller_armed ? CURRENT_DATA_READ.angleAtkFix : constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45); + } + + return atk_tilt; +} \ No newline at end of file diff --git a/src/strategy_roles/striker.cpp b/src/strategy_roles/striker.cpp index 6d47b9c..36e861f 100644 --- a/src/strategy_roles/striker.cpp +++ b/src/strategy_roles/striker.cpp @@ -39,7 +39,7 @@ void Striker::striker(){ //seguo palla int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG; - if(ball_deg >= 346 || ball_deg <= 16) plusang = STRIKER_PLUSANG_VISIONCONE; //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus + if(ball_deg >= 344 || ball_deg <= 16) plusang = STRIKER_PLUSANG_VISIONCONE; //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus if(ball_deg > 180) ball_degrees2 = ball_deg - 360; //ragiono in +180 -180 else ball_degrees2 = ball_deg; diff --git a/src/strategy_roles/striker_test.cpp b/src/strategy_roles/striker_test.cpp deleted file mode 100644 index b72b4b5..0000000 --- a/src/strategy_roles/striker_test.cpp +++ /dev/null @@ -1,100 +0,0 @@ -#include "behaviour_control/status_vector.h" -#include "sensors/sensors.h" -#include "strategy_roles/striker_test.h" -#include "vars.h" - -#include "math.h" - -StrikerTest::StrikerTest() : Game() { - init(); -} - -StrikerTest::StrikerTest(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) { - init(); -} - -void StrikerTest::init(){ - atk_speed = 0; - atk_direction = 0; -} - -void StrikerTest::realPlay(){ - if(CURRENT_DATA_READ.ballSeen) striker(); - else ps->goCenter(); -} - -void StrikerTest::striker() -{ - /*First implementation of "orbital striker", a new way to approach the problem with less lines. - It works with robot's positions, calculating the final one using the angle shift over - and over again until we have an acceptable result. That result is used to drive->prepareDrive with the speed.*/ - // if (CURRENT_DATA_READ.ballDistance > ) - // drive->prepareDrive(CURRENT_DATA_READ.ballAngle, STRIKER_SPD); - // else - // { - // if (CURRENT_DATA_READ.ballAngle > 340 && CURRENT_DATA_READ.ballAngle < 20) - // { - // drive->prepareDrive(0, 100, 0); - // } - // else - // { - // int ballAngle = (90 - CURRENT_DATA_READ.ballAngle + 360) % 360; - // robotAngle = (ballAngle - 180 + 360) % 360; - - // DEBUG.print("Ball is at angle (goniometric circle): "); - // DEBUG.println(ballAngle); - // DEBUG.print("Robot is at angle "); - // DEBUG.println(robotAngle); - - // float robotAngle_rad = robotAngle*3.14 / 180; - // robotX = CURRENT_DATA_READ.ballDistance * cos(robotAngle_rad); - // robotY = CURRENT_DATA_READ.ballDistance * sin(robotAngle_rad); - - // DEBUG.print("Coords of the robot relative to the ball: ("); - // DEBUG.print(robotX); - // DEBUG.print(", "); - // DEBUG.print(robotY); - // DEBUG.println(")"); - - // // angleDiff = min(((-robotAngle + 360) % 360), ((robotAngle + 360) % 360)); - // // angleShift = min(angleDiff, ANGLE_SHIFT_STEP); - - // angleShift = ANGLE_SHIFT_STEP; - - // if (robotAngle >= 0 && robotAngle <= 180) - // newAngle = robotAngle - angleShift; - // else - // newAngle = robotAngle + angleShift; - - // DEBUG.print("New ball-robot angle: "); - // DEBUG.println(newAngle); - - // float newAngle_rad = (newAngle)*3.14 / 180; - - // robotX_new = TARGET_DIST * cos(newAngle_rad); - // robotY_new = TARGET_DIST * sin(newAngle_rad); - - // DEBUG.print("New coords of the robot relative to the ball: ("); - // DEBUG.print(robotX_new); - // DEBUG.print(", "); - // DEBUG.print(robotY_new); - // DEBUG.println(")"); - - // moveAngle = (atan2((robotX_new - robotX), (robotY_new - robotY))) * 180 / 3.14; - // moveAngle = (moveAngle + 360) % 360; - - // DEBUG.print("Direction to move in: "); - // DEBUG.println(moveAngle); - - // drive->prepareDrive(moveAngle, STRIKER_SPD); - // } - // } - - // delay(1000); - // DEBUG.println("=========="); -} - - - -void StrikerTest::ballBack() { -} \ No newline at end of file diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index 9541574..03e4222 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -60,7 +60,9 @@ void PositionSysCamera::update(){ //IMPORTANT STEP: or the direction of the plane will be flipped posx *= -1; posy *= -1; - }else{ + } + + 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; do{ diff --git a/src/system/positions/positionsys_empty.cpp b/src/system/positions/positionsys_empty.cpp index a908344..d99a9c0 100644 --- a/src/system/positions/positionsys_empty.cpp +++ b/src/system/positions/positionsys_empty.cpp @@ -15,9 +15,9 @@ void PositionSystemEmpty::test(){ } void PositionSystemEmpty::goCenter(){ - drive->prepareDrive(0,0,0); + // drive->prepareDrive(0,0,0); } void PositionSystemEmpty::centerGoal(){ - drive->prepareDrive(0,0,0); + // drive->prepareDrive(0,0,0); } diff --git a/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py index a35bac2..cb0d177 100644 --- a/utility/OpenMV/conic_eff_h7.py +++ b/utility/OpenMV/conic_eff_h7.py @@ -45,11 +45,11 @@ blue_led.on() ############################################################################## -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) +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, 220, 200) +roi = (80, 0, 240, 200) # Camera Setup ############################################################### '''sensor.reset()xxxx @@ -71,7 +71,7 @@ sensor.set_windowing(roi) sensor.set_contrast(0) sensor.set_saturation(2) sensor.set_brightness(3) -sensor.set_auto_whitebal(False, (-6.02073, -4.528669, -0.707413)) +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) @@ -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=80, merge = True): + 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()) diff --git a/utility/OpenMV/conic_eff_h7.py.autosave b/utility/OpenMV/conic_eff_h7.py.autosave index 9003ace..3b88cb2 100644 --- a/utility/OpenMV/conic_eff_h7.py.autosave +++ b/utility/OpenMV/conic_eff_h7.py.autosave @@ -36,8 +36,7 @@ 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() @@ -46,11 +45,11 @@ blue_led.on() ############################################################################## -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) +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, 220, 200) +roi = (80, 0, 240, 200) # Camera Setup ############################################################### '''sensor.reset()xxxx @@ -59,6 +58,7 @@ 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 ) @@ -72,7 +72,7 @@ sensor.set_windowing(roi) sensor.set_contrast(0) sensor.set_saturation(2) sensor.set_brightness(3) -sensor.set_auto_whitebal(False, (-6.02073, -4.528669, -0.707413)) +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) @@ -95,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=60, area_threshold=80, merge = True): + 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())