diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h index 45cf268..a98706b 100644 --- a/include/motors_movement/drivecontroller.h +++ b/include/motors_movement/drivecontroller.h @@ -22,7 +22,7 @@ // #define MAX_POSSIBLE_VEL 310 #define MAX_POSSIBLE_VEL 280 -#define MAX_VEL 100 +#define MAX_VEL 80 #define MAX_VEL_EIGTH ((int)MAX_VEL*0.8) #define MAX_VEL_HALF ((int)MAX_VEL*0.5) #define MAX_VEL_3QUARTERS ((int)MAX_VEL*0.75) diff --git a/include/strategy_roles/games.h b/include/strategy_roles/games.h index e48a036..2020856 100644 --- a/include/strategy_roles/games.h +++ b/include/strategy_roles/games.h @@ -14,6 +14,7 @@ #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/keeper.h" void initGames(); @@ -25,5 +26,6 @@ g_extr Game* pass_and_shoot; // g_extr Game* keeper; g_extr Game* tc1; +g_extr Game* tc2; g_extr Game* st_tc1; g_extr Game* st_tc3; \ No newline at end of file diff --git a/include/strategy_roles/round_robin.h b/include/strategy_roles/round_robin.h new file mode 100644 index 0000000..b039a72 --- /dev/null +++ b/include/strategy_roles/round_robin.h @@ -0,0 +1,53 @@ +#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 135 +#define RR_KICK_LIMIT_MAX 335 +#define RR_KICK_LIMIT_MIN 25 + +#define ROUND_ROBIN_VEL 30 +#define RR_YCOORD -8 +#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); + + 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/src/main.cpp b/src/main.cpp index 286fb5d..7d5af38 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -54,7 +54,7 @@ void loop() { // keeper_condition = role == LOW; if(robot_indentifier){ - striker->play(1); + tc2->play(1); // Last thing to do: movement and update status vector drive->drivePrepared(); }else{ diff --git a/src/strategy_roles/games.cpp b/src/strategy_roles/games.cpp index 543d19b..d57185e 100644 --- a/src/strategy_roles/games.cpp +++ b/src/strategy_roles/games.cpp @@ -17,6 +17,7 @@ void initGames(){ 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 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()); diff --git a/src/strategy_roles/round_robin.cpp b/src/strategy_roles/round_robin.cpp new file mode 100644 index 0000000..493fb62 --- /dev/null +++ b/src/strategy_roles/round_robin.cpp @@ -0,0 +1,178 @@ +#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/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(); + 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(!ballPresence->isInMouth()) { + // spinner_state=0; + // spinner_flag = false; + // } + + if(spinner_state == 0){ + if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX-200); + + if(ballPresence->isInMouth() && !spinner_flag){ + spinner_flag = true; + spinner_timer = millis(); + } + if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 100) { + 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, RR_YCOORD)) { + // 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 = -1.55; + limitx = RR_KICK_LIMIT_TILT1; + + }else{ + tilt1 = 0.15; + tilt2 = 1.55; limitx = 360-RR_KICK_LIMIT_TILT1; + + + } + + }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, RR_YCOORD); + + }else if(spinner_state == 2){ + roller->speed(RR_ROLLER_SPD); + + 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(RR_ROLLER_SPD); + 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) { + roller->speed(roller->MIN); + spinner_tilt = 0; + } + } +} +/* +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 = 0; + ball_catch_flag = false; + ball_catch_tilt = 0; + } + + if(ball_catch_state == 0){ + + 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; + }else if(ball_catch_state == 3){ + this->spinner(28); + } +}