#define ANGLE_SPAN 295


void Lift() {
  OnFwdRegEx(OUT_A,5,OUT_REGMODE_SPEED,RESET_NONE);
  while (MotorTachoCount(OUT_A) < 10);
  OnFwdRegEx(OUT_A,15,OUT_REGMODE_SPEED,RESET_NONE);
  while (MotorTachoCount(OUT_A) < ANGLE_SPAN-15);
  OnFwdRegEx(OUT_A,10,OUT_REGMODE_SPEED,RESET_NONE);
  while (MotorTachoCount(OUT_A) < ANGLE_SPAN-3);
  OffEx(OUT_A,RESET_NONE);
}

void Drop() {
  OnRevRegEx(OUT_A,5,OUT_REGMODE_SPEED,RESET_NONE);
  while (MotorTachoCount(OUT_A) > ANGLE_SPAN-10);
  OnRevRegEx(OUT_A,15,OUT_REGMODE_SPEED,RESET_NONE);
  while (MotorTachoCount(OUT_A) > 15);
  OnRevRegEx(OUT_A,10,OUT_REGMODE_SPEED,RESET_NONE);
  while (MotorTachoCount(OUT_A) > 10);
  OffEx(OUT_A,RESET_NONE);
}

void calibrateForks() {

  long lastPosition;
  OnFwdEx(OUT_A,10,RESET_NONE);
  Wait(200);

  OnRevRegEx(OUT_A,5,OUT_REGMODE_SPEED,RESET_NONE);
  Wait(150);

  do {
    lastPosition = MotorTachoCount(OUT_A);
    Wait(100);
  } while (MotorTachoCount(OUT_A) - lastPosition < 0);

  Coast(OUT_A);
  Wait(50);
  OnFwdEx(OUT_A,20,RESET_NONE);
  Wait(150);
  Coast(OUT_A);
  Wait(150);

  // we're at the bottom now
  ResetTachoCount(OUT_A);
}



