From f39664dcfbd6a60b9abf29fa8b260fe8a0373e1b Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Tue, 28 Jun 2022 10:41:05 +0200 Subject: [PATCH] striker: fix ball wraparound from behind avoids repeatedly hitting a line when near an edge --- include/strategy_roles/striker.h | 18 ++--- src/strategy_roles/striker.cpp | 115 ++++++++++++++++--------------- 2 files changed, 70 insertions(+), 63 deletions(-) diff --git a/include/strategy_roles/striker.h b/include/strategy_roles/striker.h index 32edc73..fc91130 100644 --- a/include/strategy_roles/striker.h +++ b/include/strategy_roles/striker.h @@ -3,11 +3,16 @@ #include "sensors/data_source_camera_vshapedmirror.h" #include "sensors/sensors.h" #include "strategy_roles/game.h" +#include "motors_movement/drivecontroller.h" #define STRIKER_ATTACK_DISTANCE 110 #define STRIKER_TILT_STOP_DISTANCE 140 -#define STRIKER_PLUSANG 70 -#define STRIKER_PLUSANG_VISIONCONE 0 +#define STRIKER_PLUSANG 50 +#define STRIKER_PLUSANG_VISIONCONE 7 +#define STRIKER_VEL MAX_VEL + +#define STRIKER_MOUTH_SX 345 +#define STRIKER_MOUTH_DX 15 class Striker : public Game{ @@ -22,11 +27,6 @@ class Striker : public Game{ int tilt(); float ballTilt(); - int atk_speed, atk_direction; - float atk_tilt, ball_tilt; - - int ball_angle_filter; - ComplementaryFilter* ball_filter; - - bool gotta_tilt; + int atk_speed, atk_direction = 0, atk_tilt, stato = 0, plusang_flag = 0; + bool flag = false; }; diff --git a/src/strategy_roles/striker.cpp b/src/strategy_roles/striker.cpp index 68043ef..6b982cb 100644 --- a/src/strategy_roles/striker.cpp +++ b/src/strategy_roles/striker.cpp @@ -1,83 +1,90 @@ -#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/striker.h" -#include "vars.h" +#include "behaviour_control/status_vector.h" #include "math.h" - +#include "sensors/data_source_ball.h" +#include "sensors/sensors.h" +#include "systems/position/positionsys_camera.h" +#include "vars.h" Striker::Striker() : Game() { - init(); + init(); } Striker::Striker(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) { - init(); + init(); } void Striker::init() { - atk_speed = 0; - atk_direction = 0; - atk_tilt = 0; - ball_angle_filter = 0; - - gotta_tilt = false; - ball_filter = new ComplementaryFilter(1); + atk_speed = 0; + atk_direction = 0; + atk_tilt = 0; } void Striker::realPlay() { - ball_angle_filter = ball_filter->calculate(CURRENT_DATA_READ.ballAngle); - - if (CURRENT_DATA_READ.ballSeen) - this->striker(); - else{ - ps->goCenter(); - } + if (CURRENT_DATA_READ.ballSeen) + this->striker(); + else + { + ps->goCenter(); + } } float ctilt = 0; unsigned long ttilt = 0; -void Striker::striker(){ - if(CURRENT_DATA_READ.ballDistance >= 125){ - drive->prepareDrive(ball_angle_filter > 180 ? ball_angle_filter*0.96 : ball_angle_filter*1.04, MAX_VEL_3QUARTERS, 0); - - }else +void Striker::striker() { - //seguo palla - int ball_degrees2, dir, ball_deg = ball_angle_filter, plusang = STRIKER_PLUSANG; - - 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; - if(ball_degrees2 > 0) dir = ball_deg + plusang; //se sto nel quadrante positivo aggiungo - else dir = ball_deg - plusang; //se sto nel negativo sottraggo + if(CURRENT_DATA_READ.lineActive != 0 ) flag = false; - dir = (dir + 360) % 360; - // drive->prepareDrive(dir, MAX_VEL_HALF, tilt()); - drive->prepareDrive(dir, MAX_VEL_3QUARTERS, CURRENT_DATA_READ.ballAngle <= 90 || CURRENT_DATA_READ.ballAngle >= 270 ? CURRENT_DATA_READ.angleAtkFix : 0); + int dir = 0, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG + 8 * (CURRENT_DATA_READ.ballDistance <= 90); - // if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED); - // else roller->speed(roller->MIN); - } - + if (CURRENT_DATA_READ.ballDistance >= 125) + { + drive->prepareDrive(ball_deg > 180 ? ball_deg * 0.96 : ball_deg * 1.04, STRIKER_VEL, 0); + } + else + { + // seguo palla + + if (ball->isInFront()) + { + dir = 0; + flag = false; + } + else + { + if (!flag) + { + + if (ball_deg <= 90) + dir = ball_deg + plusang; + else if (ball_deg >= 270) + dir = ball_deg - plusang; + else + { + flag = true; + if (ball_deg <= 180) + plusang_flag = plusang; + else + plusang_flag = -plusang; + } + }else dir = ball_deg + plusang_flag; + } + + dir = (dir + 360) % 360; + drive->prepareDrive(dir, STRIKER_VEL, tilt()); + + // if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED); + // else roller->speed(roller->MIN); + } } -int Striker::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; +int Striker::tilt() +{ + return CURRENT_DATA_READ.ballAngle <= 90 || CURRENT_DATA_READ.ballAngle >= 270 ? CURRENT_DATA_READ.angleAtkFix : 0; } \ No newline at end of file