低端单片机(8bit)的16路舵机调速分析与实现
来源：互联网 发布：淘宝为什么会被冻结 编辑：程序博客网 时间：2024/05/22 14:10
低端单片机(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 3C:0x0099 750D0A MOV 0x0D,#0x0A 3C:0x009C E509 MOV A,0x09 2C:0x009E 2401 ADD A,#0x01 2C:0x00A0 FF MOV R7,A 2C:0x00A1 E4 CLR A 1C:0x00A2 3508 ADDC A,0x08 3C:0x00A4 FE MOV R6,A 2C:0x00A5 7C00 MOV R4,#0x00 2C:0x00A7 7D28 MOV R5,#0x28 2C:0x00A9 120003 LCALL C?SIDIV(C:0003) C:0x00AC 8C08 MOV 0x08,R4 3C:0x00AE 8D09 MOV 0x09,R5 3C:0x00B0 0509 INC 0x09 4C:0x00B2 E509 MOV A,0x09 2C:0x00B4 7002 JNZ C:00B8 3C:0x00B6 0508 INC 0x08 4C:0x00B8 AE08 MOV R6,0x08 4C:0x00BA C3 CLR C 1C:0x00BB 950D SUBB A,0x0D 3C:0x00BD E50C MOV A,0x0C 2C:0x00BF 6480 XRL A,#P0(0x80) 2C:0x00C1 F8 MOV R0,A 2C:0x00C2 EE MOV A,R6 1C:0x00C3 6480 XRL A,#P0(0x80) 2C:0x00C5 98 SUBB A,R0 2C:0x00C6 5007 JNC C:00CF 3C:0x00C8 750A00 MOV 0x0A,#0x00 3C:0x00CB 750B01 MOV 0x0B,#0x01 3//C:0x00CE 22 RET 多个if情况时这里应该是一个跳转 3C:0x00CF E4 CLR A 1C:0x00D0 F50A MOV 0x0A,A 3C: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#defineCONTROLROBOT_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_PROTOCOLtypedef 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_PROTOCOLsbit P10 = P1 ^ 0; // 正在填充交换区1sbit P11 = P1 ^ 1; // 正在填充交换区2sbit 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) }}
- 低端单片机(8bit)的16路舵机调速分析与实现
- 低端单片机(8bit)的16路舵机调速分析与实现
- 16位单片机C语言实现PID调速心得
- 基于AVR单片机的电机调速
- ATmega128单片机与舵机控制板通信
- 8、16、32-BIT系列单片机区别与特点
- 关于低端单片机的轮询思想
- 关于二维数组的翻转与旋转 低端实现
- STC15F104E (8脚单片机）实现蓝牙串口控制舵机转向
- 直流电动机的调速 用单片机产生PWM控制
- 【转】如何实现多路舵机的精确控制
- 51单片机超高精度6路舵机控制程序
- 51单片机超高精度6路舵机控制程序
- arduino中舵机打直角的实现
- 舵机的脉冲与转动角度问题
- 舵机的相关原理与控制原理
- 单片机PWM舵机控制原理
- 主汽门与调速汽门的作用、区别
- 大数据量的处理 一亿个数排序
- 《C程序设计语言》——运算符
- lanmp一键安装版
- 【推荐】成功人生十大要点,认清自己，把握自己的命运，实现自己的人生价值。
- 从个人网站到淘宝网 仰观Java时代淘宝的技术发展(1)
- 低端单片机(8bit)的16路舵机调速分析与实现
- 解决方案：用户 'sa' 登录失败。原因: 未与信任 SQL Server 连接相关联。
- 做一个伟大的程序员，工程师。
- linux查看程序用到的文件描述符
- 【推荐】怎样提高学习效率？学习中的"事半功倍"--清华北大学生上千字经验分享！
- 给vs2010的编辑界面换肤
- 【推荐】科学记忆，有利于提高学习效率，有利于加速知识积累：实用记忆法则
- ---Struts2bean标签---
- How to make DirectFB screenshots
