From 3ebe64555be38571443784b9147b88698e11e135 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Mon, 19 Apr 2021 19:14:51 +0200 Subject: [PATCH] tilt: better tilt --- include/strategy_roles/striker.h | 5 ++++- src/motors_movement/drivecontroller.cpp | 2 +- src/strategy_roles/striker.cpp | 21 ++++++++++++++------- 3 files changed, 19 insertions(+), 9 deletions(-) diff --git a/include/strategy_roles/striker.h b/include/strategy_roles/striker.h index f2e8aa7..fed5264 100644 --- a/include/strategy_roles/striker.h +++ b/include/strategy_roles/striker.h @@ -4,7 +4,8 @@ #include "sensors/sensors.h" #include "strategy_roles/game.h" -#define STRIKER_ATTACK_DISTANCE 100 +#define STRIKER_ATTACK_DISTANCE 110 +#define STRIKER_TILT_STOP_DISTANCE 140 #define STRIKER_PLUSANG 55 #define STRIKER_PLUSANG_VISIONCONE 10 @@ -23,6 +24,8 @@ class Striker : public Game{ int atk_speed, atk_direction, atk_tilt; float cstorc; + bool gotta_tilt; + ComplementaryFilter* filter; diff --git a/src/motors_movement/drivecontroller.cpp b/src/motors_movement/drivecontroller.cpp index 8cfef60..5857943 100644 --- a/src/motors_movement/drivecontroller.cpp +++ b/src/motors_movement/drivecontroller.cpp @@ -63,7 +63,7 @@ float DriveController::torad(float f){ void DriveController::drive(int dir, int speed, int tilt){ speed = speedFilter->calculate(speed)*GLOBAL_SPD_MULT; - tilt = tilt > 180 ? tilt - 360 : tilt; + //tilt = tilt > 180 ? tilt - 360 : 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 diff --git a/src/strategy_roles/striker.cpp b/src/strategy_roles/striker.cpp index 0ac7414..c75bab2 100644 --- a/src/strategy_roles/striker.cpp +++ b/src/strategy_roles/striker.cpp @@ -23,6 +23,8 @@ void Striker::init() atk_tilt = 0; cstorc = 0; + gotta_tilt = false; + filter = new ComplementaryFilter(0.7); } @@ -40,7 +42,7 @@ void Striker::striker() if (CURRENT_DATA_READ.ballDistance > STRIKER_ATTACK_DISTANCE) { - drive->prepareDrive(ball_deg > 180 ? CURRENT_DATA_READ.ballAngle - 20 : CURRENT_DATA_READ.ballAngle + 20, MAX_VEL_EIGTH, 0); + drive->prepareDrive(ball_deg > 180 ? CURRENT_DATA_READ.ballAngle - 20 : CURRENT_DATA_READ.ballAngle + 20, MAX_VEL_HALF, 0); return; } @@ -61,16 +63,21 @@ void Striker::striker() else dir = dir; - drive->prepareDrive(dir, MAX_VEL_QUARTER, tilt()); + drive->prepareDrive(dir, MAX_VEL_HALF, tilt()); } int Striker::tilt() { - if (!CURRENT_DATA_READ.atkSeen) return 0; - if (CURRENT_DATA_READ.ballAngleFix >= 350 || CURRENT_DATA_READ.ballAngleFix <= 10) - atk_tilt = (constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45) + 360) % 360; - else if((CURRENT_DATA_READ.ballAngleFix > 345 && CURRENT_DATA_READ.ballAngleFix < 350) || (CURRENT_DATA_READ.ballAngleFix > 10 && CURRENT_DATA_READ.ballAngleFix < 15)) - atk_tilt = 0; + if (CURRENT_DATA_READ.ballDistance <= STRIKER_ATTACK_DISTANCE) gotta_tilt = true; + if (CURRENT_DATA_READ.ballDistance > STRIKER_ATTACK_DISTANCE && gotta_tilt) gotta_tilt = false; + + if (CURRENT_DATA_READ.atkSeen && gotta_tilt) return 0; + if (CURRENT_DATA_READ.ballAngle >= 345 || CURRENT_DATA_READ.ballAngle <= 15) atk_tilt = CURRENT_DATA_READ.angleAtkFix; + else gotta_tilt = false; + // if (CURRENT_DATA_READ.ballAngle >= 350 || CURRENT_DATA_READ.ballAngle <= 10) + // atk_tilt = (constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45) + 360) % 360; + // else if((CURRENT_DATA_READ.ballAngle > 345 && CURRENT_DATA_READ.ballAngle < 350) || (CURRENT_DATA_READ.ballAngle > 10 && CURRENT_DATA_READ.ballAngle < 15)) + // atk_tilt = 0; atk_tilt = filter->calculate(atk_tilt); return atk_tilt; } \ No newline at end of file