From fa005584d927791f29b3d1bd680a0e23425aa238 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Fri, 14 May 2021 10:30:35 +0200 Subject: [PATCH] romecup: initial working pass and shoot --- .../sensors/data_source_camera_conicmirror.h | 2 +- include/strategy_roles/games.h | 2 + include/strategy_roles/pass_and_shoot.h | 28 ++++++ include/strategy_roles/precision_shooter.h | 2 + src/main.cpp | 8 +- src/sensors/data_source_bt.cpp | 9 +- src/strategy_roles/games.cpp | 1 + src/strategy_roles/pass_and_shoot.cpp | 92 +++++++++++++++++++ utility/OpenMV/conic_eff_h7.py | 4 +- 9 files changed, 136 insertions(+), 12 deletions(-) create mode 100644 include/strategy_roles/pass_and_shoot.h create mode 100644 src/strategy_roles/pass_and_shoot.cpp diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h index 1594da6..7ad04d8 100644 --- a/include/sensors/data_source_camera_conicmirror.h +++ b/include/sensors/data_source_camera_conicmirror.h @@ -22,7 +22,7 @@ To overcome this, each coordinate needs to be shifted by some amount, defined on a per-robot basis that needs to be recalibrated each time. These values need to be subtracted from the coords used in setMoveSetpoints*/ #define CAMERA_TRANSLATION_X 0 -#define CAMERA_TRANSLATION_Y 4 +#define CAMERA_TRANSLATION_Y 0 class DataSourceCameraConic : public DataSource{ diff --git a/include/strategy_roles/games.h b/include/strategy_roles/games.h index 53d1386..3f926bc 100644 --- a/include/strategy_roles/games.h +++ b/include/strategy_roles/games.h @@ -10,10 +10,12 @@ #include "strategy_roles/game.h" #include "strategy_roles/striker.h" #include "strategy_roles/precision_shooter.h" +#include "strategy_roles/pass_and_shoot.h" // #include "strategy_roles/keeper.h" void initGames(); g_extr Game* striker; g_extr Game* precision_shooter; +g_extr Game* pass_and_shoot; // g_extr Game* keeper; \ No newline at end of file diff --git a/include/strategy_roles/pass_and_shoot.h b/include/strategy_roles/pass_and_shoot.h new file mode 100644 index 0000000..dee366b --- /dev/null +++ b/include/strategy_roles/pass_and_shoot.h @@ -0,0 +1,28 @@ +#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 index 69efe78..7e6a06f 100644 --- a/include/strategy_roles/precision_shooter.h +++ b/include/strategy_roles/precision_shooter.h @@ -25,4 +25,6 @@ class PrecisionShooter : public Game{ float cstorc; bool gotta_tilt; + + unsigned long pas_counter; }; diff --git a/src/main.cpp b/src/main.cpp index 173ae7b..b409ac3 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -17,8 +17,8 @@ bool striker_condition = false; bool keeper_condition = false; void setup() { - pinMode(LED_BUILTIN, OUTPUT); pinMode(BUZZER, OUTPUT); + tone(BUZZER, 220, 250); delay(1500); DEBUG.begin(115200); @@ -52,8 +52,10 @@ void loop() { drive->resetDrive(); striker_condition = role == HIGH; - striker->play(striker_condition); - // precision_shooter->play(1); + // striker->play(striker_condition); + + if(role) precision_shooter->play(1); + else pass_and_shoot->play(1); // keeper_condition = role == LOW; // keeper->play(keeper_condition); diff --git a/src/sensors/data_source_bt.cpp b/src/sensors/data_source_bt.cpp index a2d906e..3a5dfc4 100644 --- a/src/sensors/data_source_bt.cpp +++ b/src/sensors/data_source_bt.cpp @@ -29,7 +29,8 @@ DataSourceBT :: DataSourceBT(HardwareSerial* ser_, int baud) : DataSource(ser_, last_received = 0; t = 0; - tosend = 'B'; + tosend = 'A'; + received = '0'; connect(); } @@ -57,7 +58,6 @@ void DataSourceBT :: receive(){ last_received = millis(); received = (char) Serial1.read(); comrade = true; - DEBUG.println(received); } if(millis() - last_received > 2000) comrade = false; @@ -66,8 +66,6 @@ void DataSourceBT :: receive(){ void DataSourceBT::send(){ if(millis() - t >= 250){ Serial1.print(tosend); - // DEBUG.print("Sending: "); - DEBUG.println(tosend); } } @@ -75,8 +73,7 @@ void DataSourceBT::update(){ // if(!bt_bombarded && can_bombard) connect(); receive(); send(); - digitalWriteFast(LED_BUILTIN, comrade); - digitalWriteFast(BUZZER, received == 'B') ; + // if(comrade)Serial2.write(0b00000100); } void DataSourceBT :: test(){ diff --git a/src/strategy_roles/games.cpp b/src/strategy_roles/games.cpp index c927ec5..c95ff38 100644 --- a/src/strategy_roles/games.cpp +++ b/src/strategy_roles/games.cpp @@ -13,6 +13,7 @@ void initGames(){ // 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 LineSystemEmpty(), 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 new file mode 100644 index 0000000..a65fbc3 --- /dev/null +++ b/src/strategy_roles/pass_and_shoot.cpp @@ -0,0 +1,92 @@ +#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(); +} + +void PassAndShoot::realPlay() +{ + if (CURRENT_DATA_READ.ballSeen){ + if(robot_indentifier == HIGH){ + if(millis() - pass_counter <= 1500) { + striker(); + }else{ + ((PositionSysCamera*)ps)->setMoveSetpoints(CAMERA_GOAL_MIN_X, CAMERA_GOAL_Y); + bt->tosend = 'C'; + } + }else{ + if(bt->received == 'C'){ + // //Show on 32u4 + Serial2.write(0b00000100); + 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/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py index a1aa0c7..1661ce3 100644 --- a/utility/OpenMV/conic_eff_h7.py +++ b/utility/OpenMV/conic_eff_h7.py @@ -46,10 +46,10 @@ blue_led.on() thresholds = [ (70, 100, -36, 8, 38, 112), # thresholds yellow goalz - (46, 65, -18, 19, -59, -15)] # thresholds blue goal (6, 31, -15, 4, -35, 0) + (34, 54, -23, 13, -61, -18)] # thresholds blue goal (6, 31, -15, 4, -35, 0) -roi = (80, 0, 240, 200) +roi = (50, 0, 250, 200) # Camera Setup ############################################################### '''sensor.reset()xxxx