EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

striker: use striker based on old Latino's code

code_newgen
EmaMaker 2021-03-01 16:00:11 +01:00
parent 019831e237
commit b657d88846
5 changed files with 117 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
include/strategy_roles/striker.h
View File

@ -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{

18
include/strategy_roles/striker_test.h
View File

@ -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{

6
src/main.cpp
View File

@ -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();

105
src/strategy_roles/striker.cpp
View File

@ -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(){
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.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);
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 (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;
void Striker::ballBack() {
int ball_degrees2;
int dir;
if (ball_degrees2 > 0)
dir = ball_deg + plusang; //se sto nel quadrante positivo aggiungo
else
dir = ball_deg - plusang; //se sto nel negativo sottraggo
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 (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()
{
}

110
src/strategy_roles/striker_test.cpp
View File

@ -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;
// 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> 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;
}
// DEBUG.print("Coords of the robot relative to the ball: (");
// DEBUG.print(robotX);
// DEBUG.print(", ");
// DEBUG.print(robotY);
// DEBUG.println(")");
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;
}
// // angleDiff = min(((-robotAngle + 360) % 360), ((robotAngle + 360) % 360));
// // angleShift = min(angleDiff, ANGLE_SHIFT_STEP);
drive->prepareDrive(atk_direction, atk_speed);
// 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("==========");
}