working serial code

ArduinoDriver/ArduinoDriver.ino

@@ -53,7 +53,7 @@ void loop(void)
 
   // Check for connection problems, if nothing in for more than .1 secs stop motors
   while (Serial.available() < SIZE) {
-    if (timer0 > 100) {
+    if (timer0 > 200) {
       timer0 = 0;
       motorL.writeMicroseconds(1500);
       motorR.writeMicroseconds(1500);
@@ -62,23 +62,29 @@ void loop(void)
     delay(10);
   }
 
+  // We now have a full message read, search for SOM while keeping led lit.
+  digitalWrite(LED_BUILTIN, HIGH);
+  while(Serial.read() != 250); // Wait for SOM
+  bytes[0] = 250;
+  digitalWrite(LED_BUILTIN, LOW);
+  
   // Fill bytes array with serial data
-  while (Serial.available() >= SIZE) {
-    for (int i = 0; i < SIZE; i++) {
+  while (Serial.available() >= SIZE-1) { 
+    for (int i = 1; i < SIZE; i++) {
       bytes[i] = Serial.read();
     }
-    blinks(1,50);
+    //blinks(1,50);
   }
 
   //  0 meanst stop, 1 forward and 2 rev
   // SOM, L_dir, L_speed, R_dir, R_speed, EOM
 
   // If serial data looks ok, fill data array
-  if (bytes[0] == 250 && bytes[SIZE - 1] == 251) {
+  if (bytes[SIZE - 1] == 251) {
       //digitalWrite(LED_BUILTIN, HIGH);
       if (bytes[1] == 0) {
         motors[0] = 0;
-        motors[1] = 1;
+        motors[1] = 0;
         
       } else if (bytes[1] == 1) {
         motors[0] = 1;
@@ -90,7 +96,7 @@ void loop(void)
 
       if (bytes[3] == 0) {
         motors[2] = 0;
-        motors[3] = 1;
+        motors[3] = 0;
         
       } else if (bytes[3] == 1) {
         motors[2] = 1;
@@ -107,15 +113,17 @@ void loop(void)
       //blinks(3,50);
 
   } else {
-    blinks(1,50);
+    //blinks(2,50);
+    digitalWrite(LED_BUILTIN, HIGH);
     motorL.writeMicroseconds(1500);
     motorR.writeMicroseconds(1500);
+    digitalWrite(LED_BUILTIN, LOW);
   }
 
 
 
   timer0 = 0;
-  delay(10);
+  //delay(10);
 }
 
 

src/main.rs

@@ -169,7 +169,9 @@ fn main() {
                     if sucessful_receive {
                         sucessful_receive = false;
                         match port.write(&message[0..message_length]) {
-                            Ok(n) => {println!("Controller: sent {} bytes to arduino!", n);},
+                            Ok(_) => { 
+                                //println!("Controller: sent {} bytes to arduino!", n);
+                                },
                             Err(e) => {println!("Controller: could not write to arduino: {}", e); },
                         }
                     }