Fix merge

crawler_software/arduino/crawler_slave/crawler_slave.ino

@@ -10,16 +10,28 @@
 // This servo is used to wipe and clean the camera lens
 Servo windowWiperServo;
 
+// Variables populated over i2c from master
+int id;
+int val;
+
 void setup() {
+  // For debugging
+  //Serial.begin(115200);
+  
   // Attach the wiper servo to pin 9
   windowWiperServo.attach(9);
 
   // This is the address the pi will speak to us at
-  Wire.begin(0x8);
+  Wire.begin(0x4);
   
   // Call receiveEvent when data received                
   Wire.onReceive(receiveEvent);
-  
+
+  // Setup LED
+  pinMode(LED_BUILTIN, OUTPUT);
+  digitalWrite(LED_BUILTIN, LOW);
+
+  //Serial.println("Started");
 }
 
 // Just loop to keep the running code alive, and wait for events to happen.
@@ -28,9 +40,24 @@ void loop() {
 }
 
 // This method runs when we receive a message
-void receiveEvent(int howMany) {
-  while (Wire.available()) { // loop through all but the last
-    int pos = Wire.read(); // receive byte as a int
-    windowWiperServo.write(pos);
+void receiveEvent(int n) {
+  Wire.read(); // Remove smbus trash
+  if (true) { // Dont do anything if this is not true
+    id  = Wire.read(); // ID of the servo/device to access
+    val = Wire.read(); // Value to assign
+
+    //Serial.println(id);
+    //Serial.println(val);
+  
+    switch(id) {
+      case 1:
+        windowWiperServo.write(val);
+        break;
+    }
+  }
+
+  // Prevents a bug where if bytes are left in buffer, arduino crashes.
+  while (Wire.available()) {
+    Wire.read();
   }
 }