基于STM32的双轮平衡小车开发_平衡小车代码

目录

一、硬件设计

 1、硬件选择

2、硬件框图

3、分模块说明以及相关代码

3.1、电机驱动TB6612

3.2、N20编码器减速电机(我的是6V300转)

3.3、MPU6050

二、软件设计

1、软件框图

2、PID算法

2.1、简介

2.2、PID调参

2.3、相关代码

3、main.c代码

三、代码资料

---

一、硬件设计

 1、硬件选择

2、硬件框图

3、分模块说明以及相关代码

3.1、电机驱动TB6612

具体原理以及逻辑真值表：

通过接收PWM信号，根据输入信号的逻辑电平确定电机的运动方向，可以使电机正转、反转或停止。

具体接线：

代码部分（含PWM）：

#define Left_Motor_turn_Positive            GPIO_SetBits(GPIOA,GPIO_Pin_4),GPIO_ResetBits(GPIOA,GPIO_Pin_5)    //左电机正转
#define Left_Motor_turn_Negative            GPIO_SetBits(GPIOA,GPIO_Pin_5),GPIO_ResetBits(GPIOA,GPIO_Pin_4)    //左电机反转
 
#define Right_Motor_turn_Positive           GPIO_SetBits(GPIOA,GPIO_Pin_0),GPIO_ResetBits(GPIOA,GPIO_Pin_1)    //右电机正转
#define Right_Motor_turn_Negative           GPIO_SetBits(GPIOA,GPIO_Pin_1),GPIO_ResetBits(GPIOA,GPIO_Pin_0)    //右电机反转
 
void PWM_Init(void)
{
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
 
    GPIO_InitTypeDef GPIO_InitStruct;
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
    TIM_OCInitTypeDef TIM_OCInitStruct;
    
    //PWM
    GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_AF_PP;              //复用推挽输出
    GPIO_InitStruct.GPIO_Pin   = GPIO_Pin_8|GPIO_Pin_9;         //TIM1_CH1/2
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA,&GPIO_InitStruct);
    
    //输出IO
    GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_Out_PP;                              //推挽输出
    GPIO_InitStruct.GPIO_Pin   = GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_4|GPIO_Pin_5;
    GPIO_Init(GPIOA,&GPIO_InitStruct);
    GPIO_ResetBits(GPIOA,GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_4|GPIO_Pin_5);          //默认低电平(停止)
 
    TIM_InternalClockConfig(TIM1);              //选择时基单元
    TIM_TimeBaseInitStruct.TIM_ClockDivision     = TIM_CKD_DIV1;
    TIM_TimeBaseInitStruct.TIM_CounterMode       = TIM_CounterMode_Up;
    TIM_TimeBaseInitStruct.TIM_Period            = 7200-1;      //ARR重装载值  1khz
    TIM_TimeBaseInitStruct.TIM_Prescaler         = 10-1;        //PSC预分频值
    TIM_TimeBaseInitStruct.TIM_RepetitionCounter = 0;
    TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);
 
    TIM_OCStructInit(&TIM_OCInitStruct);
    TIM_OCInitStruct.TIM_OCMode      = TIM_OCMode_PWM1;                    //PWM1模式
    TIM_OCInitStruct.TIM_OCPolarity  = TIM_OCPolarity_High;                //有效电平为高电平
    TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;             //输出使能
    TIM_OCInitStruct.TIM_Pulse       = 0;                                  //CCR比较值
    
    TIM_OC1Init(TIM1,&TIM_OCInitStruct);        //通道1,2初始化
    TIM_OC2Init(TIM1,&TIM_OCInitStruct);
 
    TIM_CtrlPWMOutputs(TIM1,ENABLE);    //MOE主输出使能
    TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable);   //CH1预装载使能
    TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Enable);   //CH2预装载使能
    TIM_ARRPreloadConfig(TIM1, ENABLE);                 //使能TIMx在ARR上的预装载寄存器
 
    TIM_Cmd(TIM1,ENABLE);               //开启定时器
}
 
//设置比较值
void Set_compare(uint8_t Oc,int16_t count)
{
    if(Oc == 1)
    {
        if(count<0)
        {
            count *= -1;
            Left_Motor_turn_Negative;
        }
        else
        {
            Left_Motor_turn_Positive;
        }
        TIM_SetCompare1(TIM1,count);
    }
    if(Oc == 2)
    {
        if(count<0)
        {
            count *= -1;
            Right_Motor_turn_Positive;
        }
        else
        {
            Right_Motor_turn_Negative;
        }
        TIM_SetCompare2(TIM1,count);
    }
}

3.2、N20编码器减速电机(我的是6V300转)

编码器测速原理：电机转动时会产生A、B两相的脉冲信号，且这两路脉冲信号的相位差为90度（即正交），然后通过STM32定时器的编码器接口可接收增量（正交）编码器的信号，根据编码器旋转产生的正交信号脉冲，自动控制CNT自增或自减，从而指示编码器的位置、旋转方向和旋转速度。

脉冲次数计算方法：根据编码器的不同转动一圈产生的信号量也不同，以电机转动一圈产生7个信号，电机减速比为1:50为例子，那么转动一圈产生的脉冲信号为7*50=350个，再通过STM32的编码器接口的四倍频，也就是一圈会产生1400个脉冲。

具体接线：

代码部分：

int16_t L_Temp;
int16_t R_Temp;
 
void Encoder_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct;
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
    TIM_ICInitTypeDef TIM_ICInitStruct;
    
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);             //L_Encoder
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
 
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);             //R_Encoder
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
 
    GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_IN_FLOATING;             //浮空输入
    GPIO_InitStruct.GPIO_Pin   = GPIO_Pin_6 | GPIO_Pin_7;           //L:PA6 PA7||R:PB6 PB7
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA,&GPIO_InitStruct);
    GPIO_Init(GPIOB,&GPIO_InitStruct);
 
    TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);
    TIM_TimeBaseInitStruct.TIM_ClockDivision     = TIM_CKD_DIV1;            //无用 被编码器托管
    TIM_TimeBaseInitStruct.TIM_CounterMode       = TIM_CounterMode_Up;      //无用 被编码器托管
    TIM_TimeBaseInitStruct.TIM_Period            = 65536-1;                 //ARR
    TIM_TimeBaseInitStruct.TIM_Prescaler         = 1-1;                     //PSC
    TIM_TimeBaseInitStruct.TIM_RepetitionCounter = 0;                       //关闭重复次数计数器
    TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStruct);
 
 
    TIM_ICStructInit(&TIM_ICInitStruct);                              //结构体配置不完整时，默认给初始值
    TIM_ICInitStruct.TIM_Channel     = TIM_Channel_1;                 //L_A相
    TIM_ICInitStruct.TIM_ICFilter    = 0xF;;                          //滤波
    TIM_ICInit(TIM3,&TIM_ICInitStruct);
    TIM_ICInitStruct.TIM_Channel     = TIM_Channel_2;                 //L_B相
    TIM_ICInitStruct.TIM_ICFilter    = 0xF;;
    TIM_ICInit(TIM3,&TIM_ICInitStruct);
 
 
    TIM_ICInitStruct.TIM_Channel     = TIM_Channel_1;//R_A相
    TIM_ICInitStruct.TIM_ICFilter    = 0xF;
    TIM_ICInit(TIM4,&TIM_ICInitStruct);
    TIM_ICInitStruct.TIM_Channel     = TIM_Channel_2;//R_B相
    TIM_ICInitStruct.TIM_ICFilter    = 0xF;
    TIM_ICInit(TIM4,&TIM_ICInitStruct);
 
    //使用编码器模式3
    //参数一TIMx：定时器的选择
    //参数二TIM_EncoderMode：编码器模式的选择
    //参数三TIM_IC1Polarity：通道一的极性选择
    //参数四TIM_IC2Polarity：通道二的极性选择
    TIM_EncoderInterfaceConfig(TIM3,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
    TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);
 
    //清除TIM的更新标志位
    TIM_ClearFlag(TIM3, TIM_FLAG_Update);
    TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
    TIM_ClearFlag(TIM4, TIM_FLAG_Update);
    TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);
 
    //复位计数器
    TIM_SetCounter(TIM3,0);
    TIM_SetCounter(TIM4,0);
 
    TIM_Cmd(TIM3,ENABLE);               //开启定时器
    TIM_Cmd(TIM4,ENABLE);
}
 
int16_t Get_Left_Counter(void)
{
    L_Temp = TIM_GetCounter(TIM3);
    TIM_SetCounter(TIM3,0);
    return L_Temp;
}
 
int16_t Get_Right_Counter(void)
{
    R_Temp = TIM_GetCounter(TIM4);
    TIM_SetCounter(TIM4,0);
    return R_Temp;
}

3.3、MPU6050

简介：MPU6050是一款六轴传感器，包括三轴陀螺仪和三轴加速度计。它的原理是利用陀螺仪测量物体的旋转速度，加速度计测量物体的线性运动加速度。通过结合这两种数据，可以得到物体的运动状态，例如倾斜角度或者在空间中的方向。

具体接线：

代码部分：

        mpu_dmp_get_data(&pitch,&roll,&yaw);                //获取姿态角
        MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);            //获取角速度

所需要的是这两个接口函数，由于代码过多，关于MPU6050的初始化、IIC通信驱动、DMP库获取姿态角可以自行搜索学习，本文不便展示。

二、软件设计

1、软件框图

2、PID算法

2.1、简介

PID 控制器（比例-积分-微分控制器）是一个在工业控制应用中常见的反馈回路部件，由比例单元 P、积分单元 I 和微分单元 D 组成。PID 控制的基础是比例控制；积分控制可消除稳态误差，但可能增加超调；微分控制可加快大惯性系统响应速度以及减弱超调趋势。具体原理可以自行搜索学习。

2.2、PID调参

直立环：

Kp极性：任意给定值后拿起小车前倾，车轮前转说明极性正确，如果极性错误前面加个负号即可

Kp大小：当给定值不断增大，当小车有低频振荡时说明Kp值为最大值，可引入Kd

Kd极性:   Kp值设为0，任意给定Kd值后拿起小车绕电机轴旋转，车轮同相转动，说明极性正确

Kd大小： 恢复Kp值，Kd值不断增大，当小车有高频振荡是说明Kd值为最大值，可引入速度环

注意：直立环调完后两个系数乘0.6再调速度环

速度环：

Kp&Ki极性：手动转动一个车轮，另一个车轮会同向加速，直至最大说明极性正确

Kp&Ki大小：不断增加Kp&Ki值，直至小车保持平衡的同时，速度接近于零。且回位效果好

2.3、相关代码

uint8_t Run_Off(float Angle)
{
    uint8_t off_flag;
    if(Angle > 40 || Angle < -40)
    {
        off_flag = 1;
        GPIO_ResetBits(GPIOA,GPIO_Pin_4),GPIO_ResetBits(GPIOA,GPIO_Pin_5);
        GPIO_ResetBits(GPIOA,GPIO_Pin_0),GPIO_ResetBits(GPIOA,GPIO_Pin_1);
    }
    else off_flag = 0;
    return off_flag;
}
 
void PID_Init()
{
    /*直立PID环控制参数初始化*/
    //P对应反应快慢
    pid.balance_stand = 1.5;
    pid.Kp_stand = -540;
    pid.Kd_stand = -1.8;
 
    /*速度PID环控制参数初始化*/
    pid.Kp_speed  = 115;
    pid.Ki_speed  = pid.Kp_speed / 200;
 
    /*转向PID环控制参数初始化*/
    pid.Kp_turn    = 60;
    pid.Kd_turn    = 0;
    
}
 
/*
函数功能：直立环PD控制
入口参数：Angle 测量实际角度
返回  值：直立控制PWM
注    释：微分变量为直接读取的角速度
*/
extern short gyroy;
int balance(float Angle)
{
    signed int Bias;
    int balance;
    Bias = Angle - pid.balance_stand;           //角度误差
    balance = (pid.Kp_stand * Bias) + (gyroy * pid.Kd_stand);
    return balance;
}
 
/*
函数功能：速度环PI控制
入口参数：Encoder 左右电机转速
返回  值：速度控制PWM
*/
int velocity(int encoder_left,int encoder_right)
{
    static float Velocity,Encoder_Err,Encoder_Last,Encoder_lowout,Movement;
    static float Encoder_Integral;
    static float Target_Velocity = 100;
 
    if(Flag_Front == 1)  Movement = Target_Velocity / 2;
    else if(Flag_Rear == 1)  Movement = -Target_Velocity / 2;
    else if(Flag_Stop == 1)  Movement = 0;
    else Movement = 0;
    
    Encoder_Err = (encoder_left+encoder_right) - 0;          //获取最新速度偏差=测量速度（左右编码器之和）-目标速度（此处为零）
    Encoder_lowout = 0.3 * Encoder_Err+0.7 * Encoder_Last;
    Encoder_Last = Encoder_lowout;
    Encoder_Integral += Encoder_lowout;                               //积分出位移 积分时间：10ms
    Encoder_Integral  = Encoder_Integral + Movement;             //接收遥控器数据，控制前进后退
    if(Encoder_Integral> 10000) Encoder_Integral =  10000;     //积分限幅
    if(Encoder_Integral<-10000) Encoder_Integral = -10000;     //积分限幅
    Velocity = Encoder_Err * pid.Kp_speed + Encoder_Integral * pid.Ki_speed;
    
    if(Run_Off(pitch) == 1)  Encoder_Integral = 0;            //电机关闭,清除积分
    return Velocity;
}
 
/*
函数功能：转向PD控制
入口参数：Z轴角速度
返回  值：转向控制PWM
*/
 
int turn(int16_t gyroz)
{
    static float Turn_Target, Turn_Amplitude = 60;
    int turn_out;
    float Kp = pid.Kp_turn,Kd;
    
    if(Flag_Left == 1) Turn_Target = Turn_Amplitude/2;
    else if(Flag_Right ==1 ) Turn_Target = -Turn_Amplitude/2;
    else if(Flag_Turn_Stop == 1) Turn_Target = 0;
    else Turn_Target = 0;
    
    if(1 == Flag_Front || 1 == Flag_Rear)  Kd = pid.Kd_turn;        
    else Kd = 0;   
    
    turn_out = Turn_Target * Kp + gyroz * Kd;
    
    return turn_out;
}

3、main.c代码

float pitch,roll,yaw;       //欧拉角
int16_t aacx,aacy,aacz;     //加速度原始数据
int16_t gyrox,gyroy,gyroz;  //角速度原始数据
 
int8_t Flag_Front,Flag_Rear,Flag_Stop,Flag_Left,Flag_Right,Flag_Turn_Stop; //蓝牙遥控相关的变量
int16_t L_temp,R_temp,Moto1,Moto2;                          //陀螺仪原始数据 0
int balance_out = 0,velocity_out = 0,Turn_out = 0;     //输出变量
 
uint8_t rxtest;
uint16_t j;
 
int main(void)
{
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    delay_init();
    SystemInit();
    Serial_TX_RX_Init(9600);
    OLED_Init();
    PID_Init();
    PWM_Init();
    Encoder_Init();
    while(MPU_Init()!=0);       //初始化MPU6050
    while(mpu_dmp_init()!=0);   //初始化DMP
    Exti_Init();                //IN中断初始化
    Timer_Init();               //100Hz
 
    while(1)
    {
        OLED_ShowSignedNum(1,1,pitch,3);
        OLED_ShowSignedNum(2,1,roll,3);
        OLED_ShowSignedNum(3,1,yaw,3);
    }
}
 
 
//得到MPU6050数据,进行pid算法设置pwm
void EXTI15_10_IRQHandler(void)   //外部中断1(高优先级)
{
    int PWM_out;
    if(EXTI_GetITStatus(EXTI_Line14)==SET)
    {
        mpu_dmp_get_data(&pitch,&roll,&yaw);                //获取姿态角
        MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);            //获取角速度
        
        balance_out  = balance(pitch);              //直立环
        velocity_out = velocity(L_temp,R_temp);     //速度环
        Turn_out = turn(gyroz);                     //转向环
        PWM_out = balance_out + velocity_out;
                
        Moto1 = PWM_out - Turn_out;  //获得PWM输出值
        Moto2 = PWM_out + Turn_out;
        
        Moto1 = Limit_Pwm(Moto1,7200);
        Moto2 = Limit_Pwm(Moto2,7200);  
        
        if(Run_Off(pitch) == 0)
        {
            Set_compare(1,Moto1);
            Set_compare(2,Moto2);
        }
        
        EXTI_ClearITPendingBit(EXTI_Line14);
    }
}
 
//蓝牙控制
void USART2_IRQHandler(void)    //串口内部中断2(次高优先级)
{
    if (USART_GetITStatus(USART2, USART_IT_RXNE) == SET)       //标志位RXNE置1(RDR寄存器为非空)
    {
        //串口接收功能函数 
        uint8_t rxdata = USART_ReceiveData(USART2);
        switch(rxdata)
        {
            case 'f': Flag_Front = 1,Flag_Rear = 0,Flag_Left = 0,Flag_Right = 0;//前
                break;
            case 'b': Flag_Front = 0,Flag_Rear = 1,Flag_Left = 0,Flag_Right = 0;//后
                break;
            case 'a': Flag_Front = 0,Flag_Rear = 0,Flag_Left = 1,Flag_Right = 0;//左
                break;
            case 'c': Flag_Front = 0,Flag_Rear = 0,Flag_Left = 0,Flag_Right = 1;//右
                break;
            case 's': Flag_Front = 0,Flag_Rear = 0,Flag_Left = 0,Flag_Right = 0,Flag_Stop = 1,Flag_Turn_Stop = 1;//停
                break;
            default :
                break;
        }
        USART_ClearITPendingBit(USART2, USART_IT_RXNE);         //清除RXNE位
    }
}
 
//获取电机速度
void TIM2_IRQHandler(void)      //100Hz定时器中断(低优先级)
{
    if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)        //判断是否中断溢出
    {
        L_temp = Get_Left_Counter();
        R_temp = -Get_Right_Counter();
        
        TIM_ClearITPendingBit(TIM2, TIM_IT_Update);             //清除中断标志位
    }
}

三、代码资料

链接：百度网盘 请输入提取码

提取码：kax4