having troubles testing

include/data_source_controller.h

@@ -1,3 +1,4 @@
+#pragma once
 #include "data_source.h"
 #include "vars.h"
 #include <Arduino.h>

include/game.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include "data_source.h"
+#include "data_source_ball.h"
+#include "motor.h"
+#include "drivecontroller.h"
+#include "sensors.h"
+#include "vars.h"
+#include "data_source_bno055.h"
+
+class Game{
+    public:
+        Game();
+        void keeper();
+        void goalie();
+        bool role, attackGoal; //1->goalie 0->keeper, 1->yellow 0->blue
+        //~Game();
+};

include/sensors.h

@@ -6,19 +6,22 @@
 #include "motor.h"
 #include "data_source_controller.h"
 #include "drivecontroller.h"
-
+#pragma once
+#include "game.h"
 #ifdef SENSORS_CPP
 #define extr 
 #else
 #define extr extern
 #endif
 
+class Game;
 void initSensors();
 void updateSensors();
 
-extr DataSource* compass;
-extr DataSource* ball;
-extr DataSource* camera;
+extr DataSourceBNO055* compass;
+extr DataSourceBall* ball;
+extr DataSourceCamera* camera;
 //extr DataSource* us;
 extr DataSourceController* usCtrl;
 extr DriveController* drive;
+extr Game* game;

src/game.cpp

@@ -0,0 +1,21 @@
+#include "game.h"
+
+Game::Game(){
+
+}
+
+void Game::goalie(){
+    digitalWrite(LED_G, HIGH);
+    //if(ball->ballSeen){
+        //if(ball->angle >= 0) {
+            drive->prepareDrive(0, 150, 0);
+            drive->drive(0, 150, 0);
+    //}
+    //} else drive->prepareDrive(0, 0, 0);
+}
+
+void Game::keeper(){
+    digitalWrite(LED_Y, HIGH);
+}
+
+

src/main.cpp

@@ -1,8 +1,6 @@
 #include <Arduino.h>
-#include "vars.h"
-#include "data_source_bno055.h"
-#include "drivecontroller.h"
-#include "sensors.h"
+
+#include "game.h"
 
 
 void setup() {
@@ -17,6 +15,10 @@ void loop() {
   updateSensors();
   /*if(millis() % 100 == 0)
   DEBUG_PRINT.println(us->getValue());*/
-  usCtrl->test();
+  if(game->role) game->goalie();
+  else game->keeper();
+  game->role = digitalRead(SWITCH_DX);
+  game->attackGoal = digitalRead(SWITCH_SX);
+  //usCtrl->test();
   delay(200);
 }

src/sensors.cpp

@@ -12,6 +12,8 @@ void initSensors(){
     ball = new DataSourceBall(&Serial4, 57600);
     camera = new DataSourceCamera(&Serial2, 19200);
     usCtrl = new DataSourceController(dUs);
+
+    game = new Game();
 }
 
 void updateSensors(){