 // Sets left Motor Speed -100 to 100
 void leftMotorSpeed(int Speed)
 {
-    if (Speed == 0)
+   char Direction = 'N';
+   if (Speed == 0 ||
+       Speed > 100 ||
+       Speed < -100)
    {
-     analogWrite(PIN_LEFT_REV, 0);
-     analogWrite(PIN_LEFT_FOR, 0);
+     Speed = 0;
+     digitalWrite(PIN_LEFT_FOR, LOW);
+     digitalWrite(PIN_LEFT_REV, LOW);
+     analogWrite(PIN_LEFT_SPEED,0);
    }
    else if (Speed < 0)
    {
+     Direction = 'R';
      Speed = Speed*-1;
      // Convert from 0-100 to 0-255
-     analogWrite(PIN_LEFT_FOR, 0);
-     analogWrite(PIN_LEFT_REV, (Speed/100)*255);
+     digitalWrite(PIN_LEFT_FOR, LOW);
+     digitalWrite(PIN_LEFT_REV, HIGH);
+     analogWrite(PIN_LEFT_SPEED,(Speed*255)/100);
    }
    else
    {
-     analogWrite(PIN_LEFT_REV, 0);
-     analogWrite(PIN_LEFT_FOR, (Speed/100)*255);
+     Direction = 'F';
+     digitalWrite(PIN_LEFT_FOR, HIGH);
+     digitalWrite(PIN_LEFT_REV, LOW);
+     analogWrite(PIN_LEFT_SPEED,(Speed*255)/100);
    }
-   Serial.print(CMD_OUT_DRIVE_MOTOR_LEFT_SPEED+":"+Speed+"\n");
+   Serial.print(CMD_OUT_DRIVE_MOTOR_LEFT_SPEED+":"+Speed+","+Direction+"\n");
 }
 
 void rightMotorSpeed(int Speed)
   {
-    if (Speed == 0)
+    char Direction = 'N';
+    if (Speed == 0 ||
+       Speed > 100 ||
+       Speed < -100)
      {
-       analogWrite(PIN_RIGHT_REV, 0);
-       analogWrite(PIN_RIGHT_FOR, 0);
+       Speed = 0;
+       digitalWrite(PIN_RIGHT_FOR, LOW);
+       digitalWrite(PIN_RIGHT_REV, LOW);
+       analogWrite(PIN_RIGHT_SPEED,0);
      }
      else if (Speed < 0)
      {
        Speed = Speed*-1;
+       Direction = 'R';
        // Convert from 0-100 to 0-255
-       analogWrite(PIN_RIGHT_FOR, 0);
-       analogWrite(PIN_RIGHT_REV, (Speed/100)*255);
+       digitalWrite(PIN_RIGHT_FOR, LOW);
+       digitalWrite(PIN_RIGHT_REV, HIGH);
+       analogWrite(PIN_RIGHT_SPEED,(Speed*255)/100);
      }
      else
      {
-       analogWrite(PIN_RIGHT_REV, 0);
-       analogWrite(PIN_RIGHT_FOR, (Speed/100)*255);
+       Direction = 'F';
+       digitalWrite(PIN_RIGHT_FOR, HIGH);
+       digitalWrite(PIN_RIGHT_REV, LOW);
+       analogWrite(PIN_RIGHT_SPEED,(Speed*255)/100);
      }
-     Serial.print(CMD_OUT_DRIVE_MOTOR_RIGHT_SPEED+":"+Speed+"\n");
-}
-
+     Serial.print(CMD_OUT_DRIVE_MOTOR_RIGHT_SPEED+":"+Speed+","+Direction+"\n");
+}
+

       digitalWrite(PIN_SPI_SCK,  HIGH);
       
       Serial.print(CMD_OUT_READY+" Program\n");
     }
  }
  
}

    char inChar = (char)Serial.read();
    // add it to the inputString:
    inputString += inChar;
    
    // if the incoming character is a newline, set a flag
    // so the main loop can do something about it:
    if (inChar == '\n') {
