From 57fe2fd18c58f63d777afa1f619d1a906d6595d2 Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker0@gmail.com>
Date: Thu, 17 Dec 2020 13:17:00 +0100
Subject: [PATCH] test: add a couple of tests for 32u4

---
 src/test_menu.cpp | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/src/test_menu.cpp b/src/test_menu.cpp
index 4340afa..1800a3b 100644
--- a/src/test_menu.cpp
+++ b/src/test_menu.cpp
@@ -31,6 +31,8 @@ void TestMenu :: testMenu(){
     DEBUG.println("6)Camera test");
     DEBUG.println("7)Line Sensors camera test");
     DEBUG.println("8)Line Sensors test");
+    DEBUG.println("u)Read Serial messages from 32u4");
+    DEBUG.println("s)Send test to 32u4 status LEDs");
     do{
         testNum = DEBUG.read();
         DEBUG.println();
@@ -46,6 +48,8 @@ void TestMenu :: testMenu(){
         else if (testNum == '6') DEBUG.println("Camera Test");
         else if (testNum == '7') DEBUG.println("Line Sensors camera test, turn on SW_ELETT");
         else if (testNum == '8') DEBUG.println("Line Sensors test, turn on SW_ELETT");
+        else if (testNum == 'u') DEBUG.println("Reading from 32u4");
+        else if (testNum == 's') DEBUG.println("Testing LEDs. Sending bytes to 32u4");
     else {
       DEBUG.println("UNKNOWN COMMAND");
       flagtest = false;
@@ -102,6 +106,27 @@ void TestMenu :: testMenu(){
                     }
                 }
             break;
+            case 'u':
+              while(Serial2.available()) DEBUG.print((char)Serial2.read());
+            break;
+            case 's':
+                DEBUG.println("Remember LED1 is not used by teensy");
+                DEBUG.println("LED2");
+                Serial2.write(0b00000001);
+                delay(1500);
+
+                DEBUG.println("LED3");
+                Serial2.write(0b00000010);
+                delay(1500);
+
+
+                DEBUG.println("LED4");
+                Serial2.write(0b00000100);
+                
+                delay(1500);
+                Serial2.write(0);
+                delay(1500);
+            break;
             default:
             break;
         }