From f7a5b072df7aa8e153a931aa91bcff50fd3271fe Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Tue, 22 Jun 2021 10:39:14 +0200 Subject: [PATCH] precision shooter: catches ball and goes towards goal depending on the angle it catches the ball at, sometimes it hides the ball, sometimes it does a random spinning kick when it founds the line --- include/motors_movement/drivecontroller.h | 5 +- .../sensors/data_source_camera_conicmirror.h | 2 +- src/motors_movement/drivecontroller.cpp | 4 ++ src/strategy_roles/precision_shooter.cpp | 51 +++++++++++-------- utility/OpenMV/conic_eff_h7.py | 4 +- 5 files changed, 39 insertions(+), 27 deletions(-) diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h index 6bd356a..9c1f253 100644 --- a/include/motors_movement/drivecontroller.h +++ b/include/motors_movement/drivecontroller.h @@ -10,7 +10,7 @@ //BEST NUMBERS YET //USE MOVING AVERAGE AND ANGLE WRAP -#define KP 1.35 +#define KP 1.2 #define KI 0.0 #define KD 0.025 @@ -20,7 +20,7 @@ //Max possible vel 310 -#define MAX_VEL 50 +#define MAX_VEL 60 #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) @@ -37,6 +37,7 @@ class DriveController{ void drive(int dir=0, int speed=0, int tilt=0); void prepareDrive(int dir, int speed, int tilt=0); void drivePrepared(); + int directionAccountingForTilt(int, int); float updatePid(); float torad(float f); void resetDrive(); diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h index 12d1cf7..33621be 100644 --- a/include/sensors/data_source_camera_conicmirror.h +++ b/include/sensors/data_source_camera_conicmirror.h @@ -27,7 +27,7 @@ These values need to be subtracted from the coords used in setMoveSetpoints*/ // #define CAMERA_TRANSLATION_Y 7 //Robot with roller -#define CAMERA_TRANSLATION_X -5 +#define CAMERA_TRANSLATION_X 0 #define CAMERA_TRANSLATION_Y 0 class DataSourceCameraConic : public DataSource{ diff --git a/src/motors_movement/drivecontroller.cpp b/src/motors_movement/drivecontroller.cpp index 65ba45e..84ee895 100644 --- a/src/motors_movement/drivecontroller.cpp +++ b/src/motors_movement/drivecontroller.cpp @@ -167,4 +167,8 @@ void DriveController::stopAll(){ m2->stop(); m3->stop(); m4->stop(); +} + +int DriveController::directionAccountingForTilt(int wanted_direction, int tilt){ + return ((360-tilt+wanted_direction)+360)%360; } \ No newline at end of file diff --git a/src/strategy_roles/precision_shooter.cpp b/src/strategy_roles/precision_shooter.cpp index 4097bc3..bf495a3 100644 --- a/src/strategy_roles/precision_shooter.cpp +++ b/src/strategy_roles/precision_shooter.cpp @@ -29,12 +29,28 @@ void PrecisionShooter::init() ballAngleFilter = new ComplementaryFilter(0.85); } +float tilt1 = 0; +float tilt2 = 0; +int spinner_state = 0; +bool spinner_flag = false; +unsigned long spinner_timer = 0; +float spinner_tilt = 0; +int ball_catch_state = 0; +bool ball_catch_flag = false; +unsigned long ball_catch_timer = 0; +float ball_catch_tilt = 0; + void PrecisionShooter::realPlay() { if (CURRENT_DATA_READ.ballSeen) - this->spinner(5); - else + this->catchBall(); + else{ ps->goCenter(); + ball_catch_flag = false; + spinner_flag = false; + ball_catch_state = 0; + spinner_state = 0; + } } /* @@ -43,13 +59,6 @@ Spinning kick state machine 1: tilt toward 180 deg 2: tilt back to 0 in the needed direction, stopping the roller whenn needed */ -float tilt1 = 0; -float tilt2 = 0; -int spinner_state = 0; -bool spinner_flag = false; -unsigned long spinner_timer = 0; -float spinner_tilt = 0; - void PrecisionShooter::spinner(int targetx){ // if(!ballPresence->isInMouth()) { // spinner_state=0; @@ -128,10 +137,6 @@ Ball catch state machine 1: slowly return facing to north (slowly otherwise we might lose the ball) 2: reach the spot */ -int ball_catch_state = 0; -bool ball_catch_flag = false; -unsigned long ball_catch_timer = 0; -float ball_catch_tilt = 0; void PrecisionShooter::catchBall(){ @@ -144,27 +149,29 @@ void PrecisionShooter::catchBall(){ } if(ball_catch_state == 0){ - drive->prepareDrive(0, 30, ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix)); + drive->prepareDrive(0, MAX_VEL_HALF, ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix)); if(ballPresence->isInMouth() && !ball_catch_flag){ ball_catch_flag = true; ball_catch_timer = millis(); } - if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 250) { + if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 350) { ball_catch_state++; ball_catch_tilt = CURRENT_DATA_READ.IMUAngle; } }else if(ball_catch_state == 1){ - if(ball_catch_tilt > 180) ball_catch_tilt += 0.15; - else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.15; + int val = ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix); + drive->prepareDrive(drive->directionAccountingForTilt(CURRENT_DATA_READ.angleAtkFix, val ),MAX_VEL_HALF,val); - drive->prepareDrive(0,0,ball_catch_tilt); + // if(ball_catch_tilt > 180) ball_catch_tilt += 0.15; + // else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.15; - if(CURRENT_DATA_READ.IMUAngle >= 355 || CURRENT_DATA_READ.IMUAngle <= 5) ball_catch_state = 0; - }else if(ball_catch_state == 2){ - ((PositionSysCamera*)ps)->setMoveSetpoints(CURRENT_DATA_READ.xAtkFix, 0); - if(((PositionSysCamera*)ps)->isInTheVicinityOf(CURRENT_DATA_READ.xAtkFix, 0)) ball_catch_state++; + // drive->prepareDrive(0,0,ball_catch_tilt); + + // if(CURRENT_DATA_READ.IMUAngle >= 355 || CURRENT_DATA_READ.IMUAngle <= 5) ball_catch_state = 0; + // ((PositionSysCamera*)ps)->setMoveSetpoints(CURRENT_DATA_READ.xAtkFix, 0); + // if(((PositionSysCamera*)ps)->isInTheVicinityOf(CURRENT_DATA_READ.xAtkFix, 0)) ball_catch_state++; } } diff --git a/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py index d0c7a05..b03ce43 100644 --- a/utility/OpenMV/conic_eff_h7.py +++ b/utility/OpenMV/conic_eff_h7.py @@ -46,10 +46,10 @@ blue_led.on() thresholds = [ (70, 100, -36, 8, 38, 112), # thresholds yellow goalz - (31, 74, -31, 15, -50, -9)] # thresholds blue goal (6, 31, -15, 4, -35, 0) + (37, 57, -19, 8, -52, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0) -roi = (50, 0, 250, 200) +roi = (50, 0, 270, 200) # Camera Setup ############################################################### '''sensor.reset()xxxx