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

单片机 > 单片机程序设计 > 详情

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

发布时间：

// main.c

[cpp] view plaincopyprint?

#include
#include "ControlRobot.h"
#include
#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
for (delayVar2 = 0; delayVar2
_nop_();
}
void InitPwmPollint()
{
UCHAR8 i;
UCHAR8 j;
UCHAR8 k;
UINT16 temp;
for (i = 0; i
{
for (j = 0; j
{
for (k = j; 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
{
for (j = 0; j
{
if (INVALID_JOINT_VALUE == g_diffAngle[i][j])
{
g_diffAngle[i][j] = STEERING_ENGINE_FULL_CYCLE;
}
}
}
for (i = 0; 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
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
counter += g_diffAngle[PIN_GROUP_##group][i];
for (i = 0; i
{
GROUP_##group##_CONTROL_PIN = 0xFF;
Delay10us(STEERING_ENGINE_INIT_DELAY);
for (j = 0; 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
{
g_angle[0][i] = 45;
g_angle[1][i] = 45;
}
for (i = 0; i
for (j = 0; 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)
}
}