低端微控制器(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個指令週期。我們看下面的程式：
#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;
}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接下來我們來分析軟體延時的方法進行調速，首先我們需要一個延時函式，那麼我們定義一個延時10us的函式如下所示：
void Delay10us(UINT16 ntimes)
{
for (delayVar1 = 0; delayVar1 < ntimes; ++delayVar1)
for (delayVar2 = 0; delayVar2 < 21; ++delayVar2)
_nop_();
}同時對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);
}總結：
對於只有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 */
#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)
}
}相關文章
- 【.NET 與樹莓派】控制舵機樹莓派
- 舵機的基本原理的控制方法
- java 實現微控制器與PC串列埠通訊Java串列埠
- 富集分析的原理與實現
- 計算機網路實驗4：鏈路層分析與組網計算機網路
- 低端手機也能玩轉VR?VR
- win10怎麼調機箱風扇轉速_win10如何調機箱風扇轉速Win10
- Android 原始碼分析 --Handler 機制的實現與工作原理Android原始碼
- Matlab實現模擬調製與解調Matlab
- 基於8266WIFI模組實現智慧手機與51微控制器的通訊入門WiFi
- Mininet主機與真實網路互通方案實現
- 三節課：2016年度大學生網際網路就業現狀與態度調查分析報告就業
- 農行網路流量回溯與分析實現新突破
- Redux流程分析與實現Redux
- 軟體效能測試分析與調優實踐之路-Java應用程式的效能分析與調優-手稿節選Java
- 小米手機太低端？雷軍：有技術含量的
- 瞭解舵機以及MG996R的控制方法996
- redux簡單實現與分析Redux
- 實現遊戲飛速發展：全球手遊玩家分析報告遊戲
- Binder機制分析（3）—— 實現自己的Service
- Android 的 Handler 機制實現原理分析Android
- 基於51微控制器的小車避障電路實現-PCB下載站
- Tarjan中棧的分析與SLT棧的實現
- 速推微控制器原理和應用：中斷
- 基於arduino+as608+360舵機的宿舍指紋鎖UI
- [深入理解Java虛擬機器]第五章 調優案例分析與實戰Java虛擬機
- octavia的實現與分析（二）·原理，架構與基本流程架構
- HashMap 實現原理與原始碼分析HashMap原始碼
- JAVA的反射機制==>用反射分析類的實現Java反射
- GPT-4要革 程式設計師的命？智慧開發的理想與現實 | 愛分析調研GPT程式設計師
- Virtualbox 虛擬機器實現與本地、網際網路互通虛擬機
- 一款工具的逆向分析與再實現
- Python 中 import 的機制與實現PythonImport
- Redis 設計與實現 （五）--多機資料庫的實現Redis資料庫
- 洗碗機廠家如何實現遠端監控與資料分析
- SOA網路調研分析報告
- AdaBoost演算法分析與實現演算法
- 離心泵的調節方式與能耗分析