EmaMaker
/
SPQR-Team-2019-REVAMPED
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

striker: fix ball wraparound from behind 

avoids repeatedly hitting a line when near an edge
pull/1/head
EmaMaker 2022-06-28 10:41:05 +02:00
parent 131819f02a
commit f39664dcfb
2 changed files with 70 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
include/strategy_roles/striker.h
View File

@ -3,11 +3,16 @@
#include "sensors/data_source_camera_vshapedmirror.h" #include "sensors/data_source_camera_vshapedmirror.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "strategy_roles/game.h" #include "strategy_roles/game.h"
#include "motors_movement/drivecontroller.h"
#define STRIKER_ATTACK_DISTANCE 110 #define STRIKER_ATTACK_DISTANCE 110
#define STRIKER_TILT_STOP_DISTANCE 140 #define STRIKER_TILT_STOP_DISTANCE 140
#define STRIKER_PLUSANG 70 #define STRIKER_PLUSANG 50
#define STRIKER_PLUSANG_VISIONCONE 0 #define STRIKER_PLUSANG_VISIONCONE 7
#define STRIKER_VEL MAX_VEL
#define STRIKER_MOUTH_SX 345
#define STRIKER_MOUTH_DX 15
class Striker : public Game{ class Striker : public Game{
@ -22,11 +27,6 @@ class Striker : public Game{
int tilt(); int tilt();
float ballTilt(); float ballTilt();
int atk_speed, atk_direction; int atk_speed, atk_direction = 0, atk_tilt, stato = 0, plusang_flag = 0;
float atk_tilt, ball_tilt; bool flag = false;
int ball_angle_filter;
ComplementaryFilter* ball_filter;
bool gotta_tilt;
}; };

111
src/strategy_roles/striker.cpp
View File

@ -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 "strategy_roles/striker.h"
#include "vars.h" #include "behaviour_control/status_vector.h"
#include "math.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() Striker::Striker() : Game()
{ {
init(); init();
} }
Striker::Striker(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) Striker::Striker(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_)
{ {
init(); init();
} }
void Striker::init() void Striker::init()
{ {
atk_speed = 0; atk_speed = 0;
atk_direction = 0; atk_direction = 0;
atk_tilt = 0; atk_tilt = 0;
ball_angle_filter = 0;
gotta_tilt = false;
ball_filter = new ComplementaryFilter(1);
} }
void Striker::realPlay() void Striker::realPlay()
{ {
ball_angle_filter = ball_filter->calculate(CURRENT_DATA_READ.ballAngle); if (CURRENT_DATA_READ.ballSeen)
this->striker();
if (CURRENT_DATA_READ.ballSeen) else
this->striker(); {
else{ ps->goCenter();
ps->goCenter(); }
}
} }
float ctilt = 0; float ctilt = 0;
unsigned long ttilt = 0; unsigned long ttilt = 0;
void Striker::striker(){ 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
{ {
//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(CURRENT_DATA_READ.lineActive != 0 ) flag = false;
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 int dir = 0, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG + 8 * (CURRENT_DATA_READ.ballDistance <= 90);
else dir = ball_deg - plusang; //se sto nel negativo sottraggo
dir = (dir + 360) % 360; if (CURRENT_DATA_READ.ballDistance >= 125)
// 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); drive->prepareDrive(ball_deg > 180 ? ball_deg * 0.96 : ball_deg * 1.04, STRIKER_VEL, 0);
}
else
{
// seguo palla
// if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED); if (ball->isInFront())
// else roller->speed(roller->MIN); {
} 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() { int Striker::tilt()
if (ball->isInMouth() /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true; {
else gotta_tilt = false; return CURRENT_DATA_READ.ballAngle <= 90 || CURRENT_DATA_READ.ballAngle >= 270 ? CURRENT_DATA_READ.angleAtkFix : 0;
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;
} }