 }
 
 // 'digitalWrite' command
-// ex: "W031" or "W3 1" (write 1 to pin 3)
-void dwrite()
+// ex: W031 - set pin 3 HIGH, W100 - set pin 10 LOW
+void _write()
 {
   byte pin = bctoi(1, 2);
   if (pin < 2 || pin > 10) { err(); return; }
   pinMode(pin, OUTPUT);
-  if (buf[3] == '0') digitalWrite(pin, 0); else digitalWrite(pin, 1);
+  if (buf[3] == '0') digitalWrite(pin, LOW); else digitalWrite(pin, HIGH);
+}
+
+//=======================================
+
+// 'read' command
+void _read()
+{
+  cmd = buf[1];
+  if (cmd == 'D' || cmd == 'd') dread();
+  else
+  if (cmd == 'A' || cmd == 'a') aread();
+  else
+    nak();
 }
 
 // 'digitalRead' command
-// ex: RD5 or RD10
+// ex: RD5 or RD05 or RD10
 void dread()
 {
   byte pin = bctoi(2, 2);
   if (pin < 2 || pin > 10) { err(); return; }
   pinMode(pin, INPUT_PULLUP);
-  unsigned int a;
-  a = digitalRead(pin);
+  unsigned int a = digitalRead(pin);
   Serial.print("RD");
-  Serial.print((unsigned int) pin);
+  Serial.print((unsigned int)pin);
   Serial.print("=");
   Serial.println(a);
 }
 
 // 'analogRead' command
 // ex: RA0
+// result in mV
 void aread()
 {
   byte pin = bctoi(2, 1);

#include <BMSerial.h>
#include <RoboClaw.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

#define LED_PIN 13



// 'drive' command. both motors in one command
// ex: DF - drive forward at 'motor_velocity'
// ex: DD127127 - drive differential. full forward
// ex: DD000000 - drive differential. full backward
void drive()
{
  cmd = buf[1];
