   else
   if (cmd == 'P' || cmd == 'p') power();
   else
-  if (cmd == 'S' || cmd == 's') settings();
+  if (cmd == 'S' || cmd == 's') set();
   else
   if (cmd == 'B' || cmd == 'b') battery();
   else
-  if (cmd == 'R' || cmd == 'r')
-  {
-    cmd = buf[1];
-    if (cmd == 'D' || cmd == 'd') dread();
-    else
-    if (cmd == 'A' || cmd == 'a') aread();
-    else
-    {
-      nak(); return;
-    }
-  
-    OnValidCommand();
-    return;
-  }
-  else
-  if (cmd == 'W' || cmd == 'w') dwrite();
+  if (cmd == 'R' || cmd == 'r') _read();
+  else
+  if (cmd == 'W' || cmd == 'w') _write();
   else
   if (cmd == 'Z' || cmd == 'z') { OnDisconnected(); return; }
   else

unsigned long lastCommandTime;
unsigned long commandTime;
unsigned long cmdTimeout = 2500;
boolean ledCmdOn = false;
boolean IsConnected = false;

// RoboClaw //

  else
  if (cmd == 'R' || cmd == 'r') { motor1_target = MOTOR_VELOCITY_0 + motor_velocity; motor2_target = MOTOR_VELOCITY_0 - motor_velocity; }
  else
  if (cmd == 'S' || cmd == 's') { motor1_target = MOTOR_VELOCITY_0; motor2_target = MOTOR_VELOCITY_0; }
  else
  if (cmd == 'E' || cmd == 'e') driveEnable(true);
  else
  if (cmd == 'O' || cmd == 'o') driveEnable(false);
  else
  if (cmd == 'V' || cmd == 'v')
  {
