   AFMS.begin();  // create with the default frequency 1.6KHz
 }
 
+// ----- state ------
+int speed = 200;
+int suddenness = 100;
+int trim = 0;
+
+int Vrf = 0;
+int Vlf = 0;
+int Vrb = 0;
+int Vlb = 0;
+int speed_rf = 0;
+int speed_lf = 0;
+int speed_rb = 0;
+int speed_lb = 0;
+
+unsigned long current_time = 0;
+
 void loop() {
   int command;
   if (Serial.available()) {
      command = Serial.read();
      switch (command) {
+       case 'A':
+         Vlf = 10;
+         break;
+       case 'B':
+         Vrf = 10;
+         break;
+       case 'C':
+         Vlb = 10;
+         break;
+       case 'D':
+         Vrb = 10;
+         break;
        case 'f':
          do_forward();
          break;

         break;
     }
  }
}

void do_forward() {
  M1->setSpeed(200);
  M2->setSpeed(200);
  M3->setSpeed(200);
  M4->setSpeed(200);
  M1->run(FORWARD);
  M2->run(FORWARD);
  M3->run(FORWARD);
  M4->run(FORWARD);
}

void do_reverse() {
  M1->setSpeed(200);
  M2->setSpeed(200);
  M3->setSpeed(200);
  M4->setSpeed(200);
  M1->run(BACKWARD);
  M2->run(BACKWARD);
  M3->run(BACKWARD);
  M4->run(BACKWARD);
}

void do_right_turn() {
  M1->setSpeed(200);
  M2->setSpeed(0);
  M3->setSpeed(200);
  M4->setSpeed(0);
  M1->run(FORWARD);
  M2->run(FORWARD);
  M3->run(FORWARD);
  M4->run(FORWARD);
}

void do_left_turn() {
  M1->setSpeed(0);
  M2->setSpeed(200);
  M3->setSpeed(0);
  M4->setSpeed(200);
  M1->run(FORWARD);
  M2->run(FORWARD);
  M3->run(FORWARD);
  M4->run(FORWARD);
}

void do_pivot_left() {
  M1->setSpeed(100);
  M2->setSpeed(100);
  M3->setSpeed(100);
  M4->setSpeed(100);
  M1->run(BACKWARD);
  M2->run(FORWARD);
  M3->run(BACKWARD);
  M4->run(FORWARD);
}

void do_stop() {
  M1->setSpeed(0);
  M2->setSpeed(0);
  M3->setSpeed(0);
  M4->setSpeed(0);
  M1->run(FORWARD);
  M2->run(FORWARD);
  M3->run(FORWARD);
  M4->run(FORWARD);
}



//#include "utility/Adafruit_MS_PWMServoDriver.h"

Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
Adafruit_DCMotor *Mrf = AFMS.getMotor(1);
Adafruit_DCMotor *Mlf = AFMS.getMotor(4);
Adafruit_DCMotor *Mrb = AFMS.getMotor(2);
Adafruit_DCMotor *Mlb = AFMS.getMotor(3);

void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
