stm32平衡小车- 03

姿态解算：

cs 复制代码

#include "imu.h"
#include "imath.h"
#include "math.h"
#include "mpu6050.h"
#include "timer.h"

_Matrix Mat = {0};
_Attitude att = {0};
#define mahonyPERIOD        5.0f            //姿态解算周期（ms）
#define kp 	    0.5f                        //proportional gain governs rate of convergence to accelerometer/magnetometer 
#define ki 	    0.0001f                     //integral gain governs rate of convergenceof gyroscope biases

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;       //quaternion elements representing theestimated orientation
float exInt = 0, eyInt = 0, ezInt = 0;      //scaled integral error  

/*
 * 函数名：mahony_update
 * 描述  ：姿态解算
 * 输入  ：陀螺仪三轴数据（单位：弧度/秒），加速度三轴数据（单位：g）    
 * 返回  ：     
 */
//Gyroscope units are radians/second, accelerometer  units are irrelevant as the vector is normalised.
void mahony_update(float gx, float gy, float gz, float ax, float ay, float az) 
{
	float norm;
	float vx, vy, vz;
	float ex, ey, ez;
 
    if(ax*ay*az==0)
        return;
    
    //[ax,ay,az]是机体坐标系下加速度计测得的重力向量(竖直向下)
	norm = invSqrt(ax*ax + ay*ay + az*az);
	ax = ax * norm;
	ay = ay * norm;
	az = az * norm;

	//VectorA = MatrixC * VectorB
	//VectorA ：参考重力向量转到在机体下的值
	//MatrixC ：地理坐标系转机体坐标系的旋转矩阵  
	//VectorB ：参考重力向量（0,0,1）      
    //[vx,vy,vz]是地理坐标系重力分向量[0,0,1]经过DCM旋转矩阵(C(n->b))计算得到的机体坐标系中的重力向量(竖直向下)    

    vx = Mat.DCM_T[0][2];
    vy = Mat.DCM_T[1][2];
    vz = Mat.DCM_T[2][2];
    
    //机体坐标系下向量叉乘得到误差向量，误差e就是测量得到的vˉ和预测得到的 v^之间的相对旋转。这里的vˉ就是[ax,ay,az]',v^就是[vx,vy,vz]'
    //利用这个误差来修正DCM方向余弦矩阵(修正DCM矩阵中的四元素)，这个矩阵的作用就是将b系和n正确的转化直到重合。
    //实际上这种修正方法只把b系和n系的XOY平面重合起来，对于z轴旋转的偏航，加速度计无可奈何，
    //但是，由于加速度计无法感知z轴上的旋转运动，所以还需要用地磁计来进一步补偿。
    //两个向量的叉积得到的结果是两个向量的模与他们之间夹角正弦的乘积a×v=|a||v|sinθ,
    //加速度计测量得到的重力向量和预测得到的机体重力向量已经经过单位化，因而他们的模是1，
    //也就是说它们向量的叉积结果仅与sinθ有关，当角度很小时，叉积结果可以近似于角度成正比。

    ex = ay*vz - az*vy;
	ey = az*vx - ax*vz;
	ez = ax*vy - ay*vx;
 
    //对误差向量进行积分
	exInt = exInt + ex*ki;
	eyInt = eyInt + ey*ki;
	ezInt = ezInt + ez*ki;

//姿态误差补偿到角速度上，修正角速度积分漂移，通过调节Kp、Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度。
	gx = gx + kp*ex + exInt;
	gy = gy + kp*ey + eyInt;
	gz = gz + kp*ez + ezInt;

    //一阶龙格库塔法更新四元数 
	q0 = q0 + (-q1*gx - q2*gy - q3*gz)* mahonyPERIOD * 0.0005f;
	q1 = q1 + ( q0*gx + q2*gz - q3*gy)* mahonyPERIOD * 0.0005f;
	q2 = q2 + ( q0*gy - q1*gz + q3*gx)* mahonyPERIOD * 0.0005f;
	q3 = q3 + ( q0*gz + q1*gy - q2*gx)* mahonyPERIOD * 0.0005f; 

    //把上述运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。
	norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
	q0 = q0 * norm;
	q1 = q1 * norm;
	q2 = q2 * norm;
	q3 = q3 * norm;
    
    //四元素转欧拉角
	att.pit =  atan2(2.0f*(q0*q1 + q2*q3),q0*q0 - q1*q1 - q2*q2 + q3*q3) * rad_to_angle;
	att.rol =  asin(2.0f*(q0*q2 - q1*q3)) * rad_to_angle;       
 
    //z轴角速度积分的偏航角
    att.yaw += Mpu.deg_s.z  * mahonyPERIOD * 0.001f;   
}
/*
 * 函数名：rotation_matrix
 * 描述  ：旋转矩阵：机体坐标系 -> 地理坐标系
 * 输入  ： 
 * 返回  ：     
 */
void rotation_matrix(void)
{
    Mat.DCM[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
    Mat.DCM[0][1] = 2.0f * (q1*q2 -q0*q3);
    Mat.DCM[0][2] = 2.0f * (q1*q3 +q0*q2);

    Mat.DCM[1][0] = 2.0f * (q1*q2 +q0*q3);
    Mat.DCM[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;
    Mat.DCM[1][2] = 2.0f * (q2*q3 -q0*q1);
     
    Mat.DCM[2][0] = 2.0f * (q1*q3 -q0*q2);
    Mat.DCM[2][1] = 2.0f * (q2*q3 +q0*q1);
    Mat.DCM[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;   
}
/*
 * 函数名：rotation_matrix_T
 * 描述  ：旋转矩阵的转置矩阵：地理坐标系 -> 机体坐标系
 * 输入  ： 
 * 返回  ：     
 */
void rotation_matrix_T(void)
{
    Mat.DCM_T[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
    Mat.DCM_T[0][1] = 2.0f * (q1*q2 +q0*q3);    
    Mat.DCM_T[0][2] = 2.0f * (q1*q3 -q0*q2); 
    
    Mat.DCM_T[1][0] = 2.0f * (q1*q2 -q0*q3);
    Mat.DCM_T[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;  
    Mat.DCM_T[1][2] = 2.0f * (q2*q3 +q0*q1);    
    
    Mat.DCM_T[2][0] = 2.0f * (q1*q3 +q0*q2);
    Mat.DCM_T[2][1] = 2.0f * (q2*q3 -q0*q1);
    Mat.DCM_T[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;   
}
/*
 * 函数名：Matrix_ready
 * 描述  ：矩阵更新准备，为姿态解算使用
 * 输入  ： 
 * 返回  ：     
 */
void Matrix_ready(void)
{
    rotation_matrix();                      //旋转矩阵更新
    rotation_matrix_T();                    //旋转矩阵的逆矩阵更新
}

pid:

cs 复制代码

#include "pid.h"

_ALL_PID all;

//存储pid控制器参数
const float  controller_parameter[3][5] =
{
    //0.kp 1.ki 2.kd 3.积分限幅  4.pid输出限幅值  
    {5.6, 0.0,  2,  500 , 980 },                        //rol_angle     内环角度环       //调试电机：7.5    6.1
    {0.45, 0.0,  0,  550 , 950 },                        //vel_encoder   外环速度环       //调试电机：0.5(购买的16线编码器)  / 0.65(自制编码器11线)   0.45
    {14,  0.0,  0,  550 , 950 },                        //gyro          内环角速度环     //调试电机：17        15
};

//pid参数初始化配置
void pid_init(_PID *controller,uint8_t label)
{
    controller->kp              = controller_parameter[label][0];         
    controller->ki              = controller_parameter[label][1];         
    controller->kd              = controller_parameter[label][2];         
    controller->integral_max    = controller_parameter[label][3];         
    controller->out_max         = controller_parameter[label][4];               
}
//pid参数初始化
void all_pid_init(void)
{
    pid_init(&all.rol_angle,0);
    pid_init(&all.vel_encoder,1);
    pid_init(&all.rol_gyro,2);
} 
//pid控制器
float pid_controller(_PID *controller)
{
    controller->err_last = controller->err;                                                 //保存上次偏差
    controller->err = controller->expect - controller->feedback;                            //偏差计算
    controller->integral += controller->ki * controller->err;                               //积分  
    //积分限幅
    if(controller->integral >  controller->integral_max)     controller->integral =  controller->integral_max;
    if(controller->integral < -controller->integral_max)     controller->integral = -controller->integral_max;
    //pid运算
    controller->out =  controller->kp*controller->err + controller->integral + controller->kd*(controller->err-controller->err_last);
    //输出限幅
    if(controller->out >  controller->out_max)   controller->out =  controller->out_max;
    if(controller->out < -controller->out_max)   controller->out = -controller->out_max;
    return controller->out;
}
//清除积分
void clear_integral(_PID *controller)
{
    controller->integral = 0.0f;
}

pwm:

cs 复制代码

#include "pwm.h"
#include "imath.h"


/*
 * 函数名：pwm_init
 * 描述  ：PWM配置初始化
 * 输入  ：    
 * 返回  ：     
 */
void pwm_init(void)
{
	GPIO_InitTypeDef GPIO_initStructure;
	TIM_TimeBaseInitTypeDef TIM_timeBaseInitStructure;
	TIM_OCInitTypeDef TIM_OCInitStructure;
		
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO,ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);	
    
	GPIO_initStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
	GPIO_initStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_initStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&GPIO_initStructure);

	//定时器周期 t =( 72 / 7200 ) * 10^6 =  10khz    
	
	//配置时基
	TIM_timeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;         //不分频
	TIM_timeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;		//向上计数
	TIM_timeBaseInitStructure.TIM_Period = 7200-1;						//设置ARR值
	TIM_timeBaseInitStructure.TIM_Prescaler = 0;						//时钟预分频值
	TIM_TimeBaseInit(TIM2,&TIM_timeBaseInitStructure);
	
	//配置OC输出通道
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;					//采用PWM模式1输出波形
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;			//设置CH通道的有效电平
    TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;		//设置CH通道的空闲状态的电平
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;		//使能CH通道
	TIM_OCInitStructure.TIM_Pulse = 0;									//设置TIM1的CCR值
	
    TIM_OC1Init(TIM2,&TIM_OCInitStructure);
	TIM_OC1PreloadConfig(TIM2,TIM_OCPreload_Enable);                  
	
	TIM_OC2Init(TIM2,&TIM_OCInitStructure);
    TIM_OC2PreloadConfig(TIM2,TIM_OCPreload_Enable);                                   

	//使能TIM的ARR和CRR，以及使能TIM定时器,开启pwm输出
	TIM_ARRPreloadConfig(TIM2,ENABLE);								    //预加载使能	
         
	TIM_Cmd(TIM2,ENABLE);
	
	//开始启动定时器输出pwm,这句是高级定时器才有的，输出pwm必须打开
//	TIM_CtrlPWMOutputs(TIM1,ENABLE);  

    TIM2->CCR1 = 0;
    TIM2->CCR2 = 0;
}
/*
 * 函数名：motor_pwm_out
 * 描述  ：控制两路PWM输出
 * 输入  ：两路的pwm    
 * 返回  ：     
 */
void motor_pwm_out(uint16_t pwm1,uint16_t pwm2)
{
	TIM2->CCR1 = to_limit(pwm1,0,7200);
	TIM2->CCR2 = to_limit(pwm2,0,7200);
}
/*
 * 函数名：driver_pin_init
 * 描述  ：TB6612电机驱动IO初始化配置
 * 输入  ：    
 * 返回  ：     
 */
void driver_pin_init(void)
{
    GPIO_InitTypeDef  GPIO_InitStructure;
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB,ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);
    GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);                    //JTAG失能 PB3/4 用于普通IO口使用
    
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Pin = AIN1_pin;
    GPIO_Init(AIN1_port, &GPIO_InitStructure);  

    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Pin = AIN2_pin;
    GPIO_Init(AIN2_port, &GPIO_InitStructure);  
    
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Pin = BIN1_pin;
    GPIO_Init(BIN1_port, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Pin = BIN2_pin;
    GPIO_Init(BIN2_port, &GPIO_InitStructure);    
    
    //STBY pin
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Pin = STBY_pin;
    GPIO_Init(STBY_port, &GPIO_InitStructure);  
    
    //使能
    STBY_HIGH;  
}

/*
 * 函数名：Encoder_A_init
 * 描述  ：编码器A端口配置初始化
 * 输入  ：    
 * 返回  ：     
 */
void Encoder_A_init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    TIM_ICInitTypeDef TIM_ICInitStructure;      

    //PA6 ch1, PA7 ch2 
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

    GPIO_StructInit(&GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;         
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);                           

    TIM_DeInit(TIM3);
    TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
    TIM_TimeBaseStructure.TIM_Period = MAX_VALUE;
    TIM_TimeBaseStructure.TIM_Prescaler = 0;
    TIM_TimeBaseStructure.TIM_ClockDivision =TIM_CKD_DIV1 ;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);              

    //编码器接口配置：采用模式3：双边沿触发，并且在双边沿都进行计数
    TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_BothEdge ,TIM_ICPolarity_BothEdge);
    TIM_ICStructInit(&TIM_ICInitStructure);
    TIM_ICInitStructure.TIM_ICFilter = 6;               //硬件捕获滤波
    TIM_ICInit(TIM3, &TIM_ICInitStructure);

    TIM_ClearFlag(TIM3, TIM_FLAG_Update);
    TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
    //Reset counter
    TIM3->CNT = 0;

    TIM_Cmd(TIM3, ENABLE);
}
/*
 * 函数名：Encoder_B_init
 * 描述  ：编码器B端口配置初始化
 * 输入  ：    
 * 返回  ：     
 */
void Encoder_B_init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    TIM_ICInitTypeDef TIM_ICInitStructure;      

    //PB6 ch1, PB7 ch2 
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);

    GPIO_StructInit(&GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;         
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOB, &GPIO_InitStructure);                           

    TIM_DeInit(TIM4);
    TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
    TIM_TimeBaseStructure.TIM_Period = MAX_VALUE;
    TIM_TimeBaseStructure.TIM_Prescaler = 0;
    TIM_TimeBaseStructure.TIM_ClockDivision =TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);              

    TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_BothEdge ,TIM_ICPolarity_BothEdge);
    TIM_ICStructInit(&TIM_ICInitStructure);
    TIM_ICInitStructure.TIM_ICFilter = 6;
    TIM_ICInit(TIM4, &TIM_ICInitStructure);

    TIM_ClearFlag(TIM4, TIM_FLAG_Update);
    TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);
    //Reset counter
    TIM4->CNT = 0;

    TIM_Cmd(TIM4, ENABLE);
}

int encoder_val_a;
int encoder_val_b;
/*
 * 函数名：read_encoder
 * 描述  ：编码器数据读取
 * 输入  ：encoder_num：要读取的编码器号    
 * 返回  ：编码值     
 */
int read_encoder(uint8_t encoder_num)
{
    int encoder_val;
    switch(encoder_num)
    {
        case ENCODER_A: encoder_val = (int)TIM3->CNT; TIM3->CNT = 0;break;
        case ENCODER_B: encoder_val = (int)TIM4->CNT; TIM4->CNT = 0;break;
        default: encoder_val = 0; break;
    }
    return encoder_val;
}
/*
 * 函数名：encoder_manage
 * 描述  ：编码器数据处理为 ：-A ~ +A 范围
 * 输入  ：data：编码器值首地址    
 * 返回  ：  
 */
void encoder_manage(int *data)
{
    //编码器反向时，转为与正向时对应的负数
    if(*data>(MAX_VALUE*0.5f))
        *data = *data - MAX_VALUE;
    else 
        *data = *data;
}

滤波算法：

cs 复制代码

#include "filter.h"


/*
 * 函数名：butterworth_lpf
 * 描述  ：二阶巴特沃斯滤波器原型
 * 输入  ：now_input输入数据， buffer中间数据缓存，parameter滤波参数
 * 返回  ：滤波之后的数据     
 */
float butterworth_lpf(float now_input,_Butterworth_data *buffer, _Butterworth_parameter *parameter)
{
    buffer->input_data[2] = now_input;

    /* Butterworth LPF */
    buffer->output_data[2] =   parameter->b[0] * buffer->input_data[2]
                             + parameter->b[1] * buffer->input_data[1]
                             + parameter->b[2] * buffer->input_data[0]
                             - parameter->a[1] * buffer->output_data[1]
                             - parameter->a[2] * buffer->output_data[0];
    /* x(n) 保存 */
    buffer->input_data[0] = buffer->input_data[1];
    buffer->input_data[1] = buffer->input_data[2];
    /* y(n) 保存 */
    buffer->output_data[0] = buffer->output_data[1];
    buffer->output_data[1] = buffer->output_data[2];
  
    return buffer->output_data[2];
}