Easyway两轮自平衡车开发历程
2026-09-121
作者：Edison钟
日期：2013.7
产品&技术开发背景：
1. Segway作为双轮平衡车的始祖，其优异的操控性在当时引起一阵轰动。国内类似产品可选择性有限且价格昂贵。
2. 此项目启动的初衷是本人对此新的技术概念前景的看好及对该技术的进一步探索。
3. 基于以上情况，萌生了开发一款更有性价比产品的念头，同时想通过此项目进一步巩固PID自动控制理论及锻炼动手能力，纯娱乐学习。
设计目标及要求：
1. 载重：80kg
2. 最大速度: 20km/h
3. 最大爬坡角度：30°
4. 控制方式：手动/遥控双模式
原型机3D建模效果图
设计思路及原理：
1. MCU主控通过读取加速度及陀螺仪传感器数据计算出平衡车的实际倾斜角度，通过角度数值计算出驱动车轮的电机转速的PWM控制量。
2. PID闭环控制角度值，检测到倾角变大时，电机的控制量进行相应的增加驱动车轮加速转动确保车身重心的平衡。
3. 左右转向通过握把驱动电位器的方式实现，MCU的ADC检测到电位器的转向信号后将其值叠加到2个车轮电机转速的PWM控制信号上。比如左转时，左侧车轮原始PWM值-转向信号值而右车轮原始PWM值+转向信号值，这样就形成了左右轮的转速差达到转向及原地掉头的目的。
硬件选型：
1. 电源：DC24/36V
2. 核心控制器：STC12C5A60S2 1T单片机，硬件资源：两个16位定时器，两路8位PWM输出，两个自带独立波特率发生器串口，两路外部中断
3. 传感器：MPU-6050三轴加速度及三轴陀螺仪并附带温度传感器
4. 电机：
· 电机类型：DC永磁有刷电机
· 额定功率：350W/24V
· 额定扭矩：TBC
· 转速：250RPM
· 减速比：16.5:1
· 输出轴额定扭矩：TBC
· 输出轴轴径：TBC
· 输出轴连接方式：普通平键连接轮毂
5. 电机驱动板：
· 电压：12－36V
· 持续工作电流：20A以上
· 控制方式：电机正反控制，电机PWM占空比0％－100％无极调速
· 类型：双H桥功率MOSFET驱动2路电机
6. 车轮：直径300-400mm
7. 车架：组合架
减速电机拆解
电机驱动单元
电机调速主控联机测试
车架装配效果图
整机装配效果图
控制把手
原型机全家福
原型机测试视频：平衡车原型机测试视频-CSDN直播
设计注意事项：
1. MPU-6050陀螺仪存在积分累计误差导致角度检测不准，需引入重力加速度传感器构建互补滤波器确保车身倾角的准确性。
2. 左右车轮阻力偏差较大时如何保证车体不快速旋转。
3. 确保左右车轮最大速度差不能超过极限值，以免车体快速旋转。
4. 路况较差时如何保证车的运行速度与设定值基本一致。
5. 如何保证控制系统的安全性，稳定性和可靠性。
项目回顾及展望：
1. 此项目测试非常成功，控制效果达到预期。
2. 实际原型机的测试非常稳定，对类似产品的开发有很好的参考借鉴意义。
3. 通过此项目，加深了对PID自动控制算法的理解并且精确实现了对运动的控制，对后续其它有姿态控制的项目有很好的借鉴意义。
控制软件版本更改记录：
V0.1:
1. 原始版本，适配小型原型机
2. 传感器采样周期22ms
3. Kp=995,Ki=6250,Kd=38,K_deadband=0
4. offset_ax补偿为500
5. 电源为5V供电
6. 控制方式为四路RF控制
7. PWM控制频率为50Hz
8. 小车站立时来回晃动很大，不能长时间站立
V0.2：
1. 角度控制增加积分项，即角度采用PID控制
2. 增加速度传感器，即增加速度PI控制
3. 传感器采样周期采用定时器1定时10ms
4. 角度数据读取及PWM计算在定时器1中断中完成
5. Kp_angle=14.4; Ki_angle=1;Kd_angle=2.4;Kp_speed=0.08;Ki_speed=0.18;deadband=16;
6. offset_ax =-20
7. 电源为7.4V
8. 控制方式为蓝牙无线控制
9. PWM频率调整为1KHz
10. 增加数据上传使能控制
11. 小车能基本站立，但快速运行时仍会往一边倒
V0.3
1. 速度低通滤波参数改为0.9和0.1（之前为0.7和0.3）
2. 改善不明显
V0.4
1. 增加左右电机补偿使小车在站立或者直行时左右电机转速一致，因自制的测速模块开关频率最高只有1.5KHz，无法满足实际要求，取消此功能
2. 增加左右电机脉冲数上传上位机功能，用于调试时监控速度采集是否正确
3. 基于第一点，取消速度控制环节，改用角度PID控制，同时采用改变角度angle_offset来控制小车前进后退
4. Kp_angle=34; Ki_angle=1.8;Kd_angle=3.2;Kp_speed=0;Ki_speed=0;deadband=16;（换刚充电完成的电池后Kp_angle太大，导致电机驱动烧坏）
5. angle_offset值为+/-1度时，能长时间站立并能行驶较长距离,但如果是地面平坦阻力小，小车还是会逐渐加速, 如果路面阻力太大小车无法克服阻力前进（使用一段时间后的电池）。
V0.5
1. 电机驱动改为L298P,控制方式和L293D一样
2. Kp_angle=12.8; Ki_angle=1;Kd_angle=1.8;Kp_speed=0;Ki_speed=0;deadband=16;
3. 性能能达到V0.4版本的效果
V0.6
1. 将自制的测速模块的上拉电阻从100K改为10K后测速频率可达6KHz以上，能满足小车的测速要求，因此增加左右电机补偿使小车在站立或者直行时左右电机转速一致、
2. 角度采用PID控制，速度采用PI控制,其中角度积分angle_I需小于速度积分speed_I以对车体进行速度控制
3. Kp_angle=14.4; Ki_angle=0.4;Kd_angle=1.8;Kp_speed=0.32;Ki_speed=0.01;deadband=16;
4. 车体可以长时间站立，速度可以控制并保持，但速度控制不平滑，小车加速一段距离后又减速一段距离，如此循环
5. 速度低通滤波参数改为0.6和0.4后，小车抖动厉害，改为0.94和0.06后效果较好
6. 转向偏移量改为20
7. 方向控制改为“点动”
V0.7
1. 角度采用PD控制，速度采用PID控制,小车能长时间站立，运行较平稳，速度积分的引入使得小车能自动调节重心，负载偏离中心时，小车能自动调整角度使重心保持在两轮轴的正上方。
2. 速度调整时每10ms调节1个单位，小车调速时很平稳无抖动。
3. 控制变量所占空间已接近STC12C5A60S2存储极限。
4. 小车在站立或者直行时引入左右电机转速差补偿PI控制，使得小车两轮基本保持相同的速度，同时可以避免其中一电机驱动出现问题时小车的高速旋转。
V0.8
1. 增加安全监测函数safety_check(), 一旦检查到speed_limit, rotation_speed_limit, speed_diff_limit, angle_limit超出设定值将关闭电机。
V0.9
1. 增加两路ADC用于检测速度控制量及方向控制量。
V1.0
1. 软件移植至大型原型机
2. 陀螺仪IIC通信接口改到P0.0，P0.1
3. ADC使用0，1路采集电位器信号
V1.4
1. 因方向控制电位零点位置偏移，启动时增加方向控制杆零点识别功能，开机前将方向控制操作杆转至中间位置，启动时将此时的位置设为零点位置。
最新软件代码：
/**********************************************V1.4**********************************************/
//****************************************
// 功能: 两轮自平衡车控制程序
// 2013.01.13
//****************************************
// 使用单片机STC12C5A60S2
// 晶振：32M
// 编译环境 Keil uVision4
// 电机：永康久久24V/350W减速电机,减速比16.5:1
// 电源：24V锂离子电池
// 电机驱动：IRLR7843
//****************************************
#include <STC12C5A.h>
#include <math.h> //Keil library
#include <stdio.h> //Keil library
#include <stdlib.h>
#include <string.h>
#include <INTRINS.H>
#include <MPU6050_driver.h>
#include <I2C.h>
#include "STC_ISP.h"
//#define FOSC 32000000L //System frequency
//#define BAUD 9600 //UART baudrate
//#define PERIOD 26666; //0xffff-timer0_set; //65535-38869=26666
typedef unsigned char uchar;
typedef unsigned short ushort;
typedef unsigned int uint;
//****************************************
// 定义51单片机端口
//****************************************
//P0^0 = SCL
//P0^1 = SDA
//P1^0 = ADC0
//P1^1 = ADC1
sbit PWM1 = P1^3;
sbit PWM2 = P1^4;
sbit buzzer = P1^5;
sbit power_sw_1 = P1^6; //低电平有效
sbit power_sw_2 = P1^7; //低电平有效
sbit mode_sw = P2^7;
sbit Dir_ml_1 = P3^4;
sbit Dir_ml_2 = P3^5;
sbit Dir_mr_1 = P3^6;
sbit Dir_mr_2 = P3^7;
//****************************************
//全局变量定义
//****************************************
//(1)姿态传感器输入变量
float data angle_x;
float data angle_I;
float data angle_ax,angle_gx;
float data gy;
float data gz;
//(2)控制输入变量
bit mode_SW=0;
uchar count_10ms_dir;
//uchar count_10ms_speed;
uchar count_10ms_buzzer;
uchar buzzer_ON_flag;
uchar motion_control;
uchar count_RXD=0;
float speed_bias,speed_adj=1;
float commd_dir,origin_dir,commd_speed,commd_dir_pre,commd_speed_pre,dir_temp; //速度,方向控制参数
float commd_speed_IV,commd_dir_IV; //速度方向控制中间变量
uint commd_dir_temp,commd_speed_temp;
uint ADC_dir_count=0,ADC_speed_count=0;
float commd_dir_sum=0,commd_speed_sum=0;
//float speed_current;
//(3)电机控制核心算法变量
float data PWM,PWM_angle,PWM_speed,PWM_ml,PWM_mr;
float data speed,speed_E,speed_D,speed_I=0;
float data speed_setpoint;
float dir_offset;
float data PWM_speed_comp,PWM_speed_comp_pre;
int data pulse_ml,pulse_mr,pulse_m_l,pulse_m_r;
int data speed_diff=0,speed_diff_I=0;
uchar xdata uart_RXD[10];
uint speed_limit,rotation_speed_limit,speed_diff_limit,angle_limit,battery_status;
uchar safety_flag;
uchar channel=1;
uchar ADC_RES_pre;
//(4)上位机调试变量
uint PWM_abs;
int PWM_c;
float PWM_mr_temp,PWM_ml_temp;
uchar STC_flag;
bit data_trigger=0;
/*****************************************
float PID[2][8]=
{
3.60,0.00,0.18,0.00,0.00,0.00,0.00,0.00, //无人驾驶参数
16.8,0.00,0.72,0.00,0.00,0.00,0.00,0.00 //载人驾驶参数
};*/
float Kp_angle=32; //16.8
float Ki_angle=0;
float Kd_angle=0.88; //0.72
float Kp_speed=0.2;
float Ki_speed=0.01; //0.0008
float Kd_speed=1.5;
float Kp_speed_diff=0;
float Ki_speed_diff=0;
uchar deadband=0; //电机死区补偿量
//*****************************************/
/*****************************************
float code Kp_angle=19.2;
float code Ki_angle=0;
float code Kd_angle=1.8;
float code Kp_speed=0.64;
float code Ki_speed=0.0008;
float code Kd_speed=2.4;
float code Kp_speed_diff=6.4;
float code Ki_speed_diff=0.6;
uchar code deadband=16; //电机死区补偿量
//*****************************************/
/*****************************************
float code Kp_angle=10; //调试电机时参数
float code Ki_angle=0;
float code Kd_angle=0;
float code Kp_speed=0;
float code Ki_speed=0;
float code Kd_speed=0;
float code Kp_speed_diff=0;
float code Ki_speed_diff=0;
uchar code deadband=0; //电机死区补偿量
//*****************************************/
//*******************************************函数定义及声明***********************************************
//*********************************************************
//中断控制初始化
//*********************************************************
void init_int(void)
{
EA = 1; //开总中断
EX0 = 1; //开外部中断INT0
EX1 = 1; //开外部中断INT1
IT0 = 1; //INT0下降沿触发
IT1 = 1; //INT1下降沿触发
ET1 = 1; //开定时器1中断
ES = 1; //Enable UART interrupt
//IE2 = 0x01; //Enable UART2 interrupt(不可位寻址)
EADC= 1; //开模数转换中断
IPH = 0x0d; //0x0d，0x15设置外部中断级别3，串口中断优先级别1，T1中断优先级别2，ADC中断优先级别0
IP = 0x15; //0x15，0x0d设置外部中断级别3，串口中断优先级别2，T1中断优先级别1，ADC中断优先级别0
}
//*********************************************************
//串口初始化函数
//*********************************************************
void init_uart(void)
{
//启用独立串口波特率发生器
//BRT = -(FOSC/12/32/BAUD); //波特率发生器预置初始值
//AUXR|= 0x15; //选择独立波特率发生器并启动,BRT工作在1T模式
AUXR|= 0x11; //选择独立波特率发生器并启动,BRT工作在12T模式
//PCON|= 0x80; //1T模式波特率加倍
BRT = 0xF7; //9600@32mhz,12T模式
//BRT = 0xdd; //57600@32mhz,1T模式
SCON = 0x50; //串口工作模式1（8位可变波特率模式）
}
//*********************************************************
//串口2初始化函数
/*********************************************************
void init_uart2(void)
{
S2CON = 0x50; //8-bit variable UART,S2REN=1允许接收
//S2CON = 0xda; //9-bit variable UART, parity bit initial to 1
//S2CON = 0xd5; //9-bit variable UART, parity bit initial to 0
BRT = 0xF7; //9600@32mhz,12T模式
//BRT = 0xdd; //57600@32mhz,1T模式
AUXR|= 0x10; //选择独立波特率发生器并启动,BRT工作在12T模式
//AUXR = 0x14; //Baudrate generator work in 1T mode
AUXR1|=0x10; //P4口为第二串口P4.2=RX,P4.3=TX
}
*/
//*********************************************************
//设置Timer0为8位自动重载模式，作为PWM时钟源
//*********************************************************
void init_timer0()
{
AUXR |= 0x00; //timer0 work in 12T mode
TMOD |= 0x02; //set timer0 counter mode2 (8-bit auto-reload)
TH0=TL0=0xf6; //PWM 0x30 for 50Hz,0x98 for 100hz,0xf6 for 1khz,0xff for 10khz T=1/(f*256)
//TR0 = 1; //timer0 start running(as PWM clk)
TR0 = 0; //timer0 shutup(as PWM clk)
}
//*********************************************************
//定时器100Hz数据更新初始化
//*********************************************************
void init_timer1(void) //10毫秒@32MHz,100Hz刷新频率
{
AUXR |= 0x00; //定时器时钟12T模式
TMOD |= 0x10; //设置定时器模式
TH1 = 0x97; //设置定时初值
TL1 = 0xd5; //设置定时初值
TF1 = 0; //清除TF1标志
TR1 = 1; //定时器1开始计时
}
//*********************************************************
//PWM模式设置
//*********************************************************
void init_PWM()
{
CCON = 0; //Initial PCA control register(PCA timer stop,Clear CF flag,Clear all module interrupt flag)
CL = 0; //Reset PCA base timer
CH = 0;
//CMOD = 0x04; //Set PCA timer clock source as timer0 overflow,Disable PCA timer overflow interrupt
CMOD = 0x0A; //Set PCA timer clock source as SYSclk/4=32MHZ/4=8MHZ,F_PWM=8,000,000/256=31,25KHZ,Disable PCA timer overflow interrupt
CCAP0H = CCAP0L = 255; //PWM0 port output 0% duty cycle square wave
CCAPM0 = 0x42; //PCA module-0 work in 8-bit PWM mode and no PCA interrupt
CCAP1H = CCAP1L = 255; //PWM1 port output 0% duty cycle square wave
CCAPM1 = 0x42; //PCA module-1 work in 8-bit PWM mode and no PCA interrupt
CR = 1; //PCA timer start run
}
//*********************************************************
//模数转换模块初始化函数
//*********************************************************
void init_ADC( )
{
P1ASF = 0x07; //0x24 set P1.2,P1.5 and 0x03 set P1.0,1.1,0x03 set P1.0,1.1,P1.2 as analog input port
ADC_RES = 0; //Clear previous result
ADC_CONTR = ADC_POWER | ADC_SPEEDLL | ADC_START | channel;
delay(1); //ADC power-on delay and Start A/D conversion
//ADC_CONTR = ADC_POWER | ADC_SPEEDLL | ADC_START | channel;
}
//*********************************************************
//控制系统初始化函数
//*********************************************************
void init_Edway(void)
{
power_sw_1=1; //高电平关闭电机电源继电器1
power_sw_2=1; //高电平关闭电机电源继电器2
safety_flag=5; //safety_flag为5表示是安全的
commd_speed=0; //速度控制量为0
//origin_dir=648.5; //648.5满格电
//commd_dir=origin_dir; //方向控制量为0
//commd_dir_pre=origin_dir;
commd_speed_IV=0; //速度控制量为0
commd_speed_pre=0;
commd_dir_IV=0; //方向控制量为0
speed_setpoint=0; //速度控制量为0
dir_offset=0; //方向控制量为0
motion_control=0;
P3M1 = 0x00; //P3^4,P3^5,P3^6,P3^7口为推挽模式以驱动电机控制板
P3M0 = 0xF0;
delay(500); //上电延时
init_MPU6050();
init_uart();
//init_uart2();
init_timer0();
init_PWM();
init_timer1();
init_ADC();
init_int();
delay(500);
angle_x=angle_ax;
angle_gx=angle_ax;
}
//*********************************************************
//串口数据发送
//*********************************************************
void SeriSend(uchar *send_data)
{
uchar n=0;
while(*(send_data+n)!='\0')
{
SBUF=*(send_data+n);
while(!TI);
TI=0;
n++;
}
}
//*********************************************************
//数据上传上位机显示函数
//*********************************************************
void data_upload(void)
{
//uchar xdata str_angle_ax[200],str_angle_gy[200];
uchar xdata str_angle_x[200],str_pulse_ml[20],str_pulse_mr[20],str_r_speed[20],str_commd_speed[20],str_commd_dir[20],str_actual_speed[20];//,str_pwm[20];
uchar xdata str_PWM_ml[10],str_PWM_mr[10];
char comma[]=",";
char huiche[]="\n";
//sprintf(str_angle_ax,"%.2f",angle_ax);
//sprintf(str_angle_gy,"%.2f",angle_gx);
sprintf(str_angle_x,"%.2f",angle_x);
sprintf(str_r_speed,"%d",rotation_speed_limit);
sprintf(str_pulse_ml,"%d",pulse_ml);
sprintf(str_pulse_mr,"%d",pulse_mr);
sprintf(str_commd_speed,"%.2f",speed_setpoint);
sprintf(str_actual_speed,"%.2f",speed);
sprintf(str_commd_dir,"%.2f",dir_offset);
//sprintf(str_pwm,"%d",PWM_c);
sprintf(str_PWM_ml,"%.2f",PWM_ml_temp);
sprintf(str_PWM_mr,"%.2f",PWM_mr_temp);
//sprintf(str_PWM_ml,"%.2f",PWM_ml);
//sprintf(str_PWM_mr,"%.2f",PWM_mr);
//SeriSend(str_angle_ax);
//SeriSend(comma);
//SeriSend(str_angle_gy);
//SeriSend(comma);
SeriSend(str_angle_x); //角度数据
SeriSend(comma);
SeriSend(str_r_speed); //角速度数据
SeriSend(comma);
SeriSend(str_pulse_ml); //左电机脉冲数据
SeriSend(comma);
SeriSend(str_pulse_mr); //右电机脉冲数据
SeriSend(comma);
SeriSend(str_commd_speed); //速度控制目标值
SeriSend(comma);
SeriSend(str_actual_speed); //实际速度检测值
SeriSend(comma);
SeriSend(str_commd_dir); //转向控制目标值
//SeriSend(comma);
//SeriSend(str_pwm);
SeriSend(comma);
SeriSend(str_PWM_ml); //带符号的左电机控制量
SeriSend(comma);
SeriSend(str_PWM_mr); //带符号的右电机控制量
SeriSend(huiche);
}
//*********************************************************
//蓝牙控制函数
//*********************************************************
void BT_remote(void)
{/*
switch(motion_control)
{
case 0: //停止
commd_speed=0;
commd_dir=0;
break;
case 1: //前进
commd_speed=150;
break;
case 2: //后退
commd_speed=-150;
break;
case 3: //左转
commd_dir=dir_temp;
break;
case 4: //右转
commd_dir=-dir_temp;
break;
case 5: //快进
commd_speed=300;
break;
case 6: //快退
commd_speed=-300;
break;
/***************************************************************
字符串命令转换控制
***************************************************************/
/* case 48: //停止，48为字符0
commd_speed=0;
commd_dir=0;
break;
case 49: //前进，49为字符1
commd_speed=150;
break;
case 50: //后退，50为字符2
commd_speed=-150;
break;
case 51: //左转，51为字符3
commd_dir=dir_temp;
break;
case 52: //右转，52为字符4
commd_dir=-dir_temp;
break;
case 53: //快进，53为字符5
commd_speed=300;
break;
case 54: //快退，54为字符6
commd_speed=-300;
break;
default:
motion_control=0;
}*/
}
//*********************************************************
//控制命令预处理函数
//*********************************************************
void commd_process(void)
{
//commd_speed_IV=1.5*(commd_speed-64); //64="@",前进后退控制
commd_speed_IV=0.15*commd_speed; //只能前进控制
//commd_dir_IV=9*(commd_dir-origin_dir); //10 x
commd_dir_IV=(9-0.05*speed_limit)*(commd_dir-origin_dir); //速度越快，转向控制量越小以避免较快行驶速度时大角度转弯把人摔出去
if(commd_dir_IV>0)commd_dir_IV*=1.4; //方向控制量线性化,2 x
commd_speed_IV=0.08*commd_speed_IV+0.92*commd_speed_pre; //ADC速度控制转换数据低通滤波
commd_speed_pre=commd_speed_IV;
commd_dir_IV=0.2*commd_dir_IV+0.8*commd_dir_pre; //ADC方向控制转换数据低通滤波
commd_dir_pre=commd_dir_IV;
dir_offset=commd_dir_IV;
if(mode_SW==1)
{
count_10ms_dir++;
if(count_10ms_dir==50){count_10ms_dir=0;dir_offset=commd_dir_IV=0;commd_dir=origin_dir;} //遥控模式转弯点动调整时间为0.5s
}
speed_setpoint=commd_speed_IV;
/*
if(speed_setpoint!=commd_speed_IV)
{ //速度定额调整
speed_bias=commd_speed_IV-speed_setpoint; //速度控制量每10ms调整speed_adj控制量以改善速度
if(speed_bias>speed_adj) speed_setpoint+=speed_adj; //调整时由于控制量突变出现的抖动，使之平滑过渡
else if(speed_bias<-speed_adj) speed_setpoint-=speed_adj;//该方法速度调整时很平稳，但控制量越大调整时间
else speed_setpoint=commd_speed_IV; //越长，控制延时越严重。
}
*/
if(dir_offset>8)dir_offset-=8; //方向控制量在一区间内为0以避免方向控制柄在中间位置时控制量的少量偏差导致转向
else if(dir_offset<-8)dir_offset+=8;
else dir_offset=0;
if(speed_setpoint>40)speed_setpoint-=40; //方向控制量在一区间内为0以避免方向控制柄在中间位置时控制量的少量偏差导致转向
//else if(dir_offset<-25)speed_setpoint+=25;
else speed_setpoint=0;
if(dir_offset>70)dir_offset=70; //方向控制量限幅
else if(dir_offset<-70)dir_offset=-70;
if(speed_setpoint>70)speed_setpoint=70; //速度控制量限幅8KM/H
else if(speed_setpoint<-70)speed_setpoint=-70;
}
//*********************************************************
//车速计算函数
//*********************************************************
void speed_cal(void)
{
float speed_pre;
speed_pre=speed;
speed=(pulse_ml+pulse_mr)*1.096; //speed={[(pulse_ml+pulse_mr)/2]/(16.5*100*0.01)}*(2*3.14*320/2)*(3600/1,000,000)*1000;//单位：100m/Hr
speed=0.92*speed_pre+0.08*speed; //速度低通滤波0.94/0.06
speed_limit=fabs(speed); //速度上限检测
//speed_actual=speed; //速度检测值
speed_D=speed-speed_pre; //加速度计算
speed_E=speed_setpoint-speed; //速度误差计算
speed_I=speed_I+Ki_speed*speed_E; //对速度误差进行积分
//if(angle_limit<5)speed_I=speed_I+Ki_speed*speed_E; //对速度误差进行积分
//if(speed_I>240) speed_I = 240; //速度误差积分进行饱和处理
//else if(speed_I<-240) speed_I = -240;
if(speed_I>5*Kp_angle) speed_I = 5*Kp_angle; //速度误差积分进行饱和处理
else if(speed_I<-5*Kp_angle) speed_I = -5*Kp_angle;
/*
speed_diff=pulse_ml-pulse_mr; //左右电机转速差计算
speed_diff_limit=abs(speed_diff);
speed_diff_I=speed_diff_I+Ki_speed_diff*speed_diff;//左右电机转速差积分
if(speed_diff_I>455)speed_diff_I=455; //左右两轮速度差进行饱和处理
else if(speed_diff_I<-455)speed_diff_I=-455;
if(dir_offset!=0) {speed_diff=0;speed_diff_I=0;} //转弯时不进行左右车轮转速差补偿
*/
}
//*********************************************************
//电机PWM计算函数
//*********************************************************
void PWM_cal(void)
{
//dir_offset=0; //调试电机时使用
PWM_angle=Kp_angle*angle_x+Ki_angle*angle_I+Kd_angle*gy; //角度PID控制
PWM_speed=Kp_speed*speed_E+speed_I+Kd_speed*speed_D; //速度PID控制
PWM=PWM_angle-PWM_speed;
if(PWM<-250) PWM=-250; //PWM进行饱和处理
else if(PWM>250) PWM=250;
/*
PWM_speed_comp_pre=PWM_speed_comp;
PWM_speed_comp=Kp_speed_diff*speed_diff+speed_diff_I; //左右电机转速差补偿PI控制
PWM_speed_comp=0.4*PWM_speed_comp_pre+0.6*PWM_speed_comp; //转速差补偿量平滑处理
if(PWM>0) //左右电机转速差补偿量饱和处理，补偿量<=PWM
{
if(PWM_speed_comp>PWM)PWM_speed_comp=PWM;
else if(PWM_speed_comp<-PWM)PWM_speed_comp=-PWM;
}
else if(PWM<0)
{
if(PWM_speed_comp>-PWM)PWM_speed_comp=-PWM;
else if(PWM_speed_comp<PWM)PWM_speed_comp=PWM;
}
else PWM_speed_comp=0;
PWM_mr=PWM+dir_offset+PWM_speed_comp; //当左右电机运行阻力相差较大或者其中一电机
PWM_ml=PWM-dir_offset-PWM_speed_comp; //出现故障停车时对电机转速进行补偿以避免车体高速旋转
*/ //但如果是一侧车轮打滑时，该补偿效果将适得其反。是否可以考虑用陀螺仪检测车体高速旋转
PWM_mr=PWM+dir_offset;
PWM_ml=PWM-dir_offset; //无左右电机转速差补偿控制
PWM_mr_temp=PWM_mr;
PWM_ml_temp=PWM_ml;
}
//*********************************************************
//电机控制函数
//*********************************************************
void motor_control()
{
if(PWM_ml<0)
{
Dir_ml_1=0; //左电机后退
Dir_ml_2=1;
PWM_ml = -PWM_ml;
}
else
{
Dir_ml_1=1; //左电机前进
Dir_ml_2=0;
}
/*
else if(PWM_ml>0)
{
Dir_ml_1=1; //左电机前进
Dir_ml_2=0;
}
else
{
Dir_ml_1=0; //左电机刹车
Dir_ml_2=0;
}
*/
if(PWM_mr<0)
{
Dir_mr_1=0; //右电机后退
Dir_mr_2=1;
PWM_mr = -PWM_mr;
}
else
{
Dir_mr_1=1; //右电机前进
Dir_mr_2=0;
}
/*
else if(PWM_mr>0)
{
Dir_mr_1=1; //右电机前进
Dir_mr_2=0;
}
else
{
Dir_mr_1=0; //右电机刹车
Dir_mr_2=0;
}
*/
PWM_ml+=deadband; //左电机死区补偿
PWM_mr+=deadband; //右电机死区补偿
if(PWM_ml>230) PWM_ml = 230 ; //左电机饱和处理，防止PWM值超过230(电机驱动模块支持的最大占空比为90%)
if(PWM_mr>230) PWM_mr = 230 ; //右电机饱和处理，防止PWM值超过230(电机驱动模块支持的最大占空比为90%)
//CCAP0H = CCAP0L = 0xff-PWM_ml; //设定PWM0占空比(CCAP0H=0，速度最大)
//CCAP1H = CCAP1L = 0xff-PWM_mr; //设定PWM1占空比(CCAP1H=0，速度最大)
CCAP0H = 0xff-PWM_ml; //设定PWM0占空比(CCAP0H=0，速度最大)
CCAP1H = 0xff-PWM_mr; //设定PWM1占空比(CCAP1H=0，速度最大)
}
//*********************************************************
//工作模式检测函数
//*********************************************************
void mode_check()
{
if(mode_sw==0||safety_flag==4) //调速把控制||速度超过安全值后抬头以减低速度
{
Kp_speed=0.2;
Ki_speed=0.01;
Kd_speed=1.5;
}
else //姿势控制
{
Kp_speed=0;
Ki_speed=0;
Kd_speed=0;
if(speed_I>0.01)speed_I-=0.01;
else if(speed_I<-0.01)speed_I+=0.01;
else speed_I=0;
}
}
//*********************************************************
//安全监测函数
//*********************************************************
void safety_check(void)
{
if(speed_limit>120||rotation_speed_limit>200||angle_limit>22)//||speed_diff_limit>30]
{
safety_flag=1; //safety_flag=1表示小车运行超出安全设定值，已失控
}
//else if(battery_status<200) safety_flag=2; //safety_flag=2表示小车电池电量不足
else if(safety_flag==3) //safety_flag=3系统复位
{
_nop_();
}
else if(speed_limit>70&&mode_sw==1)
{
safety_flag=4; //safety_flag=4表示速度超过安全值
}
else safety_flag=5; //safety_flag=5表示系统是安全
}
void buzzer_confg(uchar buzzer_flag)
{
switch(buzzer_flag)
{
case 0: //短鸣短停-急促声
if(count_10ms_buzzer>=4){buzzer=!buzzer;count_10ms_buzzer=0;}
break;
case 1: //中鸣中停-中等急促
if(count_10ms_buzzer>=50){buzzer=!buzzer;count_10ms_buzzer=0;}
break;
case 2: //长鸣长停
if(count_10ms_buzzer>=100){buzzer=!buzzer;count_10ms_buzzer=0;}
break;
case 3: //短鸣长停
if(count_10ms_buzzer>=10){buzzer_ON_flag++;buzzer=1;count_10ms_buzzer=0;}
if(buzzer_ON_flag>=80){buzzer=0;buzzer_ON_flag=0;}
break;
case 4: //中鸣长停
if(count_10ms_buzzer>=80){buzzer_ON_flag++;buzzer=1;count_10ms_buzzer=0;}
if(buzzer_ON_flag>=10){buzzer=0;buzzer_ON_flag=0;}
break;
default:
buzzer=1;
}
}
//*/
//*********************************************************
//主函数
//*********************************************************
void main()
{
uchar buzzer_flag;
PWM1=0; //PWM1置0，避免单片机复位时IO口高电平驱动电机
PWM2=0; //PWM2置0，避免单片机复位时IO口高电平驱动电机
init_Edway(); //系统初始化
delay(10000); //初始化延时
origin_dir=commd_dir; //取当前方向控制手柄的位置为零位
commd_dir_pre=commd_dir;
delay(1000);
while(1)
{
power_sw_1=1; //复位后关闭电机电源开关
power_sw_2=1;
safety_flag=5; //复位后默认系统是安全的
while(fabs(speed_setpoint)>5||fabs(dir_offset)>20) //当启动前检查到速度及方向控制量超过设定值时，不启动电机
{
buzzer_flag=0;
buzzer_confg(buzzer_flag);
}
while(angle_limit>0.2) //当倾斜角度值大于1°时，不开启电机电源开关
{
angle_I=0;
speed_I=0;
PWM_speed_comp=0;
buzzer_flag=2;
buzzer_confg(buzzer_flag);
}
power_sw_1=0; //当倾斜角度值小于1°时开启电机电源开关
power_sw_2=0;
while(1)
{
//BT_remote();
//STC_ISP(); //免断电下载
if(safety_flag==1) //safety_flag==1表明系统不安全
{
power_sw_1=1; //关闭电机电源，需手动复位
power_sw_2=1;
//CCAP0H=0xff; //PWM0输出占空比为0,该方法有刹车效果，同时刹车时冲击比较厉害
//CCAP1H=0xff; //PWM1输出占空比为0,该方法有刹车效果，同时刹车时冲击比较厉害
buzzer_flag=0;
}
else if(safety_flag==2) //safety_flag==2表明电池电量低，自动停车
{
speed_setpoint=0;
buzzer_flag=1;
}
else if(safety_flag==3) //safety_flag==3跳出while循环复位
{
break;
}
else if(safety_flag==4) //safety_flag==4表明小车行驶速度超过安全设定速度
{
buzzer_flag=1;
}
else if(safety_flag==5) //safety_flag==5表明系统安全
{
//power_sw_1=0;
//power_sw_2=0;
buzzer_flag=3;
}
if(power_sw_1==1&&safety_flag!=1) buzzer_flag=2; //如果在安全的情况下检测到继电器已被关闭时，蜂鸣器提示音改为长鸣长停，继电器需手动开启
//if(power_sw_1==1&&safety_flag!=1) break; //调试时使用，在安全的情况下检测到继电器已被关闭时，直接返回并做出判断是否重新开启继电器
buzzer_confg(buzzer_flag);
mode_check();
if(data_trigger)data_upload(); //当data_trigger为1时才上传角度数据到上位机
//data_upload();
}
}
}
//*********************************************************
//左电机测速中断函数
//*********************************************************
void int_ext0(void) interrupt 0
{
if(PWM>0) pulse_m_l++; //左电机脉冲累计
else if(PWM<0) pulse_m_l--;
//LED1=~LED1;
}
//*********************************************************
//右电机测速中断函数
//*********************************************************
void int_ext1(void) interrupt 2
{
if(PWM>0) pulse_m_r++; //右电机脉冲累计
else if(PWM<0) pulse_m_r--;
//LED2=~LED2;
}
//*********************************************************
//timer1 10ms定时中断函数
//*********************************************************
void int_timer1(void) interrupt 3
{
TH1 = 0x97; //设置定时初值10MS
TL1 = 0xd5;
pulse_mr=pulse_m_r; //速度脉冲赋予中间变量供后续速度计算时使用，并及时清除当前脉冲值以重新开始计数
pulse_ml=pulse_m_l;
pulse_m_r=pulse_m_l=0; //电机计数脉冲清零
count_10ms_buzzer++;
//ADC_CONTR = ADC_POWER | ADC_SPEEDLL | ADC_START | channel; //启动ADC,此处无法启动ADC
commd_process(); //外部控制量处理函数
//STC_ISP(); //免断电下载
angle_cal(); //倾角计算
speed_cal(); //速度计算
safety_check(); //安全检测
if(safety_flag==1) return; //表明系统不安全
PWM_cal(); //PWM值计算
motor_control(); //电机PWM控制
//delay(10); //用于测试处理时间是否还有盈余
}
//*********************************************************
//串口中断函数
//*********************************************************
void int_uart(void) interrupt 4
{
if(RI==1)
{
uart_RXD[count_RXD]=SBUF;
count_RXD++;
RI=0;
//***************************************************************************************
//(1)第一个字节数据为0xff或者'f'=102，表示是运动控制指令,运动控制指令由3个字节数据组成
//***************************************************************************************
if((uart_RXD[0]==0xff||uart_RXD[0]==102)&&count_RXD==4) //102为字符f的ascii码值
{
if(mode_SW==1)
{
motion_control=uart_RXD[1];
//if(motion_control==1||motion_control==49) commd_speed=uart_RXD[2]; //速度控制,49=“1”
if(motion_control==1||motion_control==49){commd_speed_temp=uart_RXD[2];commd_speed_temp<<=8;commd_speed_temp|=uart_RXD[3];commd_speed=commd_dir_temp;}
//else if(motion_control==2||motion_control==50){commd_dir=uart_RXD[2];count_10ms_dir=0;} //方向控制偏移值,50=“2”
else if(motion_control==2||motion_control==50){commd_dir_temp=uart_RXD[2];commd_dir_temp<<=8;commd_dir_temp|=uart_RXD[3];commd_dir=commd_dir_temp;}
else if(motion_control==3||motion_control==51){power_sw_1=1;power_sw_2=1;} //紧急刹车,51="3"
else if(motion_control==4||motion_control==52){safety_flag=3;} //重启系统,52="4"
}
count_RXD=0;
}
//***************************************************************************************
//(2)第一个字节数据为0xee，表示是PID控制参数，PID控制指令由10个字节数据组成
//***************************************************************************************
else if(uart_RXD[0]==0xee&&count_RXD==10)
{
Kp_angle=1*uart_RXD[1];
Ki_angle=0.01*uart_RXD[2];
Kd_angle=0.01*uart_RXD[3];
Kp_speed=0.01*uart_RXD[4];
Ki_speed=0.0001*uart_RXD[5];
Kd_speed=0.1*uart_RXD[6];
Kp_speed_diff=0.1*uart_RXD[7];
Ki_speed_diff=0.1*uart_RXD[8];
deadband=uart_RXD[9];
count_RXD=0;
}
//***************************************************************************************
//(3)第一个字节数据为0xdd，开启/关闭角度数据上传上位机开关，数据上传指令由1个字节数据组成
//***************************************************************************************
else if(uart_RXD[0]==0xdd){data_trigger=~data_trigger;count_RXD=0;}
//***************************************************************************************
//(4)第一个字节数据为0xcc或者'c'=99，控制模式切换，手动/遥控=mode_SW==0/mode_SW==1
//***************************************************************************************
else if(uart_RXD[0]==0xcc||uart_RXD[0]==99) //99为字符c的ascii码值
{
commd_speed=0; //控制模式切换前，速度及方向控制量置零
commd_dir=origin_dir;
commd_speed_IV=0;
commd_dir_IV=0;
speed_setpoint=0;
dir_offset=0;
mode_SW=~mode_SW;
count_RXD=0;
}
//***************************************************************************************
//(5)指令为多字节指令并且指令数据未接收完时返回以接收剩余的指令数据
//***************************************************************************************
else if(uart_RXD[0]==0xff||uart_RXD[0]==102||uart_RXD[0]==0xee) return;
//***************************************************************************************
//(6)如果串口接收数据不是以上打头（0xff,102,0xee）的指令数据将被忽略
//***************************************************************************************
else count_RXD=0;
}
}
//*********************************************************
//模数转换模块中断函数
//*********************************************************
void int_ADC() interrupt 5
{
ADC_CONTR &= !ADC_FLAG; //Clear ADC interrupt flag
if(channel==2)
{
channel=1;
//if(mode_SW==0)commd_speed=ADC_RES; //只取ADC的高八位数据舍弃低两位ADC_LOW2,并且仅当mode_SW==0时才采纳ADC通道的方向及速度指令
if(mode_SW==0)
{
commd_speed_temp=ADC_RES;
commd_speed_temp<<=2;
commd_speed_temp|=ADC_LOW2;
commd_speed=commd_speed_temp;
//commd_speed_sum+=commd_dir_temp;
//if(ADC_dir_count>399){commd_speed=commd_speed_sum/400;commd_speed_sum=0;ADC_speed_count=0;} //ADC数据取平均值
}
}
else if(channel==1)
{
channel=2;
//if(mode_SW==0)commd_dir=ADC_RES; //只取ADC的高八位数据舍弃低两位ADC_LOW2
//if(mode_SW==0){commd_dir_temp=ADC_RES;commd_dir_temp<<=2;commd_dir_temp|=ADC_LOW2;commd_dir=commd_dir_temp;}
if(mode_SW==0)
{
ADC_dir_count++;
commd_dir_temp=ADC_RES;
commd_dir_temp<<=2;
commd_dir_temp|=ADC_LOW2;
commd_dir_sum+=commd_dir_temp;
if(ADC_dir_count>49){commd_dir=commd_dir_sum/50;commd_dir_sum=0;ADC_dir_count=0;} //ADC数据取平均值
}
}
ADC_CONTR = ADC_POWER | ADC_SPEEDLL | ADC_START | channel; //重新启动ADC
}
/*
//*********************************************************
//串口2中断函数
//*********************************************************
void serial2_int(void) interrupt 4
{
if (S2CON & S2RI)
{
S2CON &= ~S2RI; //Clear receive interrupt flag
P0 = S2BUF; //P0 show UART data
P2 = (S2CON & S2RB8); //P2.2 show parity bit
}
if (S2CON & S2TI)
{
S2CON &= ~S2TI; //Clear transmit interrupt flag
//busy = 0; //Clear transmit busy flag
}
}
*/
