/*
Program ID  : FL-L12-Move.nxc
Create date : 3-4-2010
Author      : CH, Chen (Taiwan)
Move to a specific position
*/

#define L12_PORT     OUT_A

#define FULL_PWR    100
#define HALF_PWR    50
#define QUAT_PWR    25

#define CHECK_STALL_CNT  5
#define CHECK_INTERVAL 300

/* ********************************************************
 * Move arm till reached specific position or met stall
 * x_port: NXT output port
 * x_pwr : Output power level, >0 for retraction
 *                             <0 for extention
 * x_dest_pos : Destination position
 * Position counter: +/- 100/200 of 50mm/100mm arm length
 *                   Decrease if extend, increase if retract
 * ******************************************************** */
 void Do_MoveTo_rtn(const byte x_port, int x_pwr, int x_dest_pos)
 {
    long curr_time, last_time;
    int from_pos, curr_pos, last_pos, stall_cnt=0;
    bool isDone=FALSE, isStall=FALSE, isExtend=FALSE, isRetract=FALSE;

    ClearScreen();

    TextOut(0, LCD_LINE1, "   From: " );
    TextOut(0, LCD_LINE2, "Move To: " );
    NumOut(54, LCD_LINE2, x_dest_pos);
    TextOut(0, LCD_LINE4, "Current: " );

    from_pos = MotorRotationCount(x_port);
    NumOut(54, LCD_LINE1, from_pos);

    //Compare current position with target to determine running direction of motor
    if (from_pos > x_dest_pos) //Extend
    {
       OnRev(x_port, abs(x_pwr));
       isExtend = TRUE;
    }
    else if (from_pos < x_dest_pos) //Retract
         {
            OnFwd(x_port, abs(x_pwr));
            isRetract = TRUE;
         }
         else isDone = TRUE;
    
    last_time = CurrentTick();
    last_pos  = MotorRotationCount(x_port);
    while (!(isDone || isStall))
    {
          curr_time = CurrentTick();
          if ((curr_time - last_time) >= CHECK_INTERVAL) //Reach check interval
          {
             TextOut(54, LCD_LINE4, "       " );
             curr_pos = MotorRotationCount(x_port);
             NumOut(54, LCD_LINE4, curr_pos);
             if ((isExtend  && (curr_pos <= x_dest_pos)) ||
                 (isRetract && (curr_pos >= x_dest_pos)))
             {
                isDone = TRUE;
                continue;
             }
             else
             {
                if (curr_pos == last_pos) //Stall detect
                {
                   stall_cnt++;
                   if (stall_cnt >= CHECK_STALL_CNT) //if over the check times
                   {
                      isStall = TRUE;
                      continue;
                   }
                }
                else
                {
                   stall_cnt = 0;
                   last_pos = curr_pos;
                }
             }
             last_time = curr_time;
          }
    }
    Off(x_port);

    Wait(100);
    if (isDone)  TextOut(0, LCD_LINE5, "Reached target  " );
    if (isStall) TextOut(0, LCD_LINE6, "Arm is stalled  " );
    Wait(3000);
    //TextOut(0, LCD_LINE8, "ENTER To Next" );
    //until((ButtonPressed(BTNCENTER, TRUE)));
 }


/* ********************************************************
 * main()
 * ******************************************************** */
 task main()
 {
     ClearScreen();
     TextOut(0, LCD_LINE1, "<<FIRGELLI L12>>" );
     TextOut(0, LCD_LINE3, "Move to specific" );
     TextOut(0, LCD_LINE4, "        position" );
     //TextOut(0, LCD_LINE8, "ENTER To Run    " );
     Wait(2000);
     //until((ButtonPressed(BTNCENTER, TRUE)));

     //L12 50mm test scenario
     Do_MoveTo_rtn(L12_PORT, FULL_PWR, -100);
     Do_MoveTo_rtn(L12_PORT, FULL_PWR, 0);
     
     Do_MoveTo_rtn(L12_PORT, HALF_PWR, -50);
     Do_MoveTo_rtn(L12_PORT, HALF_PWR, -100);
     
     Do_MoveTo_rtn(L12_PORT, HALF_PWR, -75);
     Do_MoveTo_rtn(L12_PORT, HALF_PWR, -50);
     Do_MoveTo_rtn(L12_PORT, HALF_PWR, -25);
     Do_MoveTo_rtn(L12_PORT, HALF_PWR, 0);
     //L12 100mm test Secenario
     /*
     Do_MoveTo_rtn(L12_PORT, FULL_PWR, -50);
     Do_MoveTo_rtn(L12_PORT, FULL_PWR, -100);
     Do_MoveTo_rtn(L12_PORT, FULL_PWR, -150);
     Do_MoveTo_rtn(L12_PORT, HALF_PWR, -175);
     Do_MoveTo_rtn(L12_PORT, HALF_PWR, -200);
     Do_MoveTo_rtn(L12_PORT, QUAT_PWR, -190);
     Do_MoveTo_rtn(L12_PORT, FULL_PWR, -150);
     Do_MoveTo_rtn(L12_PORT, HALF_PWR, 0);
     */

     Stop(TRUE);
 }