ball presence sensor

basically a line sensor with split led and phototransistors

include/behaviour_control/status_vector.h

@@ -29,14 +29,14 @@
 #define CURRENT_INPUT_WRITE ( inputs[((currentSVIndex))] )
 
 typedef struct input{
-    int IMUAngle, USfr, USsx, USdx, USrr, BT;
+    int IMUAngle, USfr, USsx, USdx, USrr, BT, ballPresenceVal;
     byte cameraByte, ballByte, lineByte, xb, yb, xy, yy;
     bool SW_DX, SW_SX;
     String ballString;
 }input;
 
 typedef struct data{
-    int IMUAngle, ballAngle, ballAngleFix, ballDistance, 
+    int IMUAngle, ballAngle, ballAngleFix, ballDistance, ballPresenceVal, 
         yAngle, bAngle, yAngleFix, bAngleFix, 
         yDist, bDist, 
         angleAtk, angleAtkFix, angleDef, angleDefFix, yAtk, yAtkFix, xAtk, xAtkFix, yDef, yDefFix, xDef, xDefFix,
@@ -53,7 +53,7 @@ typedef struct data{
     bool mate, 
         ATKgoal, DEFgoal, 
         atkSeen, defSeen, bSeen, ySeen,
-        ballSeen;
+        ballSeen, ballPresent;
 }data;
 
 sv_extr input inputs[dim];

include/sensors/data_source_ball_presence.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#define BALL_PRESENCE_TRESH 500
+
+#include <Arduino.h>
+#include "behaviour_control/data_source.h"
+
+class DataSourceBallPresence : public DataSource{
+
+    public:
+        DataSourceBallPresence(int, bool);
+        void postProcess() override;
+        void test() override;
+        bool isInMouth();
+
+    public:
+        bool present;
+};

include/sensors/sensors.h

@@ -17,6 +17,7 @@
 #include "systems/lines/linesys_camera.h"
 #include "systems/position/positionsys_zone.h"
 #include "sensors/data_source_ball.h"
+#include "sensors/data_source_ball_presence.h"
 #include "sensors/data_source_bt.h"
 #include "sensors/data_source_bno055.h"
 #include "sensors/data_source_camera_conicmirror.h"
@@ -35,6 +36,7 @@ s_extr DataSourceBall* ball;
 s_extr DataSourceCameraConic* camera;
 s_extr DriveController* drive;
 s_extr DataSourceBT* bt;
+s_extr DataSourceBallPresence* ballPresence;
 
 s_extr int role;
 s_extr int robot_indentifier;

src/main.cpp

@@ -9,8 +9,6 @@
 #include "test_menu.h"
 #include "motors_movement/roller.h"
 
-void updateRoller();
-
 TestMenu* testmenu;
 
 #define BALL_IN A13
@@ -53,12 +51,12 @@ void loop() {
   drive->resetDrive();
   
   // striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike;
-  keeper_condition = role == LOW;
+  // keeper_condition = role == LOW;
 
-  striker->play(1);
+  precision_shooter->play(1);
 
   testmenu->testMenu();
-  
+
   // Last thing to do: movement and update status vector
   drive->drivePrepared();  
   updateStatusVector();

src/sensors/data_source_ball_presence.cpp

@@ -0,0 +1,29 @@
+#include "sensors/data_source_ball_presence.h"
+#include "behaviour_control/status_vector.h"
+#include "vars.h"
+
+DataSourceBallPresence::DataSourceBallPresence(int pin_, bool analog) : DataSource(pin_, analog){
+    present = false;
+    value = 0;
+} 
+
+void DataSourceBallPresence::postProcess(){
+    present = value <= BALL_PRESENCE_TRESH;
+
+    CURRENT_INPUT_WRITE.ballPresenceVal = value;
+    CURRENT_DATA_WRITE.ballPresenceVal = value;
+    CURRENT_DATA_WRITE.ballPresent = present;
+}
+
+bool DataSourceBallPresence::isInMouth(){
+    return ball->isInFront() && present;
+}
+
+void DataSourceBallPresence::test(){
+    DEBUG.print("Reading value: ");
+    DEBUG.print(CURRENT_DATA_READ.ballPresenceVal);
+    DEBUG.print(" -> ");
+    DEBUG.print(CURRENT_DATA_READ.ballPresent);
+    DEBUG.print(" | Ball in mouth: ");
+    DEBUG.print(isInMouth());
+}

src/sensors/sensors.cpp

@@ -20,6 +20,7 @@ void initSensors(){
     // delay(350);
     bt = new DataSourceBT(&Serial1, 9600);
     roller = new Roller(30, 31, 1000, 2000, 500);
+    ballPresence = new DataSourceBallPresence(A22, true);
  }
 
 void updateSensors(){
@@ -30,6 +31,7 @@ void updateSensors(){
     
     compass->update();
     ball->update();
+    ballPresence->update();
     camera->update();
 
     bt->update();

src/test_menu.cpp

@@ -110,6 +110,13 @@ void TestMenu::testMenu()
             while (Serial2.available())
                 DEBUG.print((char)Serial2.read());
             break;
+        case 'b':
+            ballPresence->test();
+            break;
+        case 'r':
+            drive->stopAll();
+            flagtest = false;
+            break;
         case 's':
             DEBUG.println("32u4 send Test");
             DEBUG.println("Remember LED1 is not used by teensy");
@@ -128,10 +135,6 @@ void TestMenu::testMenu()
             Serial2.write(0);
             delay(1500);
             break;
-        case 'r':
-            drive->stopAll();
-            flagtest = false;
-            break;
         case 'h':
         default:
             DEBUG.println("Unknown command, here's a lil help for you :)");