lines: specific line system for keeper

include/systems/lines/linesys_camera_keeper.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <Arduino.h>
+
+#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<DataSource*> in_, vector<DataSource*> out);
+
+        void update() override;
+        void test() override;
+        void outOfBounds();
+        void checkLineSensors();
+        
+    private:
+        vector<DataSource*> 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;
+};

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());
 }

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<DataSource*> in_, vector<DataSource*> 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();
+}