 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
+  Serial.println("Arm starting");
+
+  AFMS.begin();  // create with the default frequency 1.6KHz
+  Serial.println ("OK");
 }
+
 
 // ----- state ------
 int speed = 100;

#include <Adafruit_MotorShield.h>
//#include "utility/Adafruit_MS_PWMServoDriver.h"

Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x62); 
Adafruit_DCMotor *M1 = AFMS.getMotor(4);

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();
