#define SPEED_RANGE 10

int a_speed, b_speed, c_speed;

void varMotor(int s, const int m)
{
   int l=s;
   if (l>0)
   {
      OnFwd(m);
   }
   else if (l<0)
   {
      l=-l;
      OnRev(m);
   }
   else
   {
      Off(m);
   }
   Wait(l);      // On Time
      if (l > 0)
   {
      if (l > 3)
      {
         Float(m);
         Wait ((SPEED_RANGE-l) * 3);   // Off Time for > 3 w/ float
         if (l<9)
             SetPower(m, 4);
         else
             SetPower(m, 7);
      }
      else
      {
         Off(m);
         Wait (SPEED_RANGE-l);   // Off Time for < 3
      }
   }
}


task runMotorA()
{
   while (true)
   {
      varMotor(a_speed, OUT_A);
   }
}

task runMotorB()
{
   while (true)
   {
      varMotor(b_speed, OUT_B);
   }
}

task runMotorC()
{
   while (true)
   {
      varMotor(c_speed, OUT_C);
   }
}

void forward(int speed)
{
   PlaySound(SOUND_CLICK);
   a_speed = 0; b_speed = -speed; c_speed = speed;
}

task main()
{
   a_speed = 0; b_speed = 0; c_speed = 0;
   start runMotorA;
   start runMotorB;
   start runMotorC;

//   a_speed = 9; b_speed = -6; c_speed = -6; Wait(300);
//   a_speed = 0; b_speed = 0; c_speed = 0; Wait(10);


   // Square - no turn
   b_speed = -10; c_speed = 10; Wait(300);
   a_speed = 0; b_speed = 0; c_speed = 0; Wait(10);

   a_speed = 10; b_speed = -6; c_speed = -6; Wait(300);
   a_speed = 0; b_speed = 0; c_speed = 0; Wait(10);

   b_speed = 10; c_speed = -10; Wait(200);
   a_speed = 0; b_speed = 0; c_speed = 0; Wait(10);

   a_speed = -10; b_speed = 6; c_speed = 6; Wait(300);
   a_speed = 0; b_speed = 0; c_speed = 0; Wait(10);

   // Circle while turning
   a_speed = 7; b_speed = -10; c_speed = 10; Wait(900);
   a_speed = 0; b_speed = 0; c_speed = 0; Wait(10);


   // Turn about 1 wheel
   a_speed = 0; b_speed = 9; c_speed = 9; Wait(400);
   a_speed = 0; b_speed = 0; c_speed = 0; Wait(10);

   // Forward a little
   a_speed = 0; b_speed = -8; c_speed = 8; Wait(200);
   a_speed = 0; b_speed = 0; c_speed = 0; Wait(10);

   // Spin
   a_speed = -10; b_speed = -10; c_speed = -10; Wait(300);
   a_speed = 0; b_speed = 0; c_speed = 0; Wait(10);


   stop runMotorA; stop runMotorB; stop runMotorC;
   Off(OUT_A+OUT_B+OUT_C);
}