/*
Program ID  : RoboArm-Demo.nxc
Create date : 7-21-2010
Author      : CH, Chen (Taiwan)

Output power level : >0 for FLLA retraction and grabber holding
                     <0 for FLLA extention  and grabber releasing
Position counter   : +/- 100/200 of 50mm/100mm arm length
Decrease if extend, increase if retract

*/

#define PITCH_ARM         OUT_A
#define ROLL_ARM          OUT_B
#define GRABBER           OUT_C
//Applied power level
#define FULL_PWR            100
#define HALF_PWR             50
#define QUAD_PWR             30
//FLLA moving direction
#define PITCH_UP            (1)
#define PITCH_DOWN         (-1)

#define ROLL_LEFT           (1)
#define ROLL_RIGHT         (-1)
//Nxt servo motor moving direction
#define GRAB_HOLD           (1)
#define GRAB_RELEASE       (-1)

#define PITCH_LIMIT         100
#define PITCH_ZERO           40

#define ROLL_LIMIT          100
#define ROLL_ZERO            45

#define CHECK_STALL_CNT       5
#define CHECK_PERIOD        100
#define DISPLAY_PERIOD      200

//Grabber status
#define GB_UN_DEFINED         0
#define GB_IN_REL             1
#define GB_IN_HOLD            2
//Pitch arm location
#define FLLA_UN_DEFINED     999

#define PITCH_IN_TOP          0
#define PITCH_IN_LEVEL      -40
#define PITCH_IN_BOTTOM    -100

#define ROLL_IN_LEFT          0
#define ROLL_IN_LEVEL       -45
#define ROLL_IN_RIGHT      -100

#define OP_PAUSE            200

int grabber_limit, pitchArm_loc, rollArm_loc, grabber_status;

/* ********************************************************
 * Move arm specific distance or till stalled status was detected.
 * x_port     : PITCH_ARM(A), ROLL_ARM(B)
 * x_pwr      : Output power level, ingored sign always using positive value
 * x_distance : Moving distance, if > 0 set FLLA retracting
 *                               if < 0 set FLLA extending
 * ******************************************************** */
 void RoboArm_Ctrl(const byte x_port, int x_pwr, int x_distance)
 {
    long curr_time, last_time;
    int curr_pos, last_pos, x_move_distance, stall_cnt=0;
    bool isDone=FALSE, isStall=FALSE, isReverse=FALSE, isForward=FALSE;

    ClearScreen();
    TextOut(0,  LCD_LINE1, "RoboArm Control " );
    TextOut(0,  LCD_LINE3, "Action:         " );
    TextOut(48, LCD_LINE3, SubStr("PITCHROLL ",5*x_port,5));
    TextOut(0,  LCD_LINE4, "Target:         " );
    NumOut(48,  LCD_LINE4, x_distance);

    ResetRotationCount(x_port);
    Wait(50);
    if (x_distance < 0) //FLLA extend
    {
       OnRev(x_port, abs(x_pwr));
       isReverse = TRUE;
    }
    else if (x_distance > 0) //FLLA retract
         {
            OnFwd(x_port, abs(x_pwr));
            isForward = TRUE;
         }
         else isDone = TRUE;

    last_time = CurrentTick();
    last_pos  = MotorRotationCount(x_port);
    while (!(isDone || isStall))
    {
          curr_time = CurrentTick();
          if ((curr_time - last_time) >= CHECK_PERIOD) //Reach check interval
          {
             curr_pos = MotorRotationCount(x_port);
             if ((isReverse && (curr_pos <= x_distance)) ||
                 (isForward && (curr_pos >= x_distance)))
             {
                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;
          }
          
          if ((curr_time - last_time) >= DISPLAY_PERIOD) //Reach display interval
          {
             TextOut(0, LCD_LINE5, "Travel:         " );
             NumOut(48, LCD_LINE5, MotorRotationCount(x_port));
          }
    }

    Off(x_port);
    Wait(50);
    ClearScreen();
 }

/* ********************************************************
 * Control the grabber rotating specific distance
 * x_pwr      : Output power level, ingored sign always using positive value
 * x_distance : Moving distance, if > 0 set grabber holding
 *                               if < 0 set grabber releasing
 * ******************************************************** */
 void Grabber_Ctrl(int x_pwr, int x_distance)
 {
    long curr_time, last_time;
    int curr_pos;
    bool isDone=FALSE, isReverse=FALSE, isForward=FALSE;

    ClearScreen();
    TextOut(0, LCD_LINE1, "Grabber Control " );
    TextOut(0, LCD_LINE4, "Target:         " );
    NumOut(48, LCD_LINE4, x_distance);

    ResetRotationCount(GRABBER);
    Wait(50);
    if (x_distance < 0) //Grabber release
    {
       OnRev(GRABBER, abs(x_pwr));
       isReverse = TRUE;
    }
    else if (x_distance > 0) //Grabber hold
         {
            OnFwd(GRABBER, abs(x_pwr));
            isForward = TRUE;
         }
         else isDone = TRUE;

    last_time = CurrentTick();
    while (!isDone)
    {
          curr_time = CurrentTick();
          if ((curr_time - last_time) >= CHECK_PERIOD) //Reach check interval
          {
             curr_pos = MotorRotationCount(GRABBER);
             if ((isReverse && (curr_pos <= x_distance)) ||
                 (isForward && (curr_pos >= x_distance)))
             {
                isDone = TRUE;
                continue;
             }
             last_time = curr_time;
          }

          if ((curr_time - last_time) >= DISPLAY_PERIOD) //Reach display interval
          {
             TextOut(0, LCD_LINE5, "Travel:         " );
             NumOut(48, LCD_LINE5, MotorRotationCount(GRABBER));
          }
    }

    Off(GRABBER);
    Wait(50);
    ClearScreen();
 }

/* ********************************************************
 * Setting Zero position of Grabber(Release)
 * Releasing grabber till reached the position
 * that will be set it as the Zero position of grabber.
 * ******************************************************** */
 void Set_GrabberZeroPos()
 {
    ClearScreen();
    TextOut(0, LCD_LINE1, "1>Set Grabber   " );
    TextOut(0, LCD_LINE2, "  Zero Position " );
    bool isDone=FALSE;
    do
    {
      TextOut(0, LCD_LINE8, "Rele  Stop  Hold" );
      while(ButtonPressed(BTNLEFT,   FALSE) ||
            ButtonPressed(BTNCENTER, FALSE) ||
            ButtonPressed(BTNRIGHT,  FALSE));
      while(TRUE)
      {
           if (ButtonPressed(BTNLEFT, FALSE))
           {
              Off(GRABBER);
              Wait(50);
              TextOut(0, LCD_LINE4, "Releasing ...   " );
              OnFwd(GRABBER, GRAB_RELEASE * FULL_PWR);
              Wait(50);
           }
           if (ButtonPressed(BTNCENTER, FALSE))
           {
              Off(GRABBER);
              Wait(50);
              TextOut(0, LCD_LINE4, "                " );
              break;
           }
           if (ButtonPressed(BTNRIGHT, FALSE))
           {
              Off(GRABBER);
              Wait(50);
              TextOut(0, LCD_LINE4, "Holding ...     " );
              OnFwd(GRABBER, GRAB_HOLD * FULL_PWR);
              Wait(50);
           }
      }
      //Confirm setting or re-setting again
      TextOut(0, LCD_LINE8, "Redo  Exit   Set" );
      while(ButtonPressed(BTNLEFT,   FALSE) ||
            ButtonPressed(BTNCENTER, FALSE) ||
            ButtonPressed(BTNRIGHT,  FALSE));
      while(TRUE)
      {
           if (ButtonPressed(BTNLEFT,   FALSE))
           {
              TextOut(0, LCD_LINE4, "                " );
              break;
           }
           if (ButtonPressed(BTNCENTER, FALSE)) Stop(TRUE);
           if (ButtonPressed(BTNRIGHT,  FALSE))
           {
              isDone=TRUE;
              break;
           }
      }
    } until(isDone);
 }

/* ********************************************************
 * Setting limit position of Grabber(Hold)
 * Holding grabber till reached the position
 * that will be set it as the limit position of grabber.
 * ******************************************************** */
 int Set_GrabberLimitPos()
 {
    int limit_pos=0, curr_pos=0;
    bool isDone=FALSE, isValid=FALSE;

    ClearScreen();
    TextOut(0, LCD_LINE1, "2>Set Grabber   " );
    TextOut(0, LCD_LINE2, "  Limit Position" );
    grabber_status = GB_UN_DEFINED;
    ResetRotationCount(GRABBER);
    Wait(50);
    do
    {
      TextOut(0, LCD_LINE8, "Rele  Stop  Hold" );
      while(ButtonPressed(BTNLEFT,   FALSE) ||
            ButtonPressed(BTNCENTER, FALSE) ||
            ButtonPressed(BTNRIGHT,  FALSE));
      while(TRUE)
      {
           if (ButtonPressed(BTNLEFT, FALSE))
           {
              Off(GRABBER);
              Wait(50);
              if (MotorRotationCount(GRABBER) > 0)
              {
                 TextOut(0, LCD_LINE4, "Releasing ...   " );
                 OnFwd(GRABBER, GRAB_RELEASE * FULL_PWR);
                 Wait(50);
              }
              else TextOut(0, LCD_LINE4, "Invalid action  " );
           }
           if (ButtonPressed(BTNCENTER, FALSE))
           {
              Off(GRABBER);
              Wait(50);
              TextOut(0, LCD_LINE4, "Encoder:        " );
              NumOut(54, LCD_LINE4, MotorRotationCount(GRABBER));
              break;
           }
           if (ButtonPressed(BTNRIGHT, FALSE))
           {
              Off(GRABBER);
              Wait(50);
              TextOut(0, LCD_LINE4, "Holding ...     " );
              OnFwd(GRABBER, GRAB_HOLD * FULL_PWR);
              Wait(50);
           }
      }
      //Confirm setting or re-setting again
      if (MotorRotationCount(GRABBER) < 0)
      {
         TextOut(0, LCD_LINE5, "Invalid LimitPos" );
         TextOut(0, LCD_LINE8, "Redo  Exit      " );
      }
      else
      {
         isValid = TRUE;
         TextOut(0, LCD_LINE8, "Redo  Exit   Set" );
      }
         
      while(ButtonPressed(BTNLEFT,   FALSE) ||
            ButtonPressed(BTNCENTER, FALSE) ||
            ButtonPressed(BTNRIGHT,  FALSE));
      while(TRUE)
      {
           if (ButtonPressed(BTNLEFT, FALSE))
           {
              TextOut(0, LCD_LINE4, "                " );
              TextOut(0, LCD_LINE5, "                " );
              break;
           }
           if (ButtonPressed(BTNCENTER, FALSE)) Stop(TRUE);
           if (ButtonPressed(BTNRIGHT,  FALSE) && isValid)
           {
              limit_pos=MotorRotationCount(GRABBER);
              Wait(50);
              isDone=TRUE;
              break;
           }
      }
    } until(isDone);
    
    grabber_status = GB_IN_HOLD;
    return(limit_pos);
 }
 
/* ********************************************************
 * Setup Grabber zero/limit positions
 * Step 1. Set_GrabberZeroPos()
 *         Release grabber till reached the position that will be
 *         set it as zero position.
 *
 * Step 2. Set_GrabberLimitPos()
 *         Hold grabber till reached the position that will be
 *         set it as most limit position.
 *         This limit position will be saved in global variable: grabber_limit
 * ******************************************************** */
 bool Set_Grabber_Pos()
 {
    Set_GrabberZeroPos();
    grabber_limit = Set_GrabberLimitPos();
    return (TRUE);
 }

/* ********************************************************
 * Zero position
 * ******************************************************** */
 void Move_To_ZeroPos()
 {

    RoboArm_Ctrl(PITCH_ARM, FULL_PWR, (PITCH_UP   * PITCH_LIMIT));
    RoboArm_Ctrl(PITCH_ARM, FULL_PWR, (PITCH_DOWN * PITCH_ZERO));
    pitchArm_loc = PITCH_IN_LEVEL;

    RoboArm_Ctrl(ROLL_ARM,  FULL_PWR, (ROLL_LEFT  * ROLL_LIMIT));
    RoboArm_Ctrl(ROLL_ARM,  FULL_PWR, (ROLL_RIGHT * ROLL_ZERO));
    rollArm_loc = ROLL_IN_LEVEL;

    if (grabber_status == GB_IN_HOLD)
    {
       Grabber_Ctrl(FULL_PWR, (GRAB_RELEASE * grabber_limit));
       grabber_status = GB_IN_REL;
    }
 }
 
/* ********************************************************
 * Operate robot arm / graber control process
 * x_port : PITCH_ARM / ROLL_ARM / GRABBER / OP_PAUSE
 * newLoc : destination location
 * ******************************************************** */
 int OperateCtrl_rtn(const byte x_port, int newLoc)
 {

    if (x_port == OP_PAUSE)
    {
       TextOut(0, LCD_LINE8, "Continue    Exit" );
       while(ButtonPressed(BTNLEFT,   FALSE) ||
             ButtonPressed(BTNCENTER, FALSE) ||
             ButtonPressed(BTNRIGHT,  FALSE));
       while(TRUE)
       {
            if (ButtonPressed(BTNLEFT, FALSE))
            {
               TextOut(0, LCD_LINE8, "                " );
               return (0);
            }
            if (ButtonPressed(BTNRIGHT, FALSE)) Stop (TRUE);
       }
    }

    if (((x_port == PITCH_ARM) && (pitchArm_loc == FLLA_UN_DEFINED)) ||
        ((x_port == ROLL_ARM ) && (rollArm_loc  == FLLA_UN_DEFINED)))
       return (FLLA_UN_DEFINED);
       
    if ((x_port == GRABBER) && (grabber_status == GB_UN_DEFINED))
       return (GB_UN_DEFINED);
       
    if (((x_port == PITCH_ARM) && (pitchArm_loc   == newLoc)) ||
        ((x_port == ROLL_ARM ) && (rollArm_loc    == newLoc)) ||
        ((x_port == GRABBER  ) && (grabber_status == newLoc)))
       return (newLoc);
       
    switch (x_port)
    {
           case PITCH_ARM:
                RoboArm_Ctrl(x_port, FULL_PWR, (newLoc - pitchArm_loc));
                break;
           case ROLL_ARM:
                RoboArm_Ctrl(x_port, FULL_PWR, (newLoc - rollArm_loc));
                break;
           case GRABBER:
                if (newLoc == GB_IN_REL)
                   Grabber_Ctrl(FULL_PWR, (GRAB_RELEASE * grabber_limit));
                else
                {
                   if (newLoc == GB_IN_HOLD)
                      Grabber_Ctrl(FULL_PWR, (GRAB_HOLD * grabber_limit));
                }
                break;
    }
    return (newLoc);
 }

/* ********************************************************
 * Demo routine
 * Grabber Status:
 *(grabber_status) GB_UN_DEFINED / GB_IN_REL / GB_IN_HOLD : 0/1/2
 *
 * RoboArm location:
 *               FLLA_UN_DEFINED / 999
 *(pitchArm_loc) PITCH_IN_TOP / PITCH_IN_LEVEL / PITCH_IN_BOTTOM : 0/-40/-100
 *(rollArm_loc ) ROLL_IN_LEFT / ROLL_IN_LEVEL  / ROLL_IN_RIGHT   : 0/-45/-100
 * ******************************************************** */
 void Run_Demo_rtn()
 {
    pitchArm_loc   = OperateCtrl_rtn(PITCH_ARM, PITCH_IN_TOP);
    Wait(1000);
    rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_RIGHT);
    rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_LEFT);
    rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_LEVEL);
    grabber_status = OperateCtrl_rtn(GRABBER,   GB_IN_HOLD);
    Wait(2000);
    grabber_status = OperateCtrl_rtn(GRABBER,   GB_IN_REL);
    //
    pitchArm_loc   = OperateCtrl_rtn(PITCH_ARM, PITCH_IN_BOTTOM);
    Wait(1000);
    rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_LEFT);
    rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_RIGHT);
    grabber_status = OperateCtrl_rtn(GRABBER,   GB_IN_HOLD);
    Wait(2000);
    grabber_status = OperateCtrl_rtn(GRABBER,   GB_IN_REL);
    rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_LEVEL);
    //
    pitchArm_loc   = OperateCtrl_rtn(PITCH_ARM, PITCH_IN_LEVEL);
    Wait(1000);
    rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_RIGHT);
    rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_LEFT);
    grabber_status = OperateCtrl_rtn(GRABBER,   GB_IN_HOLD);
    Wait(2000);
    grabber_status = OperateCtrl_rtn(GRABBER,   GB_IN_REL);
    
    rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_LEVEL);
 
 }
 
/* ********************************************************
 * PickUp CD demo routine
 * ******************************************************** */
 void Run_PickUp_rtn()
 {
    int x_dummy;

    x_dummy        = OperateCtrl_rtn(OP_PAUSE,  0);
    pitchArm_loc   = OperateCtrl_rtn(PITCH_ARM, PITCH_IN_BOTTOM);
    //x_dummy        = OperateCtrl_rtn(OP_PAUSE,  0);
    //rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_LEFT);
    //x_dummy        = OperateCtrl_rtn(OP_PAUSE,  0);
    Wait(2000);
    grabber_status = OperateCtrl_rtn(GRABBER,   GB_IN_HOLD);
    //x_dummy        = OperateCtrl_rtn(OP_PAUSE,  0);
    
    pitchArm_loc   = OperateCtrl_rtn(PITCH_ARM, PITCH_IN_LEVEL);
    rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_RIGHT);
    rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_LEFT);
    rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_LEVEL);
    
    pitchArm_loc   = OperateCtrl_rtn(PITCH_ARM, PITCH_IN_TOP);
    rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_LEFT);
    rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_RIGHT);
     rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_LEVEL);
    
    pitchArm_loc   = OperateCtrl_rtn(PITCH_ARM, PITCH_IN_BOTTOM);
    Wait(1000);
    //x_dummy        = OperateCtrl_rtn(OP_PAUSE,  0);
    grabber_status = OperateCtrl_rtn(GRABBER,   GB_IN_REL);
    Wait(1000);
    //x_dummy        = OperateCtrl_rtn(OP_PAUSE,  0);
    
    pitchArm_loc   = OperateCtrl_rtn(PITCH_ARM, PITCH_IN_LEVEL);
    rollArm_loc    = OperateCtrl_rtn(ROLL_ARM,  ROLL_IN_LEVEL);
  }

/* ********************************************************
 * main()
 * ******************************************************** */
 task main()
 {
     bool isExit = FALSE, isZeroPos = FALSE, isSetGrabberPos_OK = FALSE;
     
     pitchArm_loc   = FLLA_UN_DEFINED; //Initialize robot arm location
     rollArm_loc    = FLLA_UN_DEFINED; //Initialize robot arm location
     grabber_status = GB_UN_DEFINED;   //Initialize grabber status
     grabber_limit  = 0; //Global variable for saving the grabber's limit position
     do
     {
       ClearScreen();
       TextOut(0, LCD_LINE1, "RobotArm-Demo   " );
       if (isZeroPos) TextOut(0, LCD_LINE3, "Zero Position   " );
       else           TextOut(0, LCD_LINE3, "NonZero Position" );
           
       TextOut(0, LCD_LINE8, "Zero  Exit  Demo" );
       while(ButtonPressed(BTNLEFT,   FALSE) ||
             ButtonPressed(BTNCENTER, FALSE) ||
             ButtonPressed(BTNRIGHT,  FALSE));
       while(TRUE)
       {
            if (ButtonPressed(BTNLEFT, FALSE))
            {
               if (!isSetGrabberPos_OK) isSetGrabberPos_OK = Set_Grabber_Pos();
                  Move_To_ZeroPos();
                  isZeroPos=TRUE;
                  break;
               }
               if (ButtonPressed(BTNRIGHT, FALSE))
               {
                  Run_Demo_rtn();
                  Run_PickUp_rtn();
                  isZeroPos=FALSE;
                  break;
               }
               if (ButtonPressed(BTNCENTER, FALSE))
               {
                  isExit=TRUE;
                  break;
               }
       }
     } until(isExit);

 }