camera lines

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;

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

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;

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{

include/sensors/linesys_camera.h

@@ -0,0 +1,42 @@
+#pragma once
+
+#include <Arduino.h>
+
+#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<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;
+        elapsedMillis exitTimer;
+        int outDir, outVel;
+        byte linesens;
+};

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"

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

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

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;

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

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<DataSource*> lIn = { new DataSource(S1I, true), new DataSource(S2I, true), new DataSource(S3I, true), new DataSource(S4I, true) };
     vector<DataSource*> 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());
 }

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;

src/strategy_roles/keeper.cpp

@@ -1,7 +1,7 @@
 #include <Arduino.h>
 
 #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"