阅读量:0
一、引言
关于STM32两轮平衡车的设计，我想在读者阅读本文之前应该已经有所了解，所以本文的重点是代码的分享和分析。至于具体的原理，我觉得读者不必阅读长篇大论的文章，只需按照本文分享的代码自己亲手制作一辆平衡车，其原理并不言而喻了。源完整代码工程在文章末尾百度网盘链接，请需要的读者自行下载即可。
另外，由于平衡车的精髓在于PID算法的运用，有需要了解PID算法的读者可以参考以下两篇文章：
PID算法详解（代码详解篇），位置式PID、增量式PID（通用）_pid 代码-CSDN博客
PID算法详解（精华知识汇总）_小小_扫地僧的博客-CSDN博客
二、所需材料
1、STM32F03C8T6
2、MPU6050
3、蓝牙模块
4、编码电机
5、TB6612
6、电源+稳压模块
7、OLED显示模块
三、接线强调
1、TB6612接线
2、蓝牙模块与单片机之间
单片机 蓝牙模块
TX ——> RX
RX ——> TX
3、MPU6050
使用IIC通信，所以对照代码接SDA、SCL、GND、VCC、IN（中断触发线）
四、功能介绍
1、两轮平衡直立
2、蓝牙APP控制运动状态
3、遥控手柄控制
4、超声波避障
五、关键算法
PID算法对编码电机的控制
1.位置闭环控制
位置闭环控制就是根据编码器的脉冲累加测量电机的位置信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。 位置闭环控制就是根据编码器的脉冲累加测量电机的位置信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程.
1.1理论分析
1.2控制原理图
1.3C语言实现
int Position_PID (int Encoder, int Target) { static float Bias, Pwm,Integral_bias,Last_Bias; Bias=Encoder-Target;//计算偏差 Integral_bias+=Bias; //求出偏差的积分 Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias);Last_Bias=Bias; //保存上一次偏差 return Pwm; //输出 } 入口参数为编码器的位置测量值和位置控制的目标值，返回值为电机控制PWM(现在再看一下上面的控制原理图是不是更加容易明白了)。
第一行是相关内部变量的定义。
第二行是求出速度偏差，由测量值减去目标值。第三行通过累加求出偏差的积分。
第四行使用位置式PID控制器求出电机 PWM。第五行保存上一次偏差，便于下次调用。最后一行是返回。
然后，在定时中断服务函数里面调用该函数实现我们的控制目标:Moto=Position_PID(Encoder, Target_Position);
Set_Pwm(Moto) ;//===赋值给PWM寄存器
2、速度闭环控制
速度闭环控制就是根据单位时间获取的脉冲数(这里使用了M法测速)测量电机的速度信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。
一些PID的要点在位置控制中已经有讲解，这里不再赘叙。
需要说明的是，这里速度控制20ms一次，一般建议10ms或者5ms，因为在这里电机是使用USB供电，速度比较慢，20ms可以延长获取速度的单位时间，提高编码器的采值。
2.1理论分析
根据增量式离散PID公式 根据增量式离散PID公式
Pwm+=Kp[e(k)-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k):本次偏差
e(k-1):上一次的偏差e (k-2):上上次的偏差
Pwm 代表增量输出在我们的速度控制闭环系统里面只使用PI控制，因此对PID控制器可简化为以下公式:
Pwm+=Kp[e(k)-e(k-1)]+Ki*e(k)
2.2 控制原理图
2.3 C语言实现
增量式PI控制器具体通过C语言实现的代码如下:
int Incremental_PI (int Encoder,int Target) { static float Bias, Pwm, Last_bias; Bias=Encoder-Target;//计算偏差 Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;//增量式PI控制器 Last_bias=Bias;//保存上一次偏差 return Pwm;//增量输出 }入口参数为编码器的速度测量值和速度控制的目标值，返回值为电机控制PWM。
第一行是相关内部变量的定义。
第二行是求出速度偏差，由测量值减去目标值。第三行使用增量PI控制器求出电机PWM。
第四行保存上一次偏差，便于下次调用。最后一行是返回。
然后，在定时中断服务函数里面调用该函数实现我们的控制目标:Moto=Incremental_PI(Encoder, Target_Velocity);Set_Pwm(Moto);//===赋值给对应MCU的PWM寄存器
六、关键代码分析
1、编码电机PID算法控制
#include "control.h" #include "usart2.h" /************************************************************************** 函数功能：所有的控制代码都在这里面 5ms定时中断由MPU6050的INT引脚触发 严格保证采样和数据处理的时间同步 在MPU6050的采样频率设置中，设置成100HZ，即可保证6050的数据是10ms更新一次。 读者可在imv_mpu.h文件第26行的宏定义进行修改(#define DEFAULT_MPU_HZ (100)) **************************************************************************/ #define SPEED_Y 100 //俯仰(前后)最大设定速度 #define SPEED_Z 80//偏航(左右)最大设定速度 int Balance_Pwm,Velocity_Pwm,Turn_Pwm,Turn_Kp; float Mechanical_angle=8; float Target_Speed=0; //期望速度（俯仰）。用于控制小车前进后退及其速度。 float Turn_Speed=0; //期望速度（偏航） //针对不同车型参数，在sys.h内设置define的电机类型 float balance_UP_KP=BLC_KP; // 小车直立环PD参数 float balance_UP_KD=BLC_KD; float velocity_KP=SPD_KP; // 小车速度环PI参数 float velocity_KI=SPD_KI; float Turn_Kd=TURN_KD;//转向环KP、KD float Turn_KP=TURN_KP; void EXTI9_5_IRQHandler(void) { static u8 Voltage_Counter=0; if(PBin(5)==0) { EXTI->PR=1<<5; //清除LINE5上的中断标志位 mpu_dmp_get_data(&pitch,&roll,&yaw); //得到欧拉角（姿态角）的数据 MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据 Encoder_Left=Read_Encoder(2); //读取编码器的值，保证输出极性一致 Encoder_Right=-Read_Encoder(3); //读取编码器的值 Led_Flash(100); Voltage_Counter++; if(Voltage_Counter==20) //100ms读取一次电压 { Voltage_Counter=0; Voltage=Get_battery_volt(); //读取电池电压 } if(KEY_Press(100)) //长按按键切换模式并触发模式切换初始化 { if(++CTRL_MODE>=101) CTRL_MODE=97; Mode_Change=1; } Get_RC(); Target_Speed=Target_Speed>SPEED_Y?SPEED_Y:(Target_Speed<-SPEED_Y?(-SPEED_Y):Target_Speed);//限幅 Turn_Speed=Turn_Speed>SPEED_Z?SPEED_Z:(Turn_Speed<-SPEED_Z?(-SPEED_Z):Turn_Speed);//限幅( (20*100) * 100) Balance_Pwm =balance_UP(pitch,Mechanical_angle,gyroy); //===直立环PID控制 Velocity_Pwm=velocity(Encoder_Left,Encoder_Right,Target_Speed); //===速度环PID控制 Turn_Pwm =Turn_UP(gyroz,Turn_Speed); //===转向环PID控制 Moto1=Balance_Pwm-Velocity_Pwm+Turn_Pwm; //===计算左轮电机最终PWM Moto2=Balance_Pwm-Velocity_Pwm-Turn_Pwm; //===计算右轮电机最终PWM Xianfu_Pwm(); //===PWM限幅 Turn_Off(pitch,12); //===检查角度以及电压是否正常 Set_Pwm(Moto1,Moto2); //===赋值给PWM寄存器 } } /************************************************************************** 函数功能：直立PD控制 入口参数：角度、机械平衡角度（机械中值）、角速度 返回 值：直立控制PWM **************************************************************************/ int balance_UP(float Angle,float Mechanical_balance,float Gyro) { float Bias; int balance; Bias=Angle-Mechanical_balance; //===求出平衡的角度中值和机械相关 balance=balance_UP_KP*Bias+balance_UP_KD*Gyro; //===计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数 return balance; } /************************************************************************** 函数功能：速度PI控制 入口参数：电机编码器的值 返回 值：速度控制PWM **************************************************************************/ int velocity(int encoder_left,int encoder_right,int Target_Speed) { static float Velocity,Encoder_Least,Encoder; static float Encoder_Integral; //=============速度PI控制器=======================// Encoder_Least =(Encoder_Left+Encoder_Right);//-target; //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度 Encoder *= 0.8; //===一阶低通滤波器 Encoder += Encoder_Least*0.2; //===一阶低通滤波器 Encoder_Integral +=Encoder; //===积分出位移 积分时间：10ms Encoder_Integral=Encoder_Integral - Target_Speed; //===接收遥控器数据，控制前进后退 if(Encoder_Integral>10000) Encoder_Integral=10000; //===积分限幅 if(Encoder_Integral<-10000) Encoder_Integral=-10000; //===积分限幅 Velocity=Encoder*velocity_KP+Encoder_Integral*velocity_KI; //===速度控制 if(pitch<-40||pitch>40) Encoder_Integral=0; //===电机关闭后清除积分 return Velocity; } /************************************************************************** 函数功能：转向PD控制 入口参数：电机编码器的值、Z轴角速度 返回 值：转向控制PWM **************************************************************************/ int Turn_UP(int gyro_Z, int RC) { int PWM_out; /*转向约束*/ if(RC==0) Turn_Kd=TURN_KD; //若无左右转向指令，则开启转向约束 else Turn_Kd=0; //若左右转向指令接收到，则去掉转向约束 PWM_out=Turn_Kd*gyro_Z + Turn_KP*RC; return PWM_out; } void Tracking() { TkSensor=0; TkSensor+=(C1<<3); TkSensor+=(C2<<2); TkSensor+=(C3<<1); TkSensor+=C4; } void Get_RC() { static u8 SR04_Counter =0; static float RATE_VEL = 1; float RATE_TURN = 1.6; float LY,RX; //PS2手柄控制变量 int Yuzhi=2; //PS2控制防抖阈值 switch(CTRL_MODE) { case 97: SR04_Counter++; if(SR04_Counter>=20) //100ms读取一次超声波的数据 { SR04_Counter=0; SR04_StartMeasure(); //读取超声波的值 } if(SR04_Distance<=30) { Target_Speed=0,Turn_Speed=40; } else { Target_Speed=30,Turn_Speed=0; } break; case 98://蓝牙模式 if((Fore==0)&&(Back==0)) Target_Speed=0;//未接受到前进后退指令-->速度清零，稳在原地 if(Fore==1) Target_Speed--;//前进1标志位拉高-->需要前进 if(Back==1) Target_Speed++;// /*左右*/ if((Left==0)&&(Right==0)) Turn_Speed=0; if(Left==1) Turn_Speed-=30; //左转 if(Right==1) Turn_Speed+=30; //右转 break; case 99://循迹模式 Tracking(); switch(TkSensor) { case 15: Target_Speed=0; Turn_Speed=0; break; case 9: Target_Speed--; Turn_Speed=0; break; case 2://向右转 Target_Speed--; Turn_Speed=15; break; case 4://向左转 Target_Speed--; Turn_Speed=-15; break; case 8: Target_Speed=-10; Turn_Speed=-80; break; case 1: Target_Speed=-10; Turn_Speed=80; break; } break; case 100://PS2手柄遥控 if(PS2_Plugin) { LY=PS2_LY-128; //获取偏差 RX=PS2_RX-128; //获取偏差 if(LY>-Yuzhi&&LY<Yuzhi) LY=0; //设置小角度的死区 if(RX>-Yuzhi&&RX<Yuzhi) RX=0; //设置小角度的死区 if(Target_Speed>-LY/RATE_VEL) Target_Speed--; else if(Target_Speed<-LY/RATE_VEL) Target_Speed++; Turn_Speed=RX/RATE_TURN; } else { Target_Speed=0,Turn_Speed=0; } break; } } 2、编码电机编码值采集
#include "encoder.h" /************************************************************************** 函数功能：把TIM2初始化为编码器接口模式 入口参数：无 返回 值：无 **************************************************************************/ void Encoder_Init_TIM2(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_ICInitTypeDef TIM_ICInitStructure; GPIO_InitTypeDef GPIO_InitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);//使能定时器4的时钟 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);//使能PB端口时钟 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1; //端口配置 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入 GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOB TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 预分频器 TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD; //设定计数器自动重装值 TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频：不分频 TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;TIM向上计数 TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3 TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICInitStructure.TIM_ICFilter = 10; TIM_ICInit(TIM2, &TIM_ICInitStructure); TIM_ClearFlag(TIM2, TIM_FLAG_Update);//清除TIM的更新标志位 TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE); //Reset counter TIM_SetCounter(TIM2,0); TIM_Cmd(TIM2, ENABLE); } /************************************************************************** 函数功能：把TIM3初始化为编码器接口模式 入口参数：无 返回 值：无 **************************************************************************/ void Encoder_Init_TIM3(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_ICInitTypeDef TIM_ICInitStructure; GPIO_InitTypeDef GPIO_InitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);//使能定时器4的时钟 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);//使能PB端口时钟 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //端口配置 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入 GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOB TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 预分频器 TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD; //设定计数器自动重装值 TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频：不分频 TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;TIM向上计数 TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3 TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICInitStructure.TIM_ICFilter = 10; TIM_ICInit(TIM3, &TIM_ICInitStructure); TIM_ClearFlag(TIM3, TIM_FLAG_Update);//清除TIM的更新标志位 TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE); //Reset counter TIM_SetCounter(TIM3,0); TIM_Cmd(TIM3, ENABLE); } /************************************************************************** 函数功能：单位时间读取编码器计数 入口参数：定时器 返回 值：速度值 **************************************************************************/ int Read_Encoder(u8 TIMX) { int Encoder_TIM; switch(TIMX) { case 2: Encoder_TIM= (short)TIM2 -> CNT; TIM2 -> CNT=0; break; case 3: Encoder_TIM= (short)TIM3 -> CNT; TIM3 -> CNT=0; break; default: Encoder_TIM=0; } return Encoder_TIM; } 3、PWM配置
#include "pwm.h" //PWM输出初始化 //arr：自动重装值 //psc：时钟预分频数 //TIM1_PWM_Init(7199,0);//PWM频率=72000/(7199+1)=10Khz void TIM1_PWM_Init(u16 arr,u16 psc) { GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);// RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA , ENABLE); //使能GPIO外设时钟使能 //设置该引脚为复用输出功能,输出TIM1 CH1 CH4的PWM脉冲波形 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_11; //TIM_CH1 //TIM_CH4 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOA, &GPIO_InitStructure); TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值 TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值 不分频 TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式 TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位 TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式1 TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能 TIM_OCInitStructure.TIM_Pulse = 0; //设置待装入捕获比较寄存器的脉冲值 TIM_OCInitStructure.TIM_Pulse = arr >> 1; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性:TIM输出比较极性高 TIM_OC1Init(TIM1, &TIM_OCInitStructure); //根据TIM_OCInitStruct中指定的参数初始化外设TIMx TIM_OC4Init(TIM1, &TIM_OCInitStructure); //根据TIM_OCInitStruct中指定的参数初始化外设TIMx TIM_CtrlPWMOutputs(TIM1,ENABLE); //MOE 主输出使能 TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable); //CH1预装载使能 TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable); //CH4预装载使能 TIM_ARRPreloadConfig(TIM1, ENABLE); //使能TIMx在ARR上的预装载寄存器 TIM_Cmd(TIM1, ENABLE); //使能TIM1 } 4、蓝牙控制
#include "usart2.h" /************************************************************************** 函数功能：串口2初始化 入口参数： bound:波特率 返回 值：无 **************************************************************************/ void uart2_init(u32 bound) { //GPIO端口设置 GPIO_InitTypeDef GPIO_InitStructure; USART_InitTypeDef USART_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //使能UGPIOB时钟 RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); //使能USART2时钟 //USART2_TX GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2; //PA2 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出 GPIO_Init(GPIOA, &GPIO_InitStructure); //USART2_RX GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;//PA3 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//浮空输入 GPIO_Init(GPIOA, &GPIO_InitStructure); //USART 初始化设置 USART_InitStructure.USART_BaudRate = bound;//串口波特率 USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式 USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位 USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位 USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制 USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //收发模式 USART_Init(USART2, &USART_InitStructure); //初始化串口2 USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);//开启串口接受中断 USART_Cmd(USART2, ENABLE); //使能串口2 } /************************************************************************** 函数功能：串口2接收中断 入口参数：无 返回 值：无 **************************************************************************/ u8 Fore,Back,Left,Right; void USART2_IRQHandler(void) { int Uart_Receive; if(USART_GetITStatus(USART2,USART_IT_RXNE)!=RESET)//接收中断标志位拉高 { Uart_Receive=USART_ReceiveData(USART2);//保存接收的数据 BluetoothCMD(Uart_Receive); } } void BluetoothCMD(int Uart_Receive) { switch(Uart_Receive) { case 90://停止 Fore=0,Back=0,Left=0,Right=0; break; case 65://前进 Fore=1,Back=0,Left=0,Right=0; break; case 72://左前 Fore=1,Back=0,Left=1,Right=0; break; case 66://右前 Fore=1,Back=0,Left=0,Right=1; break; case 71://左转 Fore=0,Back=0,Left=1,Right=0; break; case 67://右转 Fore=0,Back=0,Left=0,Right=1; break; case 69://后退 Fore=0,Back=1,Left=0,Right=0; break; case 70://左后,向右旋 Fore=0,Back=1,Left=0,Right=1; break; case 68://右后，向左旋 Fore=0,Back=1,Left=1,Right=0; break; default://停止 Fore=0,Back=0,Left=0,Right=0; break; } } void Uart2SendByte(char byte) //串口发送一个字节 { USART_SendData(USART2, byte); //通过库函数 发送数据 while( USART_GetFlagStatus(USART2,USART_FLAG_TC)!= SET); //等待发送完成。 检测 USART_FLAG_TC 是否置1； //见库函数 P359 介绍 } void Uart2SendBuf(char *buf, u16 len) { u16 i; for(i=0; i<len; i++)Uart2SendByte(*buf++); } void Uart2SendStr(char *str) { u16 i,len; len = strlen(str); for(i=0; i<len; i++)Uart2SendByte(*str++); } 5、中断处理函数
void EXTI9_5_IRQHandler(void) { static u8 Voltage_Counter=0; if(PBin(5)==0) { EXTI->PR=1<<5; //清除LINE5上的中断标志位 mpu_dmp_get_data(&pitch,&roll,&yaw); //得到欧拉角（姿态角）的数据 MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据 Encoder_Left=Read_Encoder(2); //读取编码器的值，保证输出极性一致 Encoder_Right=-Read_Encoder(3); //读取编码器的值 Led_Flash(100); Voltage_Counter++; if(Voltage_Counter==20) //100ms读取一次电压 { Voltage_Counter=0; Voltage=Get_battery_volt(); //读取电池电压 } if(KEY_Press(100)) //长按按键切换模式并触发模式切换初始化 { if(++CTRL_MODE>=101) CTRL_MODE=97; Mode_Change=1; } Get_RC(); Target_Speed=Target_Speed>SPEED_Y?SPEED_Y:(Target_Speed<-SPEED_Y?(-SPEED_Y):Target_Speed);//限幅 Turn_Speed=Turn_Speed>SPEED_Z?SPEED_Z:(Turn_Speed<-SPEED_Z?(-SPEED_Z):Turn_Speed);//限幅( (20*100) * 100) Balance_Pwm =balance_UP(pitch,Mechanical_angle,gyroy); //===直立环PID控制 Velocity_Pwm=velocity(Encoder_Left,Encoder_Right,Target_Speed); //===速度环PID控制 Turn_Pwm =Turn_UP(gyroz,Turn_Speed); //===转向环PID控制 Moto1=Balance_Pwm-Velocity_Pwm+Turn_Pwm; //===计算左轮电机最终PWM Moto2=Balance_Pwm-Velocity_Pwm-Turn_Pwm; //===计算右轮电机最终PWM Xianfu_Pwm(); //===PWM限幅 Turn_Off(pitch,12); //===检查角度以及电压是否正常 Set_Pwm(Moto1,Moto2); //===赋值给PWM寄存器 } }七、PCB板设计
八、代码开源
1、寄存器版本
链接：https://pan.baidu.com/s/1NlMHsgMF2Cu8sz955n27Eg?pwd=zxf1
提取码：zxf1
--来自百度网盘超级会员V2的分享
2、HAL库版本
链接：https://pan.baidu.com/s/1rW5M7Dz-TK4IWJxNp57mBw?pwd=zxf1
提取码：zxf1
--来自百度网盘超级会员V2的分享
