/*
Program ID  : RoboArm-NP-Ctrl
Create date : 03-25-2011
Author      : CH, Chen (Taiwan)
Description :
1. Building a robot arm with 4 degrees of freedom and grabber
2. This arm contains 5 RC Servos (Hitec HS-311 with lego mounting kit)
3. Using the Mindsensors.com NxtServo sensor to control the RC Servos
4. This project testing group control the whole RC Servos to let the robot
   arm could play some pre-defined actions.
5. Servo actions :
   Servo 1   : Control the robot arm horizontal rotation
               Left/Center/Right - 50/150/250
   Servo 2/3 : Control the Robot arm vertical rotation
               Servo 2 Front/Center/Right - 50/150/250
               Servo 3 = 300 - Position value of Servo 2
   Servo 4   : Control the grabber vertical rotation
               Upward/Center/Downward - 200/150/100
   <Grabber Orientation control:>
            (1)Keep grabber horizontal : Set Pos2 value
               When Pos2 range is 100-200
               Keep Pos4 + Pos2 = 300
               
            (2)Keep geabber vertical down
               When Pos2 range is 100 - 50(Front)
               Keep Pos4 + Pos2 = 200
               
            (3)Keep grabber straight up
               When Pos2 range is 200 - 250 (Back)
               Keep Pos4 + Pos2 = 400
               
   Servo 5   : Control grabber holding / releasing action = 150/50,250

6. Control functions with NumericPad.
*/
#define NXTSERVO_PORT       S1     //Connect NxtServo sensor to this port
#define NXTSERVO            0xb0
#define SERVO_BATTERY       0x41
#define SERVO_POS           0x42   //0x42~0x51 of Servo channel 1-8(2 bytes set)
#define SERVO_SPEED         0x52   //0x52~0x59 of Servo channel 1-8
#define SERVO_Q_POS         0x5a   //0x5A~0x61 of Servo channel 1-8

#define NUMPAD_PORT         S2     //Connect NumericPad sensor to this port
#define NUMPAD              0xb4
#define NUMPAD_KEY          0x00
#define CHECK_INTERVAL      4000

#define DELAY               3000    //Depends on speed of setting & Servo spec

// Channel No. of Servo Role
#define SERVO_ARM_HPOS      0
#define SERVO_ARM_VPOS      1
#define SERVO_ARM_VPOS_SYNC 2
#define SERVO_GRABBER_VPOS  3
#define SERVO_GRABBER       4

// Robot arm action type
// Control action
#define ROBOT_HOME          0
#define ROBOT_PAUSE         1
// Servo 1 - Robot Arm horizontal rotation
#define ARM_CENTER          150
#define ARM_LEFT            (ARM_CENTER - 100)
#define ARM_RIGHT           (ARM_CENTER + 100)
// Servo 2/3 - Robot Arm vertical moving front and back
#define ARM_UPRIGHT         (150+10)
#define ARM_HALF_FRONT      (ARM_UPRIGHT - 50)
#define ARM_HALF_BACK       (ARM_UPRIGHT + 50)
// Servo 4 - Grabber orientation flag
#define GRABBER_UP          1
#define GRABBER_STRAIGHT    2
#define GRABBER_DOWN        3
// Servo 5 - Grabber holding and releasing
#define GRABBER_HOLD        150
#define GRABBER_RELEASE     60
// Light Control
#define TURN_ON_LIGHT       OnFwd(OUT_A, 100)
#define TURN_OFF_LIGHT      Off(OUT_A)

// NumericPad key status after adjusting the sequence of bits
// [11 IS_GRAB_RELEASE    ]-[7 IS_LIGHT        ]-[3 IS_GRAB_HOLD      ]
// [10 IS_FRONT_HORIZONTAL]-[6 IS_UP_HORIZONTAL]-[2 IS_BACK_HORIZONTAL]
// [ 9 IS_FRONT_DOWN      ]-[5 IS_CENTER       ]-[1 IS_BACK_UP        ]
// [ 8 IS_LEFT            ]-[4 IS_HOME         ]-[0 IS_RIGHT          ]
#define IS_HOME             0x0010  // 0000 0000,0001 0000

#define IS_RIGHT            0x0001  // 0000 0000,0000 0001
#define IS_CENTER           0x0020  // 0000 0000,0010 0000
#define IS_LEFT             0x0100  // 0000 0001,0000 0000

#define IS_BACK_UP          0x0002  // 0000 0000,0000 0010
#define IS_BACK_HORIZONTAL  0x0004  // 0000 0000,0000 0100
#define IS_UP_HORIZONTAL    0x0040  // 0000 0000,0100 0000

#define IS_FRONT_HORIZONTAL 0x0400  // 0000 0100,0000 0000
#define IS_FRONT_DOWN       0x0200  // 0000 0010,0000 0000

#define IS_GRAB_HOLD        0x0008  // 0000 0000,0000 1000
#define IS_GRAB_RELEASE     0x0800  // 0000 1000,0000 0000

#define IS_LIGHT            0x0080  // 0000 0000,1000 0000

byte Nxtservo_q_pos[];
byte Nxtservo_speed[];
byte l_arm, l_shoulder, l_elbow, l_hand;
bool l_light;

/* ********************************************************
 * Adjust bit seqence bit6 & bit7 of byte 1
 *                and bit2 & bit3 of byte 2
 * ******************************************************** */
 int AdjustBitSeq(byte &I2C_get_buf[])
 {
    byte Byte1_b6 = (I2C_get_buf[0] & 0x80) >> 1; //get bit 7 then right shift
    byte Byte1_b7 = (I2C_get_buf[0] & 0x40) << 1; //get bit 6 then left  shift
    byte Byte1    = (I2C_get_buf[0] & 0x3F) + Byte1_b6 + Byte1_b7;

    byte Byte2_b2 = (I2C_get_buf[1] & 0x08) >> 1; //get bit 3 then right shift
    byte Byte2_b3 = (I2C_get_buf[1] & 0x04) << 1; //get bit 2 then left  shift
    byte Byte2    = (I2C_get_buf[1] & 0x03) + Byte2_b2 + Byte2_b3;

    return ((Byte1 + (Byte2 << 8)) & 0x0FFF);
 }

/* ********************************************************
 * Get NumericPad key-pressed status value
 * ******************************************************** */
 int Get_NumericPad_key ()
 {
    byte I2C_req_buf[];
    ArrayBuild(I2C_req_buf, NUMPAD, NUMPAD_KEY);

    byte I2C_get_buf[];
    ArrayInit(I2C_get_buf, 0, 2);

    while(I2CCheckStatus(NUMPAD_PORT) == STAT_COMM_PENDING);
    LowspeedWrite(NUMPAD_PORT, 2, I2C_req_buf);

    while(I2CCheckStatus(NUMPAD_PORT) == STAT_COMM_PENDING);
    if(LowspeedRead(NUMPAD_PORT, 2, I2C_get_buf) < 0) return 0;

    return (AdjustBitSeq(I2C_get_buf));
 }
 
/* ********************************************************
 * Get Battery voltage (mV) from register 0x41
 * The register contain value 0~255
 * where value of 127 corresponds to 4700 mV
 * ******************************************************** */
 int Get_Battery_Voltage ()
 {
    byte I2C_req_buf[];
    ArrayBuild(I2C_req_buf, NXTSERVO, SERVO_BATTERY);

    byte I2C_get_buf[];
    ArrayInit(I2C_get_buf, 0, 1);

    while(I2CCheckStatus(NXTSERVO_PORT) == STAT_COMM_PENDING);
    if (LowspeedWrite(NXTSERVO_PORT, 1, I2C_req_buf) < 0) return 0;

    while(I2CCheckStatus(NXTSERVO_PORT) == STAT_COMM_PENDING);
    if (LowspeedRead(NXTSERVO_PORT, 1, I2C_get_buf) < 0)  return 0;
    
    return ((I2C_get_buf[0] * 4700) / 127);
 }
 
 /* ********************************************************
 * Get specific Servo's positon(0x41,0x42)~(0x50,0x51)
 * Return value : Quick position of the request servo channel
 * Parameter :
 * byte x_servo_no : Servo channel (0-7)
 * ******************************************************** */
 int Get_Servo_QuickPosition (byte x_servo_no)
 {
    byte I2C_req_buf[];
    ArrayInit(I2C_req_buf, 0, 2);

    byte I2C_get_buf[];
    ArrayInit(I2C_get_buf, 0, 2);

    I2C_req_buf[0] = NXTSERVO;
    I2C_req_buf[1] = SERVO_POS + 2*x_servo_no;

    while(I2CCheckStatus(NXTSERVO_PORT) == STAT_COMM_PENDING);
    if (LowspeedWrite(NXTSERVO_PORT, 2, I2C_req_buf) < 0) return 0;

    while(I2CCheckStatus(NXTSERVO_PORT) == STAT_COMM_PENDING);
    if (LowspeedRead(NXTSERVO_PORT, 2, I2C_get_buf) < 0)  return 0;

    return ((I2C_get_buf[1] * 256 + I2C_get_buf[0])/10);
 }

/* ********************************************************
 * Set Servo speed to whole 8 servo channels with Nxtservo_speed array
 * ******************************************************** */
 void Set_ServoGroup_Speed ()
 {
    byte I2C_req_buf[];
    ArrayInit(I2C_req_buf, 0, 10);
    
    I2C_req_buf[0] = NXTSERVO;
    I2C_req_buf[1] = SERVO_SPEED;
    for (int xii=0; xii<8; xii++)
    {
        I2C_req_buf[xii+2] = Nxtservo_speed[xii];
    }

    while(I2CCheckStatus(NXTSERVO_PORT) == STAT_COMM_PENDING);
    LowspeedWrite(NXTSERVO_PORT, 0, I2C_req_buf);
    Wait(100);
 }

 /* ********************************************************
 * Set speed to specific servo channel
 * parameters :
 * byte xchannel: servo channel(0-7)
 * byte xpos    : speed to be setting(50-250)
 * ******************************************************** */
 void Set_Servo_Speed(byte xchannel, byte xspeed)
 {
    byte I2C_req_buf[];
    ArrayInit(I2C_req_buf, 0, 3);

    I2C_req_buf[0] = NXTSERVO;
    I2C_req_buf[1] = SERVO_SPEED + xchannel;
    I2C_req_buf[2] = xspeed;

    while(I2CCheckStatus(NXTSERVO_PORT) == STAT_COMM_PENDING);
    LowspeedWrite(NXTSERVO_PORT, 0, I2C_req_buf);
    Wait(100);
}

/* ********************************************************
 * Set quick position to whole 8 servo channels with Nxtservo_q_pos array
 * All servo motors will move to target position in the same time
 * ******************************************************** */
 void Set_ServoGroup_Position()
 {
    byte I2C_req_buf[];
    ArrayInit(I2C_req_buf, 0, 10);
    
    I2C_req_buf[0] = NXTSERVO;
    I2C_req_buf[1] = SERVO_Q_POS;
    for (int xii=0; xii<8; xii++)
    {
       I2C_req_buf[xii+2] = Nxtservo_q_pos[xii];
    }

    while(I2CCheckStatus(NXTSERVO_PORT) == STAT_COMM_PENDING);
    LowspeedWrite(NXTSERVO_PORT, 0, I2C_req_buf);
    Wait(DELAY);
}

/* ********************************************************
 * Set quick position to specific servo channel
 * Parameters :
 * byte xchannel : servo channel(0-7)
 * byte xpos     : quick position to be setting(50-250)
 * ******************************************************** */
 void Set_Servo_Position(byte xchannel, byte xpos)
 {
    byte I2C_req_buf[];
    ArrayInit(I2C_req_buf, 0, 3);

    I2C_req_buf[0] = NXTSERVO;
    I2C_req_buf[1] = SERVO_Q_POS + xchannel;
    I2C_req_buf[2] = xpos;

    while(I2CCheckStatus(NXTSERVO_PORT) == STAT_COMM_PENDING);
    LowspeedWrite(NXTSERVO_PORT, 0, I2C_req_buf);
    Wait(DELAY);
}

/* *******************************************************
 * Set all Servo's position to let Robot do actions
 * ******************************************************* */
 void Do_Robot_Action(byte arm, byte shoulder, byte elbow, byte hand)
 {
    Nxtservo_q_pos[SERVO_ARM_HPOS]      = arm;
    Nxtservo_q_pos[SERVO_ARM_VPOS]      = shoulder;
    Nxtservo_q_pos[SERVO_ARM_VPOS_SYNC] = 300 - shoulder;

    if (shoulder == ARM_HALF_BACK)  // Pivot point
    {
        if (elbow == GRABBER_UP)
            Nxtservo_q_pos[SERVO_GRABBER_VPOS] = 400-shoulder;    // Up
        else
            Nxtservo_q_pos[SERVO_GRABBER_VPOS] = 300-shoulder+20; // Horizontal
    }
    else
    if (shoulder == ARM_UPRIGHT)    // ARM is UPRIGHT
        Nxtservo_q_pos[SERVO_GRABBER_VPOS] = 300-shoulder+25;     // Horizontal
    else
    if (shoulder == ARM_HALF_FRONT) // Pivot point
    {
        if (elbow == GRABBER_STRAIGHT)
            Nxtservo_q_pos[SERVO_GRABBER_VPOS] = 300-shoulder+25;  // Horizontal
        else
            Nxtservo_q_pos[SERVO_GRABBER_VPOS] = 200-shoulder+30;  // Down
    }

    Nxtservo_q_pos[SERVO_GRABBER] = hand;
    
    Set_ServoGroup_Position();
 }
/* *******************************************************
 * Reset Robot to ready status
 * ******************************************************* */
 void Set_Robot_Home()
 {
    TURN_OFF_LIGHT;
    Do_Robot_Action(ARM_CENTER, ARM_HALF_BACK, GRABBER_STRAIGHT, GRABBER_HOLD);
    
    l_arm      = ARM_CENTER;
    l_shoulder = ARM_HALF_BACK;
    l_elbow    = GRABBER_STRAIGHT;
    l_hand     = GRABBER_HOLD;
    l_light    = FALSE;
 }

/* *******************************************************
 * Controlling NXTServo via NumericPad's key pressed status
 * ******************************************************* */
 void NXTServo_Control_Rtn(int keyStatus, bool isNPmode)
 {
    byte arm      = l_arm;
    byte shoulder = l_shoulder;
    byte elbow    = l_elbow;
    byte hand     = l_hand;
    
    if ((keyStatus & IS_HOME) <> 0)
    {
       Set_Robot_Home();
    }
    else
    {
        // Controlling the direction of the robot arm
        string amsg = " - - " ;
        if ((keyStatus & IS_CENTER) <> 0)
        {
           arm  = ARM_CENTER;
           amsg = " -C- " ;
        }
        else
        if ((keyStatus & IS_RIGHT)  <> 0)
        {
           arm  = ARM_RIGHT;
           amsg = " - -R" ;
        }
        else
        if ((keyStatus & IS_LEFT)   <> 0)
        {
           arm  = ARM_LEFT;
           amsg = "L- - " ;
        }
        
        // Controlling the posture of the robot arm's shoulder and elbow
        string smsg = "  - -  " ;
        string emsg = " - - " ;
        if ((keyStatus & IS_BACK_UP) <> 0)
        {
           shoulder = ARM_HALF_BACK;
           elbow    = GRABBER_UP;
           smsg     = "  - -HB" ;
           emsg     = " - -U" ;
        }
        else
        if ((keyStatus & IS_BACK_HORIZONTAL) <> 0)
        {
           shoulder = ARM_HALF_BACK;
           elbow    = GRABBER_STRAIGHT;
           smsg     = "  - -HB" ;
           emsg     = " -S- " ;
        }
        else
        if ((keyStatus & IS_UP_HORIZONTAL) <> 0)
        {
           shoulder = ARM_UPRIGHT;
           elbow    = GRABBER_STRAIGHT;
           smsg     = "  -U-  " ;
           emsg     = " -S- " ;
        }
        else
        if ((keyStatus & IS_FRONT_HORIZONTAL) <> 0)
        {
           shoulder = ARM_HALF_FRONT;
           elbow    = GRABBER_STRAIGHT;
           smsg     = "HF- -  " ;
           emsg     = " -S- " ;
        }
        else
        if ((keyStatus & IS_FRONT_DOWN) <> 0)
        {
           shoulder = ARM_HALF_FRONT;
           elbow    = GRABBER_DOWN;
           smsg     = "HF- -  " ;
           emsg     = "D- - " ;
        }
        
        // Controlling holding/releasing actions of the robot arm's grabber
        string hmsg = " - " ;
        if ((keyStatus & IS_GRAB_HOLD)    <> 0)
        {
           hand = GRABBER_HOLD;
           hmsg = " -H" ;
        }
        else
        if ((keyStatus & IS_GRAB_RELEASE) <> 0)
        {
           hand = GRABBER_RELEASE;
           hmsg = "R- " ;
        }
        
        string lmsg = "   " ;
        if ((keyStatus & IS_LIGHT) <> 0)
        {
           if (l_light)
           {
              TURN_OFF_LIGHT;
              l_light = FALSE;
              lmsg    = "OFF";
           }
           else
           {
              TURN_ON_LIGHT;
              l_light = TRUE;
              lmsg    = "ON ";
           }
        }

        if (isNPmode)
        {
           TextOut(60, LCD_LINE3, lmsg);
           TextOut(60, LCD_LINE4, hmsg);
           TextOut(54, LCD_LINE5, emsg);
           TextOut(48, LCD_LINE6, smsg);
           TextOut(54, LCD_LINE7, amsg);
        }
        
        if (!((l_arm   == arm)   && (l_shoulder == shoulder) &&
              (l_elbow == elbow) && (l_hand     == hand)))
        {
           Do_Robot_Action(arm, shoulder, elbow, hand);
           
           l_arm      = arm;
           l_shoulder = shoulder;
           l_elbow    = elbow;
           l_hand     = hand;
        }
    }
 }
 
/* ********************************************************
 * Clear screen
 * ******************************************************** */
 void ClearScreen_Rtn()
 {
    TextOut(60, LCD_LINE3, "   " );     //60, LCD_LINE3(lmsg)
    TextOut(60, LCD_LINE4, " - " );     //60, LCD_LINE4(hmsg)
    TextOut(54, LCD_LINE5, " - - " );   //54, LCD_LINE5(emsg)
    TextOut(48, LCD_LINE6, "  - -  " ); //48, LCD_LINE6(smsg)
    TextOut(54, LCD_LINE7, " - - " );   //54, LCD_LINE7(amsg)
 }

/* *******************************************************
 * Get NumericPad's key pressed status for controlling the NXTServo
 * ******************************************************* */
 void NumPadCtrl_Rtn()
 {
    int keyStatus, last_keyStatus;
    long currTick;
    
    //                    "1234567890123456"
    TextOut(0, LCD_LINE2, "    <NumPad>    " );
    TextOut(0, LCD_LINE3, "Light    [   ]  " ); //60, LCD_LINE3
    TextOut(0, LCD_LINE4, "Hand     [ - ]  " ); //60, LCD_LINE4
    TextOut(0, LCD_LINE5, "Elbow   [ - - ] " ); //54, LCD_LINE5
    TextOut(0, LCD_LINE6, "Shulder[  - -  ]" ); //48, LCD_LINE6
    TextOut(0, LCD_LINE7, "Arm     [ - - ] " ); //54, LCD_LINE7
    TextOut(0, LCD_LINE8, "          Return" );
    
    Set_Robot_Home();
    currTick = CurrentTick();
    do
    {
         keyStatus = Get_NumericPad_key ();
         if (keyStatus <> last_keyStatus)
         {
            NXTServo_Control_Rtn(keyStatus, TRUE);
            last_keyStatus = keyStatus;
            currTick       = CurrentTick();
         }
         else if ((CurrentTick() - currTick) > CHECK_INTERVAL)
              {
                 ClearScreen_Rtn();
                 currTick = CurrentTick();
              }
         Wait(50);
    }    until (ButtonPressed(BTNRIGHT, TRUE));
    Set_Robot_Home();
 }

/* *******************************************************
 * Demo routine
 * ******************************************************* */
 void RoboArm_Demo_Rtn()
 {
    Set_Robot_Home();

    TextOut(0, LCD_LINE2, "    < Demo >    " );
    TextOut(0, LCD_LINE3, "                " );
    TextOut(0, LCD_LINE4, "Using NumPad can" );
    TextOut(0, LCD_LINE5, "control 4 kinds " );
    TextOut(0, LCD_LINE6, "of Robot action." );
    TextOut(0, LCD_LINE7, "                " );
    TextOut(0, LCD_LINE8, "Run             " );
    
    Wait(50);
    until(ButtonPressed(BTNLEFT, TRUE));
    // 3 orientations
    TextOut(0, LCD_LINE4, "1. Orientation  " );
    TextOut(0, LCD_LINE5, "Turn RIGHT/LEFT " );
    TextOut(0, LCD_LINE6, "     and CENTER." );
    TextOut(0, LCD_LINE8, "                " );
    NXTServo_Control_Rtn((0x0FFF & (IS_RIGHT)),  FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_CENTER)), FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_LEFT)),   FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_CENTER)), FALSE);
    
    // 5 arm postures
    TextOut(0, LCD_LINE4, "2. Arm Posture  " );
    TextOut(0, LCD_LINE5, "(1) Arm:        " );
    TextOut(0, LCD_LINE6, "BACK/UP/FOR-WARD" );
    TextOut(0, LCD_LINE7, "(2) Hand:       " );
    TextOut(0, LCD_LINE8, "UP/STRAIGHT/DOWN" );
    NXTServo_Control_Rtn((0x0FFF & (IS_BACK_UP)),          FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_UP_HORIZONTAL)),    FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_FRONT_HORIZONTAL)), FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_FRONT_DOWN)),       FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_FRONT_HORIZONTAL)), FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_UP_HORIZONTAL)),    FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_BACK_HORIZONTAL)),  FALSE);
    
    // Grabber control
    TextOut(0, LCD_LINE4, "3. Grabber      " );
    TextOut(0, LCD_LINE5, "   Release/Hold " );
    TextOut(0, LCD_LINE6, "                " );
    TextOut(0, LCD_LINE7, "                " );
    TextOut(0, LCD_LINE8, "                " );
    NXTServo_Control_Rtn((0x0FFF & (IS_GRAB_RELEASE)), FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_GRAB_HOLD)),    FALSE);
    
    // Light control
    TextOut(0, LCD_LINE4, "4. Light        " );
    TextOut(0, LCD_LINE5, "   ON/OFF       " );
    for (short xii=0; xii<10; xii++)
    {
        NXTServo_Control_Rtn((0x0FFF & (IS_LIGHT)), FALSE);
        Wait(300);
        NXTServo_Control_Rtn((0x0FFF & (IS_LIGHT)), FALSE);
        Wait(100);
    }
    
    // Combined actions
    TextOut(0, LCD_LINE4, "  It also can   " );
    TextOut(0, LCD_LINE5, "   combine      " );
    TextOut(0, LCD_LINE6, "  multi-actions." );
    Set_Robot_Home();
    
    NXTServo_Control_Rtn((0x0FFF & (IS_UP_HORIZONTAL)),                     FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_RIGHT)),                             FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_FRONT_HORIZONTAL | IS_GRAB_RELEASE)),FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_FRONT_DOWN)),                        FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_GRAB_HOLD)),                         FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_FRONT_HORIZONTAL)),                  FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_UP_HORIZONTAL)),                     FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_LEFT | IS_BACK_HORIZONTAL)),         FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_CENTER | IS_UP_HORIZONTAL)),         FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_FRONT_HORIZONTAL)),                  FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_FRONT_DOWN | IS_GRAB_RELEASE)),      FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_UP_HORIZONTAL)),                     FALSE);
    NXTServo_Control_Rtn((0x0FFF & (IS_GRAB_HOLD)),                         FALSE);
    
    Set_Robot_Home();
 }
 
/* *******************************************************
 * Main menu routine
 * ******************************************************* */
 void Start_Rtn()
 {
    ClearScreen();
    TextOut(0, LCD_LINE1, "NXTServo RoboArm" );

    //ArrayInit(Nxtservo_speed, SET_SPEED,8);
    ArrayBuild(Nxtservo_speed, 30, 30, 30, 30, 30, 0, 0, 0);
    Set_ServoGroup_Speed();
    ArrayInit(Nxtservo_q_pos,150,8);
    Set_Robot_Home();
    
    bool isStopRun = FALSE;
    while(!isStopRun)
    {
         TextOut(0, LCD_LINE2, " <Main  Menual> " );
         TextOut(0, LCD_LINE3, "                " );
         TextOut(0, LCD_LINE4, "                " );
         TextOut(0, LCD_LINE5, "  CH Lego(2011) " );
         TextOut(0, LCD_LINE6, "                " );
         TextOut(0, LCD_LINE7, "                " );
         TextOut(0, LCD_LINE8, "NumPad Demo Quit" );

         while(TRUE)
         {
              Wait(50);
              if (ButtonPressed(BTNRIGHT, TRUE))
              {
                 isStopRun = TRUE;
                 break;
              }
              else if (ButtonPressed(BTNCENTER, TRUE))
                   {
                      RoboArm_Demo_Rtn();
                      break;
                   }
                   else if (ButtonPressed(BTNLEFT, TRUE))
                        {
                           NumPadCtrl_Rtn();
                           break;
                        }
         }
    }
 }


/* ********************************************************
 * Set NumericPad sensor value
 * ******************************************************** */
 void Set_NumericPad (const byte & Group[])
 {
    byte I2C_req_buf[];
    ArrayBuild(I2C_req_buf, NUMPAD, Group);

    while(LowspeedCheckStatus(NUMPAD_PORT) == STAT_COMM_PENDING);
    LowspeedWrite(NUMPAD_PORT, 0, I2C_req_buf);

    while(LowspeedCheckStatus(NUMPAD_PORT) == STAT_COMM_PENDING);
 }
 
/* ********************************************************
 * Initialize Numeric Pad
 * ******************************************************** */
 void NumPad_Init()
 {
    byte Group1[] = {0x41, 0x0F, 0x0A, 0x0F, 0x0A, 0x0F, 0x0A, 0x0F, 0x0A, 0x0F};//2
    byte Group2[] = {0x4A, 0x0A, 0x0F, 0x0A, 0x0F, 0x0A, 0x0F, 0x0A, 0x0F};//3
    byte Group3[] = {0x52, 0x0A, 0x0F, 0x0A, 0x0F, 0x0A, 0x0F, 0x0A, 0x0F};//4
    byte Group4[] = {0x5C, 0x0b, 0x20, 0x0C};//5
    byte Group5[] = {0x2B, 0x01, 0x01, 0x00, 0x00, 0x01, 0x01, 0xFF, 0x02};//1
    byte Group6[] = {0x7B, 0x0B};
    byte Group7[] = {0x7D, 0x9C, 0x65, 0x8C};//6

    Set_NumericPad(Group1);
    Set_NumericPad(Group2);
    Set_NumericPad(Group3);
    Set_NumericPad(Group4);
    Set_NumericPad(Group5);
    Set_NumericPad(Group6);
    Set_NumericPad(Group7);
}

/* *******************************************************
 * Initialize the sensors
 * ******************************************************* */
 void I2CSensor_Init()
 {
    SetSensorLowspeed(NXTSERVO_PORT);
    SetSensorLowspeed(NUMPAD_PORT);
    NumPad_Init();
 }

/* ********************************************************
 * main()
 * ******************************************************** */
 task main()
 {
    I2CSensor_Init();
    Start_Rtn();
    Stop(true);
 }