 #include <Adafruit_MotorShield.h>
-//#include "utility/Adafruit_MS_PWMServoDriver.h"
+#include "utility/Adafruit_MS_PWMServoDriver.h"
 
 Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x62); 
 Adafruit_DCMotor *M1 = AFMS.getMotor(4);

Adafruit_DCMotor *M3 = AFMS.getMotor(2);
Adafruit_DCMotor *M4 = AFMS.getMotor(1);

void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Arm OK!");
  Serial.print(analogRead(5));
  Serial.print(' ');
  Serial.print(analogRead(4));
  Serial.print(' ');
  Serial.println(analogRead(3));
  
  //AFMS.begin();  // create with the default frequency 1.6KHz
}

// ----- state ------
int speed = 100;

int M3_on = 0;
int M4_on = 0;

unsigned long counter = 0;

void loop() {
  int command;
  if (millis() - counter > 10000) {
     counter = millis();
     Serial.print   ("=A0 ");
     Serial.println (analogRead(0));
     Serial.print   ("=A1 ");
     Serial.println (analogRead(1));
     Serial.print   ("=A2 ");
     Serial.println (analogRead(2));
  }
  if (Serial.available()) {
     command = Serial.read();
