From c346312bc90e00f0dd13096a1b6acc006cde35a5 Mon Sep 17 00:00:00 2001
From: Siria <siria@siria.localdomain>
Date: Mon, 2 Dec 2019 17:25:22 +0100
Subject: [PATCH] having troubles testing

---
 include/data_source_ball.h       |  2 +-
 include/data_source_controller.h |  1 +
 include/game.h                   | 18 ++++++++++++++++++
 include/sensors.h                | 11 +++++++----
 src/game.cpp                     | 21 +++++++++++++++++++++
 src/main.cpp                     | 12 +++++++-----
 src/sensors.cpp                  |  2 ++
 7 files changed, 57 insertions(+), 10 deletions(-)
 create mode 100644 include/game.h
 create mode 100644 src/game.cpp

diff --git a/include/data_source_ball.h b/include/data_source_ball.h
index dc6da55..6beb7be 100644
--- a/include/data_source_ball.h
+++ b/include/data_source_ball.h
@@ -8,6 +8,6 @@ class DataSourceBall : public DataSource{
         void postProcess() override;
         void test() override;
         
-        int angle,distance;
+        int angle, distance;
         bool ballSeen;
 }; 
\ No newline at end of file
diff --git a/include/data_source_controller.h b/include/data_source_controller.h
index 90b6c3c..e83332f 100644
--- a/include/data_source_controller.h
+++ b/include/data_source_controller.h
@@ -1,3 +1,4 @@
+#pragma once
 #include "data_source.h"
 #include "vars.h"
 #include <Arduino.h>
diff --git a/include/game.h b/include/game.h
new file mode 100644
index 0000000..8eaf387
--- /dev/null
+++ b/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();
+};
\ No newline at end of file
diff --git a/include/sensors.h b/include/sensors.h
index b5e1ad1..7ded117 100644
--- a/include/sensors.h
+++ b/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;
\ No newline at end of file
diff --git a/src/game.cpp b/src/game.cpp
new file mode 100644
index 0000000..05c974f
--- /dev/null
+++ b/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);
+}
+
+
diff --git a/src/main.cpp b/src/main.cpp
index cc0a32e..95f1375 100644
--- a/src/main.cpp
+++ b/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);
 }
\ No newline at end of file
diff --git a/src/sensors.cpp b/src/sensors.cpp
index b14aeec..c262ef9 100644
--- a/src/sensors.cpp
+++ b/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(){