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

precision shooter: catch and spinning kick

pull/1/head
EmaMaker 2021-06-20 23:32:45 +02:00
parent 6c81dbf998
commit c425472f58
5 changed files with 153 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
include/strategy_roles/precision_shooter.h
View File

@ -1,6 +1,7 @@
#pragma once
#include "sensors/data_source_camera_vshapedmirror.h"
#include "behaviour_control/complementary_filter.h"
#include "sensors/sensors.h"
#include "strategy_roles/game.h"
@ -9,6 +10,9 @@
#define PS_PLUSANG 55
#define PS_PLUSANG_VISIONCONE 10
// There needs to be a little bit of space between the target point and the spot to be in
#define PS_SPINNER_OVERHEAD 10
class PrecisionShooter : public Game{
public:
@ -18,13 +22,13 @@ class PrecisionShooter : public Game{
private:
void realPlay() override;
void init() override;
void striker();
void catchBall();
void spinner();
int tilt();
private:
int atk_speed, atk_direction, atk_tilt;
float cstorc;
bool gotta_tilt;
unsigned long pas_counter;
ComplementaryFilter* ballAngleFilter;
};

3
include/systems/position/positionsys_camera.h
View File

@ -22,6 +22,8 @@
#define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2)
#define DIST_MULT 1.7
#define VICINITY_DIST_TRESH 3
#define Kpx 1
#define Kix 0
#define Kdx 0
@ -42,6 +44,7 @@ class PositionSysCamera : public PositionSystem{
void setCameraPID();
void CameraPID();
int calcOtherGoalY(int goalY);
bool isInTheVicinityOf(int, int);
double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;
int MAX_DIST, vx, vy, axisx, axisy;

143
src/strategy_roles/precision_shooter.cpp
View File

@ -25,62 +25,129 @@ void PrecisionShooter::init()
cstorc = 0;
gotta_tilt = false;
ballAngleFilter = new ComplementaryFilter(0.85);
}
void PrecisionShooter::realPlay()
{
if (CURRENT_DATA_READ.ballSeen)
this->striker();
this->catchBall();
else
ps->goCenter();
}
unsigned long t3 = 0;
unsigned long t4 = 0;
boolean ignited = false;
/*
Spinning kick state machine
0: wait for the ball to be in mouth, calculate movement direction
1: tilt toward 180 deg
2: tilt back to 0 in the needed direction, stopping the roller whenn needed
*/
float tilt1 = 0;
float tilt2 = 0;
int spinner_state = 0;
bool spinner_flag = false;
unsigned long spinner_timer = 0;
float spinner_tilt = 0;
void PrecisionShooter::striker(){
void PrecisionShooter::spinner(int targetx){
if(!ballPresence->isInMouth()) {
spinner_state=0;
spinner_flag = false;
}
#ifdef MAX_VEL
#undef MAX_VEL
#define MAX_VEL 100
#endif
//seguo palla
int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = PS_PLUSANG;
if(ball_deg >= 346 || ball_deg <= 16) plusang = PS_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
dir = (dir + 360) % 360;
drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
// if(ball->isInFront()) roller->speed(ROLLER_DEFAULT_SPEED);
// else roller->speed(roller->MIN);
if(ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 78 && ( (CURRENT_DATA_READ.posy >= 32 && (CURRENT_DATA_READ.posx >= 15 || CURRENT_DATA_READ.posx <= -15)) || abs(tilt()) > 65 ) ) {
// Just let the robot slowly approach the ball
if(!ignited){
ignited = true;
t4 = millis();
if(spinner_state == 0){
if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
if(ballPresence->isInMouth() && !spinner_flag){
spinner_flag = true;
spinner_timer = millis();
}
if(millis() - t4 > 250 && ignited){
t3 = millis();
if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 250) {
spinner_state++;
spinner_tilt = CURRENT_DATA_READ.IMUAngle;
}
}else if(spinner_state == 1){
int spotx;
if(targetx > 0) spotx = targetx-PS_SPINNER_OVERHEAD;
else spotx = targetx+PS_SPINNER_OVERHEAD;
((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0);
if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 0)){
ball_catch_state++;
if(CURRENT_DATA_READ.posx > targetx){
tilt1 = -0.1;
tilt2 = 0.35;
}else{
tilt1 = 0.1;
tilt2 = -0.35;
}
}
}else if(spinner_state == 2){
spinner_tilt += tilt1;
drive(0,0,spinner_tilt);
if(CURRENT_DATA_READ.IMUAngle > 175 && CURRENT_DATA_READ.IMUAngle < 185) {
spinner_state++;
spinner_tilt = CURRENT_DATA_READ.IMUAngle;
}
}else if(spinner_state == 3){
spinner_tilt += tilt2;
drive(0,0,spinner_tilt);
if(CURRENT_DATA_READ.IMUAngle > 225 || CURRENT_DATA_READ.IMUAngle < 135){
roller->speed(roller->MIN);
spinner_state++;
}
}
if(millis() - t3 < 800){
// roller->speed(roller->MAX);
drive->prepareDrive(180, MAX_VEL_3QUARTERS, 0);
ignited = false;
}
/*
Ball catch state machine
0: go towards the ball, until it's been in robot's mouth for 250ms
1: slowly return facing to north (slowly otherwise we might lose the ball)
2: reach the spot
*/
int ball_catch_state = 0;
bool ball_catch_flag = false;
unsigned long ball_catch_timer = 0;
float ball_catch_tilt = 0;
void PrecisionShooter::catchBall(){
if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
else roller->speed(roller->MIN);
if(!ball->isInFront()){
ball_catch_state = 0;
ball_catch_flag = false;
}
if(ball_catch_state == 0){
drive->prepareDrive(0, 30, ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix));
if(ballPresence->isInMouth() && !ball_catch_flag){
ball_catch_flag = true;
ball_catch_timer = millis();
}
if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 250) {
ball_catch_state++;
ball_catch_tilt = CURRENT_DATA_READ.IMUAngle;
}
}else if(ball_catch_state == 1){
if(ball_catch_tilt > 180) ball_catch_tilt += 0.15;
else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.15;
if(CURRENT_DATA_READ.IMUAngle >= 355 || CURRENT_DATA_READ.IMUAngle <= 5) ball_catch_state = 0;
}else if(ball_catch_state == 2){
((PositionSysCamera*)ps)->setMoveSetpoints(CURRENT_DATA_READ.xAtkFix, 0);
if(((PositionSysCamera*)ps)->isInTheVicinityOf(CURRENT_DATA_READ.xAtkFix, 0)) ball_catch_state++;
}
}
int PrecisionShooter::tilt() {

35
src/strategy_roles/striker.cpp
View File

@ -40,7 +40,7 @@ float ctilt = 0;
unsigned long ttilt = 0;
void Striker::striker(){
/*void Striker::striker(){
//seguo palla
if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
else roller->speed(roller->MIN);
@ -58,21 +58,36 @@ void Striker::striker(){
// old_ball_Angle = ball_angle;
// old_ball_tilt = (int) ball_tilt;
}
}*/
float Striker::ballTilt(){
if(CURRENT_DATA_READ.ballAngle > 180 && CURRENT_DATA_READ.ballAngle < 340) ball_tilt -= 0.15f;
else if(CURRENT_DATA_READ.ballAngle <= 180 && CURRENT_DATA_READ.ballAngle > 20) ball_tilt += 0.15f;
return ball_tilt;
}
void Striker::striker(){
//seguo palla
int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, 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
dir = (dir + 360) % 360;
drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
else roller->speed(roller->MIN);
}
int Striker::tilt() {
if(CURRENT_DATA_READ.atkSeen && (CURRENT_DATA_READ.ballAngleFix > 340 || CURRENT_DATA_READ.ballAngleFix < 20)){
atk_tilt = roller->roller_armed ? CURRENT_DATA_READ.angleAtkFix : constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45);
}else{
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;

17
src/system/positions/positionsys_camera.cpp
View File

@ -4,6 +4,7 @@
#include "vars.h"
#include "math.h"
PositionSysCamera::PositionSysCamera() {
MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y);
Inputx = 0;
@ -122,6 +123,11 @@ int PositionSysCamera::calcOtherGoalY(int goalY){
return otherGoalY;
}
bool PositionSysCamera::isInTheVicinityOf(int x_, int y_){
// Distance using pytagorean theorem
return pow(CURRENT_DATA_READ.posx-x_, 2) + pow(CURRENT_DATA_READ.posy-y_, 2) <= VICINITY_DIST_TRESH*VICINITY_DIST_TRESH;
}
void PositionSysCamera::CameraPID(){
if(givenMovement){
@ -139,12 +145,11 @@ void PositionSysCamera::CameraPID(){
int dir = -90-(atan2(Outputy,Outputx)*180/3.14);
dir = (dir+360) % 360;
int dist = sqrt( ( (CURRENT_DATA_WRITE.posx-Setpointx)*(CURRENT_DATA_WRITE.posx-Setpointx) ) + (CURRENT_DATA_WRITE.posy-Setpointy)*(CURRENT_DATA_WRITE.posy-Setpointy) );
// int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, MAX_VEL);
speed = speed > 30 ? speed : 0;
dir = filterDir->calculate(dir);;
//speed = filterSpeed->calculate(speed);
int dist = sqrt( ( pow(CURRENT_DATA_WRITE.posx-Setpointx,2) ) + pow(CURRENT_DATA_WRITE.posy-Setpointy, 2) );
int speed = dist*DIST_MULT;
speed = speed > 10 ? speed : 0;
dir = filterDir->calculate(dir);
#ifdef DRIVE_VECTOR_SUM
vx = ((speed * cosins[dir]));