From dcfc28aab5e581f3a73d8beee45d7488bf807aee Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Tue, 22 Jun 2021 20:53:11 +0200 Subject: [PATCH] robocup: first technical challenge still could be better, faster and more precise in some parts --- include/motors_movement/drivecontroller.h | 4 +-- include/strategy_roles/games.h | 4 ++- src/main.cpp | 2 +- src/strategy_roles/games.cpp | 1 + src/strategy_roles/striker_roller.cpp | 32 +++++++---------------- 5 files changed, 16 insertions(+), 27 deletions(-) diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h index edbca77..990d8fe 100644 --- a/include/motors_movement/drivecontroller.h +++ b/include/motors_movement/drivecontroller.h @@ -10,8 +10,8 @@ //BEST NUMBERS YET //USE MOVING AVERAGE AND ANGLE WRAP -#define KP 1.2 -#define KI 0.0 +#define KP 0.8 +#define KI 0.5 #define KD 0.025 #define KSPD 0.3 diff --git a/include/strategy_roles/games.h b/include/strategy_roles/games.h index 6d17f59..fb8c21b 100644 --- a/include/strategy_roles/games.h +++ b/include/strategy_roles/games.h @@ -20,4 +20,6 @@ g_extr Game* striker; g_extr Game* striker_roller; g_extr Game* precision_shooter; g_extr Game* pass_and_shoot; -// g_extr Game* keeper; \ No newline at end of file +// g_extr Game* keeper; + +g_extr Game* tc1; \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 5c4de03..c5da63d 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -53,7 +53,7 @@ void loop() { // striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike; // keeper_condition = role == LOW; - striker_roller->play(1); + tc1->play(1); testmenu->testMenu(); diff --git a/src/strategy_roles/games.cpp b/src/strategy_roles/games.cpp index dabdec2..c98c00d 100644 --- a/src/strategy_roles/games.cpp +++ b/src/strategy_roles/games.cpp @@ -16,5 +16,6 @@ void initGames(){ pass_and_shoot = new PassAndShoot(new LineSysCamera(lIn, lOut), new PositionSysCamera()); precision_shooter = new PrecisionShooter(new LineSysCamera(lIn, lOut), new PositionSysCamera()); striker_roller = new StrikerRoller(new LineSysCamera(lIn, lOut), new PositionSysCamera()); + tc1 = new StrikerRoller(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/striker_roller.cpp b/src/strategy_roles/striker_roller.cpp index cde2d74..f54ca6c 100644 --- a/src/strategy_roles/striker_roller.cpp +++ b/src/strategy_roles/striker_roller.cpp @@ -62,18 +62,16 @@ void StrikerRoller::spinner(int targetx){ spinner_timer = millis(); } - if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { + if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 50) { spinner_state=1; spinner_flag = false; } }else if(spinner_state == 1){ roller->speed(roller->MAX); - if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { - spinner_state=2; - spinner_flag = false; - spinner_tilt = CURRENT_DATA_READ.IMUAngle; - } + spinner_state=2; + spinner_flag = false; + spinner_tilt = CURRENT_DATA_READ.IMUAngle; if(CURRENT_DATA_READ.posx < targetx) { tilt1 = -0.15; @@ -86,7 +84,6 @@ void StrikerRoller::spinner(int targetx){ limitx = STRL_KICK_LIMIT_TILT1; } - spinner_state=1; }else if(spinner_state == 2){ roller->speed(roller->MAX); @@ -139,33 +136,25 @@ void StrikerRoller::catchBall(){ ball_catch_timer = millis(); } - if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 350) { + if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 50) { ball_catch_state++; - ball_catch_tilt = CURRENT_DATA_READ.IMUAngle; } }else if(ball_catch_state == 1){ int val = ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix); + // val = val > 10 || val < 10 ? val : 0; + drive->prepareDrive(drive->directionAccountingForTilt(CURRENT_DATA_READ.angleAtkFix, val ),MAX_VEL_HALF,val); - if(CURRENT_DATA_READ.distAtk <= 30){ + if(CURRENT_DATA_READ.distAtk <= 25){ ball_catch_state++; spinner_state = 0; spinner_flag = false; spinner_tilt = 0; - drive->prepareDrive(0,0,0); + // drive->prepareDrive(0,0,0); } }else if(ball_catch_state == 2){ spinner(0); } - - // if(ball_catch_tilt > 180) ball_catch_tilt += 0.15; - // else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.15; - - // 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++; } int StrikerRoller::tilt() { @@ -173,9 +162,6 @@ int StrikerRoller::tilt() { 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); }