/*
Program ID  : FL-L12-FuncTest.nxc
Create date : 3-3-2010
Author      : CH, Chen (Taiwan)
Functionality testing of Firgelli L12-NXT
1. Fully moving the arm till stall condition is detected.
2. Watch moving time and position counter value
*/

#define L12_PORT   OUT_A

#define EXTEND_FULL_PWR   -100
#define EXTEND_HALF_PWR    -50
#define EXTEND_QUAT_PWR    -25
#define RETRACT_FULL_PWR   100
#define RETRACT_HALF_PWR    50
#define RETRACT_QUAT_PWR    25

#define CHECK_INTERVAL     200
#define CHECK_STALL_CNT      5
/* ********************************************************
 * Move the L12 arm till stalled
 * x_port: NXT output port
 * x_pwr : Output power level applied, >0 for retraction
 *                                     <0 for extention
 *  Position counter: +/- 100/200 of 50mm/100mm arm length
 *                   Decrease if extend, increase if retract
 * ******************************************************** */
 void Do_Full_Move_rtn(const byte x_port, int x_pwr)
 {
    long beg_time, curr_time, last_time;
    int  beg_pos,  curr_pos,  last_pos, stall_cnt=0;
    bool isStall=FALSE;

    ClearScreen();
    if (x_pwr > 0) TextOut(0, LCD_LINE1, "Retracting" );
    else
    if (x_pwr < 0) TextOut(0, LCD_LINE1, "Extending" );
    TextOut(0, LCD_LINE3, "Curr Pos: " );
    
    OnFwd(x_port, x_pwr);
    beg_time  = CurrentTick();
    last_time = beg_time;
    beg_pos   = MotorRotationCount(x_port);
    last_pos  = beg_pos;
    while (!isStall)
    {
          curr_time = CurrentTick();
          if ((curr_time - last_time) >= CHECK_INTERVAL)
          {
             TextOut(60, LCD_LINE3, "      " );
             curr_pos = MotorRotationCount(x_port);
             NumOut(60, LCD_LINE3, curr_pos);
             if (curr_pos == last_pos)  //Stall detected
             {
                stall_cnt++;
                if (stall_cnt >= CHECK_STALL_CNT)
                {
                   isStall   = TRUE;
                   last_pos  = curr_pos; //for displaying results
                   last_time = curr_time;
                   continue;
                }
             }
             else
             {
                stall_cnt = 0;
                last_pos = curr_pos;
             }
             last_time = curr_time;
          }
    }
    Off(x_port);
    
    if (x_pwr > 0) TextOut(0, LCD_LINE1, "Retract to end  " );
    else
    if (x_pwr < 0) TextOut(0, LCD_LINE1, "Extend to top   " );
    TextOut(0, LCD_LINE5, "Move Pos: " );
    NumOut(60, LCD_LINE5, (last_pos - beg_pos));
    TextOut(0, LCD_LINE6, "Move time: " );
    NumOut(66, LCD_LINE6, (last_time - beg_time));

    Wait(100);
    TextOut(0, LCD_LINE8, "ENTER To Next" );
    until((ButtonPressed(BTNCENTER, TRUE)));
 }

/* ********************************************************
 * main()
 * ******************************************************** */
 task main()
 {
     while (TRUE)
     {
           ClearScreen();
           TextOut(0, LCD_LINE1, "<<FIRGELLI L12>>" );
           TextOut(0, LCD_LINE3, "Watch values of " );
           TextOut(0, LCD_LINE4, "moving time and " );
           TextOut(0, LCD_LINE5, "position counter" );
     
           TextOut(0, LCD_LINE8, "ENTER To Run    " );
           until((ButtonPressed(BTNCENTER, TRUE)));

           Do_Full_Move_rtn(L12_PORT, EXTEND_FULL_PWR);
           Do_Full_Move_rtn(L12_PORT, RETRACT_FULL_PWR);
     }
     Stop(TRUE);
 }