From 5e88a21293db0fe6d4b46db5544bb86b097010f2 Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker0@gmail.com>
Date: Mon, 19 Apr 2021 16:05:42 +0200
Subject: [PATCH] sensors: add the robot identifier switch

and add test for switches
---
 include/sensors/sensors.h |  3 ++-
 include/vars.h            |  6 +++---
 src/sensors/sensors.cpp   | 10 ++++++----
 src/test_menu.cpp         | 13 ++++++++++++-
 4 files changed, 23 insertions(+), 9 deletions(-)

diff --git a/include/sensors/sensors.h b/include/sensors/sensors.h
index 95b323d..d1132c8 100644
--- a/include/sensors/sensors.h
+++ b/include/sensors/sensors.h
@@ -35,4 +35,5 @@ s_extr DataSourceCameraConic* camera;
 s_extr DriveController* drive;
 s_extr DataSourceBT* bt;
 
-s_extr int role;
\ No newline at end of file
+s_extr int role;
+s_extr int robot_indentifier;
\ No newline at end of file
diff --git a/include/vars.h b/include/vars.h
index 0bb05e7..166d0d9 100755
--- a/include/vars.h
+++ b/include/vars.h
@@ -14,8 +14,8 @@ the new setup is the one commented, leds have to be written
 in the 32U4 code*/
 
 #define BUZZER 6
-#define SWITCH_1 39
-#define SWITCH_2 38
-#define SWITCH_3 33
+#define SWITCH_SX 39
+#define SWITCH_DX 38
+#define SWITCH_ID 33
 
 extr float sins[360], cosins[360];
diff --git a/src/sensors/sensors.cpp b/src/sensors/sensors.cpp
index e7bfa1c..433c0a7 100644
--- a/src/sensors/sensors.cpp
+++ b/src/sensors/sensors.cpp
@@ -2,8 +2,9 @@
 #include "sensors/sensors.h"
 
 void initSensors(){
-    pinMode(SWITCH_1, INPUT);
-    pinMode(SWITCH_2, INPUT);
+    pinMode(SWITCH_DX, INPUT);
+    pinMode(SWITCH_SX, INPUT);
+    pinMode(SWITCH_ID, INPUT);
     
     drive = new DriveController(new Motor(12, 11, 4, 55), new  Motor(25, 24, 5, 135), new Motor(27, 26, 2, 225), new Motor(29, 28, 3, 305));
         //drive = new DriveController(new Motor(12, 11, 2, 45),new  Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
@@ -17,8 +18,9 @@ void initSensors(){
  }
 
 void updateSensors(){
-    role = digitalRead(SWITCH_1);
-    camera->goalOrientation = digitalRead(SWITCH_2);
+    role = digitalRead(SWITCH_DX);
+    camera->goalOrientation = digitalRead(SWITCH_SX);
+    robot_indentifier = digitalRead(SWITCH_ID);
     
     compass->update();
     ball->update();
diff --git a/src/test_menu.cpp b/src/test_menu.cpp
index a2a1f3c..501b8d7 100644
--- a/src/test_menu.cpp
+++ b/src/test_menu.cpp
@@ -79,7 +79,17 @@ void TestMenu::testMenu()
         case '8':
             DEBUG.println("Camera tilt Test");
             drive->resetDrive();
-            drive->prepareDrive(0, 0, (CURRENT_DATA_READ.angleAtkFix + 360) % 360);
+            drive->prepareDrive(0, 0, CURRENT_DATA_READ.angleAtkFix);
+            break;
+        case '9':
+            DEBUG.println("Switches as seen from behind:");
+            DEBUG.print("Right switch (role): ");
+            DEBUG.println(role);
+            DEBUG.print("Left switch (goalOrientation): ");
+            DEBUG.println(camera->goalOrientation);
+            DEBUG.print("Robot Identifier: ");
+            DEBUG.println(robot_indentifier);
+            delay(50);
             break;
         case 'u':
             DEBUG.println("32u4 receive Test");
@@ -118,6 +128,7 @@ void TestMenu::testMenu()
             DEBUG.println("6)Camera test");
             DEBUG.println("7)Line Sensors camera test");
             DEBUG.println("8)Camera tilt test");
+            DEBUG.println("9)Switches test");
             DEBUG.println("u)Read Serial messages from 32u4");
             DEBUG.println("s)Send test to 32u4 status LEDs");
             flagtest = false;