#define DRIVE OUT_A + OUT_B + OUT_C
#define CONTROL SENSOR_1

#define RAMPTIME 80

task main () {
  SetSensor(CONTROL, SENSOR_TOUCH);
  SelectDisplay(DISPLAY_OUT_C);
  
  while (true) {
    ClearMessage();
    until (Message() != 0);
    if (Message() == 1) {
      SpinUp();
      Off(DRIVE);
    } else if (Message() == 2) {
      SpinUp();
      WaitMessage(3);
      Off(DRIVE);
    }
  }
}         

void SetAndWait(const int speed) {
  SetPower(DRIVE, speed);
  Wait(RAMPTIME);
}

void WaitMessage (int msg) {
  ClearMessage();
  until (Message() == msg);
  ClearMessage();
}

void SpinUp () {
  SetPower(DRIVE, 0);
  On(DRIVE);
  Wait(RAMPTIME);
  SetAndWait(1);
  SetAndWait(2);
  SetAndWait(3);
  SetAndWait(4);
  SetAndWait(5);
  SetAndWait(6);
  SetAndWait(7);
}