From f34cb81c7ad69239b5bbb938005c9780555ff777 Mon Sep 17 00:00:00 2001
From: KenwoodFox <kenwood364@gmail.com>
Date: Sun, 4 Jul 2021 20:32:04 -0400
Subject: [PATCH] Update new sorting/recv mechanism

---
 .../arduino/crawler_slave/crawler_slave.ino   | 44 +++++--------------
 1 file changed, 12 insertions(+), 32 deletions(-)

diff --git a/crawler_software/arduino/crawler_slave/crawler_slave.ino b/crawler_software/arduino/crawler_slave/crawler_slave.ino
index 820dccb..e3e6cad 100644
--- a/crawler_software/arduino/crawler_slave/crawler_slave.ino
+++ b/crawler_software/arduino/crawler_slave/crawler_slave.ino
@@ -10,27 +10,20 @@
 // This servo is used to wipe and clean the camera lens
 Servo windowWiperServo;
 
-// This is a variable capable of storing up to 4 items
-char instruction[32] = "";
-
-// Values for the id, and raw value of an instruction
-char * id;
-char * raw_value;
+// Variables populated over i2c from master
+int id;
+int val;
 
 void setup() {
   // Attach the wiper servo to pin 9
   windowWiperServo.attach(9);
 
-  // This is only useful for debugging
-  //Serial.begin(9600);
-
   // This is the address the pi will speak to us at
-  Wire.begin(0x4);
+  Wire.begin(0x8);
   
   // Call receiveEvent when data received                
   Wire.onReceive(receiveEvent);
-
-  //Serial.println("Finished Setup.");  
+  
 }
 
 // Just loop to keep the running code alive, and wait for events to happen.
@@ -40,27 +33,14 @@ void loop() {
 
 // This method runs when we receive a message
 void receiveEvent(int n) {
-
-  for (int i = 0; i < n; i++) {
-    instruction[i] = Wire.read();
-    instruction[i + 1] = '\0'; //add null after ea. char
+  while (Wire.available()) { // loop through all but the last
+    id  = Wire.read(); // ID of the servo/device to access
+    val = Wire.read(); // Value to assign
   }
 
-  //RPi first byte is cmd byte so shift everything to the left 1 pos so temp contains our string
-  for (int i = 0; i < n; ++i)
-    instruction[i] = instruction[i + 1];
-
-  id = strtok (instruction, "-");
-  raw_value = strtok (NULL, "-");
-
-  int value = atoi(raw_value);
-
-  //Serial.println(id);
-  //Serial.println(value);
-
-  // Switch statements dont work on char *
-  if (strcmp(id,"WIPE") == 0) // if id, and WIPE cancel out
-  {
-    windowWiperServo.write(value);
+  switch(id) {
+    case 1:
+      windowWiperServo.write(val);
+      break;
   }
 }