低端单片机(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个指令周期。我们看下面的程序：
#include <STC_NEW_8051.H>
void main()
{
volatile int cnt = 0;
volatile int pwm = 0;
volatile int pwmCycle = 10;
cnt = (cnt + 1) % 40;
if (++cnt < pwmCycle)
pwm = 1;
else
pwm = 0;
}
这段程序是模拟定时器中断中对一路舵机进行调速的情况，注意，这里一定要使用volatile关键字，否则程序会由于编译器的优化而错误。对于所有中断中用到的全局变量，我们都需要加上volatile关键字防止编译器优化。下面，我们对这段代码进行反汇编，并计算指令周期数。反汇编代码如下：
C:0x0097 F50C MOV 0x0C,A 3 C:0x0099 750D0A MOV 0x0D,#0x0A 3 C:0x009C E509 MOV A,0x09 2 C:0x009E 2401 ADD A,#0x01 2 C:0x00A0 FF MOV R7,A 2 C:0x00A1 E4 CLR A 1 C:0x00A2 3508 ADDC A,0x08 3 C:0x00A4 FE MOV R6,A 2 C:0x00A5 7C00 MOV R4,#0x00 2 C:0x00A7 7D28 MOV R5,#0x28 2 C:0x00A9 120003 LCALL C?SIDIV(C:0003) C:0x00AC 8C08 MOV 0x08,R4 3 C:0x00AE 8D09 MOV 0x09,R5 3 C:0x00B0 0509 INC 0x09 4 C:0x00B2 E509 MOV A,0x09 2 C:0x00B4 7002 JNZ C:00B8 3 C:0x00B6 0508 INC 0x08 4 C:0x00B8 AE08 MOV R6,0x08 4 C:0x00BA C3 CLR C 1 C:0x00BB 950D SUBB A,0x0D 3 C:0x00BD E50C MOV A,0x0C 2 C:0x00BF 6480 XRL A,#P0(0x80) 2 C:0x00C1 F8 MOV R0,A 2 C:0x00C2 EE MOV A,R6 1 C:0x00C3 6480 XRL A,#P0(0x80) 2 C:0x00C5 98 SUBB A,R0 2 C:0x00C6 5007 JNC C:00CF 3 C:0x00C8 750A00 MOV 0x0A,#0x00 3 C:0x00CB 750B01 MOV 0x0B,#0x01 3 //C:0x00CE 22 RET 多个if情况时这里应该是一个跳转 3 C:0x00CF E4 CLR A 1 C:0x00D0 F50A MOV 0x0A,A 3 C:0x00D2 F50B MOV 0x0B,A 3
由以上代码可知，一个舵机的调速周期为79+[LCALL C?SIDIV(C:0003) ]条指令周期，所以，对于10us的定时器中断，最多可以调速2路舵机。
接下来我们来分析软件延时的方法进行调速，首先我们需要一个延时函数，那么我们定义一个延时10us的函数如下所示：
void Delay10us(UINT16 ntimes)
{
for (delayVar1 = 0; delayVar1 < ntimes; ++delayVar1)
for (delayVar2 = 0; delayVar2 < 21; ++delayVar2)
_nop_();
}
如果对每个舵机分别进行调速，那么我们的时间开销为0.6 * 16 = 9.6s，这个时间是不可接受的。由于8位单片机有P0-P3的管脚阵列，那么我们可以利用其中的某一列，同时调速8路舵机，那么我们的时间开销为0.6 * 2 = 1.2s，这个对于我们的机器人来说是完全可以接受的。
同时对8路舵机进行调速的话，我们首先需要对舵机的调速角度进行排序（升序），然后计算出两两舵机舵机的差值，这样就可以同时对8路舵机进行调速了。对于出现的非法角度，我们给这路舵机送全为高电平的PWM信号，使此路舵机保持先前状态。代码如下所示：
void InitPwmPollint()
{
UCHAR8 i;
UCHAR8 j;
UCHAR8 k;
UINT16 temp;
for (i = 0; i < USED_PIN; ++i)
{
for (j = 0; j < 7; ++j)
{
for (k = j; k < 8; ++k)
{
if (g_diffAngle[i][j] > g_diffAngle[i][k])
{
temp = g_diffAngle[i][j];
g_diffAngle[i][j] = g_diffAngle[i][k];
g_diffAngle[i][k] = temp;
temp = g_diffAngleMask[i][j];
g_diffAngleMask[i][j] = g_diffAngleMask[i][k];
g_diffAngleMask[i][k] = temp;
}
}
}
}
for (i = 0; i < USED_PIN; ++i)
{
for (j = 0; j < 8; ++j)
{
if (INVALID_JOINT_VALUE == g_diffAngle[i][j])
{
g_diffAngle[i][j] = STEERING_ENGINE_FULL_CYCLE;
}
}
}
for (i = 0; i < USED_PIN; ++i)
{
for (j = 7; j >= 1; --j)
{
g_diffAngle[i][j] = g_diffAngle[i][j] - g_diffAngle[i][j - 1];
}
}
}
（注：对于上面用到的一些常量，请大家参考本文最后给出的完整程序。
下面我们举例说明上述代码，假设 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路舵机同时调速。调速部分代码如下：
#define PWM_STEERING_ENGINE(group) \
{ \
counter = STEERING_ENGINE_INIT_DELAY; \
for (i = 0; i < 8; ++i) \
counter += g_diffAngle[PIN_GROUP_##group][i]; \
\
for (i = 0; i < 30; ++i) \
{ \
GROUP_##group##_CONTROL_PIN = 0xFF; \
Delay10us(STEERING_ENGINE_INIT_DELAY); \
\
for (j = 0; j < 8; ++j) \
{ \
Delay10us(g_diffAngle[PIN_GROUP_##group][j]); \
GROUP_##group##_CONTROL_PIN &= ~(g_diffAngleMask[PIN_GROUP_##group][j]); \
} \
\
Delay10us(STEERING_ENGINE_CYCLE - counter); \
} \
}
while (1)
{
PWM_STEERING_ENGINE(1);
}
通过上面的操作，我们便完成了同时对8路舵机的调速操作。
总结：
对于只有1、2路舵机的情况，我们可以使用定时器中断进行调速，其具有实现简单，通俗易懂的有点。而对于多路舵机的情形，就需要使用软件延时进行操作，其优点是可以同时对8路舵机进行调速，效率高，但是其代码不容易实现与调试。
附完整程序：
（注：由于需求变动很大，包括功能和管脚分配等，所以本程序大量使用宏及enum）
/**
* @file ControlRobot.h
* @author 马冬亮
* @brief 用于对机器人进行控制.
*/
#ifndef CONTROLROBOT_H
#define CONTROLROBOT_H
/**
* @brief 关节角度非法值.
* @ingroup ControlRobot
*
* @details 当关节角度无法确定或捕获关节失败时, 设置为此数值, 机器人接收到此数值则不转动舵机.
*/
#define INVALID_JOINT_VALUE 200
/**
* @brief 定义关节常量用于存取关节角度.
* @ingroup ControlRobot
*/
typedef enum RobotJoint
{
ROBOT_LEFT_SHOULDER_VERTICAL = 0, ///< 左肩膀, 前后转动的舵机
ROBOT_LEFT_SHOULDER_HORIZEN = 1, ///< 左肩膀, 上下转动的舵机
ROBOT_LEFT_ELBOW = 2, ///< 左肘, 左臂肘关节的舵机
ROBOT_RIGHT_SHOULDER_VERTICAL = 3, ///< 右肩膀, 前后转动的舵机
ROBOT_RIGHT_SHOULDER_HORIZEN = 4, ///< 右肩膀, 上下转动的舵机
ROBOT_RIGHT_ELBOW = 5, ///< 右肘, 右臂肘关节的舵机
ROBOT_LEFT_HIP_VERTICAL = 6, ///< 左髋, 左右转动的舵机
ROBOT_LEFT_HIP_HORIZEN = 7, ///< 左髋, 前后转动的舵机
ROBOT_LEFT_KNEE = 8, ///< 左膝盖, 左膝关节的舵机
ROBOT_LEFT_ANKLE = 9, ///< 左踝, 这个舵机不使用
ROBOT_RIGHT_HIP_VERTICAL = 10, ///< 右髋, 左右转动的舵机
ROBOT_RIGHT_HIP_HORIZEN = 11, ///< 右髋, 前后转动的舵机
ROBOT_RIGHT_KNEE = 12, ///< 右膝盖, 右膝关节的舵机
ROBOT_RIGHT_ANKLE = 13, ///< 右踝, 这个舵机不使用
ROBOT_LEFT_FOOT = 14, ///< 左脚, 这个舵机不使用
ROBOT_RIGHT_FOOT = 15, ///< 右脚, 这个舵机不使用
ROBOT_HEAD = 16, ///< 头, 这个舵机不使用
ROBOT_JOINT_AMOUNT = ROBOT_HEAD + 1
} RobotJoint;
typedef enum RobotSteeringEnginePinIndex
{
ROBOT_PIN_INDEX_LEFT_SHOULDER_VERTICAL = 0,
ROBOT_PIN_INDEX_LEFT_SHOULDER_HORIZEN = 1,
ROBOT_PIN_INDEX_LEFT_ELBOW = 2,
ROBOT_PIN_INDEX_RIGHT_SHOULDER_VERTICAL = 3,
ROBOT_PIN_INDEX_RIGHT_SHOULDER_HORIZEN = 4,
ROBOT_PIN_INDEX_RIGHT_ELBOW = 5,
ROBOT_PIN_INDEX_LEFT_HIP_VERTICAL = 6,
ROBOT_PIN_INDEX_RIGHT_HIP_VERTICAL = 7,
ROBOT_PIN_INDEX_LEFT_HIP_HORIZEN = 0,
ROBOT_PIN_INDEX_LEFT_KNEE = 1,
ROBOT_PIN_INDEX_LEFT_ANKLE = 2,
ROBOT_PIN_INDEX_LEFT_FOOT = 3,
ROBOT_PIN_INDEX_RIGHT_HIP_HORIZEN = 4,
ROBOT_PIN_INDEX_RIGHT_KNEE = 5,
ROBOT_PIN_INDEX_RIGHT_ANKLE = 6,
ROBOT_PIN_INDEX_RIGHT_FOOT = 7
} RobotSteeringEnginePinIndex;
typedef enum RobotSteeringEnginePinMask
{
ROBOT_PIN_MASK_LEFT_SHOULDER_VERTICAL = 0x01,
ROBOT_PIN_MASK_LEFT_SHOULDER_HORIZEN = 0x02,
ROBOT_PIN_MASK_LEFT_ELBOW = 0x04,
ROBOT_PIN_MASK_RIGHT_SHOULDER_VERTICAL = 0x08,
ROBOT_PIN_MASK_RIGHT_SHOULDER_HORIZEN = 0x10,
ROBOT_PIN_MASK_RIGHT_ELBOW = 0x20,
ROBOT_PIN_MASK_LEFT_HIP_VERTICAL = 0x40,
ROBOT_PIN_MASK_RIGHT_HIP_VERTICAL = 0x80,
ROBOT_PIN_MASK_LEFT_HIP_HORIZEN = 0x01,
ROBOT_PIN_MASK_LEFT_KNEE = 0x02,
ROBOT_PIN_MASK_LEFT_ANKLE = 0x04,
ROBOT_PIN_MASK_LEFT_FOOT = 0x08,
ROBOT_PIN_MASK_RIGHT_HIP_HORIZEN = 0x10,
ROBOT_PIN_MASK_RIGHT_KNEE = 0x20,
ROBOT_PIN_MASK_RIGHT_ANKLE = 0x40,
ROBOT_PIN_MASK_RIGHT_FOOT = 0x80
} RobotSteeringEnginePinMask;
#define PROTOCOL_HEADER "\xC9\xCA"
#define PROTOCOL_END "\xC9\xCB"
#define PROTOCOL_END_LENGTH 2
#define PROTOCOL_HEADER_LENGTH 2
#define COMMAND_STR_LENGTH (PROTOCOL_HEADER_LENGTH + ROBOT_JOINT_AMOUNT + PROTOCOL_END_LENGTH)
#define COMMAND_STR_BUFFER_SIZE ((COMMAND_STR_LENGTH) + 2)
#endif /* CONTROLROBOT_H */
// main.c
#include <STC_NEW_8051.H>
#include "ControlRobot.h"
#include<intrins.h>
#define DEBUG_PROTOCOL
typedef unsigned char UCHAR8;
typedef unsigned int UINT16;
#undef TRUE
#undef FALSE
#define TRUE 1
#define FALSE 0
#define MEMORY_MODEL
UINT16 MEMORY_MODEL delayVar1;
UCHAR8 MEMORY_MODEL delayVar2;
#define BAUD_RATE 0xF3
#define USED_PIN 2
#define PIN_GROUP_1 0
#define PIN_GROUP_2 1
#define GROUP_1_CONTROL_PIN P0
#define GROUP_2_CONTROL_PIN P2
#define STEERING_ENGINE_CYCLE 2000
#define STEERING_ENGINE_INIT_DELAY 50
#define STEERING_ENGINE_FULL_CYCLE ((STEERING_ENGINE_CYCLE) - (STEERING_ENGINE_INIT_DELAY))
volatile UCHAR8 MEMORY_MODEL g_angle[2][ROBOT_JOINT_AMOUNT];
volatile bit MEMORY_MODEL g_fillingBufferIndex = 0;
volatile bit MEMORY_MODEL g_readyBufferIndex = 1;
volatile bit MEMORY_MODEL g_swapBuffer = FALSE;
volatile UINT16 MEMORY_MODEL g_diffAngle[USED_PIN][8];
volatile UCHAR8 MEMORY_MODEL g_diffAngleMask[USED_PIN][8] =
{
{
ROBOT_PIN_MASK_LEFT_SHOULDER_VERTICAL,
ROBOT_PIN_MASK_LEFT_SHOULDER_HORIZEN,
ROBOT_PIN_MASK_LEFT_ELBOW,
ROBOT_PIN_MASK_RIGHT_SHOULDER_VERTICAL,
ROBOT_PIN_MASK_RIGHT_SHOULDER_HORIZEN,
ROBOT_PIN_MASK_RIGHT_ELBOW,
ROBOT_PIN_MASK_LEFT_HIP_VERTICAL,
ROBOT_PIN_MASK_RIGHT_HIP_VERTICAL
},
{
ROBOT_PIN_MASK_LEFT_HIP_HORIZEN,
ROBOT_PIN_MASK_LEFT_KNEE,
ROBOT_PIN_MASK_LEFT_ANKLE,
ROBOT_PIN_MASK_LEFT_FOOT,
ROBOT_PIN_MASK_RIGHT_HIP_HORIZEN,
ROBOT_PIN_MASK_RIGHT_KNEE,
ROBOT_PIN_MASK_RIGHT_ANKLE,
ROBOT_PIN_MASK_RIGHT_FOOT
}
};
#ifdef DEBUG_PROTOCOL
sbit P10 = P1 ^ 0; // 正在填充交换区1
sbit P11 = P1 ^ 1; // 正在填充交换区2
sbit P12 = P1 ^ 2; // 交换区变换
sbit P13 = P1 ^ 3; // 协议是否正确
#endif
void Delay10us(UINT16 ntimes)
{
for (delayVar1 = 0; delayVar1 < ntimes; ++delayVar1)
for (delayVar2 = 0; delayVar2 < 21; ++delayVar2)
_nop_();
}
void InitPwmPollint()
{
UCHAR8 i;
UCHAR8 j;
UCHAR8 k;
UINT16 temp;
for (i = 0; i < USED_PIN; ++i)
{
for (j = 0; j < 7; ++j)
{
for (k = j; k < 8; ++k)
{
if (g_diffAngle[i][j] > g_diffAngle[i][k])
{
temp = g_diffAngle[i][j];
g_diffAngle[i][j] = g_diffAngle[i][k];
g_diffAngle[i][k] = temp;
temp = g_diffAngleMask[i][j];
g_diffAngleMask[i][j] = g_diffAngleMask[i][k];
g_diffAngleMask[i][k] = temp;
}
}
}
}
for (i = 0; i < USED_PIN; ++i)
{
for (j = 0; j < 8; ++j)
{
if (INVALID_JOINT_VALUE == g_diffAngle[i][j])
{
g_diffAngle[i][j] = STEERING_ENGINE_FULL_CYCLE;
}
}
}
for (i = 0; i < USED_PIN; ++i)
{
for (j = 7; j >= 1; --j)
{
g_diffAngle[i][j] = g_diffAngle[i][j] - g_diffAngle[i][j - 1];
}
}
}
void InitSerialPort()
{
AUXR = 0x00;
ES = 0;
TMOD = 0x20;
SCON = 0x50;
TH1 = BAUD_RATE;
TL1 = TH1;
PCON = 0x80;
EA = 1;
ES = 1;
TR1 = 1;
}
void OnSerialPort() interrupt 4
{
static UCHAR8 previousChar = 0;
static UCHAR8 currentChar = 0;
static UCHAR8 counter = 0;
if (RI)
{
RI = 0;
currentChar = SBUF;
if (PROTOCOL_HEADER[0] == currentChar) // 协议标志
{
previousChar = currentChar;
}
else
{
if (PROTOCOL_HEADER[0] == previousChar && PROTOCOL_HEADER[1] == currentChar) // 协议开始
{
counter = 0;
previousChar = currentChar;
g_swapBuffer = FALSE;
}
else if (PROTOCOL_END[0] == previousChar && PROTOCOL_END[1] == currentChar) // 协议结束
{
previousChar = currentChar;
if (ROBOT_JOINT_AMOUNT == counter) // 协议接受正确
{
if (0 == g_fillingBufferIndex)
{
g_readyBufferIndex = 0;
g_fillingBufferIndex = 1;
}
else
{
g_readyBufferIndex = 1;
g_fillingBufferIndex = 0;
}
g_swapBuffer = TRUE;
#ifdef DEBUG_PROTOCOL
P13 = 0;
#endif
}
else
{
g_swapBuffer = FALSE;
#ifdef DEBUG_PROTOCOL
P13 = 1;
#endif
}
counter = 0;
}
else // 接受协议正文
{
g_swapBuffer = FALSE;
previousChar = currentChar;
if (counter < ROBOT_JOINT_AMOUNT)
g_angle[g_fillingBufferIndex][counter] = currentChar;
++counter;
}
} // if (PROTOCOL_HEADER[0] == currentChar)
SBUF = currentChar;
while (!TI)
;
TI = 0;
} // (RI)
}
void FillDiffAngle()
{
// 设置舵机要调整的角度
g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_VERTICAL] = g_angle[g_readyBufferIndex][ROBOT_LEFT_SHOULDER_VERTICAL];
g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_HORIZEN] = g_angle[g_readyBufferIndex][ROBOT_LEFT_SHOULDER_HORIZEN];
g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_ELBOW] = g_angle[g_readyBufferIndex][ROBOT_LEFT_ELBOW];
g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_VERTICAL] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_SHOULDER_VERTICAL];
g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_HORIZEN] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_SHOULDER_HORIZEN];
g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_ELBOW] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_ELBOW];
g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_HIP_VERTICAL] = g_angle[g_readyBufferIndex][ROBOT_LEFT_HIP_VERTICAL];
g_diffAngle[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_HIP_VERTICAL] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_HIP_VERTICAL];
g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_HIP_HORIZEN] = g_angle[g_readyBufferIndex][ROBOT_LEFT_HIP_HORIZEN];
g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_KNEE] = g_angle[g_readyBufferIndex][ROBOT_LEFT_KNEE];
g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_ANKLE] = g_angle[g_readyBufferIndex][ROBOT_LEFT_ANKLE];
g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_FOOT] = g_angle[g_readyBufferIndex][ROBOT_LEFT_FOOT];
g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_HIP_HORIZEN] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_HIP_HORIZEN];
g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_KNEE] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_KNEE];
g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_ANKLE] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_ANKLE];
g_diffAngle[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_FOOT] = g_angle[g_readyBufferIndex][ROBOT_RIGHT_FOOT];
// 复位舵机管脚索引
g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_VERTICAL] = ROBOT_PIN_MASK_LEFT_SHOULDER_VERTICAL;
g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_SHOULDER_HORIZEN] = ROBOT_PIN_MASK_LEFT_SHOULDER_HORIZEN;
g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_ELBOW] = ROBOT_PIN_MASK_LEFT_ELBOW;
g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_VERTICAL] = ROBOT_PIN_MASK_RIGHT_SHOULDER_VERTICAL;
g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_SHOULDER_HORIZEN] = ROBOT_PIN_MASK_RIGHT_SHOULDER_HORIZEN;
g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_ELBOW] = ROBOT_PIN_MASK_RIGHT_ELBOW;
g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_LEFT_HIP_VERTICAL] = ROBOT_PIN_MASK_LEFT_HIP_VERTICAL;
g_diffAngleMask[PIN_GROUP_1][ROBOT_PIN_INDEX_RIGHT_HIP_VERTICAL] = ROBOT_PIN_MASK_RIGHT_HIP_VERTICAL;
g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_HIP_HORIZEN] = ROBOT_PIN_MASK_LEFT_HIP_HORIZEN;
g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_KNEE] = ROBOT_PIN_MASK_LEFT_KNEE;
g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_ANKLE] = ROBOT_PIN_MASK_LEFT_ANKLE;
g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_LEFT_FOOT] = ROBOT_PIN_MASK_LEFT_FOOT;
g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_HIP_HORIZEN] = ROBOT_PIN_MASK_RIGHT_HIP_HORIZEN;
g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_KNEE] = ROBOT_PIN_MASK_RIGHT_KNEE;
g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_ANKLE] = ROBOT_PIN_MASK_RIGHT_ANKLE;
g_diffAngleMask[PIN_GROUP_2][ROBOT_PIN_INDEX_RIGHT_FOOT] = ROBOT_PIN_MASK_RIGHT_FOOT;
}
#define PWM_STEERING_ENGINE(group) \
{ \
counter = STEERING_ENGINE_INIT_DELAY; \
for (i = 0; i < 8; ++i) \
counter += g_diffAngle[PIN_GROUP_##group][i]; \
\
for (i = 0; i < 30; ++i) \
{ \
GROUP_##group##_CONTROL_PIN = 0xFF; \
Delay10us(STEERING_ENGINE_INIT_DELAY); \
\
for (j = 0; j < 8; ++j) \
{ \
Delay10us(g_diffAngle[PIN_GROUP_##group][j]); \
GROUP_##group##_CONTROL_PIN &= ~(g_diffAngleMask[PIN_GROUP_##group][j]); \
} \
\
Delay10us(STEERING_ENGINE_CYCLE - counter); \
} \
}
void main()
{
UCHAR8 i;
UCHAR8 j;
UINT16 counter;
InitSerialPort();
P1 = 0xFF;
// 初始化舵机角度
for (i = 0; i < ROBOT_JOINT_AMOUNT; ++i)
{
g_angle[0][i] = 45;
g_angle[1][i] = 45;
}
for (i = 0; i < USED_PIN; ++i)
for (j = 0; j < 8; ++j)
g_diffAngle[i][j] = 0;
FillDiffAngle();
InitPwmPollint();
while (1)
{
#ifdef DEBUG_PROTOCOL
if (g_fillingBufferIndex)
{
P11 = 0;
P10 = 1;
}
else
{
P11 = 1;
P10 = 0;
}
if (g_swapBuffer)
P12 = 0;
else
P12 = 1;
#endif
if (g_swapBuffer)
{
FillDiffAngle();
g_swapBuffer = FALSE;
InitPwmPollint();
}
PWM_STEERING_ENGINE(1)
PWM_STEERING_ENGINE(2)
}
}