 // ----- state ------
 int speed = 100;
 
-int M1_on = 0;
-int M2_on = 0;
-int M3_on = 0;
-int M4_on = 0;
+int M_on[4] = { 0, 0, 0, 0 };
+char label[4][4] = { "MA1", "MA2", "MA3", "MA4" };
 
 unsigned long counter = 0;
 

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


void setup() {

}

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


