六个舵机组成的双足机器人程序（51单片机）

一、项目概述

使用STC89C52单片机控制6个舵机，实现双足机器人的基本行走功能。机器人采用6自由度设计（每条腿3个舵机：髋关节、膝关节、踝关节），通过协调各关节运动实现前进、后退、转弯等动作。

二、系统架构

PWM
机械传动
控制
显示
输入
姿态反馈
STC89C52
舵机1-6
双足机器人
电源管理
LCD1602
按键

三、硬件设计

1. 元件清单

元件名称	型号/规格	数量	功能说明
主控芯片	STC89C52RC	1	系统控制核心
舵机	SG90/MG996R	6	关节驱动（5V/6V）
舵机驱动板	PCA9685	1	16路PWM控制（I²C接口）
LCD显示屏	LCD1602	1	状态显示
按键	轻触开关	4	控制按钮
电源	7.4V锂电池	1	系统供电
降压模块	LM2596	1	7.4V→5V降压
舵机电池	18650×2	1	7.4V/2000mAh
杜邦线	公对母/母对母	若干	连接线
结构件	3D打印/亚克力	1套	机器人骨架

2. 舵机分配

舵机编号	位置	功能	初始角度
S1	左腿髋关节	控制腿部前后摆动	90°
S2	左腿膝关节	控制小腿弯曲	90°
S3	左腿踝关节	控制脚部角度	90°
S4	右腿髋关节	控制腿部前后摆动	90°
S5	右腿膝关节	控制小腿弯曲	90°
S6	右腿踝关节	控制脚部角度	90°

3. 电路连接

模块	引脚	单片机引脚	说明
PCA9685	SDA	P2.0	I²C数据线
	SCL	P2.1	I²C时钟线
	VCC	5V	电源正极
	GND	GND	电源负极
LCD1602	RS	P1.0	寄存器选择
	RW	P1.1	读写选择
	E	P1.2	使能信号
	D4-D7	P0.4-P0.7	数据线(4位模式)
按键	K1(前进)	P3.0	前进控制
	K2(后退)	P3.1	后退控制
	K3(左转)	P3.2	左转控制
	K4(右转)	P3.3	右转控制

四、软件设计

1. 主程序流程图

是
否
系统初始化
舵机归零
读取按键状态
有按键按下?
执行对应动作
更新舵机角度
显示状态

2. 核心代码实现

头文件与宏定义

#include <reg52.h>
#include <intrins.h>
#include <string.h>

// PCA9685 I²C地址
#define PCA_ADDR 0x80

// 舵机初始角度
#define INIT_ANGLE 90

// 舵机范围限制
#define MIN_ANGLE 30
#define MAX_ANGLE 150

// 按键定义
sbit KEY_FORWARD = P3^0;
sbit KEY_BACKWARD = P3^1;
sbit KEY_LEFT = P3^2;
sbit KEY_RIGHT = P3^3;

// LCD引脚
sbit LCD_RS = P1^0;
sbit LCD_RW = P1^1;
sbit LCD_E = P1^2;

// 全局变量
unsigned char servo_angle[6] = {INIT_ANGLE, INIT_ANGLE, INIT_ANGLE, 
                               INIT_ANGLE, INIT_ANGLE, INIT_ANGLE};
unsigned char robot_state = 0; // 0:停止 1:前进 2:后退 3:左转 4:右转

I²C通信协议

// I²C起始信号
void I2C_Start() {
    SDA = 1;
    SCL = 1;
    _nop_(); _nop_();
    SDA = 0;
    _nop_(); _nop_();
    SCL = 0;
}

// I²C停止信号
void I2C_Stop() {
    SDA = 0;
    SCL = 1;
    _nop_(); _nop_();
    SDA = 1;
    _nop_(); _nop_();
}

// I²C发送字节
void I2C_SendByte(unsigned char dat) {
    unsigned char i;
    for(i=0; i<8; i++) {
        SDA = (dat & 0x80) ? 1 : 0;
        SCL = 1;
        _nop_(); _nop_();
        SCL = 0;
        dat <<= 1;
    }
    // 等待ACK
    SDA = 1;
    SCL = 1;
    _nop_(); _nop_();
    SCL = 0;
}

// PCA9685写寄存器
void PCA_WriteReg(unsigned char reg, unsigned char dat) {
    I2C_Start();
    I2C_SendByte(PCA_ADDR);
    I2C_SendByte(reg);
    I2C_SendByte(dat);
    I2C_Stop();
}

// 设置舵机角度
void Set_ServoAngle(unsigned char servo, unsigned char angle) {
    // 角度限制在30-150度
    if(angle < MIN_ANGLE) angle = MIN_ANGLE;
    if(angle > MAX_ANGLE) angle = MAX_ANGLE;
    
    // 计算PWM脉冲宽度 (0.5ms-2.5ms对应0-180度)
    unsigned int pulse = 205 + (angle * 1024) / 180;
    
    // 设置PCA9685寄存器
    unsigned char reg = 0x06 + (servo * 4);
    PCA_WriteReg(reg, 0x00);       // ON_L
    PCA_WriteReg(reg+1, 0x00);     // ON_H
    PCA_WriteReg(reg+2, pulse);    // OFF_L
    PCA_WriteReg(reg+3, pulse>>8);// OFF_H
    
    // 更新全局变量
    servo_angle[servo] = angle;
}

LCD1602驱动

// LCD忙等待
void LCD_Busy() {
    unsigned char busy;
    P0 = 0xFF;
    LCD_RS = 0;
    LCD_RW = 1;
    do {
        LCD_E = 1;
        busy = P0;
        LCD_E = 0;
    } while(busy & 0x80);
}

// LCD写命令
void LCD_WriteCmd(unsigned char cmd) {
    LCD_Busy();
    LCD_RS = 0;
    LCD_RW = 0;
    P0 = cmd;
    LCD_E = 1;
    _nop_();
    LCD_E = 0;
}

// LCD写数据
void LCD_WriteData(unsigned char dat) {
    LCD_Busy();
    LCD_RS = 1;
    LCD_RW = 0;
    P0 = dat;
    LCD_E = 1;
    _nop_();
    LCD_E = 0;
}

// LCD初始化
void LCD_Init() {
    LCD_WriteCmd(0x38);  // 8位数据，2行显示
    LCD_WriteCmd(0x0C);  // 开显示，关光标
    LCD_WriteCmd(0x06);  // 地址自动加1
    LCD_WriteCmd(0x01);  // 清屏
}

// LCD显示字符串
void LCD_ShowString(unsigned char x, unsigned char y, char *str) {
    if(y == 0) LCD_WriteCmd(0x80 + x);
    else LCD_WriteCmd(0xC0 + x);
    
    while(*str) {
        LCD_WriteData(*str++);
    }
}

// LCD显示角度
void LCD_ShowAngles() {
    char buf[16];
    sprintf(buf, "S1:%3d S2:%3d", servo_angle[0], servo_angle[1]);
    LCD_ShowString(0, 0, buf);
    sprintf(buf, "S3:%3d S4:%3d", servo_angle[2], servo_angle[3]);
    LCD_ShowString(0, 1, buf);
}

机器人动作函数

// 舵机归零
void Servo_Reset() {
    Set_ServoAngle(0, INIT_ANGLE); // S1
    Set_ServoAngle(1, INIT_ANGLE); // S2
    Set_ServoAngle(2, INIT_ANGLE); // S3
    Set_ServoAngle(3, INIT_ANGLE); // S4
    Set_ServoAngle(4, INIT_ANGLE); // S5
    Set_ServoAngle(5, INIT_ANGLE); // S6
    LCD_ShowString(0, 0, "Robot Ready");
    LCD_ShowString(0, 1, "Press Key...");
}

// 前进动作
void Robot_Forward() {
    // 左腿动作
    Set_ServoAngle(0, INIT_ANGLE + 20); // 髋关节前摆
    Set_ServoAngle(1, INIT_ANGLE - 15); // 膝关节弯曲
    Set_ServoAngle(2, INIT_ANGLE + 10); // 踝关节调整
    
    // 右腿动作
    Set_ServoAngle(3, INIT_ANGLE - 20); // 髋关节后摆
    Set_ServoAngle(4, INIT_ANGLE + 15); // 膝关节伸直
    Set_ServoAngle(5, INIT_ANGLE - 10); // 踝关节调整
    
    LCD_ShowString(0, 0, "Moving Forward");
}

// 后退动作
void Robot_Backward() {
    // 左腿动作
    Set_ServoAngle(0, INIT_ANGLE - 20); // 髋关节后摆
    Set_ServoAngle(1, INIT_ANGLE + 15); // 膝关节伸直
    Set_ServoAngle(2, INIT_ANGLE - 10); // 踝关节调整
    
    // 右腿动作
    Set_ServoAngle(3, INIT_ANGLE + 20); // 髋关节前摆
    Set_ServoAngle(4, INIT_ANGLE - 15); // 膝关节弯曲
    Set_ServoAngle(5, INIT_ANGLE + 10); // 踝关节调整
    
    LCD_ShowString(0, 0, "Moving Backward");
}

// 左转动作
void Robot_TurnLeft() {
    // 左腿不动
    Set_ServoAngle(0, INIT_ANGLE);
    Set_ServoAngle(1, INIT_ANGLE);
    Set_ServoAngle(2, INIT_ANGLE);
    
    // 右腿后摆
    Set_ServoAngle(3, INIT_ANGLE - 30); // 髋关节后摆
    Set_ServoAngle(4, INIT_ANGLE + 20); // 膝关节伸直
    Set_ServoAngle(5, INIT_ANGLE - 15); // 踝关节调整
    
    LCD_ShowString(0, 0, "Turning Left");
}

// 右转动作
void Robot_TurnRight() {
    // 右腿不动
    Set_ServoAngle(3, INIT_ANGLE);
    Set_ServoAngle(4, INIT_ANGLE);
    Set_ServoAngle(5, INIT_ANGLE);
    
    // 左腿后摆
    Set_ServoAngle(0, INIT_ANGLE + 30); // 髋关节前摆
    Set_ServoAngle(1, INIT_ANGLE - 20); // 膝关节弯曲
    Set_ServoAngle(2, INIT_ANGLE + 15); // 踝关节调整
    
    LCD_ShowString(0, 0, "Turning Right");
}

主程序

void main() {
    // 初始化
    LCD_Init();
    PCA_WriteReg(0x00, 0x00); // 进入睡眠模式
    PCA_WriteReg(0xFE, 0x79); // 设置PWM频率50Hz (预分频65)
    PCA_WriteReg(0x00, 0x01); // 退出睡眠模式
    
    Servo_Reset(); // 舵机归零
    HAL_Delay(1000);
    
    while(1) {
        // 按键检测
        if(KEY_FORWARD == 0) {
            Robot_Forward();
            robot_state = 1;
            HAL_Delay(300); // 动作持续时间
        }
        else if(KEY_BACKWARD == 0) {
            Robot_Backward();
            robot_state = 2;
            HAL_Delay(300);
        }
        else if(KEY_LEFT == 0) {
            Robot_TurnLeft();
            robot_state = 3;
            HAL_Delay(300);
        }
        else if(KEY_RIGHT == 0) {
            Robot_TurnRight();
            robot_state = 4;
            HAL_Delay(300);
        }
        else if(robot_state != 0) {
            // 无按键时回到站立姿势
            Servo_Reset();
            robot_state = 0;
        }
        
        // 更新显示
        LCD_ShowAngles();
        HAL_Delay(100);
    }
}

五、机器人步态设计

1. 行走步态原理

双足机器人行走采用交替步态（Alternating Gait）：

1. 支撑腿（负重腿）保持稳定

2. 摆动腿（迈步腿）向前移动

3. 重心在两腿间转移

2. 动作分解（前进）

阶段	左腿动作	右腿动作	持续时间
1	髋关节前摆20°，膝弯曲15°	髋关节后摆20°，膝伸直15°	300ms
2	髋关节回中，膝伸直	髋关节回中，膝弯曲	300ms
3	髋关节后摆20°，膝伸直15°	髋关节前摆20°，膝弯曲15°	300ms
4	髋关节回中，膝弯曲	髋关节回中，膝伸直	300ms

3. 动作平滑过渡

为避免机器人晃动，采用三次样条插值实现平滑角度变化：

// 平滑过渡到目标角度
void Smooth_Move(unsigned char servo, unsigned char target_angle, unsigned int duration) {
    int diff = target_angle - servo_angle[servo];
    int steps = duration / 20; // 20ms per step
    int step_size = diff / steps;
    
    for(int i=1; i<=steps; i++) {
        unsigned char new_angle = servo_angle[servo] + step_size;
        Set_ServoAngle(servo, new_angle);
        HAL_Delay(20);
    }
    Set_ServoAngle(servo, target_angle); // 最终位置
}

六、平衡控制

1. 姿态检测（简易版）

// 简易倾角检测（使用ADC读取加速度计）
float Read_Tilt() {
    // 读取ADC值（假设连接在P1.3）
    ADC_CONTR = 0x87; // 启动ADC转换
    _nop_(); _nop_(); _nop_(); _nop_();
    while(!(ADC_CONTR & 0x10)); // 等待转换完成
    unsigned char adc_val = ADC_RES;
    
    // 转换为角度（-90°~90°）
    return (adc_val - 128) * 90.0 / 128;
}

// 平衡调整
void Balance_Control() {
    float tilt = Read_Tilt();
    
    if(tilt > 5.0) {
        // 前倾，后移重心
        Set_ServoAngle(0, servo_angle[0] - 2); // 左髋后移
        Set_ServoAngle(3, servo_angle[3] - 2); // 右髋后移
    }
    else if(tilt < -5.0) {
        // 后倾，前移重心
        Set_ServoAngle(0, servo_angle[0] + 2); // 左髋前移
        Set_ServoAngle(3, servo_angle[3] + 2); // 右髋前移
    }
}

2. 进阶平衡算法（PID控制）

// PID控制器
typedef struct {
    float Kp, Ki, Kd;
    float integral;
    float prev_error;
} PID_Controller;

float PID_Update(PID_Controller *pid, float error) {
    pid->integral += error;
    float derivative = error - pid->prev_error;
    pid->prev_error = error;
    
    return pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative;
}

// 平衡PID实例
PID_Controller balance_pid = {0.5, 0.01, 0.1, 0, 0};

void Advanced_Balance_Control() {
    float tilt = Read_Tilt();
    float output = PID_Update(&balance_pid, tilt);
    
    // 调整髋关节角度
    Set_ServoAngle(0, servo_angle[0] - output);
    Set_ServoAngle(3, servo_angle[3] - output);
}

参考代码 六个舵机组成的双足机器人程序（51单片机） www.youwenfan.com/contentcss/182573.html

七、扩展功能

1. 蓝牙遥控

// 蓝牙模块初始化（HC-05）
void Bluetooth_Init() {
    SCON = 0x50;        // 串口模式1，允许接收
    TMOD |= 0x20;       // 定时器1模式2
    TH1 = 0xFD;         // 9600波特率
    TL1 = 0xFD;
    TR1 = 1;            // 启动定时器1
    ES = 1;             // 允许串口中断
    EA = 1;             // 开总中断
}

// 串口中断服务函数
void UART_ISR() interrupt 4 {
    if(RI) {
        RI = 0;
        unsigned char cmd = SBUF;
        
        switch(cmd) {
            case 'F': Robot_Forward(); break;
            case 'B': Robot_Backward(); break;
            case 'L': Robot_TurnLeft(); break;
            case 'R': Robot_TurnRight(); break;
            case 'S': Servo_Reset(); break;
        }
    }
}

2. 自动避障

// 超声波测距
float Measure_Distance() {
    // 发送10us高电平
    Trig = 1;
    _nop_(); _nop_(); _nop_(); _nop_(); _nop_(); _nop_();
    _nop_(); _nop_(); _nop_(); _nop_(); _nop_(); _nop_();
    Trig = 0;
    
    // 等待回波
    while(!Echo);
    unsigned int time = 0;
    while(Echo) {
        time++;
        _nop_();
    }
    
    // 计算距离(cm)
    return time * 0.034 / 2;
}

// 避障逻辑
void Obstacle_Avoidance() {
    float dist = Measure_Distance();
    
    if(dist < 20.0) { // 20cm内有障碍
        Robot_Backward(); // 后退
        HAL_Delay(500);
        Robot_TurnRight(); // 右转
        HAL_Delay(500);
    } else {
        Robot_Forward(); // 无障碍前进
    }
}

八、调试与优化

1. 舵机校准

// 舵机中立点校准
void Calibrate_Servo(unsigned char servo) {
    LCD_ShowString(0, 0, "Calibrate S");
    LCD_WriteData('1' + servo);
    
    while(1) {
        if(KEY_FORWARD == 0) {
            Set_ServoAngle(servo, servo_angle[servo] + 1);
            HAL_Delay(100);
        }
        else if(KEY_BACKWARD == 0) {
            Set_ServoAngle(servo, servo_angle[servo] - 1);
            HAL_Delay(100);
        }
        else if(KEY_LEFT == 0) {
            // 保存当前角度为中立点
            eeprom_write(servo, servo_angle[servo]);
            break;
        }
    }
}

2. 低功耗设计

// 进入低功耗模式
void Enter_LowPower() {
    PCON |= 0x01; // 进入IDLE模式
    // 关闭外设
    LCD_WriteCmd(0x08); // 关闭显示
    PCA_WriteReg(0x00, 0x00); // PCA9685睡眠
}

// 唤醒
void Wake_Up() {
    PCON &= 0xFE; // 退出IDLE
    LCD_WriteCmd(0x0C); // 开显示
    PCA_WriteReg(0x00, 0x01); // PCA9685唤醒
}

九、项目总结

本系统实现了基于51单片机的6舵机双足机器人控制，具有以下特点：

1. 采用6自由度设计（每条腿3个舵机）

2. 实现前进、后退、转弯等基本动作

3. 通过PCA9685控制多路舵机

4. 提供LCD状态显示和按键控制

5. 支持蓝牙遥控和自动避障扩展