目录
前言
经过几天对平衡车的理论学习和动手实践，终于完成了平衡车的基本功能，实现前进，后退，直立，转向。本项目用到了两个电机，一个两块亚力克板，一个mpu6050陀螺仪，TB6612FNG电机驱动模块，通信方式使用蓝牙模块进行简单的通信，主控芯片使用stm32f103c8t6。
一、电机驱动部分
1、TB6612FNG电机驱动模块接线方式：
VM 接12V电源，给电机供电
STBY 置1使能AIN1，AIN2，BIN1，BIN2
VCC 接5V电源
GND 接地
AO1，AO2 输出控制电机1正反转
BO1，BO2 输入控制电机2正反转
PWMA 控制AO1，AO2使能
PWMB 控制BO1，BO2使能
AIN1，AIN2 输入控制控制电机1正反转
BIN1，BIN2 输入控制控制电机2正反转
2、代码使用定时器2的4路输出pwm
具体代码如下
void time2_pwm_init(uint16_t arr,uint16_t pre)
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);//定时器2使能
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//gpio引脚使能
GPIO_InitTypeDef gpio_init={0};
gpio_init.GPIO_Pin=GPIO_Pin_2 | GPIO_Pin_3;
gpio_init.GPIO_Mode=GPIO_Mode_AF_PP;
gpio_init.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&gpio_init);
TIM_TimeBaseInitTypeDef tim2_base={0};
tim2_base.TIM_ClockDivision=TIM_CKD_DIV1;
tim2_base.TIM_CounterMode=TIM_CounterMode_Up;
tim2_base.TIM_Period=arr;
tim2_base.TIM_Prescaler=pre;
tim2_base.TIM_RepetitionCounter=DISABLE;
TIM_TimeBaseInit(TIM2,&tim2_base);
TIM_OCInitTypeDef time2_oc={0};
time2_oc.TIM_OCMode=TIM_OCMode_PWM1;
time2_oc.TIM_OCPolarity=TIM_OCPolarity_High;
time2_oc.TIM_Pulse=0;
time2_oc.TIM_OutputState=TIM_OutputState_Enable;
TIM_OC3Init(TIM2,&time2_oc);
TIM_OC4Init(TIM2,&time2_oc);
TIM_Cmd(TIM2,ENABLE);//启动定时器
}3、gpio引脚初始化，以及前进，后退引脚设置
代码
void mator_gpio_init(void)
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
GPIO_InitTypeDef gpio_init={0};
gpio_init.GPIO_Pin=GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
gpio_init.GPIO_Mode=GPIO_Mode_Out_PP;
gpio_init.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&gpio_init);
GPIO_WriteBit(GPIOB,GPIO_Pin_12,Bit_RESET);
GPIO_WriteBit(GPIOB,GPIO_Pin_13,Bit_RESET);
GPIO_WriteBit(GPIOB,GPIO_Pin_14,Bit_RESET);
GPIO_WriteBit(GPIOB,GPIO_Pin_15,Bit_RESET);
}
void mator_forward(void)
{
GPIO_WriteBit(GPIOB,GPIO_Pin_12,Bit_SET);
GPIO_WriteBit(GPIOB,GPIO_Pin_13,Bit_RESET);
GPIO_WriteBit(GPIOB,GPIO_Pin_14,Bit_RESET);
GPIO_WriteBit(GPIOB,GPIO_Pin_15,Bit_SET);
}
void mator_back(void)
{
GPIO_WriteBit(GPIOB,GPIO_Pin_12,Bit_RESET);
GPIO_WriteBit(GPIOB,GPIO_Pin_13,Bit_SET);
GPIO_WriteBit(GPIOB,GPIO_Pin_14,Bit_SET);
GPIO_WriteBit(GPIOB,GPIO_Pin_15,Bit_RESET);
}二、MPU6050陀螺仪部分
主要使用的是mpu6050官方的dmp库
代码如下
//采集俯仰角，翻滚角，偏航角
void MPU6050_Pose(void)
{
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);
if(sensors & INV_WXYZ_QUAT )
{
q0 = quat[0] / q30;
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)*57.3; // roll
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
}
}
//mpu6050初始化
void MPU6050_Init(void)
{
int result=0;
//IIC_Init();
result=mpu_init();
if(!result)
{
printf("mpu initialization complete......\n "); //mpu initialization complete
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_set_sensor
printf("mpu_set_sensor complete ......\n");
else
printf("mpu_set_sensor come across error ......\n");
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo
printf("mpu_configure_fifo complete ......\n");
else
printf("mpu_configure_fifo come across error ......\n");
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //mpu_set_sample_rate
printf("mpu_set_sample_rate complete ......\n");
else
printf("mpu_set_sample_rate error ......\n");
if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare
printf("dmp_load_motion_driver_firmware complete ......\n");
else
printf("dmp_load_motion_driver_firmware come across error ......\n");
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation
printf("dmp_set_orientation complete ......\n");
else
printf("dmp_set_orientation come across error ......\n");
if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL)) //dmp_enable_feature
printf("dmp_enable_feature complete ......\n");
else
printf("dmp_enable_feature come across error ......\n");
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //dmp_set_fifo_rate
printf("dmp_set_fifo_rate complete ......\n");
else
printf("dmp_set_fifo_rate come across error ......\n");
run_self_test(); //自检
if(!mpu_set_dmp_state(1))
printf("mpu_set_dmp_state complete ......\n");
else
printf("mpu_set_dmp_state come across error ......\n");
}
else //MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
{
GPIO_ResetBits(GPIOC, GPIO_Pin_13); //MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
while(1);
}
}三、编码器捕获部分
使用定时器3，和4的通道1和通道2进行4倍频地计数，主要代码如下：
void time3_encoder_init()
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);//定时器2使能
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//gpio引脚使能
GPIO_InitTypeDef gpio_init={0};
gpio_init.GPIO_Pin=GPIO_Pin_6 | GPIO_Pin_7;
gpio_init.GPIO_Mode=GPIO_Mode_IN_FLOATING;
gpio_init.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOA,&gpio_init);
TIM_TimeBaseInitTypeDef time3_base={0};
time3_base.TIM_ClockDivision=TIM_CKD_DIV1;
time3_base.TIM_CounterMode=TIM_CounterMode_Up;
time3_base.TIM_Period=65535;
time3_base.TIM_Prescaler=0;
time3_base.TIM_RepetitionCounter=DISABLE;
TIM_TimeBaseInit(TIM3,&time3_base);
TIM_ICInitTypeDef time3_ic={0};
time3_ic.TIM_Channel=TIM_Channel_1;
time3_ic.TIM_ICFilter=0;
time3_ic.TIM_ICPolarity=TIM_ICPolarity_Rising;
time3_ic.TIM_ICPrescaler=TIM_ICPSC_DIV1;
time3_ic.TIM_ICSelection=TIM_ICSelection_DirectTI;
TIM_ICInit(TIM3,&time3_ic);
time3_ic.TIM_Channel=TIM_Channel_2;
TIM_ICInit(TIM3,&time3_ic);
TIM_EncoderInterfaceConfig(TIM3,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置编码器计数。BothEdge（底部边缘）
//初始化标志位,计数器
TIM_ClearFlag(TIM3,TIM_FLAG_Update);//清除标志位
TIM_SetCounter(TIM3,0);//TIM3->CNT=0;
//配置中断
NVIC_InitTypeDef nvic_init={0};
nvic_init.NVIC_IRQChannel=TIM3_IRQn;//中断通道
nvic_init.NVIC_IRQChannelCmd=ENABLE;//中断使能
nvic_init.NVIC_IRQChannelPreemptionPriority=2;//抢占优先级；
nvic_init.NVIC_IRQChannelSubPriority=1;//响应优先级
NVIC_Init(&nvic_init);
TIM_ITConfig(TIM3,TIM_IT_Update | TIM_IT_CC1 |TIM_IT_CC2,ENABLE);//配置定时器，允许更新中断,CC1,CC2捕获中断
TIM_Cmd(TIM3,ENABLE);//开启定时器
}
void time4_encoder_init()
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//定时器2使能
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//gpio引脚使能
GPIO_InitTypeDef gpio_init={0};
gpio_init.GPIO_Pin=GPIO_Pin_6 | GPIO_Pin_7;
gpio_init.GPIO_Mode=GPIO_Mode_IN_FLOATING;
gpio_init.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&gpio_init);
TIM_TimeBaseInitTypeDef time4_base={0};
time4_base.TIM_ClockDivision=TIM_CKD_DIV1;
time4_base.TIM_CounterMode=TIM_CounterMode_Up;
time4_base.TIM_Period=65535;
time4_base.TIM_Prescaler=0;
time4_base.TIM_RepetitionCounter=DISABLE;
TIM_TimeBaseInit(TIM4,&time4_base);
TIM_ICInitTypeDef time4_ic={0};
time4_ic.TIM_Channel=TIM_Channel_1;
time4_ic.TIM_ICFilter=0;
time4_ic.TIM_ICPolarity=TIM_ICPolarity_Rising;
time4_ic.TIM_ICPrescaler=TIM_ICPSC_DIV1;
time4_ic.TIM_ICSelection=TIM_ICSelection_DirectTI;
TIM_ICInit(TIM4,&time4_ic);
time4_ic.TIM_Channel=TIM_Channel_2;
TIM_ICInit(TIM4,&time4_ic);
TIM_EncoderInterfaceConfig(TIM4,TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);//配置编码器计数。BothEdge（底部边缘）
//初始化标志位,计数器
TIM_ClearFlag(TIM4,TIM_FLAG_Update);//清除标志位
TIM_SetCounter(TIM4,0);//TIM3->CNT=0;
//配置中断
NVIC_InitTypeDef nvic_init={0};
nvic_init.NVIC_IRQChannel=TIM4_IRQn;//中断通道
nvic_init.NVIC_IRQChannelCmd=ENABLE;//中断使能
nvic_init.NVIC_IRQChannelPreemptionPriority=2;//抢占优先级；
nvic_init.NVIC_IRQChannelSubPriority=1;//响应优先级
NVIC_Init(&nvic_init);
TIM_ITConfig(TIM4,TIM_IT_Update | TIM_IT_CC1 |TIM_IT_CC2,ENABLE);//配置定时器，允许更新中断,CC1,CC2捕获中断
TIM_Cmd(TIM4,ENABLE);//开启定时器
}四、pid部分
使用定时器1每隔5ms进行一次mpu6050的数据采集，并进行直立环控制，每40ms，进行一次速度环控制，防止影响直立控制。
1、直立环KD
小车的偏转角度作为Kp的系数，角加速度作为Kd的系数
代码如下：
float angle_pid_realize(struct _pid *pid,float angle)
{
if(Pitch==0||Pitch<-20||Pitch>20) //MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
{
//Pitch=1;
GPIO_ResetBits(GPIOC, GPIO_Pin_13); //MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
}
else
{GPIO_SetBits(GPIOC, GPIO_Pin_13);} //MPU6050状态正常时LED灯熄灭
pid->err=angle-pid->target_val;//计算目标值与实际值的误差
pid->actual_val=pid->Kp*pid->err+pid->Kd*gyro[0];//角度PD控制,gyro[0]x轴偏转角速度
return pid->actual_val;
}2、速度环KI
速度环控制小车的位移，实现定点停下的功能，代码如下
void speed_control(void)
{
mator.car_speed = (mator.left_tal_count + mator.right_tal_count );// * 0.5 ; //左右电机脉冲数平均值作为小车当前车速
mator.left_tal_count =mator.right_tal_count = 0; //全局变量 注意及时清零
BST_fCarSpeedOld *= 0.7;//一阶滤波，占上次的70%
BST_fCarSpeedOld +=mator.car_speed*0.3;//一阶滤波，占本次的30%
BST_fCarPosition += BST_fCarSpeedOld; //路程 即速度积分 1/11 3:03
BST_fCarPosition +=BST_fBluetoothSpeed;
//积分上限设限//
if((s32)BST_fCarPosition > SPEED_INTEGRAL_MAX) BST_fCarPosition = SPEED_INTEGRAL_MAX;
if((s32)BST_fCarPosition < SPEED_INTEGRAL_MIN) BST_fCarPosition = SPEED_INTEGRAL_MIN;
mator.speed_pwm = (BST_fCarSpeedOld -CAR_SPEED_SET) * BST_fCarSpeed_P + (BST_fCarPosition - CAR_POSITION_SET) * BST_fCarSpeed_I; //速度PI算法 速度*P +位移*I=速度PWM输出
}3、转向环（PD）
主要是偏航角作为Kd的系数
代码如下
float turn_pid_realize(uint8_t ccd,short yaw)
{
float turn=0;
turn=(Turn_val-yaw)*Turn_Kd;
//printf("turn=%d\r\n",yaw);
return turn;
}五、蓝牙通信部分
使用串口3与蓝牙进行通信，手机通过蓝牙助手给蓝牙发送消息，蓝牙模块通过串口发送消息到小车。
通信协议代码如下
void USART3_IRQHandler(void) //串口x中断服务程序
{
uint8_t Res;
// uint8_t i;
if(USART_GetITStatus(USART3,USART_IT_RXNE) != RESET)//判断中断位
{
USART_ClearITPendingBit(USART3, USART_IT_RXNE);
Res = USART_ReceiveData(USART3); //接收数据
if(Res!='\n')
{
rx_buf[rx_size++]=Res;
}
else
{
// for(i=0;i<rx_size;i++)
// printf("%c",rx_buf[i]);
// printf("\r\n");
if(memcmp("a1",rx_buf,2)==0)//前进
{
BST_fBluetoothSpeed=100;
//printf("前进\r\n");
}
else if(memcmp("a2",rx_buf,2)==0)//后退
{
BST_fBluetoothSpeed=-100;
//printf("后退\r\n");
}
else if(memcmp("a3",rx_buf,2)==0)//左转
{
BST_fBluetoothSpeed=0;
Turn_val=90;
Yaw_old=Yaw;
//printf("左转\r\n");
}
else if(memcmp("a4",rx_buf,2)==0)//右转
{
BST_fBluetoothSpeed=0;
Turn_val=-90;
Yaw_old=Yaw;
//printf("右转\r\n");
}
else if(memcmp("a0",rx_buf,2)==0)//停下
{
BST_fBluetoothSpeed=0;
//printf("停下\r\n");
}
rx_size=0;
}
}
} 总结
经过对平衡车的学习，我对pid算法有了更加深刻的理解，在mpu6050陀螺仪的使用也越来越熟练，平衡小车的核心就是pid算法，所以pid算法对小车和控制类的学习是十分重要的。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
