File content as of revision 1:19d647492936:

void control_servo( void )
{
    short   torque[9]  = { 0xfa, 0xaf, 0x00, 0x00, 0x24, 0x01, 0x01, 0x01, 0x00 };
    short send_deg[10] = { 0xfa, 0xaf, 0x00, 0x00, 0x1e, 0x02, 0x01, 0x01, 0x00, 0x00 };
    //                      Hdr,  Hdr,   ID,  Flg,  Adr,  Len,  Cnt,Lower,Upper,  Sum
    
    for( int a = 0; a < LENGTH; a ++ )
    {
        torque[2] = ID_num[a];
        torque[8] = cal_sum( torque, 8 );
        for( int b = 0; b < 9; b ++ )   servo.putc( torque[b] );
        wait_ms( 5 );
    }
    
    while( 1 )
    {
        for( int a = 0; a < LENGTH; a ++ )
        {
            for( int i = 0; i < LENGTH; i ++ )
            {
                send_deg[2] = ID_num[i];
                cal_deg( degree[a], &send_deg[7], &send_deg[8] );
                send_deg[9] = cal_sum( send_deg, 9 );
                for( int b = 0; b < 10; b ++ )   servo.putc( send_deg[b] );
                wait_ms( 1000 );
            }
        }
    }
}

void cal_deg( short degree, short *lower, short *upper )
{
    *lower = degree & 0x00ff;
    *upper = ( degree >> 8 ) & 0x00ff;
}

void id_set( void )
{
    int i, j;
    short ID[LENGTH][9] = 
                         {
                             { 0xfa, 0xaf, 0x01, 0x00, 0x04, 0x01, 0x01, 0x01, 0x00 }    //ID
                         };
    
    for( i = 0; i < LENGTH; i ++ )
    {
        ID[i][7] = ID_num[i];
        ID[i][8] = cal_sum( ID[i], 8 );
        for( j = 0; j < 9; j ++ )   servo.putc( ID[i][j] );
        save( ID[i][7] );
    }
}

short cal_sum( short *data, int sum_num )
{
    int sum = 0, j;
    
    for( j = 2; j < sum_num; j ++ )
    {
        sum ^= ( int )data[j];
    }
    
    return sum;
}

void save( short id )
{
    int i, j;
    short save[2][8] =
                    {
                        { 0xfa, 0xaf, id, 0x40, 0xff, 0x00, 0x00, 0x00 },   //save
                        { 0xfa, 0xaf, id, 0x20, 0xff, 0x00, 0x00, 0x00 }    //reboot
                    };
    
    for( i = 0; i < 2; i ++ )
    {
        save[i][2] = id;
        save[i][7] = cal_sum( save[i], 7 );
        for( j = 0; j < 8; j ++ )   servo.putc( save[i][j] );
    }
}