started writing goalie(), kinda dumb but seems to work

include/game.h

@@ -7,13 +7,15 @@
 #include "sensors.h"
 #include "vars.h"
 #include "data_source_bno055.h"
+#include "goalie.h"
+#include "keeper.h"
 
 class Game{
     public:
         Game();
-        void keeper();
-        void goalie();
-        void ballBack();
+        //void keeper();
+        //void goalie();
+        //void ballBack();
         bool role, attackGoal; //1->goalie 0->keeper, 1->yellow 0->blue
         //~Game();
 };

include/goalie.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "game.h"
+
+class Goalie{
+
+    public:
+        Goalie();
+        void goalie();
+        void ballBack();
+        //void rigore(); to be implemented
+
+};

include/keeper.h

@@ -0,0 +1,11 @@
+#pragma once
+
+#include "game.h"
+
+class Keeper{
+
+    public:
+        Keeper();
+        void keeper();
+        //void keeperGoalie(); to be implemented
+};

include/sensors.h

@@ -1,4 +1,5 @@
 #include <Arduino.h>
+#pragma once
 #include "data_source_bno055.h"
 #include "data_source_ball.h"
 #include "data_source_camera.h"
@@ -6,7 +7,6 @@
 #include "motor.h"
 #include "data_source_controller.h"
 #include "drivecontroller.h"
-#pragma once
 #include "game.h"
 #ifdef SENSORS_CPP
 #define extr 
@@ -15,6 +15,8 @@
 #endif
 
 class Game;
+class Goalie;
+class Keeper;
 void initSensors();
 void updateSensors();
 
@@ -24,4 +26,7 @@ extr DataSourceCamera* camera;
 //extr DataSource* us;
 extr DataSourceController* usCtrl;
 extr DriveController* drive;
+
 extr Game* game;
+extr Goalie* goalie;
+extr Keeper* keeper;

src/game.cpp

@@ -4,16 +4,14 @@ Game::Game(){
 
 }
 
-void Game::goalie(){
-    compass->readSensor();
-    //drive->prepareDrive(0, 150, 0);
+/* void Game::goalie(){
+  /*   //drive->prepareDrive(0, 150, 0);
     digitalWrite(LED_G, HIGH);
     if(ball->ballSeen){
-        if(ball->angle >= 0 && ball->angle <= 45) drive->drive(0, 75, 0);
-        //else if(ball->angle <= 45) drive->drive(45, 75, 0);
-        else if(ball->angle > 45 && ball->angle <= 90) drive->drive(90, 75, 0);
-        //else if(ball->angle <= 135) drive->drive(135, 75, 0);
-        else if(ball->angle > 90 && ball->angle <= 180) drive->drive(180, 75, 0);
+        if(ball->angle >= 0 && ball->angle < 45) drive->drive(0, 75, 0);
+        else if(ball->angle >= 45 && ball->angle <= 60) drive->drive(45, 75, 0);
+        else if(ball->angle > 60 && ball->angle < 90) drive->drive(75, 75, 0);
+        else if(ball->angle >= 90 && ball->angle < 135) drive->drive(90, 75, 0);
     } else {
         drive->prepareDrive(0, 0, 0);
         drive->drive(0, 0, 0);
@@ -21,7 +19,6 @@ void Game::goalie(){
 }
 
 void Game::keeper(){
-    compass->readSensor();
     digitalWrite(LED_Y, HIGH);
     drive->prepareDrive(0, 0, 0);
     drive->drive(0, 0, 0);
@@ -29,6 +26,4 @@ void Game::keeper(){
 
 void ballBack(){
 
-}
-
-
+} */

src/goalie.cpp

@@ -0,0 +1,22 @@
+#include "goalie.h"
+
+Goalie::Goalie(){
+
+}
+
+void Goalie::goalie(){
+    digitalWrite(LED_G, HIGH);
+    if(ball->ballSeen){
+        if(ball->angle >= 0 && ball->angle < 45) drive->drive(ball->angle, 75, 0);
+        else if(ball->angle >= 45 && ball->angle <= 60) drive->drive(ball->angle, 75, 0);
+        else if(ball->angle > 60 && ball->angle < 90) drive->drive(ball->angle, 75, 0);
+        else if(ball->angle >= 90 && ball->angle < 135) drive->drive(ball->angle, 75, 0); 
+    } else {
+        drive->prepareDrive(0, 0, 0);
+        drive->drive(0, 0, 0);
+    }
+}
+
+void Goalie::ballBack(){
+
+}

src/keeper.cpp

@@ -0,0 +1,15 @@
+#include "keeper.h"
+
+Keeper::Keeper(){
+
+}
+
+void Keeper::keeper(){
+    digitalWrite(LED_Y, HIGH);
+    drive->prepareDrive(0, 0, 0);
+    drive->drive(0, 0, 0);
+}
+
+/* void Keeper::keeperGoalie(){
+    to be implemented
+} */

src/main.cpp

@@ -12,14 +12,14 @@ void setup() {
 }
 
 void loop() {
-  //updateSensors();
+  updateSensors();
   /*if(millis() % 100 == 0)
   DEBUG_PRINT.println(us->getValue());*/
-  game->role = digitalRead(SWITCH_DX);
+ /*  game->role = digitalRead(SWITCH_DX);
   game->attackGoal = digitalRead(SWITCH_SX);
-  if(game->role) game->goalie();
-  else game->keeper();
-  //game->goalie();
+  if(game->role) goalie->goalie();
+  else keeper->keeper(); */
+  goalie->goalie();
   //drive->drivePrepared();
   //compass->test();
   //usCtrl->test();

src/sensors.cpp

@@ -14,6 +14,8 @@ void initSensors(){
     usCtrl = new DataSourceController(dUs);
 
     game = new Game();
+    goalie = new Goalie();
+    keeper = new Keeper();
 }
 
 void updateSensors(){