目錄

前言

一、電機(jī)驅(qū)動(dòng)部分

1、TB6612FNG電機(jī)驅(qū)動(dòng)模塊接線方式：

2、代碼使用定時(shí)器2的4路輸出pwm

3、gpio引腳初始化，以及前進(jìn)，后退引腳設(shè)置

二、MPU6050陀螺儀部分

三、編碼器捕獲部分

四、pid部分

1、直立環(huán)KD

2、速度環(huán)KI

3、轉(zhuǎn)向環(huán)（PD）

五、藍(lán)牙通信部分

總結(jié)

前言

經(jīng)過幾天對(duì)平衡車的理論學(xué)習(xí)和動(dòng)手實(shí)踐，終于完成了平衡車的基本功能，實(shí)現(xiàn)前進(jìn)，后退，直立，轉(zhuǎn)向。本項(xiàng)目用到了兩個(gè)電機(jī)，一個(gè)兩塊亞力克板，一個(gè)mpu6050陀螺儀，TB6612FNG電機(jī)驅(qū)動(dòng)模塊，通信方式使用藍(lán)牙模塊進(jìn)行簡(jiǎn)單的通信，主控芯片使用stm32f103c8t6。

一、電機(jī)驅(qū)動(dòng)部分

1、TB6612FNG電機(jī)驅(qū)動(dòng)模塊接線方式：

VM 接12V電源，給電機(jī)供電

STBY 置1使能AIN1，AIN2，BIN1，BIN2

VCC 接5V電源

GND 接地

AO1，AO2 輸出控制電機(jī)1正反轉(zhuǎn)

BO1，BO2 輸入控制電機(jī)2正反轉(zhuǎn)

PWMA 控制AO1，AO2使能

PWMB 控制BO1，BO2使能

AIN1，AIN2 輸入控制控制電機(jī)1正反轉(zhuǎn)

BIN1，BIN2?輸入控制控制電機(jī)2正反轉(zhuǎn)

2、代碼使用定時(shí)器2的4路輸出pwm

具體代碼如下

void time2_pwm_init(uint16_t arr,uint16_t pre)
{
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);//定時(shí)器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);//啟動(dòng)定時(shí)器
}

3、gpio引腳初始化，以及前進(jìn)，后退引腳設(shè)置

代碼

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庫(kù)

代碼如下

//采集俯仰角，翻滾角，偏航角
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狀態(tài)指示燈 STM32核心板 PC13 綠色燈亮起為不正常
	 {
	 GPIO_ResetBits(GPIOC, GPIO_Pin_13);				//MPU6050狀態(tài)指示燈 STM32核心板 PC13 綠色燈亮起為不正常
	 while(1);
	 }
	 
}

三、編碼器捕獲部分

使用定時(shí)器3，和4的通道1和通道2進(jìn)行4倍頻地計(jì)數(shù)，主要代碼如下：

void time3_encoder_init()
{
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);//定時(shí)器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);//配置編碼器計(jì)數(shù)。BothEdge（底部邊緣）
    
    //初始化標(biāo)志位,計(jì)數(shù)器
    TIM_ClearFlag(TIM3,TIM_FLAG_Update);//清除標(biāo)志位
    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;//搶占優(yōu)先級(jí)；
    nvic_init.NVIC_IRQChannelSubPriority=1;//響應(yīng)優(yōu)先級(jí)
    NVIC_Init(&nvic_init);
    
    TIM_ITConfig(TIM3,TIM_IT_Update | TIM_IT_CC1 |TIM_IT_CC2,ENABLE);//配置定時(shí)器，允許更新中斷,CC1,CC2捕獲中斷
    TIM_Cmd(TIM3,ENABLE);//開啟定時(shí)器
}

void time4_encoder_init()
{
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//定時(shí)器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);//配置編碼器計(jì)數(shù)。BothEdge（底部邊緣）
    
    //初始化標(biāo)志位,計(jì)數(shù)器
    TIM_ClearFlag(TIM4,TIM_FLAG_Update);//清除標(biāo)志位
    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;//搶占優(yōu)先級(jí)；
    nvic_init.NVIC_IRQChannelSubPriority=1;//響應(yīng)優(yōu)先級(jí)
    NVIC_Init(&nvic_init);
    
    TIM_ITConfig(TIM4,TIM_IT_Update | TIM_IT_CC1 |TIM_IT_CC2,ENABLE);//配置定時(shí)器，允許更新中斷,CC1,CC2捕獲中斷
    TIM_Cmd(TIM4,ENABLE);//開啟定時(shí)器
}

四、pid部分

使用定時(shí)器1每隔5ms進(jìn)行一次mpu6050的數(shù)據(jù)采集，并進(jìn)行直立環(huán)控制，每40ms，進(jìn)行一次速度環(huán)控制，防止影響直立控制。

1、直立環(huán)KD

小車的偏轉(zhuǎn)角度作為Kp的系數(shù)，角加速度作為Kd的系數(shù)

代碼如下：

float angle_pid_realize(struct _pid *pid,float angle)
{
    if(Pitch==0||Pitch<-20||Pitch>20)              //MPU6050狀態(tài)指示燈 STM32核心板 PC13 綠色燈亮起為不正常
    {
          //Pitch=1;
      GPIO_ResetBits(GPIOC, GPIO_Pin_13);          //MPU6050狀態(tài)指示燈 STM32核心板 PC13 綠色燈亮起為不正常
    }
    else 
    {GPIO_SetBits(GPIOC, GPIO_Pin_13);}             //MPU6050狀態(tài)正常時(shí)LED燈熄滅
    
    pid->err=angle-pid->target_val;//計(jì)算目標(biāo)值與實(shí)際值的誤差
    pid->actual_val=pid->Kp*pid->err+pid->Kd*gyro[0];//角度PD控制,gyro[0]x軸偏轉(zhuǎn)角速度
    return pid->actual_val;
}

2、速度環(huán)KI

速度環(huán)控制小車的位移，實(shí)現(xiàn)定點(diǎn)停下的功能，代碼如下

void speed_control(void)
{
    mator.car_speed = (mator.left_tal_count  + mator.right_tal_count );// * 0.5 ;          //左右電機(jī)脈沖數(shù)平均值作為小車當(dāng)前車速
    mator.left_tal_count =mator.right_tal_count = 0;      //全局變量 注意及時(shí)清零        
    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;
    
    //積分上限設(shè)限//
    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、轉(zhuǎn)向環(huán)（PD）

主要是偏航角作為Kd的系數(shù)

代碼如下

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

五、藍(lán)牙通信部分

使用串口3與藍(lán)牙進(jìn)行通信，手機(jī)通過藍(lán)牙助手給藍(lán)牙發(fā)送消息，藍(lán)牙模塊通過串口發(fā)送消息到小車。

通信協(xié)議代碼如下

void USART3_IRQHandler(void)                	//串口x中斷服務(wù)程序
{
    uint8_t Res;
//    uint8_t i;
    if(USART_GetITStatus(USART3,USART_IT_RXNE) !=  RESET)//判斷中斷位
    {
        USART_ClearITPendingBit(USART3, USART_IT_RXNE);
        Res = USART_ReceiveData(USART3); //接收數(shù)據(jù)
        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)//前進(jìn)
            {
                BST_fBluetoothSpeed=100;
                //printf("前進(jìn)\r\n");
            }
            else if(memcmp("a2",rx_buf,2)==0)//后退
            {
                BST_fBluetoothSpeed=-100;
                //printf("后退\r\n");
            }
            else if(memcmp("a3",rx_buf,2)==0)//左轉(zhuǎn)
            {
                BST_fBluetoothSpeed=0;
                Turn_val=90;
                Yaw_old=Yaw;
                //printf("左轉(zhuǎn)\r\n");
            }
            else if(memcmp("a4",rx_buf,2)==0)//右轉(zhuǎn)
            {
                BST_fBluetoothSpeed=0;
                Turn_val=-90;
                 Yaw_old=Yaw;
                //printf("右轉(zhuǎn)\r\n");
            }
            else if(memcmp("a0",rx_buf,2)==0)//停下
            {
                BST_fBluetoothSpeed=0;
                //printf("停下\r\n");
            }
            rx_size=0;
        }
    }
} 

總結(jié)

????????經(jīng)過對(duì)平衡車的學(xué)習(xí)，我對(duì)pid算法有了更加深刻的理解，在mpu6050陀螺儀的使用也越來越熟練，平衡小車的核心就是pid算法，所以pid算法對(duì)小車和控制類的學(xué)習(xí)是十分重要的。文章來源地址http://www.zghlxwxcb.cn/news/detail-707638.html

到了這里，關(guān)于基于stm32的平衡小車的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！