[应用相关] 基于STM32的二轮自平衡小车

#include "encoder.h"



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);//配置编码器模式  T1和T2的每个跳变沿均计数  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的所有边沿计数。



*************************/

        TIM_ICStructInit(&TIM_ICInitStruct);//初始化输入捕获

        TIM_ICInitStruct.TIM_ICFilter=10;//滤波器

        TIM_ICInit(TIM2,&TIM_ICInitStruct);

        

        TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);//配置溢出更新中断标志位

        

        TIM_SetCounter(TIM2,0);//清零定时器计数值

        

        TIM_Cmd(TIM2,ENABLE);//开启定时器

}







/**********************

编码器

速度读取函数

入口参数：定时器

**********************/

int Read_Speed(int TIMx)

{

        int value_1;

        switch(TIMx)

        {

                case 2:value=(short)TIM_GetCounter(TIM2);

                                TIM_SetCounter(TIM2,0);

                                break;

                //IF是定时器2，1.采集编码器的计数值并保存。2.将定时器的计数值清零。

                case 3:value=(short)TIM_GetCounter(TIM3);

                                TIM_SetCounter(TIM3,0);

                                break;

                default:value=0;

        }

        return value;

}





void TIM2_IRQHandler(void)

{

        if(TIM_GetITStatus(TIM2,TIM_IT_Update)!=0)

        {

                TIM_ClearITPendingBit(TIM2,TIM_IT_Update); //清除中断标志位

        }

}

        

void TIM3_IRQHandler(void)

{

        if(TIM_GetITStatus(TIM3,TIM_IT_Update)!=0)

        {

                TIM_ClearITPendingBit(TIM3,TIM_IT_Update);

        }

}

#include "pwm.h"



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,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;

        TIM_TimeBaseInitStruct.TIM_CounterMode=TIM_CounterMode_Up;

        TIM_TimeBaseInitStruct.TIM_Period=Per;

        TIM_TimeBaseInitStruct.TIM_Prescaler=Psc;

        TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);

        

        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;

        TIM_OC1Init(TIM1,&TIM_OCInitStruct);

        TIM_OC4Init(TIM1,&TIM_OCInitStruct);

        

        TIM_CtrlPWMOutputs(TIM1,ENABLE);//高级定时器专属

        

        TIM_OC1PreloadConfig(TIM1,TIM_OCPreload_Enable);

        TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);

        TIM_ARRPreloadConfig(TIM1,ENABLE);//TIM1预装载寄存器使能

        

        TIM_Cmd(TIM1,ENABLE);//使能定时器。

}

#include "motor.h"



/*电机初始化函数*/

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);        

        

}





int PWM_MAX = 3000;

int PWM_MIN = -3000; 



/*限幅函数*/

void Limit(int *motoA,int *motoB)

{

        if(*motoA>PWM_MAX)

        *motoA=PWM_MAX;

        else if(*motoA<PWM_MIN)

        *motoA=PWM_MIN;

        

        if(*motoB>PWM_MAX)

        *motoB=PWM_MAX;

        else if(*motoB<PWM_MIN)

        *motoB=PWM_MIN;

}



/*绝对值函数*/

int 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,abs(moto1)); //等价直接赋值在->CRR寄存器

        

        if(moto2>0)        Bin1=0,Bin2=1;

        else                         Bin1=1,Bin2=0;        

        TIM_SetCompare4(TIM1,abs(moto2));

}







char PWM_Zero=0,stop=0;

void Stop(float *Med_Jiaodu,float *Jiaodu)

{

        if(GFP_abs(*Jiaodu-*Med_Jiaodu)>50)

    {

        Load(PWM_Zero,PWM_Zero);

    }

}

void IIC_Init(void)

{                        

        GPIO_InitTypeDef GPIO_InitStructure;

        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);   //使能PB端口时钟

        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;        //端口配置

        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;        //推挽输出

        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;       //50M

        GPIO_Init(GPIOB, &GPIO_InitStructure);                                        //根据设定参数初始化GPIOB 

}



/*不同的管脚对应的修改的地址不一样，这里需要注意*/



#define SDA_IN()  {GPIOB->CRL&=0X0FFFFFFF;GPIOB->CRL|=8<<28;}

#define SDA_OUT() {GPIOB->CRL&=0X0FFFFFFF;GPIOB->CRL|=3<<28;}



         

#define IIC_SCL    PBout(6) //SCL

#define IIC_SDA    PBout(7) //SDA

#define READ_SDA   PBin(7)  //输入SDA 

void IIC_Init(void)

{                        

        GPIO_InitTypeDef GPIO_InitStructure;

        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);   //使能PB端口时钟

        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;        //端口配置

        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;        //推挽输出

        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;       //50M

        GPIO_Init(GPIOB, &GPIO_InitStructure);                                        //根据设定参数初始化GPIOB 

}



/*不同的管脚对应的修改的地址不一样，这里需要注意*/



#define SDA_IN()  {GPIOB->CRL&=0X0FFFFFFF;GPIOB->CRL|=8<<28;}

#define SDA_OUT() {GPIOB->CRL&=0X0FFFFFFF;GPIOB->CRL|=3<<28;}



         

#define IIC_SCL    PBout(6) //SCL

#define IIC_SDA    PBout(7) //SDA

#define READ_SDA   PBin(7)  //输入SDA 

//GPIO初始化

void USART2_init(u32 bound)

{

        //GPIO端口设置

        GPIO_InitTypeDef GPIO_InitStructure;

        USART_InitTypeDef USART_InitStructure;

        NVIC_InitTypeDef NVIC_InitStruct;  

        

        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

        RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2,ENABLE); 

        

        //USART2_TX  

        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;

        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;

        GPIO_Init(GPIOC, &GPIO_InitStructure);

        //USART2_RX         

        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;

        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;

        GPIO_Init(GPIOC, &GPIO_InitStructure);  

        //USART 初始化设置

        USART_InitStructure.USART_BaudRate = bound;//一般设置为9600;

        USART_InitStructure.USART_WordLength = USART_WordLength_8b;

        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);

        

        USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);//开启中断

        USART_Cmd(USART2, ENABLE);                    //使能串口 

        

        NVIC初始化

        MY_NVIC_PriorityGroupConfig(2);

        NVIC_InitStruct.NVIC_IRQChannel=USART2_IRQn;

        NVIC_InitStruct.NVIC_IRQChannelCmd=ENABLE;

        NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority=0;

        NVIC_InitStruct.NVIC_IRQChannelSubPriority=0;

        NVIC_Init(&NVIC_InitStruct);        

        

}



u8 Fore,Back,Left,Right;



void USART2_IRQHandler(void)  //先尝试能不能进中断  不能再放外边

{

        char  Bluetooth_data;

        if(USART_GetITStatus(USART2,USART_IT_RXNE)!=RESET)//接收中断标志位拉高

        {

                Bluetooth_data=USART_ReceiveData(USART2);//保存接收的数据

                if(Bluetooth_data==0x5A)            Fore=0,Back=0,Left=0,Right=0;//刹

                else if(Bluetooth_data==0x41)        Fore=1,Back=0,Left=0,Right=0;//前

                else if(Bluetooth_data==0x45)        Fore=0,Back=1,Left=0,Right=0;//后

                //else if(Bluetooth_data==0x42)        Fore=1,Back=0,Left=1,Right=0;//右前

                else if(Bluetooth_data==0x47)        Fore=0,Back=0,Left=1,Right=0;//左

                else if(Bluetooth_data==0x43)        Fore=0,Back=0,Left=0,Right=1;//右

                else                                                Fore=0,Back=0,Left=0,Right=0;//刹

        }

                

}



//一个字符

void USART2_Send_Data(char data)

{

        USART_SendData(USART2,data);

        while(USART_GetFlagStatus(USART2,USART_FLAG_TC)!=1);

}



//一串字符

void USART2_Send_String(char *String)

{

        u16 len,j;

        

        len=strlen(String);

        for(j=0;j<len;j++)

        {

                USART2_Send_Data(*String++);

        }

}



           

                        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;        //右转

                        

                        

                        /*转向约束*/

                        if((Left==0)&&(Right==0))Turn_Kd=-0.8;//若无左右转向指令，则开启转向约束

                        else if((Left==1)||(Right==1))Turn_Kd=0;//若左右转向指令接收到，则去掉转向约束

/********************

入口：期望角度、真实角度、真实角速度

出口：直立环输出

*********************/

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;

}





/*********************

速度环PI：Kp*Ek+Ki*Ek_S

// 入口 左电机速度, 右点击速度

*********************/

int Velocity(int Target,int encoder_left,int encoder_right)

{

        static int Encoder_S,EnC_Err_Lowout_last,PWM_out,Encoder_Err,EnC_Err_Lowout;

        float a=0.7;

        

        //1.计算速度偏差

        Encoder_Err=((encoder_left+encoder_right)-Target);

        //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.速度环控制输出计算

        PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;

        return PWM_out;

        

}

float Med_Angle=2;        //机械中值。



float

        Vertical_Kp=-125

,        //直立环KP、KD   225  215*  220*   129  125**  115 

        Vertical_Kd=-0.765;  //1.1 1.18  1.21*  1.25*   0.768  0.773** 0.764



float

        Velocity_Kp=1.0,        //速度环KP、KI  1.11  1.14**  1.16  1.18

        Velocity_Ki=0.005; // Ki = 1/200 * Kp





int MOTO1=0,MOTO2=0; // 电机装载变量



float Pitch,Roll,Yaw;        //角度信息,俯仰角,翻滚角,偏航角

short gyrox,gyroy,gyroz;//陀螺仪角速度

short aacx,aacy,aacz;//加速度

int Encoder_Left,Encoder_Right;//编码器数据(速度)

int Vertical_out,Velocity_out,Turn_out;

float Med_Angle=2;        //机械中值。



float

        Vertical_Kp=-125

,        //直立环KP、KD   225  215*  220*   129  125**  115 

        Vertical_Kd=-0.765;  //1.1 1.18  1.21*  1.25*   0.768  0.773** 0.764



float

        Velocity_Kp=1.0,        //速度环KP、KI  1.11  1.14**  1.16  1.18

        Velocity_Ki=0.005; // Ki = 1/200 * Kp





int MOTO1=0,MOTO2=0; // 电机装载变量



float Pitch,Roll,Yaw;        //角度信息,俯仰角,翻滚角,偏航角

short gyrox,gyroy,gyroz;//陀螺仪角速度

short aacx,aacy,aacz;//加速度

int Encoder_Left,Encoder_Right;//编码器数据(速度)

int Vertical_out,Velocity_out,Turn_out;

int main(void)

{ 

  //char buf[16];

        uart_init(115200);

        

        GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);

        GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁用JTAG 启用 SWD

        

        

        delay_init();                        //=====延时函数初始化

        LED_Init();                 //=====LED初始化    程序灯



        I2C_Configuration();        //硬件I2C初始化

        OLED_Init();                        //OLED初始化        



        Encoder_TIM2_Init(); //编码器

        Encoder_TIM3_Init();

        

    IIC_Init();                     //=====软件IIC初始化    读取MPU6050数据

        MPU6050_initialize();           //=====MPU6050初始化        

        DMP_Init();                     //=====初始化DMP 

        

        PWM_Init_TIM1(0,7199); //72M/(1) / (7200) = 10kHZ

        

        delay_ms(1000);                //延时1s  解决小车上电轮子乱转的问题

        

        Motor_Init();//电机

          

    TIM3_Configuration();                        //超声波拿来捕获的定时器

        UltrasonicWave_Configuration(); //超声波

        UltrasonicWave_StartMeasure();        //超声波        

        

        BASIC_TIM_Init(); //TIM4基本定时器 只定时 更新数据用

  

    EXTI_Init();               //=====MPU6050 5ms定时中断初始化

while(1)

        {

                  

                int PWM_out;

             Read_DMP(); 

                  

                    printf("%d\r\n",(int)Roll);

                  printf("%d\r\n",(int)Yaw);

                  printf("%d\r\n",(int)Pitch);

                  printf("%d\r\n",gyrox);

                  printf("%d\r\n",gyroy);

                  printf("%d\r\n",gyroz);

                

                  gyrox = gyro [0];

                  gyroy = gyro [1];

                  gyroz = gyro [2];

                

                  aacx = accel[0];

                  aacy = accel[1];

                  aacz = accel[2];

                

                        //1、采集编码器数据&MPU6050角度信息。

                        Encoder_Left=-Read_Speed(2);

                        Encoder_Right=Read_Speed(3);

                

                   

                         //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;//左电机

                        MOTO2=PWM_out;//右电机

                        Limit(&MOTO1,&MOTO2);         //PWM限幅                        

                        Load(MOTO1,MOTO2);                 //加载到电机上。

                        

                        Stop(&Med_Angle,&Pitch);//安全检测

                

while(1)

        {

                  

                int PWM_out;

             Read_DMP(); 

                  

                    printf("%d\r\n",(int)Roll);

                  printf("%d\r\n",(int)Yaw);

                  printf("%d\r\n",(int)Pitch);

                  printf("%d\r\n",gyrox);

                  printf("%d\r\n",gyroy);

                  printf("%d\r\n",gyroz);

                

                  gyrox = gyro [0];

                  gyroy = gyro [1];

                  gyroz = gyro [2];

                

                  aacx = accel[0];

                  aacy = accel[1];

                  aacz = accel[2];

                

                        //1、采集编码器数据&MPU6050角度信息。

                        Encoder_Left=-Read_Speed(2);

                        Encoder_Right=Read_Speed(3);

                

                   

                         //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;//左电机

                        MOTO2=PWM_out;//右电机

                        Limit(&MOTO1,&MOTO2);         //PWM限幅                        

                        Load(MOTO1,MOTO2);                 //加载到电机上。

                        

                        Stop(&Med_Angle,&Pitch);//安全检测