From b657d888465931de0d101b483204f042191dab53 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Mon, 1 Mar 2021 16:00:11 +0100 Subject: [PATCH] striker: use striker based on old Latino's code --- include/strategy_roles/striker.h | 17 +--- include/strategy_roles/striker_test.h | 18 +--- src/main.cpp | 6 +- src/strategy_roles/striker.cpp | 119 ++++++++++---------------- src/strategy_roles/striker_test.cpp | 112 ++++++++++++++---------- 5 files changed, 117 insertions(+), 155 deletions(-) diff --git a/include/strategy_roles/striker.h b/include/strategy_roles/striker.h index 3010475..b552694 100644 --- a/include/strategy_roles/striker.h +++ b/include/strategy_roles/striker.h @@ -4,20 +4,9 @@ #include "sensors/sensors.h" #include "strategy_roles/game.h" -#define TILT_MULT 1.8 -#define TILT_DIST 170 -#define CATCH_DIST 150 -#define GOALIE_ATKSPD_LAT 150 //255 -#define GOALIE_ATKSPD_BAK 150 -#define GOALIE_ATKSPD_FRT 150 -#define GOALIE_ATKSPD_STRK 150 -#define GOALIE_ATKDIR_PLUSANG1 20 -#define GOALIE_ATKDIR_PLUSANG2 35 -#define GOALIE_ATKDIR_PLUSANG3 40 -#define GOALIE_ATKDIR_PLUSANGBAK 40 -#define GOALIE_ATKDIR_PLUSANG1_COR 60 -#define GOALIE_ATKDIR_PLUSANG2_COR 70 -#define GOALIE_ATKDIR_PLUSANG3_COR 70 +#define STRIKER_ATTACK_DISTANCE 125 +#define STRIKER_PLUSANG 55 +#define STRIKER_PLUSANG_VISIONCONE 20 class Striker : public Game{ diff --git a/include/strategy_roles/striker_test.h b/include/strategy_roles/striker_test.h index b9126e1..78eff8c 100644 --- a/include/strategy_roles/striker_test.h +++ b/include/strategy_roles/striker_test.h @@ -4,20 +4,10 @@ #include "sensors/sensors.h" #include "strategy_roles/game.h" -#define TILT_MULT 1.8 -#define TILT_DIST 170 -#define CATCH_DIST 150 -#define GOALIE_ATKSPD_LAT 120 //255 -#define GOALIE_ATKSPD_BAK 120 -#define GOALIE_ATKSPD_FRT 120 -#define GOALIE_ATKSPD_STRK 120 -#define GOALIE_ATKDIR_PLUSANG1 20 -#define GOALIE_ATKDIR_PLUSANG2 35 -#define GOALIE_ATKDIR_PLUSANG3 40 -#define GOALIE_ATKDIR_PLUSANGBAK 40 -#define GOALIE_ATKDIR_PLUSANG1_COR 60 -#define GOALIE_ATKDIR_PLUSANG2_COR 70 -#define GOALIE_ATKDIR_PLUSANG3_COR 70 +#define TARGET_DIST 120 +#define ATTACK_DIST 150 +#define ANGLE_SHIFT_STEP 5 +#define STRIKER_SPD 80 class StrikerTest : public Game{ diff --git a/src/main.cpp b/src/main.cpp index bfab336..5d881d3 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -37,11 +37,9 @@ void loop() { updateSensors(); if(DEBUG.available()) testmenu->testMenu(); - // striker_test->play(1); striker->play(1); - // keeper->play(1); - - // drive->prepareDrive(0, 100,0); + // striker_test->play(1); + // keeper->play(role==0); // Last thing to do: movement and update status vector drive->drivePrepared(); diff --git a/src/strategy_roles/striker.cpp b/src/strategy_roles/striker.cpp index 1f596fe..d7f0f51 100644 --- a/src/strategy_roles/striker.cpp +++ b/src/strategy_roles/striker.cpp @@ -6,94 +6,61 @@ #include "math.h" - -Striker::Striker() : Game() { +Striker::Striker() : Game() +{ init(); } -Striker::Striker(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) { +Striker::Striker(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) +{ init(); } -void Striker::init(){ - atk_speed = 0; - atk_direction = 0; - cstorc = 0; +void Striker::init() +{ + atk_speed = 0; + atk_direction = 0; + cstorc = 0; } -void Striker::realPlay(){ - if(CURRENT_DATA_READ.ballSeen) this->striker(); - else ps->goCenter(); +void Striker::realPlay() +{ + if (CURRENT_DATA_READ.ballSeen) + this->striker(); + else + ps->goCenter(); } -void Striker::striker() { - if(CURRENT_DATA_READ.ballAngle>= 350 || CURRENT_DATA_READ.ballAngle<= 20) { - if(CURRENT_DATA_READ.ballDistance > 190) atk_direction = 0; - else atk_direction = CURRENT_DATA_READ.ballAngle; - atk_speed = GOALIE_ATKSPD_FRT; +void Striker::striker() +{ + int plusang = STRIKER_PLUSANG, ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle; + + if (CURRENT_DATA_READ.ballDistance > STRIKER_ATTACK_DISTANCE) + { + drive->prepareDrive(ball_deg > 180 ? CURRENT_DATA_READ.ballAngle - 10 : CURRENT_DATA_READ.ballAngle + 10, 100, 0); + return; } - - if(CURRENT_DATA_READ.ballAngle>= 90 && CURRENT_DATA_READ.ballAngle<= 270) { - ballBack(); - atk_speed = GOALIE_ATKSPD_BAK; - } - - if(CURRENT_DATA_READ.ballAngle> 10 && CURRENT_DATA_READ.ballAngle< 30) { - atk_direction = CURRENT_DATA_READ.ballAngle+ GOALIE_ATKDIR_PLUSANG1; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(CURRENT_DATA_READ.ballAngle>= 30 && CURRENT_DATA_READ.ballAngle< 45) { - atk_direction = CURRENT_DATA_READ.ballAngle+ GOALIE_ATKDIR_PLUSANG2; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(CURRENT_DATA_READ.ballAngle>= 45 && CURRENT_DATA_READ.ballAngle< 90) { - atk_direction = CURRENT_DATA_READ.ballAngle+ GOALIE_ATKDIR_PLUSANG3; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(CURRENT_DATA_READ.ballAngle> 270 && CURRENT_DATA_READ.ballAngle<= 315) { - atk_direction = CURRENT_DATA_READ.ballAngle- GOALIE_ATKDIR_PLUSANG3_COR; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(CURRENT_DATA_READ.ballAngle> 315 && CURRENT_DATA_READ.ballAngle<= 330) { - atk_direction = CURRENT_DATA_READ.ballAngle- GOALIE_ATKDIR_PLUSANG2_COR; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(CURRENT_DATA_READ.ballAngle> 330 && CURRENT_DATA_READ.ballAngle< 350) { - atk_direction = CURRENT_DATA_READ.ballAngle- GOALIE_ATKDIR_PLUSANG1_COR; - atk_speed = GOALIE_ATKSPD_LAT; - } - - // if((CURRENT_DATA_READ.ballAngle>= 330 || CURRENT_DATA_READ.ballAngle<= 30) && CURRENT_DATA_READ.ballDistance > 190) { //storcimento - // atk_speed = GOALIE_ATKSPD_STRK; //dove i gigahertz hanno fallito - // drive->prepareDrive(atk_direction, atk_speed, cstorc); - // } - // else - drive->prepareDrive(atk_direction, atk_speed); -} - - - -void Striker::ballBack() { - int ball_degrees2; - int dir; - - int plusang; - if(CURRENT_DATA_READ.ballDistance > 130) plusang = GOALIE_ATKDIR_PLUSANGBAK; - else plusang = 0; - if(CURRENT_DATA_READ.ballAngle> 180) ball_degrees2 = CURRENT_DATA_READ.ballAngle- 360; - else ball_degrees2 = CURRENT_DATA_READ.ballAngle; - if(ball_degrees2 > 0) dir = CURRENT_DATA_READ.ballAngle+ plusang; //45 con 8 ruote - else dir = CURRENT_DATA_READ.ballAngle- plusang; //45 con 8 ruote - if(dir < 0) dir = dir + 360; - else dir = dir; - atk_direction = dir; + + if (ball_deg > 340 || ball_deg < 20) + 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 (dir < 0) + dir = dir + 360; //se sto nel quadrante negativo ricappotto + else + dir = dir; + drive->prepareDrive(dir, 100, 0); } - -void Striker::storcimentoPorta() { - if (CURRENT_DATA_READ.angleAtkFix >= 5 && CURRENT_DATA_READ.angleAtkFix <= 60) cstorc+=9; - else if (CURRENT_DATA_READ.angleAtkFix <= 355 && CURRENT_DATA_READ.angleAtkFix >= 210) cstorc-=9; - else cstorc *= 0.9; - cstorc = constrain(cstorc, -45, 45); +void Striker::storcimentoPorta() +{ } \ No newline at end of file diff --git a/src/strategy_roles/striker_test.cpp b/src/strategy_roles/striker_test.cpp index 912a0c9..b72b4b5 100644 --- a/src/strategy_roles/striker_test.cpp +++ b/src/strategy_roles/striker_test.cpp @@ -23,57 +23,75 @@ void StrikerTest::realPlay(){ else ps->goCenter(); } -void StrikerTest::striker() { - if(CURRENT_DATA_READ.ballAngle>= 350 || CURRENT_DATA_READ.ballAngle<= 20) { - if(CURRENT_DATA_READ.ballDistance > 190) atk_direction = 0; - else atk_direction = CURRENT_DATA_READ.ballAngle; - atk_speed = GOALIE_ATKSPD_FRT; - } +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; - if(CURRENT_DATA_READ.ballAngle>= 90 && CURRENT_DATA_READ.ballAngle<= 270) { - int ball_degrees2; - int dir; + // DEBUG.print("Ball is at angle (goniometric circle): "); + // DEBUG.println(ballAngle); + // DEBUG.print("Robot is at angle "); + // DEBUG.println(robotAngle); - int plusang; - if(CURRENT_DATA_READ.ballDistance > 130) plusang = GOALIE_ATKDIR_PLUSANGBAK; - else plusang = 0; - - if(CURRENT_DATA_READ.ballAngle> 180) ball_degrees2 = CURRENT_DATA_READ.ballAngle- 360; - else ball_degrees2 = CURRENT_DATA_READ.ballAngle; - if(ball_degrees2 > 0) dir = CURRENT_DATA_READ.ballAngle+ plusang; //45 con 8 ruote - else dir = CURRENT_DATA_READ.ballAngle- plusang; //45 con 8 ruote - if(dir < 0) dir = dir + 360; - else dir = dir; - atk_direction = dir; - atk_speed = GOALIE_ATKSPD_BAK; - } + // float robotAngle_rad = robotAngle*3.14 / 180; + // robotX = CURRENT_DATA_READ.ballDistance * cos(robotAngle_rad); + // robotY = CURRENT_DATA_READ.ballDistance * sin(robotAngle_rad); - if(CURRENT_DATA_READ.ballAngle> 10 && CURRENT_DATA_READ.ballAngle< 30) { - atk_direction = CURRENT_DATA_READ.ballAngle+ GOALIE_ATKDIR_PLUSANG1; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(CURRENT_DATA_READ.ballAngle>= 30 && CURRENT_DATA_READ.ballAngle< 45) { - atk_direction = CURRENT_DATA_READ.ballAngle+ GOALIE_ATKDIR_PLUSANG2; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(CURRENT_DATA_READ.ballAngle>= 45 && CURRENT_DATA_READ.ballAngle< 90) { - atk_direction = CURRENT_DATA_READ.ballAngle+ GOALIE_ATKDIR_PLUSANG3; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(CURRENT_DATA_READ.ballAngle> 270 && CURRENT_DATA_READ.ballAngle<= 315) { - atk_direction = CURRENT_DATA_READ.ballAngle- GOALIE_ATKDIR_PLUSANG3_COR; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(CURRENT_DATA_READ.ballAngle> 315 && CURRENT_DATA_READ.ballAngle<= 330) { - atk_direction = CURRENT_DATA_READ.ballAngle- GOALIE_ATKDIR_PLUSANG2_COR; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(CURRENT_DATA_READ.ballAngle> 330 && CURRENT_DATA_READ.ballAngle< 350) { - atk_direction = CURRENT_DATA_READ.ballAngle- GOALIE_ATKDIR_PLUSANG1_COR; - atk_speed = GOALIE_ATKSPD_LAT; - } + // DEBUG.print("Coords of the robot relative to the ball: ("); + // DEBUG.print(robotX); + // DEBUG.print(", "); + // DEBUG.print(robotY); + // DEBUG.println(")"); - drive->prepareDrive(atk_direction, atk_speed); + // // 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("=========="); }