平衡小车项目学习过程
平衡小车项目学习过程
硬件准备
硬件占用的GPIO的框架：
-
编码器1——PA0/PA1—TIM2
-
编码器2——PB6/PB7—TIM4
-
电机1——PB12/PB13
-
电机2——PB14/PB15
-
PWM1——PA8(TIM1_CH1)
-
PWM2——PA11(TIM1_CH4)
-
MPU6050中断引脚——PB5
-
MPU6050所用IIC——PB3/PB4
-
蓝牙通信——PB10(USART3_TX) / PB11(USART3_RX)
-
超声波模块——PA2/PA3
-
2.4G无线模块——PA9/PA10 /PA5(SPI1_SCK) /PA6(SPI1_MISO)/PA7(SPI1_MOSI)
软件编程思路
Encoder.c（编码器）
GPIO初始化
定时器初始化
设置编码器模式
清除标志位
中断配置
清零计数
开定时器
代码：
void Encoder_TIM2_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_ICInitTypeDef TIM_ICInitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//开启时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IN_FLOATING;//初始化GPIO--PA0、PA1
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_0 |GPIO_Pin_1;
GPIO_Init(GPIOA,&GPIO_InitStruct);
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定时器。
TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStruct.TIM_Period=65535;
TIM_TimeBaseInitStruct.TIM_Prescaler=0;
TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStruct);
TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置编码器模式
TIM_ICStructInit(&TIM_ICInitStruct);//初始化输入捕获
TIM_ICInitStruct.TIM_ICFilter=10; //滤波器
TIM_ICInit(TIM2,&TIM_ICInitStruct);
TIM_ClearFlag(TIM4, TIM_FLAG_Update);//清除TIM的更新标志位
TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);//配置溢出更新中断标志位
TIM_SetCounter(TIM2,0);//清零定时器计数值
TIM_Cmd(TIM2,ENABLE);//开启定时器
}编码器原理
TIM_EncoderInterfaceConfig (TIM2,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
参数：
TIM2 ：定时器2
TIM_EncoderMode_TI12： T1 T2的每个跳变沿均计数
TIM_ICPolarity_Rising：不反向
TIM_ICPolarity_Rising：不反向
根据两个输入信号（TI1&TI2）的跳变顺序，产生了计数脉冲和方向信号。
依据两个输入信号的跳变顺序，计数器向上或向下计数，同时硬件对TIMx_CR1寄存器的DIR位进行相应的设置。
不管计数器是依靠TI1计数、依靠TI2计数或者同时依靠TI1和TI2计数。
在任一输入端(TI1或者TI2)的跳变都会重新计算DIR位。
【正反转】
正转：T1超前T2相位90度。
反转：T1滞后T2相位90度。
【模式】
TI1模式：在T1的所有边沿 计数。
TI2模式：在T2的所有边沿 计数。
TI12模式：在T1和T2的所有边沿 计数。
PWM.c
GPIO初始化
定时器初始化
输出比较模式初始化
TIM_OCInitStruct.TIM_Pulse=0; //占空比为零？？？
函数TIM_SetCompare1(TIM_TypeDef* TIMx, uint16_t Compare1)的调用时，前一项参数为TIMx,TIMx中的x可以取1到17且除了6、7的数,Compare1是用于与TIMx比较的数，相当于用TIMx的一个周期的时间减去这个Compare1，使得TIMx的周期从后面开始的Compare1的时间为TIMx的前部分时间的反向。即若前部分时间为高电平，则Compare1段所在时间为低电平。若前部分时间为低电平，则Compare1段所在时间为高电平
MOE主输出使能(高级定时器必须开启)
通道预装载使能
使能定时器在ARR寄存器上的预装载寄存器
代码：
void PWM_Init_TIM1(u16 Psc,u16 Per)
{
GPIO_InitTypeDef GPIO_InitStruct;
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
TIM_OCInitTypeDef TIM_OCInitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_TIM1 | RCC_APB2Periph_AFIO,ENABLE);//开启时钟
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_AF_PP;//初始化GPIO--PA8、PA11为复用推挽输出
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_8 |GPIO_Pin_11;
GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&GPIO_InitStruct);
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);//初始化定时器。
TIM_TimeBaseInitStruct.TIM_ClockDivision=TIM_CKD_DIV1;//设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStruct.TIM_Period=Per;//设置在下一个更新事件装入活动的自动重装载寄存器周期的值
TIM_TimeBaseInitStruct.TIM_Prescaler=Psc;//设置用来作为TIMx时钟频率除数的预分频值 不分频
TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);/*【2】*///TIM2
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式1
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_Pulse = 0; //设置待装入捕获/比较寄存器的脉冲值
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);/*【3】*///ENABLE//OC1预装载寄存器使能
TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);//ENABLE//OC4预装载寄存器使能
TIM_ARRPreloadConfig(TIM1,ENABLE);//TIM1在ARR上预装载寄存器使能
TIM_Cmd(TIM1,ENABLE);//开定时器。
}moto.c(电机)
GPIO初始化
PWM赋值函数
PWM限幅
绝对值函数
工作频率10KHZ
代码：
/*电机初始化函数*/
void Motor_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//开启时钟
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_Out_PP;//初始化GPIO--PB12、PB13、PB14、PB15为推挽输出
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_12 |GPIO_Pin_13 |GPIO_Pin_14 |GPIO_Pin_15;
GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&GPIO_InitStruct);
}
/*限幅函数*/
void Limit(int *motoA,int *motoB)
{
//===PWM满幅是7200(自动重装载的值) 限制在7000
if(*motoA>PWM_MAX)*motoA=PWM_MAX;
if(*motoA<PWM_MIN)*motoA=PWM_MIN;
if(*motoB>PWM_MAX)*motoB=PWM_MAX;
if(*motoB<PWM_MIN)*motoB=PWM_MIN;
}
/*绝对值函数*/
int GFP_abs(int p)
{
int q;
q=p>0?p:(-p);
return q;
}
/*赋值函数*/
/*入口参数：PID运算完成后的最终PWM值*/
void Load(int moto1,int moto2)//moto1=-200：反转200个脉冲
{
//1.研究正负号，对应正反转
if(moto1>0) Ain1=1,Ain2=0;//正转
else Ain1=0,Ain2=1;//反转
//2.研究PWM值
TIM_SetCompare1(TIM1,GFP_abs(moto1));//TIM1->CCR1
if(moto2>0) Bin1=1,Bin2=0;
else Bin1=0,Bin2=1;
TIM_SetCompare4(TIM1,GFP_abs(moto2));//写入的数值会被立即传输至当前寄存器中。
}
Exit.c（MPU6050中断）
GPIO初始化
EXTI外部中断初始化
代码：
void MPU6050_EXTI_Init(void)
{
EXTI_InitTypeDef EXTI_InitStruct;
GPIO_InitTypeDef GPIO_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO,ENABLE);//开启时钟
GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IPU;/**【1】**///GPIO_Mode_AF_PP
GPIO_InitStruct.GPIO_Pin=GPIO_Pin_5;//PB5配置为上拉输入
GPIO_Init(GPIOB,&GPIO_InitStruct);
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);//
EXTI_InitStruct.EXTI_Line=EXTI_Line5;
EXTI_InitStruct.EXTI_LineCmd=ENABLE;
EXTI_InitStruct.EXTI_Mode=EXTI_Mode_Interrupt;
EXTI_InitStruct.EXTI_Trigger=EXTI_Trigger_Falling;
EXTI_Init(&EXTI_InitStruct);
}control.c
直立环 速度环 转向环的编写
中断函数服务函数
代码：
void EXTI9_5_IRQHandler(void)
{
int PWM_out;
if(EXTI_GetITStatus(EXTI_Line5)!=0)//一级判定
{
if(PBin(5)==0)//二级判定
{
EXTI_ClearITPendingBit(EXTI_Line5);//清除中断标志位
//1、采集编码器数据&MPU6050角度信息。
Encoder_Left=-Read_Encoder(2);//电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反。
Encoder_Right=Read_Encoder(4);
mpu_dmp_get_data(&Pitch,&Roll,&Yaw); //角度
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //陀螺仪
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //加速度
UltrasonicWave_Voltage_Counter++;
if(UltrasonicWave_Voltage_Counter==3) //===100ms读取一次超声波的数据以及电压
{
UltrasonicWave_StartMeasure(); //===读取超声波的值
}
if(UltrasonicWave_Distance<15&&UltrasonicWave_Distance>3)
{
Fore=1;
Left=1;
}
/*前后*/
if((Fore==0 || fore==0)&&(Back==0 || back==0))Target_Speed=0;//未接受到前进后退指令-->速度清零，稳在原地
if(Fore==1|| fore==1)Target_Speed-=15;//前进1标志位拉高-->需要前进0.1
if(Back==1||back==1)Target_Speed+=15;//
/*********************************************************************************************/
Target_Speed=Target_Speed>SPEED_Y?SPEED_Y:(Target_Speed<-SPEED_Y?(-SPEED_Y):Target_Speed);//限幅
/*左右*/
if((Left==0 || left==0)&&(Right==0 || right==0))Turn_Speed=0;
if(Left==1 || left==1)Turn_Speed+=40; //左转10
if(Right==1||right==1)Turn_Speed-=40; //右转
Turn_Speed=Turn_Speed>SPEED_Z?SPEED_Z:(Turn_Speed<-SPEED_Z?(-SPEED_Z):Turn_Speed);//限幅( (20*100) * 100 )
// /*转向约束*/
// if(UltrasonicWave_Distance>0)Turn_Kd=-0.6;//若无左右转向指令，则开启转向约束
// else if(UltrasonicWave_Distance<13&&UltrasonicWave_Distance>3)Turn_Kd=0;//若左右转向指令接收到，则去掉转向约束
/*转向约束*/
if((Left==0|| left==0)&&(Right==0|| right==0))Turn_Kd=-0.6;//若无左右转向指令，则开启转向约束
else if((Left==1|| left==1)||(Right==1|| right==1)||(UltrasonicWave_Distance<15&&UltrasonicWave_Distance>5))Turn_Kd=0;//若左右转向指令接收到，则去掉转向约束
//2、将数据压入闭环控制中，计算出控制输出量。
Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); //速度环
Vertical_out=Vertical(Velocity_out+Med_Angle,Pitch,gyroy); //直立环
Turn_out=Turn(gyroz,Turn_Speed); //转向环
PWM_out=Vertical_out;//最终输出
//3、把控制输出量加载到电机上，完成最终的的控制。
MOTO1=PWM_out-Turn_out;//左电机
MOTO2=PWM_out+Turn_out;//右电机
Xianfu_Pwm(); //PWM限幅
Set_Pwm(MOTO1,MOTO2); //加载到电机上。
Stop(&Med_Angle,&Pitch);//安全检测
AbnormalSpinDetect(MOTO1,MOTO2,&Med_Angle,&Pitch);
if(AbnormalSpinFlag == 1)
Set_Pwm(0,0);
LandingDetect(&Med_Angle,&Pitch);
}
}
}NVIC:
代码：
void NVIC_Config(void)
{
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //设置NVIC中断分组2:2位抢占优先级，2位响应优先级
//外部中断5优先级配置也就是MPU6050 INT引脚的配置///因为是控制中断，故此优先级应是最高。
NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn; //使能外部中断通道
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x00; //抢占优先级0，
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x01; //子优先级1
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //使能外部中断通道
NVIC_Init(&NVIC_InitStructure);
//定时器3中断优先级配置也就是超声波计时的定时器的配置//
NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn; //TIM3中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x01; //先占优先级1级
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x01; //从优先级3级
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道被使能
NVIC_Init(&NVIC_InitStructure); //根据NVIC_InitStruct中指定的参数初始化外设NVIC寄存器
//外部中断2优先级配置也就是超声波引脚的配置//
NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn; //使能外部中断通道
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x01; //抢占优先级2
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x02; //子优先级2
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //使能外部中断通道
NVIC_Init(&NVIC_InitStructure); //根据NVIC_InitStruct中指定的参数初始化外设NVIC寄存器
//Usart1 NVIC 中断优先级配置
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x02 ;//抢占优先级2
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x02; //抢占优先级2
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
NVIC_Init(&NVIC_InitStructure); //根据NVIC_InitStruct中指定的参数初始化外设NVIC寄存器USART1
//Usart3 NVIC 中断优先级配置也就是蓝牙串口配置//
NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x02 ;//抢占优先级2
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x02; //子优先级2
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
NVIC_Init(&NVIC_InitStructure); //根据指定的参数初始化VIC寄存器
}
UltrasonicWave.c（超声波）
开始测距，发送一个>10us的脉冲，然后测量返回的高电平时间 这里的测量是在PA2中断中用定时器3测量回响信号的高电平时间
//这里定时器3的Counter Register value每加1代表过了0.5s 具体看源代码
void EXTI2_IRQHandler(void)
{
delay_us(10); //延时10us
if(EXTI_GetITStatus(EXTI_Line2) != RESET)
{
TIM_SetCounter(TIM3,0);
TIM_Cmd(TIM3, ENABLE); //开启时钟
while(GPIO_ReadInputDataBit(GPIOA,ECHO_PIN)); //等待低电平
TIM_Cmd(TIM3, DISABLE); //定时器3失能
UltrasonicWave_Distance=TIM_GetCounter(TIM3)*1.7; //计算距离（我觉得是UltrasonicWave_Distance=TIM_GetCounter(TIM3)/2*340/2*100 不过这个代码确实显示是错的，所以希望有人解答一下）
// if(UltrasonicWave_Distance>0)
// {
// printf("distance:%f cm",UltrasonicWave_Distance);
// }
//
EXTI_ClearITPendingBit(EXTI_Line2); //清除EXTI2线路挂起位
}
}rf2G4.c
因为STM32F103C8T6定时器只有TIME1、2、 3、 4都用完了所以这里的定时采用了片内的滴答定时器 在中断中对命令有效标志清零
具体看源码
PID
PID函数（位置式PID） 离散式不用
直立环（V）
速度环（V）
转向环（X） 没有控制小车移动 只是直立
PID控制，就是对偏差进行比例、积分和微分的控制。
PID由3个单元组成，分别是比例（P）单元、积分（I）单元、微分（D）单位。
工程中P必然存在，在P的基础上又有如PD控制器、PI控制器、PID控制器等。
比例项：提高响应速度，减小静差。
积分项：消除稳态误差。只要有偏差，我就积分，有一丁点偏差，我也会积分。积积，就会非常大。直到你偏差变为0.
微分项：减小震荡以及超调。阻尼力
Pwm=Kp*e(k)+Ki*∑e(k)+Kd*[e(k)-e(k-1)]
Kp*e(k)
Ki*∑e(k)
Kd*[e(k)-e(k-1)]
分析理解
P比例项
大鱼电子PID讲解1
大鱼电子PID讲解2
位置PID
1.理论分析
位置闭环控制就是根据编码器的脉冲累加测量电机的位置信息，并与目标值进行比较，得到控制偏差，
然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。
2.公式
Pwm=Kp*e(k)+Ki*∑e(k)+Kd*[e(k)-e(k-1)]
-
e(k)：本次偏差
-
e(k-1)：上一次的偏差
-
∑e(k)：e(k)以及之前的偏差的累积和;其中k为1,2,k;
-
Pwm: 代表输出
3.结构框图
4.C语言实现
int Position_PID (int Encoder ,int Target) //Encoder 反馈值 Target 目标值
{
static float Bias, Pwm, Integral_bias,Last_Bias;// Bias 偏差 Integral_bias 积分
Bias=Encoder-Target; //计算偏差
Integral_bias+=Bias; //求出偏差的积分
Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias); //位置式PID控制器
Last_Bias=Bias; //保存上一次偏差
return Pwm; //输出
}直立环
- 理论
小车往那边倒，车轮就往哪边开，既可以保持车子的平衡。
-
公式
a=b1*θ+b2*θ**＇**; ——> 比例微分控制【PDout=Kp*Angle+Kd*( Angle-Angle_last)】
-
结构框图
直立环：让小车角度趋近0； PD
速度环：让电机速度趋近0； PI 积分作用： 尽量消除稳态误差 电机的轮子死区大 硬件轮子晃得厉害
速度环
串级PID（内环 外环）
速度环输入：1.给定速度。2.速度反馈。
输出：角度值（直立环的期望速度输入）
直立环输入：1.给定角度（速度环输出）。2.角度反馈
输出：PWM（直接控制小车）
Vertical_out=Kp1*( real_Angle- expect_Angle)+Kd*D( real_Angle- expect_Angle) //直立PD控制器
Velocity_out =Kp2*(Encoder_ real- Encoder_ expect)+Ki*S(Encoder_ real- Encoder_ expect) //速度PI控制器
（NOTE：（1）Velocity_out = expect_Angle.（2）Kp1：Vertical_Kp.（3）Kp2：Velocity_Kp.）
【中文】
直立环输出=Kp1*（真实角度-期望角度+机械中值）+Kd*角度偏差的微分 //角度偏差=真实角度-期望角度
速度环输出=Kp2*（反馈编码器值-期望编码器值）+Ki*编码器偏差的积分 //编码器偏差=反馈编码器值-期望编码器值
（NOTE：（1）速度环输出=直立环的期望角度。（2）Kp1：直立环Kp。（3）Kp2：速度环Kp。）
合并推导：
直立环输出 Vertical_out=Kp1*(θ_r-θ_e)+Kd* (θ_r-θ_e)＇
速度环输出Velocity_out=Kp2*(E_r- E_e)+Ki*Σ(E_r- E_e)
因为：θ_e = Velocity_out
所以直立环输出（PWM（直接控制小车））
Vertical_out =
= Kp1*{ θ_r-[ Kp2*(E_r- E_e)+Ki*Σ(E_r- E_e) ]}+Kd*(θ_r-θ_e)＇
= Kp1* θ_r- Kp1* Kp2*(E_r- E_e)- Kp1*Ki\ *Σ(E_r- E_e) +Kd*(θ_r-θ_e)＇
= Kp1* θ_r+ Kd*(θ_r-θ_e)＇ Kp1* [Kp2*(E_r- E_e)+ Ki *Σ(E_r- E_e)]
【中文】
=Kp1*真实角度+ Kd*角度偏差的微分-Kp1* [Kp2*编码器偏差- Ki *编码器偏差的积分]
直立环(角度偏差变为真实角度)-K p1*速度环
直立环C语言实现：
/*********************
直立环PD控制器：Kp*Ek+Kd*Ek_D
入口：期望角度、真实角度、真实角速度
出口：直立环输出
*********************/
int Vertical(float Med,float Angle,float gyro_Y)
{
int PWM_out;
PWM_out=Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
return PWM_out;
}速度环C语言实现：
/*********************
速度环PI：Kp*Ek+Ki*Ek_S
*********************/
int Velocity(int encoder_left,int encoder_right)
{
static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;//【2】
float a=0.7;//【3】
//1.计算速度偏差
Encoder_Err=(encoder_left+encoder_right)-0;//舍去误差
//2.对速度偏差进行低通滤波 避免速度环对直立环的影响
//low_out=(1-a)*Ek+a*low_out_last;
EnC_Err_Lowout=(1-a)*Encoder_Err+a*EnC_Err_Lowout_last;//使得波形更加平滑，滤除高频干扰，防止速度突变。
EnC_Err_Lowout_last=EnC_Err_Lowout;//防止速度过大的影响直立环的正常工作。
//3.对速度偏差积分，积分出位移
Encoder_S+=EnC_Err_Lowout;//【4】
//4.积分限幅
Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
//5.速度环控制输出计算
PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;//【5】
return PWM_out;
}转向环环C语言实现：
/*********************
转向环：系数*Z轴角速度
*********************/
int Turn(int gyro_Z)
{
int PWM_out;
//这不是一个严格的PD控制器，Kd针对的是转向的约束，但Kp针对的是遥控的转向。
PWM_out=Turn_Kd*gyro_Z + Turn_Kp*RC;
return PWM_out;
}即：
串级输出OUT=
Kp1* real_Angle+Kd* D( real_Angle- expect_Angle)
Kp1*[Kp2*(Encoder_ real- Encoder_ expect)+Ki* S(Encoder_ real- Encoder_ expect)]
代码视频讲解 ——转自UP主天下行走
调参步骤
-
确立机械中值。
-
直立环（内环）—Kp极性、Kp大小。Kd极性、Kd大小。
-
速度环（外环）——Kp&Ki极性、Kp&Ki大小。
-
转向环——系数极性、系数大小。
**机械中值：**把平衡小车放在地面上，从前向后以及从后向前绕电机轴旋转平衡小车，两次的向另一边倒下的角度的中值，就是机械中值。
大鱼电子调参
直立环
Kp极性：
极性错误：小车往哪边倒，车轮就往反方向开，会使得小车加速倒下。
极性正确：小车往哪边倒，车轮就往哪边开，以保证小车有直立的趋势。
Kp大小：
Kp一直增加，直到出现大幅低频震荡。
Kd极性：
极性错误：拿起小车绕电机轴旋转，车轮反向转动，无跟随。
极性正确：拿起小车绕电机轴旋转，车轮同向转动，有跟随。
Kd大小：
Kd一直增加，直到出现高频震荡。
直立环调试完毕后，对所有确立的参数乘以0.6作为最终参数。
原因：之前得到的参数都是Kp、Kd最大值，根据工程经验平衡小车的理想参数为最大参数乘以0.6求得。
结果：乘以0.6后，小车的抖动消失，但同时直立效果也变差。待下面加入速度环就能得到更好的性能。
速度环
速度环参数调节前注意：
一、
在调试【速度环参数极性】时：需要去掉（注释掉）【直立环运算】
在调试【速度环参数大小】时：再次引入（取消注释）【直立环运算】
二、
【转向环运算】始终是去掉（注释）的一个状态。若转向环已提前将参数调试好，则未注释也影响不大。
KP&KI
线性关系、Ki=(1/200)*Kp、仅调Kp即可。
KP&KI 极性：
极性错误：手动转动其中一个车轮，另一车轮会以同样速度反向旋转——典型负反馈。
极性正确：手动转动其中一个车轮，两个车伦会同向加速，直至电机最大速度——典型正反馈。
KP&KI 大小：
增加Kp&Ki，直至：小车保持平衡的同时，速度接近于零。且回位效果较好。
转向环
KP 极性：
极性错误：拿起小车，并将小车绕Z轴旋转，两车轮旋转的趋势与小车旋转趋势一致——典型正反馈。
极性正确：拿起小车，并将小车绕Z轴旋转，两车轮旋转的趋势与小车旋转趋势相反——典型负反馈。
KP 大小：
加大Kp，直至走直线效果较好，且无剧烈抖动。
同向转动，有跟随。
Kd大小：
Kd一直增加，直到出现高频震荡。
直立环调试完毕后，对所有确立的参数乘以0.6作为最终参数。
原因：之前得到的参数都是Kp、Kd最大值，根据工程经验平衡小车的理想参数为最大参数乘以0.6求得。
结果：乘以0.6后，小车的抖动消失，但同时直立效果也变差。待下面加入速度环就能得到更好的性能。
速度环
速度环参数调节前注意：
一、
在调试【速度环参数极性】时：需要去掉（注释掉）【直立环运算】
在调试【速度环参数大小】时：再次引入（取消注释）【直立环运算】
二、
【转向环运算】始终是去掉（注释）的一个状态。若转向环已提前将参数调试好，则未注释也影响不大。
KP&KI
线性关系、Ki=(1/200)*Kp、仅调Kp即可。
KP&KI 极性：
极性错误：手动转动其中一个车轮，另一车轮会以同样速度反向旋转——典型负反馈。
极性正确：手动转动其中一个车轮，两个车伦会同向加速，直至电机最大速度——典型正反馈。
KP&KI 大小：
增加Kp&Ki，直至：小车保持平衡的同时，速度接近于零。且回位效果较好。
转向环
KP 极性：
极性错误：拿起小车，并将小车绕Z轴旋转，两车轮旋转的趋势与小车旋转趋势一致——典型正反馈。
极性正确：拿起小车，并将小车绕Z轴旋转，两车轮旋转的趋势与小车旋转趋势相反——典型负反馈。
KP 大小：
加大Kp，直至走直线效果较好，且无剧烈抖动。
参考文献
-
喵呜实验室资料
-
大鱼电子资料
-
平衡小车之家资料
-
B站UP主 天下行走
万分感谢上面提到的所有老师与前辈