 #include "utility/Adafruit_MS_PWMServoDriver.h"
 
 Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x62); 
-Adafruit_DCMotor *M1 = AFMS.getMotor(4);
-Adafruit_DCMotor *M2 = AFMS.getMotor(3);
-Adafruit_DCMotor *M3 = AFMS.getMotor(2);
-Adafruit_DCMotor *M4 = AFMS.getMotor(1);
+Adafruit_DCMotor *M[4] = { AFMS.getMotor(4),
+                           AFMS.getMotor(3),
+                           AFMS.getMotor(2),
+                           AFMS.getMotor(1) };
 
 
 void setup() {

  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();
     switch (command) {
       case '1':
         M1_on = M1_on ? 0 : 1;
         if (M1_on) {
            M1->setSpeed(speed);
            M1->run(FORWARD);
         } else {
            M1->run(RELEASE);
         }
         break;
       case '2':
         M2_on = M2_on ? 0 : 1;
         if (M2_on) {
            M2->setSpeed(speed);
            M2->run(FORWARD);
         } else {
            M2->run(RELEASE);
         }
         break;
       case '3':
         M3_on = M3_on ? 0 : 1;
         if (M3_on) {
            M3->setSpeed(speed);
            M3->run(FORWARD);
         } else {
            M3->run(RELEASE);
         }
         break;
       case '4':
         M4_on = M4_on ? 0 : 1;
         if (M4_on) {
            M4->setSpeed(speed);
            M4->run(FORWARD);
         } else {
            M4->run(RELEASE);
         }
         break;
       case 'q':
         M1_on = M1_on ? 0 : 1;
         if (M1_on) {
            M1->setSpeed(speed);
            M1->run(BACKWARD);
         } else {
            M1->run(RELEASE);
         }
         break;
       case 'w':
         M2_on = M2_on ? 0 : 1;
         if (M2_on) {
            M2->setSpeed(speed);
            M2->run(BACKWARD);
         } else {
            M2->run(RELEASE);
         }
         break;
       case 'e':
         M3_on = M3_on ? 0 : 1;
         if (M3_on) {
            M3->setSpeed(speed);
            M3->run(BACKWARD);
         } else {
            M3->run(RELEASE);
         }
         break;
       case 'r':
         M4_on = M4_on ? 0 : 1;
         if (M4_on) {
            M4->setSpeed(speed);
            M4->run(BACKWARD);
         } else {
            M4->run(RELEASE);
         }
         break;
       case 'x':
         do_stop();

}

void do_stop() {
  set_motor_speed (0, 0.0);
  set_motor_speed (1, 0.0);
  set_motor_speed (2, 0.0);
  set_motor_speed (3, 0.0);
}

void set_motor_speed (int motor, float mspeed) {
  int new_speed = (int) (mspeed * (float) speed);
  if (abs(new_speed) < 5) { new_speed = 0; }
  if (new_speed == 0) { M[motor]->run(RELEASE); } // If the motor should be shut off, don't get confused.
  if (new_speed == M_on[motor]) { return; }
  announce (label[motor], new_speed);
  M_on[motor] = new_speed;
  if (new_speed < 0) {
     M[motor]->setSpeed (-new_speed);
     M[motor]->run (BACKWARD);
  } else {
     M[motor]->setSpeed (new_speed);
     M[motor]->run (FORWARD);
  }
}

void announce (char* name, int value) {
  Serial.print ("=");
  Serial.print (name);
  Serial.print (" ");
  Serial.println (value);
}


