diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h index f8c51b8..5714404 100644 --- a/include/sensors/data_source_camera_conicmirror.h +++ b/include/sensors/data_source_camera_conicmirror.h @@ -27,8 +27,8 @@ These values need to be subtracted from the coords used in setMoveSetpoints*/ // #define CAMERA_TRANSLATION_Y 7 //Robot with roller -#define CAMERA_TRANSLATION_X -8 -#define CAMERA_TRANSLATION_Y -3 +#define CAMERA_TRANSLATION_X 0 +#define CAMERA_TRANSLATION_Y 12 class DataSourceCameraConic : public DataSource{ diff --git a/include/strategy_roles/games.h b/include/strategy_roles/games.h index fb8c21b..c881a32 100644 --- a/include/strategy_roles/games.h +++ b/include/strategy_roles/games.h @@ -12,6 +12,7 @@ #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/keeper.h" void initGames(); @@ -22,4 +23,5 @@ g_extr Game* precision_shooter; g_extr Game* pass_and_shoot; // g_extr Game* keeper; -g_extr Game* tc1; \ No newline at end of file +g_extr Game* tc1; +g_extr Game* tc2; \ No newline at end of file diff --git a/include/strategy_roles/spot_finder.h b/include/strategy_roles/spot_finder.h new file mode 100644 index 0000000..05cde28 --- /dev/null +++ b/include/strategy_roles/spot_finder.h @@ -0,0 +1,21 @@ +#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/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h index 30eb46e..77a0a80 100644 --- a/include/systems/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -23,6 +23,7 @@ #define DIST_MULT 5 #define VICINITY_DIST_TRESH 2 +#define ROUGH_VICINITY_DIST_TRESH 10 #define Kpx 1 #define Kix 0 @@ -45,6 +46,7 @@ class PositionSysCamera : public PositionSystem{ void CameraPID(); int calcOtherGoalY(int goalY); bool isInTheVicinityOf(int, int); + bool isInRoughVicinityOf(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 3ba26ff..cf569a5 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -54,7 +54,7 @@ void loop() { // keeper_condition = role == LOW; if(robot_indentifier){ - tc1->play(1); + tc2->play(1); // if(roller->roller_armed) roller->speed(roller->MAX); // Last thing to do: movement and update status vector drive->drivePrepared(); diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp index b8f8773..e64698e 100644 --- a/src/sensors/data_source_camera_conicmirror.cpp +++ b/src/sensors/data_source_camera_conicmirror.cpp @@ -201,7 +201,7 @@ void DataSourceCameraConic ::computeCoordsAngles() { byte to_32u4 = 0; to_32u4 |= (CURRENT_DATA_READ.ySeen); to_32u4 |= (CURRENT_DATA_READ.bSeen) << 1; - BALL_32U4.write(to_32u4); + // BALL_32U4.write(to_32u4); } void DataSourceCameraConic::test(){ diff --git a/src/strategy_roles/games.cpp b/src/strategy_roles/games.cpp index c98c00d..487eede 100644 --- a/src/strategy_roles/games.cpp +++ b/src/strategy_roles/games.cpp @@ -17,5 +17,6 @@ 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 SpotFinder(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/spot_finder.cpp b/src/strategy_roles/spot_finder.cpp new file mode 100644 index 0000000..01558ea --- /dev/null +++ b/src/strategy_roles/spot_finder.cpp @@ -0,0 +1,33 @@ +#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/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index 88c808d..cb110d5 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -53,8 +53,8 @@ void PositionSysCamera::update(){ posx *= -1; posy *= -1; - //x = 66 is a very very strange bug I can't seem to track down. It's a dirty hack, I know - if(posx == 66 || (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == false) ) { + //Filtering error in calculation like this is a dirty hack, I know + if(posx < -MAX_X || posx > MAX_X || posy < -MAX_Y || posy > MAX_Y || (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == false) ) { // Go back in time until we found a valid status, when we saw at least one goal int i = 1; do{ @@ -128,6 +128,11 @@ bool PositionSysCamera::isInTheVicinityOf(int x_, int y_){ return pow(CURRENT_DATA_READ.posx-x_, 2) + pow(CURRENT_DATA_READ.posy-y_, 2) <= VICINITY_DIST_TRESH*VICINITY_DIST_TRESH; } +bool PositionSysCamera::isInRoughVicinityOf(int x_, int y_){ + // Distance using pytagorean theorem + return pow(CURRENT_DATA_READ.posx-x_, 2) + pow(CURRENT_DATA_READ.posy-y_, 2) <= ROUGH_VICINITY_DIST_TRESH*ROUGH_VICINITY_DIST_TRESH; +} + void PositionSysCamera::CameraPID(){ if(givenMovement){ diff --git a/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py index b03ce43..d5c8b91 100644 --- a/utility/OpenMV/conic_eff_h7.py +++ b/utility/OpenMV/conic_eff_h7.py @@ -45,11 +45,11 @@ blue_led.on() ############################################################################## -thresholds = [ (70, 100, -36, 8, 38, 112), # thresholds yellow goalz - (37, 57, -19, 8, -52, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +thresholds = [ (84, 100, -29, 1, 36, 127), # thresholds yellow goalz + (36, 100, -34, 12, -59, -19)] # thresholds blue goal (6, 31, -15, 4, -35, 0) -roi = (50, 0, 270, 200) +roi = (70, 0, 250, 200) # Camera Setup ############################################################### '''sensor.reset()xxxx