diff --git a/include/systems/lines/linesys_camera_keeper.h b/include/systems/lines/linesys_camera_keeper.h new file mode 100644 index 0000000..8f9a55d --- /dev/null +++ b/include/systems/lines/linesys_camera_keeper.h @@ -0,0 +1,43 @@ +#pragma once + +#include + +#include "behaviour_control/ds_ctrl.h" +#include "systems/systems.h" + +#include "vars.h" + +#ifndef S1I + +#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 100 +#define EXIT_TIME 100 +#endif + +class LineSysCameraKeeper : public LineSystem{ + + public: + LineSysCameraKeeper(); + LineSysCameraKeeper(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; + unsigned long exitTimer; + int outDir, outVel; + byte linesens; +}; \ No newline at end of file diff --git a/src/strategy_roles/games.cpp b/src/strategy_roles/games.cpp index 0ce8b33..0dcf1c9 100644 --- a/src/strategy_roles/games.cpp +++ b/src/strategy_roles/games.cpp @@ -2,6 +2,7 @@ /* #include "sensors/linesys_2019.h" */ #include "systems/lines/linesys_camera.h" +#include "systems/lines/linesys_camera_keeper.h" #include "systems/systems.h" #include "systems/position/positionsys_zone.h" #include "systems/position/positionsys_camera.h" @@ -21,5 +22,5 @@ void initGames(){ lOut.push_back(new DataSource(S4O, true)); striker = new Striker(new LineSysCamera(lIn, lOut), new PositionSysCamera()); - keeper = new Keeper(new LineSystemEmpty(), new PositionSystemEmpty()); + keeper = new Keeper(new LineSysCameraKeeper(lIn, lOut), new PositionSysCamera()); } \ No newline at end of file diff --git a/src/system/lines/linesys_camera_keeper.cpp b/src/system/lines/linesys_camera_keeper.cpp new file mode 100644 index 0000000..b8e928c --- /dev/null +++ b/src/system/lines/linesys_camera_keeper.cpp @@ -0,0 +1,128 @@ +#include "systems/lines/linesys_camera_keeper.h" +#include "systems/position/positionsys_camera.h" +#include "sensors/sensors.h" +#include "strategy_roles/games.h" +#include "behaviour_control/status_vector.h" + +LineSysCameraKeeper::LineSysCameraKeeper(){} +LineSysCameraKeeper::LineSysCameraKeeper(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 LineSysCameraKeeper ::update() { + CURRENT_INPUT_WRITE.lineByte = 0; + 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_CAM; + CURRENT_INPUT_WRITE.lineByte |= linetriggerI[i] << i; + } + for (auto it = out.begin(); it != out.end(); it++) + { + i = it - out.begin(); + ds = *it; + linetriggerO[i] = ds->getValue() > LINE_THRESH_CAM; + CURRENT_INPUT_WRITE.lineByte |= linetriggerO[i] << (4+i); + } + + for (int i = 0; i < 4; i++) + { + inV = inV | (linetriggerI[i] << i); + outV = outV | (linetriggerO[i] << i); + } + + if (inV > 0 || outV > 0) + { + if (millis() - exitTimer > EXIT_TIME) + { + fboundsX = true; + fboundsY = true; + } + exitTimer = millis(); + } + + linesens |= inV | outV; + CURRENT_DATA_WRITE.lineActive = linesens; + outOfBounds(); +} + +void LineSysCameraKeeper::outOfBounds(){ + // digitalWriteFast(BUZZER, LOW); + 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 (millis() - exitTimer < EXIT_TIME){ + // CURRENT_DATA_READ.game->ps->centerGoal(); + tookLine = true; + tone(BUZZER, 220.00, 250); + }else{ + // drive->canUnlock = true; + linesens = 0; + linesensOldY = 0; + linesensOldX = 0; + } +} + +void LineSysCameraKeeper::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