From 4e0ae43573966072152686807ec0616ce341f42e Mon Sep 17 00:00:00 2001 From: u-siri-ous Date: Mon, 2 Mar 2020 18:48:29 +0100 Subject: [PATCH] camera lines --- include/behaviour_control/status_vector.h | 3 +- include/position/positionsys_camera.h | 2 +- .../sensors/data_source_camera_conicmirror.h | 3 +- include/{position => sensors}/linesys_2019.h | 2 +- include/sensors/linesys_camera.h | 42 +++++++ include/sensors/sensors.h | 3 +- src/main.cpp | 5 +- .../data_source_camera_conicmirror.cpp | 68 ++++++---- src/sensors/linesys_2019.cpp | 2 +- src/sensors/linesys_camera.cpp | 116 ++++++++++++++++++ src/strategy_roles/games.cpp | 7 +- src/strategy_roles/goalie.cpp | 5 +- src/strategy_roles/keeper.cpp | 2 +- 13 files changed, 219 insertions(+), 41 deletions(-) rename include/{position => sensors}/linesys_2019.h (97%) create mode 100644 include/sensors/linesys_camera.h create mode 100644 src/sensors/linesys_camera.cpp diff --git a/include/behaviour_control/status_vector.h b/include/behaviour_control/status_vector.h index 1808892..f20048d 100644 --- a/include/behaviour_control/status_vector.h +++ b/include/behaviour_control/status_vector.h @@ -42,7 +42,8 @@ typedef struct data{ cam_xb, cam_yb, cam_xy, cam_yy, speed, tilt, dir, axisBlock[4], USfr, USsx, USdx, USrr, - lineOutDir, matePos, role; + lineOutDir, matePos, role, cam_xb_fixed, + cam_xy_fixed, cam_yb_fixed, cam_yy_fixed; Game* game; LineSystem* lineSystem; PositionSystem* posSystem; diff --git a/include/position/positionsys_camera.h b/include/position/positionsys_camera.h index 35b68b2..1cf8b3f 100644 --- a/include/position/positionsys_camera.h +++ b/include/position/positionsys_camera.h @@ -3,7 +3,7 @@ #include "position/systems.h" #define CAMERA_CENTER_X 0 -#define CAMERA_CENTER_Y -18 +#define CAMERA_CENTER_Y 0 #define CAMERA_CENTER_Y_ABS_SUM 72 //Actually it's ± MAX_VAL #define MAX_X 25 diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h index 26040e7..6c3e842 100644 --- a/include/sensors/data_source_camera_conicmirror.h +++ b/include/sensors/data_source_camera_conicmirror.h @@ -21,7 +21,8 @@ class DataSourceCameraConic : public DataSource{ int yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist; int count = 0, unkn_counter; - int xb, yb, xy, yy, true_xb, true_xy, true_yb, true_yy, calc_xb, calc_yb, calc_xy, calc_yy; + int xb, yb, xy, yy, true_xb, true_xy, true_yb, true_yy, calc_xb, calc_yb, calc_xy, calc_yy, true_xb_fixed, + true_xy_fixed, true_yb_fixed, true_yy_fixed; bool data_received = false, start = false, end = false; int goalOrientation, pAtk, pDef; diff --git a/include/position/linesys_2019.h b/include/sensors/linesys_2019.h similarity index 97% rename from include/position/linesys_2019.h rename to include/sensors/linesys_2019.h index 5f241f1..f8a7d3c 100644 --- a/include/position/linesys_2019.h +++ b/include/sensors/linesys_2019.h @@ -17,7 +17,7 @@ #define S4O A2 #define LINE_THRESH 90 -#define EXTIME 100 +#define EXTIME 200 #define LINES_EXIT_SPD 350 class LineSys2019 : public LineSystem{ diff --git a/include/sensors/linesys_camera.h b/include/sensors/linesys_camera.h new file mode 100644 index 0000000..6a301f3 --- /dev/null +++ b/include/sensors/linesys_camera.h @@ -0,0 +1,42 @@ +#pragma once + +#include + +#include "behaviour_control/ds_ctrl.h" +#include "position/systems.h" + +#include "vars.h" + +#define S1I A14 +#define S1O A15 +#define S2I A16 +#define S2O A17 +#define S3I A20 +#define S3O A0 +#define S4I A1 +#define S4O A2 + +#define LINE_THRESH_CAM 90 +#define EXIT_TIME 100 +#define LINES_EXIT_SPD 350 + +class LineSysCamera : public LineSystem{ + + public: + LineSysCamera(); + LineSysCamera(vector in_, vector out); + + void update() override; + void test() override; + void outOfBounds(); + void checkLineSensors(); + + 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; + elapsedMillis exitTimer; + int outDir, outVel; + byte linesens; +}; \ No newline at end of file diff --git a/include/sensors/sensors.h b/include/sensors/sensors.h index baff6da..0c0f255 100644 --- a/include/sensors/sensors.h +++ b/include/sensors/sensors.h @@ -12,7 +12,8 @@ #include "behaviour_control/ds_ctrl.h" #include "motors_movement/drivecontroller.h" #include "motors_movement/motor.h" -#include "position/linesys_2019.h" +#include "sensors/linesys_2019.h" +#include "sensors/linesys_camera.h" #include "position/positionsys_zone.h" #include "position/systems.h" #include "sensors/data_source_ball.h" diff --git a/src/main.cpp b/src/main.cpp index ae97ede..e84ab13 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -19,9 +19,10 @@ void setup() { void loop() { updateSensors(); + camera->test(); - goalie->play(role==1); - keeper->play(role==0); +/* goalie->play(role==1); + keeper->play(role==0); */ // 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 06919a4..16d3f57 100644 --- a/src/sensors/data_source_camera_conicmirror.cpp +++ b/src/sensors/data_source_camera_conicmirror.cpp @@ -1,12 +1,15 @@ #include "behaviour_control/status_vector.h" #include "sensors/data_source_camera_conicmirror.h" -DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : DataSource(ser_, baud) -{ +DataSourceCameraConic::DataSourceCameraConic(HardwareSerial *ser_, int baud) : DataSource(ser_, baud) { true_xb = 0; true_yb = 0; true_xy = 0; true_yy = 0; + true_xb_fixed = 0; + true_yb_fixed = 0; + true_xy_fixed = 0; + true_yy_fixed = 0; xb = 0; yb = 0; xy = 0; @@ -39,6 +42,13 @@ void DataSourceCameraConic ::readSensor() { true_xy = xy - 50; true_yy = 50 - yy; + true_xb_fixed = true_xb*(cos(CURRENT_DATA_READ.IMUAngle)) - true_yb*(sin(CURRENT_DATA_READ.IMUAngle)); + true_xy_fixed = true_xy*(cos(CURRENT_DATA_READ.IMUAngle)) - true_yy*(sin(CURRENT_DATA_READ.IMUAngle)); + true_yb_fixed = true_xb*(sin(CURRENT_DATA_READ.IMUAngle)) + true_yb*(cos(CURRENT_DATA_READ.IMUAngle)); + true_yy_fixed = true_xy*(sin(CURRENT_DATA_READ.IMUAngle)) + true_yy*(cos(CURRENT_DATA_READ.IMUAngle)); + + + //Remap from [0,100] to [-50, +49] to correctly compute angles and distances and calculate them yAngle = -90 - (atan2(true_yy, true_xy) * 180 / 3.14); bAngle = -90 - (atan2(true_yb, true_xb) * 180 / 3.14); @@ -92,8 +102,7 @@ void DataSourceCameraConic ::readSensor() { end = true; start = false; } - else - { + else{ if (start == true) { if (count == 0) @@ -119,37 +128,44 @@ void DataSourceCameraConic ::readSensor() { // else return goalOrientation == LOW ? yAngle : bAngle; // }> -void DataSourceCameraConic::test() -{ +void DataSourceCameraConic::test(){ goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu update(); - DEBUG.print("Blue: "); + DEBUG.print("Blue: Angle: "); DEBUG.print(bAngle); - DEBUG.print(" | "); + DEBUG.print(" | Fixed Angle: "); DEBUG.print(bAngleFix); - DEBUG.print(" | "); + DEBUG.print(" | Distance: "); DEBUG.print(bDist); - DEBUG.print(" | "); + DEBUG.print(" | Seen: "); DEBUG.println(CURRENT_DATA_READ.bSeen); - DEBUG.println(" --- "); - - DEBUG.print("Yellow: "); + DEBUG.print(" | Received coords: ("); + DEBUG.print(CURRENT_DATA_READ.cam_xb); + DEBUG.print(","); + DEBUG.print(CURRENT_DATA_READ.cam_yb); + DEBUG.print(")"); + DEBUG.print(" | Fixed coords: ("); + DEBUG.print(CURRENT_DATA_READ.cam_xy_fixed); + DEBUG.print(","); + DEBUG.println(CURRENT_DATA_READ.cam_yy_fixed); + DEBUG.println(")---------------"); + DEBUG.print("Yellow: Angle: "); DEBUG.print(yAngle); - DEBUG.print(" | "); + DEBUG.print(" | Fixed Angle: "); DEBUG.print(yAngleFix); - DEBUG.print(" | "); + DEBUG.print(" | Distance: "); DEBUG.print(yDist); - DEBUG.print(" | "); + DEBUG.print(" | Seen: "); DEBUG.println(CURRENT_DATA_READ.ySeen); - DEBUG.println("---------------"); - DEBUG.print("Data: "); - DEBUG.print(true_xb); - DEBUG.print("|"); - DEBUG.print(true_yb); - DEBUG.print("|"); - DEBUG.print(true_xy); - DEBUG.print("|"); - DEBUG.println(true_yy); - DEBUG.println("---------------"); + DEBUG.print(" | Received coords: ("); + DEBUG.print(CURRENT_DATA_READ.cam_xy); + DEBUG.print(","); + DEBUG.print(CURRENT_DATA_READ.cam_yy); + DEBUG.print(")"); + DEBUG.print(" | Fixed coords: ("); + DEBUG.print(CURRENT_DATA_READ.cam_xy_fixed); + DEBUG.print(","); + DEBUG.println(CURRENT_DATA_READ.cam_yy_fixed); + DEBUG.println(")---------------"); delay(150); } diff --git a/src/sensors/linesys_2019.cpp b/src/sensors/linesys_2019.cpp index 616bef6..d8771d6 100644 --- a/src/sensors/linesys_2019.cpp +++ b/src/sensors/linesys_2019.cpp @@ -1,4 +1,4 @@ -#include "position/linesys_2019.h" +#include "sensors/linesys_2019.h" #include "sensors/sensors.h" using namespace std; diff --git a/src/sensors/linesys_camera.cpp b/src/sensors/linesys_camera.cpp new file mode 100644 index 0000000..9f8bf83 --- /dev/null +++ b/src/sensors/linesys_camera.cpp @@ -0,0 +1,116 @@ +#include "sensors/linesys_camera.h" +#include "position/positionsys_camera.h" +#include "sensors/sensors.h" +#include "strategy_roles/games.h" + +LineSysCamera::LineSysCamera(){} +LineSysCamera::LineSysCamera(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 LineSysCamera ::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; + } + for(auto it = out.begin(); it != out.end(); it++){ + i = it - out.begin(); + ds = *it; + linetriggerO[i] = ds->getValue() > LINE_THRESH; + } + + for(int i = 0; i < 4; i++){ + inV = inV | (linetriggerI[i] << i); + outV = outV | (linetriggerO[i] << i); + } + + + if ((inV > 0) || (outV > 0)) { + if(exitTimer > EXIT_TIME) { + fboundsX = true; + fboundsY = true; + } + exitTimer = 0; + } + + linesens |= inV | outV; + outOfBounds(); + } + +void LineSysCamera::outOfBounds(){ + + 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 (exitTimer <= EXTIME){ + ((PositionSysCamera*)goalie->ps)->goCenter(); + tookLine = true; + }else{ + drive->canUnlock = true; + linesens = 0; + linesensOldY = 0; + linesensOldX = 0; + } +} + +void LineSysCamera::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/strategy_roles/games.cpp b/src/strategy_roles/games.cpp index bd499e9..89d4a8f 100644 --- a/src/strategy_roles/games.cpp +++ b/src/strategy_roles/games.cpp @@ -1,6 +1,7 @@ #define GAMES_CPP -#include "position/linesys_2019.h" +/* #include "sensors/linesys_2019.h" */ +#include "sensors/linesys_camera.h" #include "position/positionsys_zone.h" #include "position/positionsys_camera.h" #include "strategy_roles/games.h" @@ -9,6 +10,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) }; - goalie = new Goalie(new LineSys2019(lIn, lOut), new PositionSysCamera()); - keeper = new Keeper(new LineSys2019(lOut, lOut), new PositionSysCamera()); + goalie = new Goalie(new LineSysCamera(lIn, lOut), new PositionSysCamera()); + keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera()); } \ No newline at end of file diff --git a/src/strategy_roles/goalie.cpp b/src/strategy_roles/goalie.cpp index 86bb68e..4085f83 100644 --- a/src/strategy_roles/goalie.cpp +++ b/src/strategy_roles/goalie.cpp @@ -22,9 +22,8 @@ void Goalie::init(){ } void Goalie::realPlay(){ - ((PositionSysCamera*)ps)->goCenter(); - // if(ball->ballSeen) this->goalie(50); - // else ((PositionSysCamera*)ps)->goCenter(); + if(ball->ballSeen) this->goalie(50); + else ((PositionSysCamera*)ps)->goCenter(); } int dir, degrees2; diff --git a/src/strategy_roles/keeper.cpp b/src/strategy_roles/keeper.cpp index a081a30..6d0040b 100644 --- a/src/strategy_roles/keeper.cpp +++ b/src/strategy_roles/keeper.cpp @@ -1,7 +1,7 @@ #include #include "behaviour_control/status_vector.h" -#include "position/linesys_2019.h" +#include "sensors/linesys_2019.h" #include "sensors/sensors.h" #include "strategy_roles/keeper.h" #include "strategy_roles/games.h"