From 39a3a26249a75ddd8254fd329e1391f238f71842 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Sat, 26 Jun 2021 12:50:27 +0200 Subject: [PATCH] robocup: tc3: corner kick Co-authored-by: u-siri-ous --- include/sensors/data_source_bt.h | 2 +- include/strategy_roles/corner_kick_1.h | 29 ++++ include/strategy_roles/corner_kick_2.h | 43 +++++ include/strategy_roles/games.h | 2 + include/systems/position/positionsys_camera.h | 1 + src/main.cpp | 4 +- src/sensors/data_source_bt.cpp | 6 +- src/strategy_roles/corner_kick_1.cpp | 83 ++++++++++ src/strategy_roles/corner_kick_2.cpp | 148 ++++++++++++++++++ src/strategy_roles/games.cpp | 5 + src/system/positions/positionsys_camera.cpp | 5 + 11 files changed, 323 insertions(+), 5 deletions(-) create mode 100644 include/strategy_roles/corner_kick_1.h create mode 100644 include/strategy_roles/corner_kick_2.h create mode 100644 src/strategy_roles/corner_kick_1.cpp create mode 100644 src/strategy_roles/corner_kick_2.cpp diff --git a/include/sensors/data_source_bt.h b/include/sensors/data_source_bt.h index 6668a9f..f46bae1 100644 --- a/include/sensors/data_source_bt.h +++ b/include/sensors/data_source_bt.h @@ -13,7 +13,7 @@ class DataSourceBT : public DataSource{ void receive(); void send(); - bool can_bombard, bt_bombarded, comrade; + bool can_send, bt_bombarded, comrade; unsigned long bt_timer, last_received, t; char received, tosend; diff --git a/include/strategy_roles/corner_kick_1.h b/include/strategy_roles/corner_kick_1.h new file mode 100644 index 0000000..50a6b9d --- /dev/null +++ b/include/strategy_roles/corner_kick_1.h @@ -0,0 +1,29 @@ +#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 new file mode 100644 index 0000000..6545cb0 --- /dev/null +++ b/include/strategy_roles/corner_kick_2.h @@ -0,0 +1,43 @@ +#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 652ac29..4896313 100644 --- a/include/strategy_roles/games.h +++ b/include/strategy_roles/games.h @@ -16,6 +16,7 @@ #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" void initGames(); @@ -30,4 +31,5 @@ g_extr Game* tc1; g_extr Game* tc2; g_extr Game* st_tc1; g_extr Game* st_tc3; +g_extr Game* tc3_1; g_extr Game* tc3_2; \ No newline at end of file diff --git a/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h index 2f5c879..faf530a 100644 --- a/include/systems/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -48,6 +48,7 @@ class PositionSysCamera : public PositionSystem{ int calcOtherGoalY(int goalY); bool isInTheVicinityOf(int, int); bool isInRoughVicinityOf(int, int); + bool isAtDistanceFrom(int, int, int); double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy; int MAX_DIST, vx, vy, axisx, axisy; diff --git a/src/main.cpp b/src/main.cpp index 7d5af38..624df7b 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -54,7 +54,9 @@ void loop() { // keeper_condition = role == LOW; if(robot_indentifier){ - tc2->play(1); + tc3_2->play(role); + tc3_1->play(!role); + // Last thing to do: movement and update status vector drive->drivePrepared(); }else{ diff --git a/src/sensors/data_source_bt.cpp b/src/sensors/data_source_bt.cpp index 3a5dfc4..1e6e7d7 100644 --- a/src/sensors/data_source_bt.cpp +++ b/src/sensors/data_source_bt.cpp @@ -21,8 +21,8 @@ For now it seems that a slave can easily recovery from a master restart, but not DataSourceBT :: DataSourceBT(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){ bt_timer = millis(); - can_bombard = false; bt_bombarded = false; + can_send = false; comrade = false; @@ -64,7 +64,7 @@ void DataSourceBT :: receive(){ } void DataSourceBT::send(){ - if(millis() - t >= 250){ + if(millis() - t >= 250 && can_send ){ Serial1.print(tosend); } } @@ -73,7 +73,7 @@ void DataSourceBT::update(){ // if(!bt_bombarded && can_bombard) connect(); receive(); send(); - // if(comrade)Serial2.write(0b00000100); + 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 new file mode 100644 index 0000000..1893029 --- /dev/null +++ b/src/strategy_roles/corner_kick_1.cpp @@ -0,0 +1,83 @@ +#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 > 250) kick_state ++; + } else if(kick_state==2){ + drive->prepareDrive(0, 60, CURRENT_DATA_READ.ballAngleFix); + if(ball->isInMouth()){ + kick_state++; + if(!kick_flag) { + kick_flag = true; + kicktimer = millis(); + }else{ + if(kick_flag && millis() - kicktimer > 700){ + kick_flag = false; + kick_state++; + kicktimer = millis(); + } + } + } + } else if(kick_state==3){ + drive->prepareDrive(0, 150, CURRENT_DATA_READ.ballAngleFix); + if(millis()-kicktimer > 400){ + kick_state++; + } + } else if(kick_state==4){ + if(((PositionSysCamera*)ps)->isAtDistanceFrom(0, -28, 5)) { + kick_state++; + kicktimer = millis(); + }else (((PositionSysCamera*)ps)->setMoveSetpoints(0, -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 new file mode 100644 index 0000000..8558f5c --- /dev/null +++ b/src/strategy_roles/corner_kick_2.cpp @@ -0,0 +1,148 @@ +#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; + } + if(state == 1){ + drive->prepareDrive(265, 50, 0); + if(CURRENT_DATA_READ.bSeen && millis() - timer > 500) { + state ++; + timer = millis(); + } + }else if(state == 2){ + drive->prepareDrive(315, 50, 0); + if(millis() - timer > 200) state++; + }else if(state == 3){ + catchBall(); + }else if(state == 4){ + spinner(0); + }else if(state == 5){ + drive->stopAll(); + } +} + +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.01; + else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.01; + + 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, 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.01; + 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, 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, 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 1b0326c..438e45c 100644 --- a/src/strategy_roles/games.cpp +++ b/src/strategy_roles/games.cpp @@ -17,8 +17,13 @@ void initGames(){ 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()); diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index 3f910f2..8a5eec0 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -132,6 +132,11 @@ bool PositionSysCamera::isInRoughVicinityOf(int x_, int y_){ return pow(CURRENT_DATA_READ.posx-x_, 2) + pow(CURRENT_DATA_READ.posy-y_, 2) <= ROUGH_VICINITY_DIST_TRESH*ROUGH_VICINITY_DIST_TRESH; } +bool PositionSysCamera::isAtDistanceFrom(int x_, int y_, int dist){ + // Distance using pytagorean theorem + return pow(CURRENT_DATA_READ.posx-x_, 2) + pow(CURRENT_DATA_READ.posy-y_, 2) <= dist*dist; +} + void PositionSysCamera::CameraPID(){ if(givenMovement){