controller output update

ArduinoDriver/ArduinoDriver.ino

@@ -26,6 +26,9 @@ elapsedMillis timer0;
 
 void setup(void)
 {
+  pinMode(LED_BUILTIN, OUTPUT);
+  blinks(5, 100);
+
   // Initialize DC motor pins
   pinMode(ML, OUTPUT);
   pinMode(MR, OUTPUT);
@@ -64,6 +67,7 @@ void loop(void)
     for (int i = 0; i < SIZE; i++) {
       bytes[i] = Serial.read();
     }
+    blinks(1,50);
   }
 
   //  0 meanst stop, 1 forward and 2 rev
@@ -71,6 +75,7 @@ void loop(void)
 
   // If serial data looks ok, fill data array
   if (bytes[0] == 250 && bytes[SIZE - 1] == 251) {
+      //digitalWrite(LED_BUILTIN, HIGH);
       if (bytes[1] == 0) {
         motors[0] = 0;
         motors[1] = 1;
@@ -98,8 +103,11 @@ void loop(void)
       
       motorL.writeMicroseconds(1500+motors[0]*motors[1]);
       motorR.writeMicroseconds(1500+motors[2]*motors[3]);
+      //digitalWrite(LED_BUILTIN, LOW);
+      //blinks(3,50);
 
   } else {
+    blinks(1,50);
     motorL.writeMicroseconds(1500);
     motorR.writeMicroseconds(1500);
   }
@@ -109,3 +117,15 @@ void loop(void)
   timer0 = 0;
   delay(10);
 }
+
+
+void blinks(int n, int d) {
+  for(int i=0;i<n;i++){
+      digitalWrite(LED_BUILTIN, HIGH);
+      delay(d);
+      digitalWrite(LED_BUILTIN, LOW);
+      delay(d);
+  }
+}
+
+

src/main.rs

@@ -126,7 +126,11 @@ fn main() {
                                     message[received_bytes] = b;
                                     received_bytes+=1; // since length now is total bytes, not index
                                     //println!("Server found end of message");
-                                    println!("Cotroller got: {:?}", String::from_utf8_lossy(&message[0..received_bytes]));
+                                    print!("Controller got: ");
+                                    for k in 0..received_bytes {
+                                        print!("{}, ", message[k] );
+                                    }
+                                    
                                     //let written_size = stream.write(&[69 as u8, 96 as u8]);
                                     //let written_size = stream.write_all(b"\x02This is a bunch of lidar data...\x03");
                                     //println!("server wrote to stream.");
@@ -152,7 +156,7 @@ fn main() {
 
 
                     // get motors current speeds
-                    println!("Controller requesting pos data.");
+                    println!("Controller: requesting pos data.");
                     controller_request_location_tx.send(10 as u8).unwrap();
                     state = controller_send_location_rx.recv().unwrap();