#define    POWER                     100
#define    REGMODE                   OUT_REGMODE_IDLE

#define    STABILIZATION_DEGREES     200
#define    MEASUREMENT_DEGREES       (6*360)

#define    LCD_LINEHEIGHT            8


// with 200g: 6*360 degrees = 45.8 cm

// with 500g: 6*360 degrees = 45.4 cm

task display();

task main() {

  byte fileHandle;
  int currentRun = 0;
  long startTime;
  string tmpNumStr;

  // prepare to write data into file:
  DeleteFile ("motor_time_data.txt");
  CreateFile ("motor_time_data.txt", 1024, fileHandle);

  start display;

  /* we assume that when the program starts, the weight
   * is at it's highest position (wind it up if not) */

  ResetAllTachoCounts(OUT_A);

  /* the main program loop repeats 5 times, OR until interrupted by a button press.
   * the loop assumes the weight is at the top when the loop starts */

  while (!ButtonCount(BTNRIGHT,true) && currentRun < 5) {

    // first we drop the weight down low enough (Fwd = down)
    OnFwdEx(OUT_A,100,RESET_NONE);
    while (MotorTachoCount(OUT_A) < STABILIZATION_DEGREES + MEASUREMENT_DEGREES);

    OffEx(OUT_A,RESET_NONE);
    Wait(800);
    // now we're sitting at the bottom

    // start bringing the weight up:
    PlayTone(440,150);
    OnRevRegEx(OUT_A, POWER, REGMODE, RESET_NONE);
  
    // wait a bit for the speed and current to stabilize:
    while (MotorTachoCount(OUT_A) > MEASUREMENT_DEGREES);

    /* now we're in the constant-speed measurement range, so we
     * start the timer (and start reading the ammeter)  */
    PlayTone(660,150);
    startTime = CurrentTick();
  
    // ...and keep moving up (through MEASUREMENT_DEGREES) until we get back to zero
    while (MotorTachoCount(OUT_A) > 0);

    // record time immediately
    tmpNumStr = NumToStr(CurrentTick()-startTime);

    PlayTone(880,150);
    OffEx(OUT_A,RESET_NONE);
  
    /* record time period in data file
     * (the third parameter requires a variable to store the number of bytes
     * written; we don't care so we give it a [presently] unused variable) */
    WriteLnString (fileHandle, tmpNumStr, startTime);

    // also display info on the screen:
    TextOut(0, 64-LCD_LINEHEIGHT*(currentRun+1), "Run ");
    NumOut(35, 64-LCD_LINEHEIGHT*(currentRun+1), currentRun+1);
    TextOut(50, 64-LCD_LINEHEIGHT*(currentRun+1), tmpNumStr);

    Wait(200);

    currentRun++;

  }

  CloseFile (fileHandle);

  TextOut(0,LCD_LINE6, "done:");
  TextOut(0,LCD_LINE7, "press");
  TextOut(0,LCD_LINE8, "r-btn");

  // motor likely overshot position 0; let's try to bring it back for next time:
  RotateMotor(OUT_A,40,-MotorTachoCount(OUT_A));  // this call blocks

  // wait for a final button press before exiting:
  while (!ButtonCount(BTNRIGHT,true));

  Stop(true); // this kills the display thread too

}

task display() {
  while (true) {
    NumOut(50,LCD_LINE8, MotorTachoCount(OUT_A));
    Wait(50);
    TextOut(50,LCD_LINE8, "      ");
  }
}







