diff --git a/include/strategy_roles/corner_kick_1.h b/include/strategy_roles/corner_kick_1.h deleted file mode 100644 index 50a6b9d..0000000 --- a/include/strategy_roles/corner_kick_1.h +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#include "systems/position/positionsys_camera.h" -#include "systems/lines/linesys_camera.h" -#include "sensors/sensors.h" -#include "strategy_roles/game.h" -#include "strategy_roles/games.h" -#include "behaviour_control/status_vector.h" -#include "sensors/data_source_ball.h" -#include "strategy_roles/striker.h" -#include "sensors/data_source_ball_presence.h" -#include "sensors/data_source_bt.h" -#include "vars.h" - -class CornerKick : public Game{ - public: - CornerKick(); - CornerKick(LineSystem* ls, PositionSystem* ps); - - void init() override; - void realPlay() override; - void kick(); - unsigned long kicktimer = 0, debounce_timer = 0; - int kick_state = 99; - bool kick_flag = false; - - int old_btn = 0, new_val = 0; - ComplementaryFilter* ballAngleFilter = new ComplementaryFilter(0.85); -}; \ No newline at end of file diff --git a/include/strategy_roles/corner_kick_2.h b/include/strategy_roles/corner_kick_2.h deleted file mode 100644 index 6545cb0..0000000 --- a/include/strategy_roles/corner_kick_2.h +++ /dev/null @@ -1,43 +0,0 @@ -#pragma once - -#include "sensors/data_source_camera_vshapedmirror.h" -#include "sensors/sensors.h" -#include "strategy_roles/game.h" - - -#define CK2_SPINNER_OVERHEAD 10 - -#define CK2_KICK_LIMIT_TILT1 200 -#define CK2_KICK_LIMIT_MAX 315 -#define CK2_KICK_LIMIT_MIN 45 - -class CornerKick2 : public Game{ - - public: - CornerKick2(); - CornerKick2(LineSystem* ls, PositionSystem* ps); - - private: - void realPlay() override; - void init() override; - - void catchBall(); - void spinner(int); - - unsigned long timer = 0; - int state = 0; - - int ball_catch_state = 0; - int ball_catch_timer = 0; - bool ball_catch_flag = false; - float ball_catch_tilt = 0; - - int goal_coords = 0; - - ComplementaryFilter* ballAngleFilter = new ComplementaryFilter(0.85); - - int limitx = 0, spinner_state = 0; - bool spinner_flag = false; - float tilt1 = 0, tilt2 = 0, spinner_tilt = 0; - unsigned long spinner_timer = 0; -}; diff --git a/include/strategy_roles/games.h b/include/strategy_roles/games.h index 4896313..40085e1 100644 --- a/include/strategy_roles/games.h +++ b/include/strategy_roles/games.h @@ -9,15 +9,7 @@ #include #include "strategy_roles/game.h" #include "strategy_roles/striker.h" -#include "strategy_roles/striker_roller.h" -#include "strategy_roles/precision_shooter.h" -#include "strategy_roles/pass_and_shoot.h" -#include "strategy_roles/spot_finder.h" -#include "strategy_roles/the_spinner.h" -#include "strategy_roles/round_robin.h" -#include "strategy_roles/corner_kick_2.h" -#include "strategy_roles/corner_kick_1.h" -// #include "strategy_roles/keeper.h" +#include "strategy_roles/keeper.h" void initGames(); @@ -25,7 +17,7 @@ g_extr Game* striker; g_extr Game* striker_roller; g_extr Game* precision_shooter; g_extr Game* pass_and_shoot; -// g_extr Game* keeper; +g_extr Game* keeper; g_extr Game* tc1; g_extr Game* tc2; diff --git a/include/strategy_roles/pass_and_shoot.h b/include/strategy_roles/pass_and_shoot.h deleted file mode 100644 index dee366b..0000000 --- a/include/strategy_roles/pass_and_shoot.h +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -#include "sensors/data_source_camera_vshapedmirror.h" -#include "sensors/sensors.h" -#include "strategy_roles/game.h" - -#define PAS_ATTACK_DISTANCE 110 -#define PAS_TILT_STOP_DISTANCE 140 -#define PAS_PLUSANG 55 -#define PAS_PLUSANG_VISIONCONE 10 - -class PassAndShoot : public Game{ - - public: - PassAndShoot(); - PassAndShoot(LineSystem* ls, PositionSystem* ps); - - private: - void realPlay() override; - void init() override; - void striker(); - int tilt(); - - int atk_speed, atk_direction, atk_tilt; - float cstorc; - unsigned long pass_counter; - bool gotta_tilt; -}; diff --git a/include/strategy_roles/precision_shooter.h b/include/strategy_roles/precision_shooter.h deleted file mode 100644 index 164b4e6..0000000 --- a/include/strategy_roles/precision_shooter.h +++ /dev/null @@ -1,50 +0,0 @@ -#pragma once - -#include "sensors/data_source_camera_vshapedmirror.h" -#include "behaviour_control/complementary_filter.h" -#include "sensors/sensors.h" -#include "strategy_roles/game.h" - -#define PS_ATTACK_DISTANCE 110 -#define PS_TILT_STOP_DISTANCE 140 -#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 7 - -#define PS_KICK_LIMIT_TILT1 200 -#define PS_KICK_LIMIT_MAX 315 -#define PS_KICK_LIMIT_MIN 45 - -class PrecisionShooter : public Game{ - - public: - PrecisionShooter(); - PrecisionShooter(LineSystem* ls, PositionSystem* ps); - - private: - void realPlay() override; - void init() override; - void catchBall(); - void spinner(int); - int tilt(); - - private: - int atk_speed, atk_direction, atk_tilt; - float cstorc; - bool gotta_tilt; - ComplementaryFilter* ballAngleFilter; - - float tilt1 = 0; - float tilt2 = 0; - int spinner_state = 0; - bool spinner_flag = false; - unsigned long spinner_timer = 0; - float spinner_tilt = 0; - int ball_catch_state = 0; - bool ball_catch_flag = false; - unsigned long ball_catch_timer = 0; - float ball_catch_tilt = 0; - int limitx = 0; -}; diff --git a/include/strategy_roles/round_robin.h b/include/strategy_roles/round_robin.h deleted file mode 100644 index 6794300..0000000 --- a/include/strategy_roles/round_robin.h +++ /dev/null @@ -1,57 +0,0 @@ -#pragma once - -#include "sensors/data_source_camera_vshapedmirror.h" -#include "behaviour_control/complementary_filter.h" -#include "sensors/sensors.h" -#include "strategy_roles/game.h" - -#define RR_ATTACK_DISTANCE 110 -#define RR_TILT_STOP_DISTANCE 140 -#define RR_PLUSANG 55 -#define RR_PLUSANG_VISIONCONE 10 - -// There needs to be a little bit of space between the target point and the spot to be in -#define RR_SPINNER_OVERHEAD 7 - -#define RR_KICK_LIMIT_TILT1 200 -#define RR_KICK_LIMIT_MAX 315 -#define RR_KICK_LIMIT_MIN 45 - -#define ROUND_ROBIN_VEL 30 -#define RR_YCOORD -5 -#define RR_ROLLER_SPD 1500 - -class RoundRobin : public Game{ - - public: - RoundRobin(); - RoundRobin(LineSystem* ls, PositionSystem* ps); - - private: - void realPlay() override; - void init() override; - void catchBall(); - void spinner(int); - void push(); - void spinnerStep(); - - private: - int atk_speed, atk_direction, atk_tilt; - float cstorc; - bool gotta_tilt; - ComplementaryFilter* ballAngleFilter; - - int flip = 0; - - float tilt1 = 0; - float tilt2 = 0; - int spinner_state = 0; - bool spinner_flag = false; - unsigned long spinner_timer = 0; - float spinner_tilt = 0; - int ball_catch_state = 0; - bool ball_catch_flag = false; - unsigned long ball_catch_timer = 0; - float ball_catch_tilt = 0; - int limitx = 0; -}; diff --git a/include/strategy_roles/spot_finder.h b/include/strategy_roles/spot_finder.h deleted file mode 100644 index 05cde28..0000000 --- a/include/strategy_roles/spot_finder.h +++ /dev/null @@ -1,21 +0,0 @@ -#pragma once - -#include "sensors/data_source_camera_vshapedmirror.h" -#include "sensors/sensors.h" -#include "strategy_roles/game.h" - -#define X_COORD 10 -#define Y_COORD 15 - -class SpotFinder : public Game{ - - public: - SpotFinder(); - SpotFinder(LineSystem* ls, PositionSystem* ps); - - private: - void realPlay() override; - void init() override; - - unsigned long t = 0; -}; diff --git a/include/strategy_roles/striker_roller.h b/include/strategy_roles/striker_roller.h deleted file mode 100644 index d6d48e8..0000000 --- a/include/strategy_roles/striker_roller.h +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once - -#include "sensors/data_source_camera_vshapedmirror.h" -#include "behaviour_control/complementary_filter.h" -#include "sensors/sensors.h" -#include "strategy_roles/game.h" - -#define STRL_ATTACK_DISTANCE 110 -#define STRL_TILT_STOP_DISTANCE 140 -#define STRL_PLUSANG 55 -#define STRL_PLUSANG_VISIONCONE 10 - -// There needs to be a little bit of space between the target point and the spot to be in -#define STRL_SPINNER_OVERHEAD 7 - -#define STRL_KICK_LIMIT_TILT1 180 -#define STRL_KICK_LIMIT_MAX 300 -#define STRL_KICK_LIMIT_MIN 60 - -class StrikerRoller : public Game{ - - public: - StrikerRoller(); - StrikerRoller(LineSystem* ls, PositionSystem* ps); - - private: - void realPlay() override; - void init() override; - void catchBall(); - void spinner(int); - int tilt(); - - private: - int atk_speed, atk_direction, atk_tilt; - float cstorc; - bool gotta_tilt; - ComplementaryFilter* ballAngleFilter; - - float tilt1 = 0; - float tilt2 = 0; - int spinner_state = 0; - bool spinner_flag = false; - unsigned long spinner_timer = 0; - float spinner_tilt = 0; - int ball_catch_state = 0; - bool ball_catch_flag = false; - unsigned long ball_catch_timer = 0; - float ball_catch_tilt = 0; - int limitx = 0; - unsigned long spinner_end_time = 0; - bool spinner_end_flag = false; - -}; diff --git a/include/strategy_roles/the_spinner.h b/include/strategy_roles/the_spinner.h deleted file mode 100644 index d6f4f81..0000000 --- a/include/strategy_roles/the_spinner.h +++ /dev/null @@ -1,56 +0,0 @@ -#pragma once - -#include "sensors/data_source_camera_vshapedmirror.h" -#include "sensors/sensors.h" -#include "strategy_roles/game.h" - -#define X_COORD 10 -#define Y_COORD 15 - -class Spinner : public Game{ - - public: - Spinner(); - Spinner(LineSystem* ls, PositionSystem* ps); - - private: - void realPlay() override; - void init() override; - void circle(); - bool doingCircle = false; - bool firstCircle = true; - bool flag = false; - unsigned long t =0; - - int step = 0; - - typedef struct v{ - v(){ - x = 0; - y = 0; - } - v(int x_, int y_){ - x = x_; - y = y_; - } - int x, y; - } spot; - - /* - spot(13, 15), //top right spot - spot(-13, 15), //top left spot - spot(-13, -15) //bottom left spot - spot(13, -15), //bottom right spot - */ - - vector spots = { - /*START: centre*/ - spot(0,0), - - // /*1ST BOTTLE*/ - spot(20, 15), - }; - - int current_spot_i = 0; - spot current_spot; -}; diff --git a/include/systems/lines/linesys_camera_recovery.h b/include/systems/lines/linesys_camera_recovery.h deleted file mode 100644 index d0dc6d7..0000000 --- a/include/systems/lines/linesys_camera_recovery.h +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once - -#include - -#include "behaviour_control/ds_ctrl.h" -#include "systems/systems.h" - -#include "vars.h" - -#define S1O A7 -#define S1I A6 -#define S2O A2 -#define S2I A3 -#define S3I A9 -#define S3O A8 -#define S4I A0 -#define S4O A1 - -#define LINE_THRESH_CAM_REC 350 -#define EXIT_TIME_REC 300 - -#define X_LIMIT 4 -#define Y_LIMIT 5 - -class LineSysCameraRecovery : public LineSystem{ - - public: - LineSysCameraRecovery(); - LineSysCameraRecovery(vector in_, vector out); - - void update() override; - void test() override; - void outOfBounds(); - void checkLineSensors(); - void striker(); - - bool isInLimitX(); - bool isInLimitY(); - - private: - vector in, out; - DataSource* ds; - bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow; - int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], linepins[4], i; - unsigned long exitTimer; - int outDir, outVel; - byte linesens; - int outX, outY; -}; \ No newline at end of file diff --git a/include/systems/lines/linesys_camera_roundrobin.h b/include/systems/lines/linesys_camera_roundrobin.h deleted file mode 100644 index 5e1ea9d..0000000 --- a/include/systems/lines/linesys_camera_roundrobin.h +++ /dev/null @@ -1,41 +0,0 @@ -#pragma once - -#include - -#include "behaviour_control/ds_ctrl.h" -#include "systems/systems.h" - -#include "vars.h" - -#define S1O A7 -#define S1I A6 -#define S2O A2 -#define S2I A3 -#define S3I A9 -#define S3O A8 -#define S4I A0 -#define S4O A1 - -#define LINE_THRESH_CAM 350 -#define EXIT_TIME 300 - -class LineSysCameraRR : public LineSystem{ - - public: - LineSysCameraRR(); - LineSysCameraRR(vector in_, vector out); - - void update() override; - void test() override; - void outOfBounds(); - void checkLineSensors(); - - public: - bool tookLine = false, flag = false; - int linetriggerI[4], linetriggerO[4]; - - private: - vector in, out; - DataSource* ds; - int inV, outV, linesensOldX, linesensOldY, value, linepins[4], i; -}; \ No newline at end of file diff --git a/src/sensors/data_source_bt.cpp b/src/sensors/data_source_bt.cpp index b050c98..6d38703 100644 --- a/src/sensors/data_source_bt.cpp +++ b/src/sensors/data_source_bt.cpp @@ -50,9 +50,9 @@ void DataSourceBT::send(){ void DataSourceBT::update(){ // if(!bt_bombarded && can_bombard) connect(); - receive(); - send(); - if(comrade)Serial2.write(0b00000100); + // receive(); + // send(); + // if(comrade)Serial2.write(0b00000100); } void DataSourceBT :: test(){ diff --git a/src/strategy_roles/corner_kick_1.cpp b/src/strategy_roles/corner_kick_1.cpp deleted file mode 100644 index 04bb887..0000000 --- a/src/strategy_roles/corner_kick_1.cpp +++ /dev/null @@ -1,82 +0,0 @@ -#include "strategy_roles/corner_kick_1.h" -#include - -CornerKick::CornerKick() : Game(){ - init(); -} - -CornerKick::CornerKick(LineSystem *ls_, PositionSystem *ps_) : Game(ls_,ps_){ - init(); -} - -Bounce b = Bounce(); // Instantiate a Bounce object - -void CornerKick::init(){ - b.attach(37, INPUT); - b.interval(25); -} - -void CornerKick::realPlay(){ - b.update(); - if(CURRENT_DATA_READ.ballSeen){ - if(b.fell()) kick_state = 0; - this->kick(); - } - else drive->prepareDrive(0,0,0); -} - -void CornerKick::kick(){ - //DEBUG.println(kick_state); - if(kick_state == 0){ - kicktimer = millis(); - kick_state++; - kick_flag = false; - } else if(kick_state == 1){ - drive->prepareDrive(0, 0, CURRENT_DATA_READ.ballAngleFix); - if((CURRENT_DATA_READ.ballAngle >= 350 || CURRENT_DATA_READ.ballAngle <= 10) && millis() - kicktimer > 1000) kick_state ++; - } else if(kick_state==2){ - drive->prepareDrive(0, 100, 350); - if(ball->isInMouth()){ - if(!kick_flag) { - kick_flag = true; - kicktimer = millis(); - }else{ - if(kick_flag && millis() - kicktimer > 425 ){ - kick_flag = false; - kick_state++; - kicktimer = millis(); - } - } - } - } else if(kick_state==3){ - drive->prepareDrive(270, 60,0); - if(millis()-kicktimer > 300){ - kick_state++; - } - } else if(kick_state==4){ - if(((PositionSysCamera*)ps)->isAtDistanceFrom(1, -28, 2)) { - kick_state++; - kicktimer = millis(); - }else (((PositionSysCamera*)ps)->setMoveSetpoints(1, -28)); - } else if(kick_state == 5){ - drive->prepareDrive(0,0,0); - bt->can_send = true; - bt->tosend = 'C'; - if(millis() - kicktimer > 500){ - kick_state++; - tone(BUZZER, 220, 250); - kicktimer = millis(); - } - } else if(kick_state == 6){ - bt->can_send = true; - bt->tosend = 'B'; - if(millis() - kicktimer > 750){ - kick_state++; - kicktimer = millis(); - bt->can_send = false; - } - }else if (kick_state == 7) { - bt->can_send = false; - drive->prepareDrive(0,0,0); - } -} \ No newline at end of file diff --git a/src/strategy_roles/corner_kick_2.cpp b/src/strategy_roles/corner_kick_2.cpp deleted file mode 100644 index 8a5cc20..0000000 --- a/src/strategy_roles/corner_kick_2.cpp +++ /dev/null @@ -1,155 +0,0 @@ -#include "strategy_roles/corner_kick_2.h" -#include "systems/position/positionsys_camera.h" -#include "vars.h" -#include "math.h" - -CornerKick2::CornerKick2() : Game() {} -CornerKick2::CornerKick2(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) {} - -void CornerKick2::init() {} - -void CornerKick2::realPlay() { - if(bt->received == 'C') { - bt->can_send = true; - bt->tosend = 'B'; - bt->can_send = false; - - tone(BUZZER, 320, 250); - timer = millis(); - state=1; - - ball_catch_state = 0; - ball_catch_tilt = 0; - spinner_state = 0; - spinner_tilt = 0; - ball_catch_flag = false; - spinner_flag = false; - } - if(state == 1){ - drive->prepareDrive(265, 50, 0); - if(CURRENT_DATA_READ.bSeen && millis() - timer > 1000) { - state ++; - timer = millis(); - } - }else if(state == 2){ - drive->prepareDrive(315, 50, 0); - if(millis() - timer > 400) state++; - }else if(state == 3){ - catchBall(); - }else if(state == 4){ - spinner(-5); - }else if(state == 5){ - drive->prepareDrive(0,0,0); - } -} - -void CornerKick2::catchBall(){ - - if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX); - else roller->speed(roller->MIN); - - if(!ball->isInFront()){ - ball_catch_state = 0; - ball_catch_flag = false; - } - - if(ball_catch_state == 0){ - drive->prepareDrive(0, MAX_VEL_HALF, 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 > 350) { - 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.0075; - else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.0075; - - drive->prepareDrive(0,0,ball_catch_tilt); - - if(CURRENT_DATA_READ.IMUAngle >= 355 || CURRENT_DATA_READ.IMUAngle <= 5) state++; - // ((PositionSysCamera*)ps)->setMoveSetpoints(CURRENT_DATA_READ.xAtkFix, 0); - // if(((PositionSysCamera*)ps)->isInTheVicinityOf(CURRENT_DATA_READ.xAtkFix, 0)) ball_catch_state++; - } -} - -void CornerKick2::spinner(int targetx){ - if(spinner_state == 0){ - if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX); - - if(ballPresence->isInMouth() && !spinner_flag){ - spinner_flag = true; - spinner_timer = millis(); - } - - if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { - spinner_state=1; - spinner_flag = false; - } - }else if(spinner_state == 1){ - roller->speed(roller->MAX); - - int spotx = targetx; - // if(targetx >= 0) spotx = targetx-CK2_SPINNER_OVERHEAD; - // else spotx = targetx+CK2_SPINNER_OVERHEAD; - - if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, -1)) { - - if( !spinner_flag){ - spinner_flag = true; - spinner_timer = millis(); - } - - if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { - spinner_state=2; - spinner_flag = false; - spinner_tilt = CURRENT_DATA_READ.IMUAngle; - } - - // if(targetx >= 0) { - tilt1 = -0.0075; - tilt2 = 0.55; - - limitx = 360-CK2_KICK_LIMIT_TILT1; - // }else{ - // tilt1 = 0.01; - // tilt2 = -0.55; - - // limitx = CK2_KICK_LIMIT_TILT1; - // } - - }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, -1); - }else if(spinner_state == 2){ - roller->speed(roller->MAX); - - spinner_tilt += tilt1; - drive->prepareDrive(0,0,spinner_tilt); - - if(CURRENT_DATA_READ.IMUAngle > limitx-5 && CURRENT_DATA_READ.IMUAngle < limitx+5) { - spinner_timer = millis(); - spinner_state=3; - } - }else if(spinner_state == 3){ - roller->speed(roller->MAX); - drive->prepareDrive(0,0,spinner_tilt); - if(millis() - spinner_timer > 150) { - spinner_state=4; - spinner_tilt = CURRENT_DATA_READ.IMUAngle; - } - }else if(spinner_state == 4){ - drive->prepareDrive(0,0,0); - - // spinner_tilt += tilt2; - // spinner_tilt = constrain(spinner_tilt, CK2_KICK_LIMIT_MIN, CK2_KICK_LIMIT_MAX); - // drive->prepareDrive(0,0,spinner_tilt); - - if(CURRENT_DATA_READ.IMUAngle >= CK2_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= CK2_KICK_LIMIT_MIN) { - roller->speed(roller->MIN); - state++; - } - } -} \ No newline at end of file diff --git a/src/strategy_roles/games.cpp b/src/strategy_roles/games.cpp index 438e45c..31f6b4b 100644 --- a/src/strategy_roles/games.cpp +++ b/src/strategy_roles/games.cpp @@ -1,8 +1,7 @@ -#define GAMES_CPP + #define GAMES_CPP /* #include "sensors/linesys_2019.h" */ #include "systems/lines/linesys_camera.h" -#include "systems/lines/linesys_camera_roundrobin.h" #include "systems/systems.h" #include "systems/position/positionsys_zone.h" #include "systems/position/positionsys_camera.h" @@ -12,19 +11,6 @@ void initGames(){ vector lIn = { new DataSource(S1I, true), new DataSource(S2I, true), new DataSource(S3I, true), new DataSource(S4I, true) }; vector lOut = { new DataSource(S1O, true), new DataSource(S2O, true), new DataSource(S3O, true), new DataSource(S4O, true) }; - // striker_test = new StrikerTest(new LineSysCamera(lIn, lOut), new PositionSysCamera()); striker = new Striker(new LineSysCamera(lIn, lOut), new PositionSysCamera()); - pass_and_shoot = new PassAndShoot(new LineSysCamera(lIn, lOut), new PositionSysCamera()); - precision_shooter = new PrecisionShooter(new LineSysCamera(lIn, lOut), new PositionSysCamera()); - striker_roller = new StrikerRoller(new LineSysCamera(lIn, lOut), new PositionSysCamera()); - - tc1 = new StrikerRoller(new LineSystemEmpty(), new PositionSysCamera()); - tc2 = new RoundRobin(new LineSysCameraRR(lIn, lOut), new PositionSysCamera()); - - tc3_1 = new CornerKick(new LineSystemEmpty(), new PositionSysCamera()); - tc3_2 = new CornerKick2(new LineSystemEmpty(), new PositionSysCamera()); - - st_tc1 = new SpotFinder(new LineSystemEmpty(), new PositionSysCamera()); - st_tc3 = new Spinner(new LineSystemEmpty(), new PositionSysCamera()); - // keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera()); + keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera()); } \ No newline at end of file diff --git a/src/strategy_roles/pass_and_shoot.cpp b/src/strategy_roles/pass_and_shoot.cpp deleted file mode 100644 index 65b4f8f..0000000 --- a/src/strategy_roles/pass_and_shoot.cpp +++ /dev/null @@ -1,103 +0,0 @@ -#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/pass_and_shoot.h" -#include "vars.h" -#include "math.h" - - -PassAndShoot::PassAndShoot() : Game() -{ - init(); -} - -PassAndShoot::PassAndShoot(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) -{ - init(); -} - -void PassAndShoot::init() -{ - atk_speed = 0; - atk_direction = 0; - atk_tilt = 0; - cstorc = 0; - - gotta_tilt = false; - - pass_counter = millis(); -} - -unsigned long pass_timer = 0; -boolean flag = false; - -void PassAndShoot::realPlay() -{ - if (CURRENT_DATA_READ.ballSeen){ - if(robot_indentifier == HIGH){ - if(ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 90){ - if(!flag){ - pass_timer = millis(); - flag = true; - - //Show on 32u4 - Serial2.write(0b00000100); - } - } - if(!flag || (millis() - pass_timer <= 650)){ - striker(); - }else{ - ((PositionSysCamera*)ps)->setMoveSetpoints(CAMERA_GOAL_MIN_X, CAMERA_GOAL_Y); - bt->tosend = 'C'; - roller->speed(0); - } - }else{ - if(bt->received == 'C'){ - striker(); - }else drive->prepareDrive(0,0,0); - } - }else drive->prepareDrive(0,0,0); - - - -} - -void PassAndShoot::striker(){ - - #ifdef MAX_VEL - #undef MAX_VEL - #define MAX_VEL 100 - #endif - - //seguo palla - int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = PAS_PLUSANG; - - if(ball_deg >= 346 || ball_deg <= 16) plusang = PAS_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); - -} - -int PassAndShoot::tilt() { - if (ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 90 /* || (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; -} \ No newline at end of file diff --git a/src/strategy_roles/precision_shooter.cpp b/src/strategy_roles/precision_shooter.cpp deleted file mode 100644 index ae775d2..0000000 --- a/src/strategy_roles/precision_shooter.cpp +++ /dev/null @@ -1,186 +0,0 @@ -#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/precision_shooter.h" -#include "vars.h" -#include "math.h" - - -PrecisionShooter::PrecisionShooter() : Game() -{ - init(); -} - -PrecisionShooter::PrecisionShooter(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) -{ - init(); -} - -void PrecisionShooter::init() -{ - atk_speed = 0; - atk_direction = 0; - atk_tilt = 0; - cstorc = 0; - - gotta_tilt = false; - - ballAngleFilter = new ComplementaryFilter(0.85); -} - -void PrecisionShooter::realPlay() -{ - if (CURRENT_DATA_READ.ballSeen) - this->spinner(CURRENT_DATA_READ.xAtkFix); - // this->catchBall(); - else{ - ps->goCenter(); - ball_catch_flag = false; - spinner_flag = false; - ball_catch_state = 0; - spinner_state = 0; - } -} - -/* -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 -*/ -void PrecisionShooter::spinner(int targetx){ - // if(!ballPresence->isInMouth()) { - // spinner_state=0; - // spinner_flag = false; - // } - - if(spinner_state == 0){ - if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX); - - if(ballPresence->isInMouth() && !spinner_flag){ - spinner_flag = true; - spinner_timer = millis(); - } - - if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { - spinner_state=1; - spinner_flag = false; - } - }else if(spinner_state == 1){ - roller->speed(roller->MAX); - - int spotx = targetx; - if(targetx >= 0) spotx = targetx-PS_SPINNER_OVERHEAD; - else spotx = targetx+PS_SPINNER_OVERHEAD; - - if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 0)) { - - if( !spinner_flag){ - spinner_flag = true; - spinner_timer = millis(); - } - - if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { - spinner_state=2; - spinner_flag = false; - spinner_tilt = CURRENT_DATA_READ.IMUAngle; - } - - if(targetx >= 0) { - tilt1 = -0.15; - tilt2 = 0.55; - - limitx = 360-PS_KICK_LIMIT_TILT1; - }else{ - tilt1 = 0.15; - tilt2 = -0.55; - - limitx = PS_KICK_LIMIT_TILT1; - } - - }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0); - }else if(spinner_state == 2){ - roller->speed(roller->MAX); - - spinner_tilt += tilt1; - drive->prepareDrive(0,0,spinner_tilt); - - if(CURRENT_DATA_READ.IMUAngle > limitx-5 && CURRENT_DATA_READ.IMUAngle < limitx+5) { - spinner_timer = millis(); - spinner_state=3; - } - }else if(spinner_state == 3){ - roller->speed(roller->MAX); - drive->prepareDrive(0,0,spinner_tilt); - if(millis() - spinner_timer > 150) { - spinner_state=4; - spinner_tilt = CURRENT_DATA_READ.IMUAngle; - } - }else if(spinner_state == 4){ - drive->prepareDrive(0,0,0); - - // spinner_tilt += tilt2; - // spinner_tilt = constrain(spinner_tilt, PS_KICK_LIMIT_MIN, PS_KICK_LIMIT_MAX); - // drive->prepareDrive(0,0,spinner_tilt); - - if(CURRENT_DATA_READ.IMUAngle >= PS_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= PS_KICK_LIMIT_MIN) roller->speed(roller->MIN); - } -} -/* -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 -*/ - -void PrecisionShooter::catchBall(){ - - if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX); - else roller->speed(roller->MIN); - - if(!ball->isInFront()){ - ball_catch_state = 0; - ball_catch_flag = false; - } - - if(ball_catch_state == 0){ - drive->prepareDrive(0, MAX_VEL_HALF, 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 > 350) { - ball_catch_state++; - ball_catch_tilt = CURRENT_DATA_READ.IMUAngle; - } - }else if(ball_catch_state == 1){ - int val = ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix); - drive->prepareDrive(drive->directionAccountingForTilt(CURRENT_DATA_READ.angleAtkFix, val ),MAX_VEL_HALF,val); - - // if(ball_catch_tilt > 180) ball_catch_tilt += 0.15; - // else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.15; - - // drive->prepareDrive(0,0,ball_catch_tilt); - - // if(CURRENT_DATA_READ.IMUAngle >= 355 || CURRENT_DATA_READ.IMUAngle <= 5) ball_catch_state = 0; - // ((PositionSysCamera*)ps)->setMoveSetpoints(CURRENT_DATA_READ.xAtkFix, 0); - // if(((PositionSysCamera*)ps)->isInTheVicinityOf(CURRENT_DATA_READ.xAtkFix, 0)) ball_catch_state++; - } -} - -int PrecisionShooter::tilt() { - if (ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 90 /* || (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; -} \ No newline at end of file diff --git a/src/strategy_roles/round_robin.cpp b/src/strategy_roles/round_robin.cpp deleted file mode 100644 index 22de3c6..0000000 --- a/src/strategy_roles/round_robin.cpp +++ /dev/null @@ -1,324 +0,0 @@ -#include "behaviour_control/status_vector.h" -#include "systems/position/positionsys_camera.h" -#include "systems/lines/linesys_camera_roundrobin.h" -#include "sensors/sensors.h" -#include "sensors/data_source_ball.h" -#include "strategy_roles/round_robin.h" -#include "vars.h" -#include "math.h" - - -RoundRobin::RoundRobin() : Game() -{ - init(); -} - -RoundRobin::RoundRobin(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) -{ - init(); -} - -void RoundRobin::init() -{ - atk_speed = 0; - atk_direction = 0; - atk_tilt = 0; - cstorc = 0; - - gotta_tilt = false; - - ballAngleFilter = new ComplementaryFilter(0.1); -} - -void RoundRobin::realPlay() -{ - if (CURRENT_DATA_READ.ballSeen) - this->catchBall(); - // this->spinner(0); - // this->push(); - else{ - ps->goCenter(); - ball_catch_flag = false; - spinner_flag = false; - ball_catch_state = 0; - spinner_state = 0; - } -} - -/* -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 -*/ -void RoundRobin::spinner(int targetx){ - - if(spinner_state == 0){ - if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX); - - if(ballPresence->isInMouth() && !spinner_flag){ - spinner_flag = true; - spinner_timer = millis(); - } - - if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { - spinner_state=1; - spinner_flag = false; - } - }else if(spinner_state == 1){ - roller->speed(roller->MAX); - - int spotx = targetx; - if(targetx >= 0) spotx = targetx-RR_SPINNER_OVERHEAD; - else spotx = targetx+RR_SPINNER_OVERHEAD; - - // if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 0)) { - - // if( !spinner_flag){ - // spinner_flag = true; - // spinner_timer = millis(); - // } - - // if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { - spinner_state=2; - // spinner_flag = false; - // spinner_tilt = CURRENT_DATA_READ.IMUAngle; - // } - - // if(targetx >= 0) { - tilt1 = -0.15; - tilt2 = -35; - - limitx = 360-RR_KICK_LIMIT_TILT1; - // }else{ - // tilt1 = 0.15; - // tilt2 = -0.55; - - // limitx = RR_KICK_LIMIT_TILT1; - // } - - // }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0); - }else if(spinner_state == 2){ - roller->speed(roller->MAX); - - spinner_tilt += tilt1; - drive->prepareDrive(0,0,spinner_tilt); - - if(CURRENT_DATA_READ.IMUAngle > limitx-5 && CURRENT_DATA_READ.IMUAngle < limitx+5) { - spinner_timer = millis(); - spinner_state=3; - } - }else if(spinner_state == 3){ - roller->speed(roller->MAX); - drive->prepareDrive(0,0,spinner_tilt); - if(millis() - spinner_timer > 150) { - spinner_state=4; - spinner_tilt = CURRENT_DATA_READ.IMUAngle; - } - }else if(spinner_state == 4){ - // drive->prepareDrive(0,0,0); - spinner_tilt += tilt2; - spinner_tilt = constrain(spinner_tilt, RR_KICK_LIMIT_MIN, RR_KICK_LIMIT_MAX); - drive->prepareDrive(0,0,spinner_tilt); - - if(CURRENT_DATA_READ.IMUAngle >= RR_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= RR_KICK_LIMIT_MIN) { - ball_catch_state++; - - roller->speed(roller->MIN); - } - } -} - - -void RoundRobin::spinnerStep(){ - spinner_state++; - spinner_timer = millis(); -} - -/*void RoundRobin::push(){ - if(spinner_state == 0){ - if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX); - - if(ballPresence->isInMouth() && !spinner_flag){ - spinner_flag = true; - spinner_timer = millis(); - } - - if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { - spinner_state=1; - spinner_flag = false; - } - }else if(spinner_state == 1){ - roller->speed(roller->MAX); - - if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -6)) spinner_state=4; - else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -6); - // } else if(spinner_state == 2){ - // roller->speed(roller->MAX); - // if(((PositionSysCamera*)ps)->isInTheVicinityOf(16, -4)) spinnerStep(); - // else ((PositionSysCamera*)ps)->setMoveSetpoints(16, -4); - // }else if(spinner_state == 3){ - // drive->prepareDrive(0,0,0); - // if(millis() - spinner_timer > 750) spinnerStep(); - }else if(spinner_state == 4){ - drive->prepareDrive(45, 50, 0); - if( ((LineSysCameraRR*)ls)->tookLine ) spinnerStep(); - }else if(spinner_state == 5){ - roller->speed(roller->MIN); - if(millis()-spinner_timer > 1500) spinnerStep(); - }else if(spinner_state == 6){ - drive->prepareDrive(180, 30, 0); - if(millis() - spinner_timer > 750) spinnerStep(); - }else if(spinner_state == 7){ - roller->speed(roller->MIN); - drive->prepareDrive(0, 0, 55); - if(millis() - spinner_timer > 1000) spinnerStep(); - }else if(spinner_state == 8){ - drive->prepareDrive(drive->directionAccountingForTilt(0, 55), 100, 55); - if(millis() - spinner_timer > 500) spinnerStep(); - }else if(spinner_state == 9){ - drive->prepareDrive(drive->directionAccountingForTilt(180, 55), 50, 55); - if(millis() - spinner_timer > 750) spinnerStep(); - }else if(spinner_state == 10){ - if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -8)) spinnerStep(); - else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -8); - }else ball_catch_state++; -}*/ -void RoundRobin::push(){ - if(spinner_state == 0){ - if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX); - - if(ballPresence->isInMouth() && !spinner_flag){ - spinner_flag = true; - spinner_timer = millis(); - } - - if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) { - spinner_state=1; - spinner_flag = false; - } - }else if(spinner_state == 1){ - roller->speed(roller->MAX); - - // if(flip) - // if(CURRENT_DATA_READ.yDist <= 24) drive->prepareDrive(0, 50, 0); - // if(CURRENT_DATA_READ.yDist >= 36) drive->prepareDrive(180, 50, 0); - // else{ - // if(CURRENT_DATA_READ.bDist <= 24) drive->prepareDrive(0, 50, 0); - // if(CURRENT_DATA_READ.bDist >= 36) drive->prepareDrive(180, 50, 0); - // } - - if(flip==0){ - if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -3)) spinnerStep(); - else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -3); - }else{ - if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -12)) spinnerStep(); - else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -12); - } - } else if(spinner_state == 2){ - // roller->speed(roller->MAX); - // if(((PositionSysCamera*)ps)->isInTheVicinityOf(16, -4)) spinnerStep(); - // else ((PositionSysCamera*)ps)->setMoveSetpoints(16, -4); - // }else if(spinner_state == 3){ - // drive->prepareDrive(0,0,0); - // if(millis() - spinner_timer > 750) spinnerStep(); - // }else if(spinner_state == 4){ - // drive->prepareDrive(45, 50, 0); - drive->prepareDrive(70, 40, 0); - if( ((LineSysCameraRR*)ls)->tookLine ) { - spinner_state = 5; - spinner_timer = millis(); - } - }else if(spinner_state == 5){ - roller->speed(roller->MIN); - if(millis()-spinner_timer > 1500) spinnerStep(); - }else if(spinner_state == 6){ - drive->prepareDrive(180, 30, 0); - if(millis() - spinner_timer > 650) spinnerStep(); - }else if(spinner_state == 7){ - roller->speed(roller->MIN); - drive->prepareDrive(0, 0, 55); - if(millis() - spinner_timer > 1000) spinnerStep(); - }else if(spinner_state == 8){ - drive->prepareDrive(drive->directionAccountingForTilt(0, 90), 100, 90); - if(millis() - spinner_timer > 400) spinnerStep(); - }else if(spinner_state == 9){ - drive->prepareDrive(drive->directionAccountingForTilt(180, 90), 50, 90); - if(millis() - spinner_timer > 750) spinnerStep(); - }else if(spinner_state == 10){ - if(flip==0){ - if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -4)) spinnerStep(); - else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -4); - }else{ - if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -13)) spinnerStep(); - else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -13); - } - }else ball_catch_state++; -} - -/* -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 -*/ - -void RoundRobin::catchBall(){ - - // if(!ball->isInFront() && ball_catch_state != 3){ - // ball_catch_state = 0; - // ball_catch_flag = false; - // ball_catch_tilt = 0; - // } - - if(ball_catch_state == 0){ - ((LineSysCameraRR*)ls)->flag = true; - - if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX); - else roller->speed(roller->MIN); - - drive->prepareDrive(0, ROUND_ROBIN_VEL, 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 > 50) { - ball_catch_state++; - ball_catch_tilt = CURRENT_DATA_READ.IMUAngle; - } - }else if(ball_catch_state == 1){ - if(ball_catch_tilt > 180 && ball_catch_tilt < 360) ball_catch_tilt += 0.15; - else if(ball_catch_tilt <= 180 && ball_catch_tilt > 0) ball_catch_tilt -= 0.15; - - drive->prepareDrive(0,0,ball_catch_tilt); - - if(CURRENT_DATA_READ.IMUAngle >= 350 || CURRENT_DATA_READ.IMUAngle <= 5) ball_catch_state = 2; - }else if(ball_catch_state == 2){ - spinner_tilt = 0; - spinner_flag = false; - spinner_timer = 0; - spinner_state = 1; - ball_catch_state = 3; - - ((LineSysCameraRR*)ls)->flag = false; - }else if(ball_catch_state == 3){ - // this->spinner(25); - this->push(); - }else if(ball_catch_state == 4){ - drive->prepareDrive(270, 50, 0); - if(((LineSysCameraRR*)ls)->tookLine && CURRENT_DATA_READ.posx <= -10) ball_catch_state = 5; - }else if(ball_catch_state == 5){ - if( ((LineSysCameraRR*)ls)->linetriggerI[1] || ((LineSysCameraRR*)ls)->linetriggerO[1] > 0) drive->prepareDrive(90, 50, 0); //with 2 and 4 you go right or left accordingly - else if( ((LineSysCameraRR*)ls)->linetriggerI[3] || ((LineSysCameraRR*)ls)->linetriggerO[3] > 0 ) drive->prepareDrive(270, 50, 0); - else drive->prepareDrive(0, 30, 0); - if(CURRENT_DATA_READ.posy > 7) { - ball_catch_state=0; - - flip = 1-flip; - - CURRENT_DATA_WRITE.IMUOffset = 180 * flip; - } - }else if(ball_catch_state==6) drive->prepareDrive(0,0,0); -} diff --git a/src/strategy_roles/spot_finder.cpp b/src/strategy_roles/spot_finder.cpp deleted file mode 100644 index 01558ea..0000000 --- a/src/strategy_roles/spot_finder.cpp +++ /dev/null @@ -1,33 +0,0 @@ -#include "strategy_roles/spot_finder.h" -#include "systems/position/positionsys_camera.h" -#include "vars.h" -#include "math.h" - -SpotFinder::SpotFinder() : Game() {} -SpotFinder::SpotFinder(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) {} - -void SpotFinder::init() {} -void SpotFinder::realPlay() { - if(millis() - t > 150){ - - int zone = 6; - - if(CURRENT_DATA_READ.posx >= -X_COORD && CURRENT_DATA_READ.posx <= X_COORD && CURRENT_DATA_READ.posy >= -Y_COORD && CURRENT_DATA_READ.posy <= Y_COORD) BALL_32U4.write(0b000000110); //center - else if(CURRENT_DATA_READ.posx <= -X_COORD && CURRENT_DATA_READ.posy <= -Y_COORD) BALL_32U4.write(0b00000100); //bottom left - else if(CURRENT_DATA_READ.posx <= -X_COORD && CURRENT_DATA_READ.posy >= Y_COORD) BALL_32U4.write(2); //top left - else if(CURRENT_DATA_READ.posx >= X_COORD && CURRENT_DATA_READ.posy <= -Y_COORD) BALL_32U4.write(0b00000001); //bottom right - else if(CURRENT_DATA_READ.posx >= X_COORD && CURRENT_DATA_READ.posy >= Y_COORD) BALL_32U4.write(5); //top right - else BALL_32U4.write(0); - - // if( ((PositionSysCamera*)ps)->isInRoughVicinityOf(-X_COORD,-Y_COORD) ) BALL_32U4.write(1); //bottom left - // else if( ((PositionSysCamera*)ps)->isInRoughVicinityOf(-X_COORD,Y_COORD) ) BALL_32U4.write(2); //top left - // else if( ((PositionSysCamera*)ps)->isInRoughVicinityOf(0,0) ) BALL_32U4.write(3); //center - // else if( ((PositionSysCamera*)ps)->isInRoughVicinityOf(X_COORD,-Y_COORD) ) BALL_32U4.write(4); //bottom right - // else if( ((PositionSysCamera*)ps)->isInRoughVicinityOf(X_COORD,Y_COORD) ) BALL_32U4.write(5); //top right - - t = millis(); - - - } - drive->stopAll(); -} \ No newline at end of file diff --git a/src/strategy_roles/striker_roller.cpp b/src/strategy_roles/striker_roller.cpp deleted file mode 100644 index ba08e1d..0000000 --- a/src/strategy_roles/striker_roller.cpp +++ /dev/null @@ -1,238 +0,0 @@ -#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_roller.h" -#include "vars.h" -#include "math.h" - - -StrikerRoller::StrikerRoller() : Game() -{ - init(); -} - -StrikerRoller::StrikerRoller(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) -{ - init(); -} - -void StrikerRoller::init() -{ - atk_speed = 0; - atk_direction = 0; - atk_tilt = 0; - cstorc = 0; - - gotta_tilt = false; - - ballAngleFilter = new ComplementaryFilter(0); -} - -void StrikerRoller::realPlay() -{ - if (CURRENT_DATA_READ.ballSeen) - this->catchBall(); - else{ - ps->goCenter(); - ball_catch_flag = false; - spinner_flag = false; - ball_catch_state = 0; - spinner_state = 0; - } -} - -/* -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 -*/ -void StrikerRoller::spinner(int targetx){ - // if(!ballPresence->isInMouth()) { - // spinner_state=0; - // spinner_flag = false; - // } - - if(spinner_state == 0){ - if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX); - - if(ballPresence->isInMouth() && !spinner_flag){ - spinner_flag = true; - spinner_timer = millis(); - } - - if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 50) { - spinner_state=1; - spinner_flag = false; - } - }else if(spinner_state == 1){ - roller->speed(roller->MAX); - - spinner_state=2; - spinner_flag = false; - - // decide with direction to rotate - spinner_tilt = CURRENT_DATA_READ.IMUAngle; - - if(CURRENT_DATA_READ.angleAtk >= 0) { - tilt1 = -0.25; - tilt2 = 5; - limitx = 360-STRL_KICK_LIMIT_TILT1; - }else{ - tilt1 = 0.25; - tilt2 = -5; - limitx = STRL_KICK_LIMIT_TILT1; - } - // if(CURRENT_DATA_READ.posx >= -5 && CURRENT_DATA_READ.posx <= 5){ - // if(CURRENT_DATA_READ.IMUAngle >= 180) { - // tilt1 = -0.25; - // tilt2 = 15; - // limitx = 360-STRL_KICK_LIMIT_TILT1; - // }else{ - // tilt1 = 0.25; - // tilt2 = -15; - // limitx = STRL_KICK_LIMIT_TILT1; - // } - // }else if(CURRENT_DATA_READ.posx < -5){ - // tilt1 = 0.25; - // tilt2 = -5; - // limitx = STRL_KICK_LIMIT_TILT1; - // }else if(CURRENT_DATA_READ.posx > 5){ - // tilt1 = -0.25; - // tilt2 = 5; - // limitx = 360-STRL_KICK_LIMIT_TILT1; - // } - // if(CURRENT_DATA_READ.IMUAngle < limitx) tilt1 *= -1; //we need to invert the rotatiion if we are on the opposite side - - }else if(spinner_state == 2){ - roller->speed(roller->MAX); - - spinner_tilt = spinner_tilt + tilt1; - // This ensures no strange rotation happens - if(spinner_tilt <= -360) spinner_tilt+=360; - if(spinner_tilt >= 360) spinner_tilt -= 360; - - drive->prepareDrive(0,0,spinner_tilt); - - if(CURRENT_DATA_READ.IMUAngle > limitx-5 && CURRENT_DATA_READ.IMUAngle < limitx+5) { - spinner_state=3; - spinner_tilt = CURRENT_DATA_READ.IMUAngle; - } - }else if(spinner_state == 3){ - roller->speed(roller->MAX); - // stay there a little bit - drive->prepareDrive(0,0,spinner_tilt); - - if(millis() - spinner_timer > 150) { - spinner_state=4; - spinner_tilt = CURRENT_DATA_READ.IMUAngle; - } - }else if(spinner_state == 4){ - // fast return which gives momentum to the ball - drive->prepareDrive(0,0,spinner_tilt); - - spinner_tilt += tilt2; - spinner_tilt = constrain(spinner_tilt, 5, 355); - - // turn roller off and kick - if(CURRENT_DATA_READ.IMUAngle >= STRL_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= STRL_KICK_LIMIT_MAX) { - roller->speed(roller->MIN); - if(!spinner_end_flag){ - spinner_end_time = millis(); - spinner_end_flag = true; - } - } - // if(ballPresence->isInMouth()) { - // spinner_state = 1; - // spinner_flag = 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 -*/ - -void StrikerRoller::catchBall(){ - - if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX); - else roller->speed(roller->MIN); - - // if(!ball->isInFront() && millis() - spinner_end_time > 500 && spinner_end_flag){ - // ball_catch_state = 0; - // ball_catch_flag = false; - // spinner_end_flag = false; - // } - if(!ball->isInFront()){ - ball_catch_state = 0; - ball_catch_flag = false; - } - - if(ball_catch_state == 0){ - // tilt torwards the ball until it's in the mouth - drive->prepareDrive(0, MAX_VEL_HALF, 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 > 50) { - ball_catch_state++; - } - }else if(ball_catch_state == 1){ - // go towards the goal without resetting tilt - // this makes it so that the robot sometimes walks backwards, hiding the ball from any opponent - // a spinning kick is the only way to free the ball here - int val = ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix); - - drive->prepareDrive(drive->directionAccountingForTilt(CURRENT_DATA_READ.angleAtkFix, val ),MAX_VEL_HALF,val); - - if(CURRENT_DATA_READ.distAtk <= 26){ - // ball_catch_state++; - // spinner_state = 0; - // spinner_flag = false; - // spinner_tilt = CURRENT_DATA_READ.IMUAngle; - - drive->prepareDrive(0,0,0); - if(CURRENT_DATA_READ.IMUAngle >= STRL_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= STRL_KICK_LIMIT_MAX) roller->speed(roller->MIN); - } - }else if(ball_catch_state == 2){ - // Spinny kick - - // spinner(0); - if(CURRENT_DATA_READ.angleAtkFix >= 0) { - tilt2 = 3; - }else{ - tilt2 = -3; - } - - drive->prepareDrive(0,0,spinner_tilt); - - spinner_tilt += tilt2; - spinner_tilt = constrain(spinner_tilt, 5, 355); - - // turn roller off and kick - if(CURRENT_DATA_READ.IMUAngle >= STRL_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= STRL_KICK_LIMIT_MAX) { - roller->speed(roller->MIN); - if(!spinner_end_flag){ - spinner_end_time = millis(); - spinner_end_flag = true; - } - } - } -} - -int StrikerRoller::tilt() { - if (ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 90 /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true; - else gotta_tilt = false; - - if(!gotta_tilt || !CURRENT_DATA_READ.atkSeen) { - atk_tilt = roller->roller_armed ? CURRENT_DATA_READ.angleAtkFix : constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45); - } - - return atk_tilt; -} \ No newline at end of file diff --git a/src/strategy_roles/the_spinner.cpp b/src/strategy_roles/the_spinner.cpp deleted file mode 100644 index 3de364e..0000000 --- a/src/strategy_roles/the_spinner.cpp +++ /dev/null @@ -1,48 +0,0 @@ -#include "strategy_roles/the_spinner.h" -#include "systems/position/positionsys_camera.h" -#include "vars.h" -#include "math.h" - -Spinner::Spinner() : Game() {} -Spinner::Spinner(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) {} - -void Spinner::init() {} -void Spinner::realPlay() { - if(current_spot_i >= spots.size()) return; - - current_spot = spots[current_spot_i]; - - if(doingCircle){ - circle(); - return; - } - - if (((PositionSysCamera*)ps)->isInTheVicinityOf(current_spot.x, current_spot.y)) { - if(current_spot.x == 0 && current_spot.y == 0) { - current_spot_i++; - return; - } - doingCircle = true; - firstCircle = true; - step = 0; - } - else (((PositionSysCamera*)ps)->setMoveSetpoints(current_spot.x, current_spot.y)); -} - -void Spinner::circle(){ - if(millis() - t < 1000){ - if(step == 0) drive->prepareDrive(45, 50, 0); - else if(step == 1) drive->prepareDrive(315, 50, 0); - else if(step == 2) drive->prepareDrive(225, 50, 0); - else if(step == 3) { - if(firstCircle){ - drive->prepareDrive(135, 80, 0); - firstCircle = false; - }else doingCircle = false; - } - }else { - step++; - t = millis(); - } - -} \ No newline at end of file diff --git a/src/system/lines/linesys_camera_recovery.cpp b/src/system/lines/linesys_camera_recovery.cpp deleted file mode 100644 index 3d495a6..0000000 --- a/src/system/lines/linesys_camera_recovery.cpp +++ /dev/null @@ -1,117 +0,0 @@ -#include "systems/lines/linesys_camera_recovery.h" -#include "systems/position/positionsys_camera.h" -#include "sensors/sensors.h" -#include "strategy_roles/games.h" -#include "behaviour_control/status_vector.h" - -LineSysCameraRecovery::LineSysCameraRecovery(){} -LineSysCameraRecovery::LineSysCameraRecovery(vector in_, vector out_){ - this->in = in_; - this->out = out_; - - fboundsX = false; - fboundsY = false; - slow = false; - - linesensOldX = 0; - linesensOldY = 0; - - tookLine = false; - - for(int i = 0; i < 4; i++){ - linetriggerI[i] = 0; - linetriggerO[i] = 0; - } - - exitTimer = 0; - linesens = 0; -} - - -void LineSysCameraRecovery ::update() { - inV = 0; - outV = 0; - tookLine = false; - - for(DataSource* d : in) d->readSensor(); - for(DataSource* d : out) d->readSensor(); - - for(auto it = in.begin(); it != in.end(); it++){ - i = it - in.begin(); - ds = *it; - linetriggerI[i] = ds->getValue() > LINE_THRESH_CAM; - } - for(auto it = out.begin(); it != out.end(); it++){ - i = it - out.begin(); - ds = *it; - linetriggerO[i] = ds->getValue() > LINE_THRESH_CAM; - } - - for(int i = 0; i < 4; i++){ - inV = inV | (linetriggerI[i] << i); - outV = outV | (linetriggerO[i] << i); - } - - if (inV > 0 || outV > 0) { - if(millis() - exitTimer > EXIT_TIME) { - fboundsX = true; - fboundsY = true; - } - exitTimer = millis(); - } - - linesens |= inV | outV; - outOfBounds(); -} - -void LineSysCameraRecovery::outOfBounds(){ - // digitalWriteFast(BUZZER, LOW); - if(fboundsX == true) { - if(linesens & 0x02) linesensOldX = 2; - else if(linesens & 0x08) linesensOldX = 8; - if(linesensOldX != 0) fboundsX = false; - } - if(fboundsY == true) { - if(linesens & 0x01) linesensOldY = 1; - else if(linesens & 0x04) linesensOldY = 4; - if(linesensOldY != 0) fboundsY = false; - } - - if (millis() - exitTimer < EXIT_TIME){ - CURRENT_DATA_READ.game->ps->goCenter(); - tookLine = true; - tone(BUZZER, 220.00, 250); - }else{ - // drive->canUnlock = true; - linesens = 0; - linesensOldY = 0; - linesensOldX = 0; - } -} - -void LineSysCameraRecovery::test(){ - update(); - DEBUG.print("In: "); - for(DataSource* d : in){ - d->update(); - DEBUG.print(d->getValue()); - DEBUG.print(" | "); - } - DEBUG.print(" |---| "); - DEBUG.print("Out: "); - for(DataSource* d : out){ - d->update(); - DEBUG.print(d->getValue()); - DEBUG.print(" | "); - } - DEBUG.println(); - for(int i = 0; i < 4; i++){ - DEBUG.print(linetriggerI[i]); - DEBUG.print(" | "); - DEBUG.println(linetriggerO[i]); - } - - DEBUG.println(inV); - DEBUG.println(outV); - DEBUG.println(); -} \ No newline at end of file diff --git a/src/system/lines/linesys_camera_roundrobin.cpp b/src/system/lines/linesys_camera_roundrobin.cpp deleted file mode 100644 index 3d53554..0000000 --- a/src/system/lines/linesys_camera_roundrobin.cpp +++ /dev/null @@ -1,77 +0,0 @@ -#include "systems/lines/linesys_camera_roundrobin.h" -#include "systems/position/positionsys_camera.h" -#include "sensors/sensors.h" -#include "strategy_roles/games.h" -#include "behaviour_control/status_vector.h" - -LineSysCameraRR::LineSysCameraRR(){} -LineSysCameraRR::LineSysCameraRR(vector in_, vector out_){ - this->in = in_; - this->out = out_; - - linesensOldX = 0; - linesensOldY = 0; - - tookLine = false; - - for(int i = 0; i < 4; i++){ - linetriggerI[i] = 0; - linetriggerO[i] = 0; - } -} - - -void LineSysCameraRR ::update() { - inV = 0; - outV = 0; - tookLine = false; - - for(DataSource* d : in) d->readSensor(); - for(DataSource* d : out) d->readSensor(); - - for(auto it = in.begin(); it != in.end(); it++){ - i = it - in.begin(); - ds = *it; - linetriggerI[i] = ds->getValue() > LINE_THRESH_CAM; - } - for(auto it = out.begin(); it != out.end(); it++){ - i = it - out.begin(); - ds = *it; - linetriggerO[i] = ds->getValue() > LINE_THRESH_CAM; - } - - for(int i = 0; i < 4; i++){ - inV = inV | (linetriggerI[i] << i); - outV = outV | (linetriggerO[i] << i); - } - - tookLine = inV > 0 || outV > 0; - // if(flag && tookLine) ((PositionSysCamera*)CURRENT_DATA_READ.posSystem)->setMoveSetpoints(0, -8); -} - -void LineSysCameraRR::test(){ - update(); - DEBUG.print("In: "); - for(DataSource* d : in){ - d->update(); - DEBUG.print(d->getValue()); - DEBUG.print(" | "); - } - DEBUG.print(" |---| "); - DEBUG.print("Out: "); - for(DataSource* d : out){ - d->update(); - DEBUG.print(d->getValue()); - DEBUG.print(" | "); - } - DEBUG.println(); - for(int i = 0; i < 4; i++){ - DEBUG.print(linetriggerI[i]); - DEBUG.print(" | "); - DEBUG.println(linetriggerO[i]); - } - - DEBUG.println(inV); - DEBUG.println(outV); - DEBUG.println(); -} \ No newline at end of file diff --git a/src/test_menu.cpp b/src/test_menu.cpp index 99d110f..d2bc309 100644 --- a/src/test_menu.cpp +++ b/src/test_menu.cpp @@ -92,7 +92,6 @@ void TestMenu::testMenu() DEBUG.print("Left switch (goalOrientation): "); DEBUG.println(camera->goalOrientation); DEBUG.print("Robot Identifier: "); - DEBUG.println(robot_indentifier); delay(50); break; case 'p':