diff --git a/include/strategy_roles/precision_shooter.h b/include/strategy_roles/precision_shooter.h index 7e6a06f..0a5f946 100644 --- a/include/strategy_roles/precision_shooter.h +++ b/include/strategy_roles/precision_shooter.h @@ -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; }; diff --git a/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h index d9032cc..7abae74 100644 --- a/include/systems/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -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; diff --git a/src/strategy_roles/precision_shooter.cpp b/src/strategy_roles/precision_shooter.cpp index 875ed10..1b3173c 100644 --- a/src/strategy_roles/precision_shooter.cpp +++ b/src/strategy_roles/precision_shooter.cpp @@ -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() { diff --git a/src/strategy_roles/striker.cpp b/src/strategy_roles/striker.cpp index 33c1d39..9833fb2 100644 --- a/src/strategy_roles/striker.cpp +++ b/src/strategy_roles/striker.cpp @@ -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; diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index 882b82e..be12d8c 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -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]));