低端单片机(8bit)的16路舵机调速分析与实现

低端单片机(8bit)的16路舵机调速分析与实现

By 马冬亮(凝霜  Loki)

一个人的战争(http://blog.csdn.net/MDL13412)

        今年的英特尔杯准备做机器人方向，所以在淘宝上买了机器人套件，自己进行组装和编程。机器人装配完如下图所示：

        （注：这款机器人由17路舵机组成，其中左右手各2路，左右腿各4路，身体4路，头部1路。）

        装配好机器人，首要任务就是让机器人运动起来。看了一下卖家给的控制程序，写的很混乱，维护极差，于是准备自己写一套控制程序，也算是给学弟学妹们留下的一份礼物吧^_^。

        首先简单介绍一下我们使用的舵机参数：

        *型号.：KCS110M                  

        *速度 ：5.0V: 0.14sec/60°

        *扭力 ：5.0V: 9.5kg/cm

        *工作电压：5.0-7.4Volts  

        *转角范围  : 0~185度

        由于是控制机器人，所以我对转角范围的需求是0～180度，那么舵机由0度转到180度需要至少0.6s的时间。

        舵机的调速原理其实很简单，通过给定连续的PWM信号，舵机就可以旋转到相应的角度。我们的这款舵机和竞赛的通用舵机基本一致，PWM周期为20ms，复位需要T1=0.5ms的高电平，T2=(20 - 0.5)ms的低电平。每增加1度，需要在T1的基础上加10us。为了让舵机完整的转到对应角度，我们需要至少对舵机连续送0.6s的PWM信号。

        由于舵机调速对PWM精度要求很高，传统的12T 8051单片机每个指令周期占12个机器周期，速度不能达到要求。我们选择了STC12C5A60S2这款单片机，这款单片机较传统的12T 8051快至少6倍以上，并配合24MHZ的晶振来控制机器人。

        舵机调速一般有两种方法，一种是使用定时器中断进行调速，另外一种是使用软件延时进行调速。

        首先分析定时器中断调速的方法：

        我的调速精度要求是1度，所以定时器需要定时10us，那么，每200次定时器中断完成一个PWM调速周期。这种编程方式的优点是实现简单，代码容易读懂。其缺点是只能处理少数舵机，不能对大量舵机进行调速。因为每10us发生一次定时中断，而每个机器周期为(1 * 1000 * 1000) / (24 * 1000 * 1000)=0.0417us，则在每个定时器中断中有10 / 0.0417=240个指令周期。我们看下面的程序：

[cpp]  view plain copy print ?

1. #include <STC_NEW_8051.H>  
2.   
3. void main()  
4. {  
5.     volatile int cnt = 0;  
6.     volatile int pwm = 0;  
7.     volatile int pwmCycle = 10;  
8.       
9.     cnt = (cnt + 1) % 40;  
10.       
11.     if (++cnt < pwmCycle)  
12.         pwm = 1;  
13.     else  
14.         pwm = 0;  
15. }  

        这段程序是模拟定时器中断中对一路舵机进行调速的情况，注意，这里一定要使用volatile关键字，否则程序会由于编译器的优化而错误。对于所有中断中用到的全局变量，我们都需要加上volatile关键字防止编译器优化。下面，我们对这段代码进行反汇编，并计算指令周期数。反汇编代码如下：

[plain]  view plain copy print ?

1. C:0x0097    F50C     MOV      0x0C,A                        3  
2. C:0x0099    750D0A   MOV      0x0D,#0x0A               3  
3.   
4. C:0x009C    E509     MOV      A,0x09                        2  
5. C:0x009E    2401     ADD      A,#0x01                       2  
6. C:0x00A0    FF       MOV      R7,A                             2  
7. C:0x00A1    E4       CLR      A                                  1  
8. C:0x00A2    3508     ADDC     A,0x08                        3  
9. C:0x00A4    FE       MOV      R6,A                             2  
10. C:0x00A5    7C00     MOV      R4,#0x00                     2  
11. C:0x00A7    7D28     MOV      R5,#0x28                     2  
12. C:0x00A9    120003   LCALL    C?SIDIV(C:0003)          
13. C:0x00AC    8C08     MOV      0x08,R4                       3  
14. C:0x00AE    8D09     MOV      0x09,R5                       3  
15.   
16. C:0x00B0    0509     INC      0x09                              4  
17. C:0x00B2    E509     MOV      A,0x09                         2  
18. C:0x00B4    7002     JNZ      C:00B8                          3  
19. C:0x00B6    0508     INC      0x08                              4  
20. C:0x00B8    AE08     MOV      R6,0x08                        4  
21. C:0x00BA    C3       CLR      C                                    1  
22. C:0x00BB    950D     SUBB     A,0x0D                         3  
23. C:0x00BD    E50C     MOV      A,0x0C                         2  
24. C:0x00BF    6480     XRL      A,#P0(0x80)                    2  
25. C:0x00C1    F8       MOV      R0,A                               2  
26. C:0x00C2    EE       MOV      A,R6                               1  
27. C:0x00C3    6480     XRL      A,#P0(0x80)                     2  
28. C:0x00C5    98       SUBB     A,R0                                2  
29. C:0x00C6    5007     JNC      C:00CF                            3  
30.   
31. C:0x00C8    750A00   MOV      0x0A,#0x00                   3  
32. C:0x00CB    750B01   MOV      0x0B,#0x01                   3  
33. //C:0x00CE    22       RET        
34. 多个if情况时这里应该是一个跳转                                  3  
35.   
36. C:0x00CF    E4       CLR      A                                      1  
37. C:0x00D0    F50A     MOV      0x0A,A                            3  
38. C:0x00D2    F50B     MOV      0x0B,A                            3  

        由以上代码可知，一个舵机的调速周期为79+[LCALL C?SIDIV(C:0003) ]条指令周期，所以，对于10us的定时器中断，最多可以调速2路舵机。

       接下来我们来分析软件延时的方法进行调速，首先我们需要一个延时函数，那么我们定义一个延时10us的函数如下所示：

[cpp]  view plain copy print ?

1. void Delay10us(UINT16 ntimes)  
2. {  
3.     for (delayVar1 = 0; delayVar1 < ntimes; ++delayVar1)  
4.         for (delayVar2 = 0; delayVar2 < 21; ++delayVar2)     
5.             _nop_();       
6. }  

        如果对每个舵机分别进行调速，那么我们的时间开销为0.6 * 16 = 9.6s，这个时间是不可接受的。由于8位单片机有P0-P3的管脚阵列，那么我们可以利用其中的某一列，同时调速8路舵机，那么我们的时间开销为0.6 * 2 = 1.2s，这个对于我们的机器人来说是完全可以接受的。

        同时对8路舵机进行调速的话，我们首先需要对舵机的调速角度进行排序（升序），然后计算出两两舵机舵机的差值，这样就可以同时对8路舵机进行调速了。对于出现的非法角度，我们给这路舵机送全为高电平的PWM信号，使此路舵机保持先前状态。代码如下所示：

[cpp]  view plain copy print ?

1. void InitPwmPollint()  
2. {  
3.     UCHAR8 i;  
4.     UCHAR8 j;  
5.     UCHAR8 k;  
6.     UINT16 temp;  
7.       
8.     for (i = 0; i < USED_PIN; ++i)  
9.     {  
10.         for (j = 0; j < 7; ++j)  
11.         {  
12.             for (k = j; k < 8; ++k)  
13.             {  
14.                 if (g_diffAngle[i][j] > g_diffAngle[i][k])  
15.                 {  
16.                     temp = g_diffAngle[i][j];  
17.                     g_diffAngle[i][j] = g_diffAngle[i][k];  
18.                     g_diffAngle[i][k] = temp;  
19.                       
20.                     temp = g_diffAngleMask[i][j];  
21.                     g_diffAngleMask[i][j] = g_diffAngleMask[i][k];  
22.                     g_diffAngleMask[i][k] = temp;  
23.                 }  
24.             }  
25.         }  
26.     }  
27.       
28.     for (i = 0; i < USED_PIN; ++i)  
29.     {  
30.         for (j = 0; j < 8; ++j)  
31.         {  
32.             if (INVALID_JOINT_VALUE == g_diffAngle[i][j])  
33.             {  
34.                 g_diffAngle[i][j] = STEERING_ENGINE_FULL_CYCLE;  
35.             }  
36.         }  
37.     }  
38.       
39.     for (i = 0; i < USED_PIN; ++i)  
40.     {  
41.         for (j = 7; j >= 1; --j)  
42.         {  
43.             g_diffAngle[i][j] = g_diffAngle[i][j] - g_diffAngle[i][j - 1];  
44.         }  
45.     }      
46. }  

        （注：对于上面用到的一些常量，请大家参考本文最后给出的完整程序。

         下面我们举例说明上述代码，假设 g_diffAngle[0][] = {10, 20, 40, 40, 50, 80, 90, 10}，那么经过排序后，其变为g_diffAngle[0][] = {10, 10, 20, 40, 40, 50, 80, 90}，而g_diffAngleMask[0][]中的值对应个角度使用的管脚的掩码，例如P0.0，则其掩码为0x01，对P0 & 0x01进行掩码计算，就可以对指定的管脚进行操作。我们对角度排序的同时，需要更新管脚的掩码，以达到相互对应的目的。

        接下来对角度进行校验，对于不合法的角度，我们使用STEERING_ENGINE_FULL_CYCLE来进行填充，使这路舵机在PWM调速周期内全为高电平。
       最后计算差值，计算后g_diffAngle[0][]={10, 0, 10, 20, 0, 10, 30, 10}，这样就求出了后面的舵机相对于其前面一路的差值。我们对g_diffAngle[0][0]对应的舵机延时Delay10us(g_diffAngle[0][0])即Delay10us(10)，延时完成后我们将这路舵机给低电平，这用到了前面定义的掩码操作。延时完成后，开始延时g_diffAngle[0][1]对应的舵机，Delay10us(g_diffAngle[0][1])即Delay10us(0)，然后将此路舵机送低电平。再延时Delay10us(g_diffAngle[0][2])即Delay10us(10)，然后再移除，依次类推。这样就能保证在一个PWM周期内对8路舵机同时调速。调速部分代码如下：

[cpp]  view plain copy print ?

1. #define PWM_STEERING_ENGINE(group)                                                          \  
2. {                                                                                           \  
3.     counter = STEERING_ENGINE_INIT_DELAY;                                                   \  
4.         for (i = 0; i < 8; ++i)                                                             \  
5.             counter += g_diffAngle[PIN_GROUP_##group][i];                                   \  
6.                                                                                             \  
7.         for (i = 0; i < 30; ++i)                                                            \  
8.         {                                                                                   \  
9.             GROUP_##group##_CONTROL_PIN = 0xFF;                                             \  
10.             Delay10us(STEERING_ENGINE_INIT_DELAY);                                          \  
11.                                                                                             \  
12.             for (j = 0; j < 8; ++j)                                                         \  
13.             {                                                                               \  
14.                 Delay10us(g_diffAngle[PIN_GROUP_##group][j]);                               \  
15.                 GROUP_##group##_CONTROL_PIN &= ~(g_diffAngleMask[PIN_GROUP_##group][j]);    \  
16.             }                                                                               \  
17.                                                                                             \  
18.             Delay10us(STEERING_ENGINE_CYCLE - counter);                                     \  
19.         }                                                                                   \  
20. }  

[cpp]  view plain copy print ?

1. while (1)  
2. {  
3.     PWM_STEERING_ENGINE(1);  
4. }  

        通过上面的操作，我们便完成了同时对8路舵机的调速操作。

        总结：

        对于只有1、2路舵机的情况，我们可以使用定时器中断进行调速，其具有实现简单，通俗易懂的有点。而对于多路舵机的情形，就需要使用软件延时进行操作，其优点是可以同时对8路舵机进行调速，效率高，但是其代码不容易实现与调试。

        附完整程序：

        （注：由于需求变动很大，包括功能和管脚分配等，所以本程序大量使用宏及enum）

[cpp]  view plain copy print ?

1. /** 
2.  * @file    ControlRobot.h 
3.  * @author  马冬亮 
4.  * @brief   用于对机器人进行控制. 
5.  */  
6.   
7. #ifndef CONTROLROBOT_H  
8. #define CONTROLROBOT_H  
9.   
10. /** 
11.  * @brief   关节角度非法值. 
12.  * @ingroup ControlRobot 
13.  *  
14.  * @details 当关节角度无法确定或捕获关节失败时, 设置为此数值, 机器人接收到此数值则不转动舵机. 
15.  */  
16. #define INVALID_JOINT_VALUE 200  
17.   
18. /** 
19.  * @brief   定义关节常量用于存取关节角度. 
20.  * @ingroup ControlRobot 
21.  */  
22. typedef enum RobotJoint  
23. {  
24.     ROBOT_LEFT_SHOULDER_VERTICAL      = 0,  ///< 左肩膀, 前后转动的舵机  
25.     ROBOT_LEFT_SHOULDER_HORIZEN       = 1,  ///< 左肩膀, 上下转动的舵机  
26.     ROBOT_LEFT_ELBOW                  = 2,  ///< 左肘, 左臂肘关节的舵机  
27.   
28.     ROBOT_RIGHT_SHOULDER_VERTICAL     = 3,  ///< 右肩膀, 前后转动的舵机  
29.     ROBOT_RIGHT_SHOULDER_HORIZEN      = 4,  ///< 右肩膀, 上下转动的舵机  
30.     ROBOT_RIGHT_ELBOW                 = 5,  ///< 右肘, 右臂肘关节的舵机  
31.   
32.     ROBOT_LEFT_HIP_VERTICAL           = 6,  ///< 左髋, 左右转动的舵机  
33.     ROBOT_LEFT_HIP_HORIZEN            = 7,  ///< 左髋, 前后转动的舵机  
34.     ROBOT_LEFT_KNEE                   = 8,  ///< 左膝盖, 左膝关节的舵机  
35.     ROBOT_LEFT_ANKLE                  = 9,  ///< 左踝, 这个舵机不使用  
36.   
37.     ROBOT_RIGHT_HIP_VERTICAL          = 10, ///< 右髋, 左右转动的舵机  
38.     ROBOT_RIGHT_HIP_HORIZEN           = 11, ///< 右髋, 前后转动的舵机  
39.     ROBOT_RIGHT_KNEE                  = 12, ///< 右膝盖, 右膝关节的舵机  
40.     ROBOT_RIGHT_ANKLE                 = 13, ///< 右踝, 这个舵机不使用  
41.   
42.     ROBOT_LEFT_FOOT                   = 14, ///< 左脚, 这个舵机不使用  
43.     ROBOT_RIGHT_FOOT                  = 15, ///< 右脚, 这个舵机不使用  
44.   
45.     ROBOT_HEAD                        = 16, ///< 头, 这个舵机不使用  
46.           
47.     ROBOT_JOINT_AMOUNT                = ROBOT_HEAD + 1  
48. } RobotJoint;  
49.   
50. typedef enum RobotSteeringEnginePinIndex  
51. {  
52.     ROBOT_PIN_INDEX_LEFT_SHOULDER_VERTICAL                  = 0,  
53.     ROBOT_PIN_INDEX_LEFT_SHOULDER_HORIZEN                   = 1,  
54.     ROBOT_PIN_INDEX_LEFT_ELBOW                              = 2,  
55.       
56.     ROBOT_PIN_INDEX_RIGHT_SHOULDER_VERTICAL                 = 3,  
57.     ROBOT_PIN_INDEX_RIGHT_SHOULDER_HORIZEN                  = 4,  
58.     ROBOT_PIN_INDEX_RIGHT_ELBOW                             = 5,  
59.       
60.     ROBOT_PIN_INDEX_LEFT_HIP_VERTICAL                       = 6,  
61.     ROBOT_PIN_INDEX_RIGHT_HIP_VERTICAL                      = 7,  
62.       
63.     ROBOT_PIN_INDEX_LEFT_HIP_HORIZEN                        = 0,  
64.     ROBOT_PIN_INDEX_LEFT_KNEE                               = 1,  
65.     ROBOT_PIN_INDEX_LEFT_ANKLE                              = 2,  
66.     ROBOT_PIN_INDEX_LEFT_FOOT                               = 3,  
67.       
68.     ROBOT_PIN_INDEX_RIGHT_HIP_HORIZEN                       = 4,  
69.     ROBOT_PIN_INDEX_RIGHT_KNEE                              = 5,  
70.     ROBOT_PIN_INDEX_RIGHT_ANKLE                             = 6,  
71.     ROBOT_PIN_INDEX_RIGHT_FOOT                              = 7  
72. } RobotSteeringEnginePinIndex;  
73.   
74. typedef enum RobotSteeringEnginePinMask  
75. {  
76.     ROBOT_PIN_MASK_LEFT_SHOULDER_VERTICAL                  = 0x01,  
77.     ROBOT_PIN_MASK_LEFT_SHOULDER_HORIZEN                   = 0x02,  
78.     ROBOT_PIN_MASK_LEFT_ELBOW                              = 0x04,  
79.       
80.     ROBOT_PIN_MASK_RIGHT_SHOULDER_VERTICAL                 = 0x08,  
81.     ROBOT_PIN_MASK_RIGHT_SHOULDER_HORIZEN                  = 0x10,  
82.     ROBOT_PIN_MASK_RIGHT_ELBOW                             = 0x20,  
83.       
84.     ROBOT_PIN_MASK_LEFT_HIP_VERTICAL                       = 0x40,  
85.     ROBOT_PIN_MASK_RIGHT_HIP_VERTICAL                      = 0x80,  
86.       
87.     ROBOT_PIN_MASK_LEFT_HIP_HORIZEN                        = 0x01,  
88.     ROBOT_PIN_MASK_LEFT_KNEE                               = 0x02,  
89.     ROBOT_PIN_MASK_LEFT_ANKLE                              = 0x04,  
90.     ROBOT_PIN_MASK_LEFT_FOOT                               = 0x08,  
91.       
92.     ROBOT_PIN_MASK_RIGHT_HIP_HORIZEN                       = 0x10,  
93.     ROBOT_PIN_MASK_RIGHT_KNEE                              = 0x20,  
94.     ROBOT_PIN_MASK_RIGHT_ANKLE                             = 0x40,  
95.     ROBOT_PIN_MASK_RIGHT_FOOT                              = 0x80  
96. } RobotSteeringEnginePinMask;  
97.   
98.   
99.       
100. #define PROTOCOL_HEADER             "\xC9\xCA"  
101. #define PROTOCOL_END                "\xC9\xCB"  
102. #define PROTOCOL_END_LENGTH         2  
103. #define PROTOCOL_HEADER_LENGTH      2  
104. #define COMMAND_STR_LENGTH          (PROTOCOL_HEADER_LENGTH + ROBOT_JOINT_AMOUNT + PROTOCOL_END_LENGTH)  
105. #define COMMAND_STR_BUFFER_SIZE     ((COMMAND_STR_LENGTH) + 2)  
106.   
107. #endif  /* CONTROLROBOT_H */  

// main.c

[cpp]  view plain copy print ?

1. #include <STC_NEW_8051.H>  
2. #include "ControlRobot.h"  
3. #include<intrins.h>  
4.   
5. #define DEBUG_PROTOCOL  
6.   
7. typedef unsigned char UCHAR8;  
8. typedef unsigned int UINT16;  
9.   
10. #undef TRUE  
11. #undef FALSE  
12. #define TRUE 1  
13. #define FALSE 0  
14.   
15. #define MEMORY_MODEL   
16.   
17. UINT16 MEMORY_MODEL delayVar1;  
18. UCHAR8 MEMORY_MODEL delayVar2;  
19.   
20. #define BAUD_RATE 0xF3  
21.   
22. #define USED_PIN 2  
23. #define PIN_GROUP_1 0  
24. #define PIN_GROUP_2 1  
25.   
26. #define GROUP_1_CONTROL_PIN P0  
27. #define GROUP_2_CONTROL_PIN P2  
28.   
29. #define STEERING_ENGINE_CYCLE 2000  
30. #define STEERING_ENGINE_INIT_DELAY 50  
31. #define STEERING_ENGINE_FULL_CYCLE ((STEERING_ENGINE_CYCLE) - (STEERING_ENGINE_INIT_DELAY))  
32.   
33. volatile UCHAR8 MEMORY_MODEL g_angle[2][ROBOT_JOINT_AMOUNT];  
34. volatile bit MEMORY_MODEL g_fillingBufferIndex = 0;  
35. volatile bit MEMORY_MODEL g_readyBufferIndex = 1;  
36. volatile bit MEMORY_MODEL g_swapBuffer = FALSE;  
37.   
38. volatile UINT16 MEMORY_MODEL g_diffAngle[USED_PIN][8];  
39. volatile UCHAR8 MEMORY_MODEL g_diffAngleMask[USED_PIN][8] =   
40. {  
41.     {  
42.         ROBOT_PIN_MASK_LEFT_SHOULDER_VERTICAL,    
43.         ROBOT_PIN_MASK_LEFT_SHOULDER_HORIZEN,    
44.         ROBOT_PIN_MASK_LEFT_ELBOW,    
45.         ROBOT_PIN_MASK_RIGHT_SHOULDER_VERTICAL,  
46.         ROBOT_PIN_MASK_RIGHT_SHOULDER_HORIZEN,    
47.         ROBOT_PIN_MASK_RIGHT_ELBOW,    
48.         ROBOT_PIN_MASK_LEFT_HIP_VERTICAL,    
49.         ROBOT_PIN_MASK_RIGHT_HIP_VERTICAL   
50.     },  
51.     {  
52.         ROBOT_PIN_MASK_LEFT_HIP_HORIZEN,    
53.         ROBOT_PIN_MASK_LEFT_KNEE,    
54.         ROBOT_PIN_MASK_LEFT_ANKLE,    
55.         ROBOT_PIN_MASK_LEFT_FOOT,    
56.         ROBOT_PIN_MASK_RIGHT_HIP_HORIZEN,    
57.         ROBOT_PIN_MASK_RIGHT_KNEE,    
58.         ROBOT_PIN_MASK_RIGHT_ANKLE,    
59.         ROBOT_PIN_MASK_RIGHT_FOOT  
60.     }  
61. };  
62.   
63. #ifdef DEBUG_PROTOCOL  
64. sbit P10 = P1 ^ 0;  // 正在填充交换区1  
65. sbit P11 = P1 ^ 1;  // 正在填充交换区2  
66. sbit P12 = P1 ^ 2;  // 交换区变换  
67. sbit P13 = P1 ^ 3;  // 协议是否正确  
68. #endif   
69.   
70. void Delay10us(UINT16 ntimes)  
71. {  
72.     for (delayVar1 = 0; delayVar1 < ntimes; ++delayVar1)  
73.         for (delayVar2 = 0; delayVar2 < 21; ++delayVar2)     
74.             _nop_();       
75. }  
76.   
77. void InitPwmPollint()  
78. {  
79.     UCHAR8 i;  
80.     UCHAR8 j;  
81.     UCHAR8 k;  
82.     UINT16 temp;  
83.       
84.     for (i = 0; i < USED_PIN; ++i)  
85.     {  
86.         for (j = 0; j < 7; ++j)  
87.         {  
88.             for (k = j; k < 8; ++k)  
89.             {  
90.                 if (g_diffAngle[i][j] > g_diffAngle[i][k])  
91.                 {  
92.                     temp = g_diffAngle[i][j];  
93.                     g_diffAngle[i][j] = g_diffAngle[i][k];  
94.                     g_diffAngle[i][k] = temp;  
95.                       
96.                     temp = g_diffAngleMask[i][j];  
97.                     g_diffAngleMask[i][j] = g_diffAngleMask[i][k];  
98.                     g_diffAngleMask[i][k] = temp;  
99.                 }  
100.             }  
101.         }  
102.     }  
103.       
104.     for (i = 0; i < USED_PIN; ++i)  
105.     {  
106.         for (j = 0; j < 8; ++j)  
107.         {  
108.             if (INVALID_JOINT_VALUE == g_diffAngle[i][j])  
109.             {  
110.                 g_diffAngle[i][j] = STEERING_ENGINE_FULL_CYCLE;  
111.             }  
112.         }  
113.     }  
114.       
115.     for (i = 0; i < USED_PIN; ++i)  
116.     {  
117.         for (j = 7; j >= 1; --j)  
118.         {  
119.             g_diffAngle[i][j] = g_diffAngle[i][j] - g_diffAngle[i][j - 1];  
120.         }  
121.     }      
122. }  
123.   
124.   
125. void InitSerialPort()  
126. {  
127.     AUXR    = 0x00;  
128.     ES      = 0;  
129.     TMOD    = 0x20;  
130.     SCON    = 0x50;  
131.     TH1     = BAUD_RATE;  
132.     TL1     = TH1;  
133.     PCON    = 0x80;  
134.     EA      = 1;  
135.     ES      = 1;  
136.     TR1     = 1;  
137. }  
138.   
139. void OnSerialPort() interrupt 4  
140. {  
141.     static UCHAR8 previousChar = 0;  
142.     static UCHAR8 currentChar = 0;  
143.     static UCHAR8 counter = 0;  
144.   
145.     if (RI)  
146.     {  
147.         RI = 0;  
148.         currentChar = SBUF;  
149.   
150.         if (PROTOCOL_HEADER[0] == currentChar)       // 协议标志  
151.         {  
152.             previousChar =     currentChar;  
153.         }  
154.         else  
155.         {  
156.             if (PROTOCOL_HEADER[0] == previousChar && PROTOCOL_HEADER[1] == currentChar)       // 协议开始  
157.             {  
158.                 counter = 0;  
159.                 previousChar = currentChar;  
160.                 g_swapBuffer = FALSE;  
161.             }  
162.             else if (PROTOCOL_END[0] == previousChar && PROTOCOL_END[1] == currentChar)   // 协议结束  
163.             {  
164.                 previousChar = currentChar;  
165.   
166.                 if (ROBOT_JOINT_AMOUNT == counter)      // 协议接受正确  
167.                 {  
168.                     if (0 == g_fillingBufferIndex)  
169.                     {  
170.                         g_readyBufferIndex = 0;  
171.                         g_fillingBufferIndex = 1;  
172.                     }  
173.                     else  
174.                     {  
175.                         g_readyBufferIndex = 1;  
176.                         g_fillingBufferIndex = 0;  
177.                     }  
178.   
179.                     g_swapBuffer = TRUE;  
180.       
181. #ifdef DEBUG_PROTOCOL  
182.                     P13 = 0;  
183. #endif  
184.                 }  
185.                 else  
186.                 {  
187.                     g_swapBuffer = FALSE;  
188.                       
189. #ifdef DEBUG_PROTOCOL  
190.                     P13 = 1;  
191. #endif  
192.                 }  
193.   
194.                 counter = 0;  
195.             }  
196.             else   // 接受协议正文  
197.             {  
198.                 g_swapBuffer = FALSE;  
199.                 previousChar = currentChar;  
200.   
201.                 if (counter < ROBOT_JOINT_AMOUNT)  
202.                      g_angle[g_fillingBufferIndex][counter] = currentChar;  
203.   
204.                 ++counter;  
205.             }  
206.         }    // if (PROTOCOL_HEADER[0] == currentChar)  
207.   
208.         SBUF = currentChar;  
209.         while (!TI)  
210.             ;  
211.         TI = 0;  
212.     }    // (RI)  
213. }  
214.   
215. void FillDiffAngle()  
216. {  
217.     // 设置舵机要调整的角度  
218.     g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_VERTICAL] = g_angle[g_readyBufferIndex][ROBOT_LEFT_SHOULDER_VERTICAL];  
219.     g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_HORIZEN] = g_angle[g_readyBufferIndex][ROBOT_LEFT_SHOULDER_HORIZEN];  
220.     g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_ELBOW] = g_angle[g_readyBufferIndex][ROBOT_LEFT_ELBOW];  
221.   
222.     g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_VERTICAL] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_SHOULDER_VERTICAL];  
223.     g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_HORIZEN] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_SHOULDER_HORIZEN];  
224.     g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_ELBOW] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_ELBOW];  
225.   
226.     g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_HIP_VERTICAL] = g_angle[g_readyBufferIndex][ROBOT_LEFT_HIP_VERTICAL];  
227.     g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_HIP_VERTICAL] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_HIP_VERTICAL];  
228.       
229.     g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_HIP_HORIZEN] = g_angle[g_readyBufferIndex][ROBOT_LEFT_HIP_HORIZEN];  
230.     g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_KNEE] = g_angle[g_readyBufferIndex][ROBOT_LEFT_KNEE];  
231.     g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_ANKLE] = g_angle[g_readyBufferIndex][ROBOT_LEFT_ANKLE];  
232.     g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_FOOT] = g_angle[g_readyBufferIndex][ROBOT_LEFT_FOOT];  
233.   
234.     g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_HIP_HORIZEN] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_HIP_HORIZEN];  
235.     g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_KNEE] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_KNEE];  
236.     g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_ANKLE] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_ANKLE];  
237.     g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_FOOT] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_FOOT];  
238.   
239.     // 复位舵机管脚索引  
240.     g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_VERTICAL] = ROBOT_PIN_MASK_LEFT_SHOULDER_VERTICAL;  
241.     g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_HORIZEN] = ROBOT_PIN_MASK_LEFT_SHOULDER_HORIZEN;  
242.     g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_ELBOW] = ROBOT_PIN_MASK_LEFT_ELBOW;  
243.   
244.     g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_VERTICAL] = ROBOT_PIN_MASK_RIGHT_SHOULDER_VERTICAL;  
245.     g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_HORIZEN] = ROBOT_PIN_MASK_RIGHT_SHOULDER_HORIZEN;  
246.     g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_ELBOW] = ROBOT_PIN_MASK_RIGHT_ELBOW;  
247.   
248.     g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_HIP_VERTICAL] = ROBOT_PIN_MASK_LEFT_HIP_VERTICAL;  
249.     g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_HIP_VERTICAL] = ROBOT_PIN_MASK_RIGHT_HIP_VERTICAL;  
250.       
251.     g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_HIP_HORIZEN] = ROBOT_PIN_MASK_LEFT_HIP_HORIZEN;  
252.     g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_KNEE] = ROBOT_PIN_MASK_LEFT_KNEE;  
253.     g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_ANKLE] = ROBOT_PIN_MASK_LEFT_ANKLE;  
254.     g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_FOOT] = ROBOT_PIN_MASK_LEFT_FOOT;  
255.   
256.     g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_HIP_HORIZEN] = ROBOT_PIN_MASK_RIGHT_HIP_HORIZEN;  
257.     g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_KNEE] = ROBOT_PIN_MASK_RIGHT_KNEE;  
258.     g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_ANKLE] = ROBOT_PIN_MASK_RIGHT_ANKLE;  
259.     g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_FOOT] = ROBOT_PIN_MASK_RIGHT_FOOT;  
260. }  
261.   
262. #define PWM_STEERING_ENGINE(group)                                                          \  
263. {                                                                                           \  
264.     counter = STEERING_ENGINE_INIT_DELAY;                                                   \  
265.         for (i = 0; i < 8; ++i)                                                             \  
266.             counter += g_diffAngle[PIN_GROUP_##group][i];                                   \  
267.                                                                                             \  
268.         for (i = 0; i < 30; ++i)                                                            \  
269.         {                                                                                   \  
270.             GROUP_##group##_CONTROL_PIN = 0xFF;                                             \  
271.             Delay10us(STEERING_ENGINE_INIT_DELAY);                                          \  
272.                                                                                             \  
273.             for (j = 0; j < 8; ++j)                                                         \  
274.             {                                                                               \  
275.                 Delay10us(g_diffAngle[PIN_GROUP_##group][j]);                               \  
276.                 GROUP_##group##_CONTROL_PIN &= ~(g_diffAngleMask[PIN_GROUP_##group][j]);    \  
277.             }                                                                               \  
278.                                                                                             \  
279.             Delay10us(STEERING_ENGINE_CYCLE - counter);                                     \  
280.         }                                                                                   \  
281. }  
282.   
283. void main()  
284. {  
285.     UCHAR8 i;  
286.     UCHAR8 j;  
287.     UINT16 counter;  
288.   
289.     InitSerialPort();  
290.       
291.     P1 = 0xFF;  
292.   
293.     // 初始化舵机角度  
294.     for (i = 0; i < ROBOT_JOINT_AMOUNT; ++i)  
295.     {  
296.         g_angle[0][i] = 45;  
297.         g_angle[1][i] = 45;  
298.     }  
299.       
300.     for (i = 0; i < USED_PIN; ++i)  
301.         for (j = 0; j < 8; ++j)  
302.             g_diffAngle[i][j] = 0;  
303.               
304.     FillDiffAngle();  
305.     InitPwmPollint();  
306.   
307.     while (1)  
308.     {  
309. #ifdef DEBUG_PROTOCOL  
310.         if (g_fillingBufferIndex)  
311.         {  
312.             P11 = 0;  
313.             P10 = 1;  
314.         }  
315.         else  
316.         {  
317.             P11 = 1;  
318.             P10 = 0;  
319.         }  
320.   
321.         if (g_swapBuffer)  
322.             P12 = 0;  
323.         else  
324.             P12 = 1;  
325. #endif  
326.   
327.         if (g_swapBuffer)  
328.         {  
329.             FillDiffAngle();  
330.               
331.             g_swapBuffer = FALSE;  
332.               
333.             InitPwmPollint();  
334.         }  
335.                   
336.         PWM_STEERING_ENGINE(1)  
337.         PWM_STEERING_ENGINE(2)  
338.     }  
339.   
340. }