main.c

STM32做的自平衡小车

#include "stm32f10x_lib.h"
#include <math.h>

#define Kp    18         // 比例常数 Proportional Const 
#define Ki    0          // 积分常数 Integral Const 
#define Kd    0.5          // 微分常数 Derivative Const 

void  RCC_Configuration(void);
void  GPIO_Configuration(void);
void  ADC_Configuration(void);
void  DMA_Configuration(void);
void  SysTick_Configuration(void);
void  TIM1_Configuration(void);

void  FORWARD_Init(void);
void  BACKWARD_Init(void);
void  STOP_Init(void);

void  PID_algorithm(double filer_angle);
void  Kalman_Filter(float ,float);
void  active(void);
void  Delay_ms(u32 nTime);

void  ADC_result(void);

u16    AD_Value[16];
u32    TimingDelay;
u16    CCR1_Val =500;

float  gyro;
float  acceler;

s16    Gyro;
s16    AccleX;

int main(void){
	RCC_Configuration(); 
	GPIO_Configuration();
	DMA_Configuration(); 
	ADC_Configuration();
	SysTick_Configuration();
	TIM1_Configuration();
	while (1){
		Delay_ms(10); 
	    ADC_result();
        active();	
    }
}


void RCC_Configuration(void){  
  ErrorStatus  HSEStartUpStatus; 
  RCC_DeInit();					 //RCC复位
  RCC_HSEConfig(RCC_HSE_ON);	  //  开启HSE振荡器
  HSEStartUpStatus=RCC_WaitForHSEStartUp();	  //等待HSE完成启动
  if(HSEStartUpStatus==SUCCESS)
  {
    RCC_HCLKConfig(RCC_SYSCLK_Div1);
    RCC_PCLK2Config(RCC_HCLK_Div1);
	RCC_PCLK1Config(RCC_HCLK_Div2);		 //配置AHB,APB2,APB1的时钟
    FLASH_SetLatency(FLASH_Latency_2);
	FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); //使能预取
	RCC_PLLConfig(RCC_PLLSource_HSE_Div1,RCC_PLLMul_9);	//PLLCLK=72Mhz
	RCC_PLLCmd(ENABLE);  //使能PLL
	while(RCC_GetFlagStatus(RCC_FLAG_PLLRDY)==RESET)
	{
	}			  //PLL是否就绪
	RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);//设置PLL为系统时钟源
	while(RCC_GetSYSCLKSource()!=0x08)
	{
	}
  }
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_ALL,ENABLE); 
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_ALL , ENABLE);
  RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
}

void GPIO_Configuration(void){
	GPIO_InitTypeDef GPIO_InitStructure;
	
  //配置ADC
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
  GPIO_Init(GPIOA, &GPIO_InitStructure);

  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
  GPIO_Init(GPIOA, &GPIO_InitStructure);

  //配置PWM输出引脚
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_Init(GPIOA, &GPIO_InitStructure);

  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_Init(GPIOA, &GPIO_InitStructure);

  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; 
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  GPIO_Init(GPIOC, &GPIO_InitStructure);


  //电机方向控制端口
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;				    
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_Init(GPIOD, &GPIO_InitStructure);
  
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14|GPIO_Pin_15;				    
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_Init(GPIOD, &GPIO_InitStructure);				
}

void DMA_Configuration(void){
	DMA_InitTypeDef DMA_InitStructure;

	/* DMA channel1 configuration ----------------------------------------------*/
  	DMA_DeInit(DMA1_Channel1);
  	DMA_InitStructure.DMA_PeripheralBaseAddr = 0x4001244C;
  	DMA_InitStructure.DMA_MemoryBaseAddr = (u32)AD_Value;
  	DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
  	DMA_InitStructure.DMA_BufferSize = 16;
  	DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
  	DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
  	DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
  	DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
  	DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
  	DMA_InitStructure.DMA_Priority = DMA_Priority_High;
  	DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
  	DMA_Init(DMA1_Channel1, &DMA_InitStructure);
  
  	/* Enable DMA channel1 */
  	DMA_Cmd(DMA1_Channel1, ENABLE);
}


void ADC_Configuration(void){
	ADC_InitTypeDef ADC_InitStructure;

	/* ADC1 configuration ------------------------------------------------------*/
  ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;
  ADC_InitStructure.ADC_ScanConvMode = ENABLE;
  ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
  ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
  ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
  ADC_InitStructure.ADC_NbrOfChannel = 2;
  ADC_Init(ADC1, &ADC_InitStructure);
  ADC_TempSensorVrefintCmd(ENABLE);
  ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_71Cycles5);
  ADC_RegularChannelConfig(ADC1, ADC_Channel_1, 2, ADC_SampleTime_71Cycles5);
  ADC_DMACmd(ADC1, ENABLE);
  ADC_Cmd(ADC1, ENABLE);
  ADC_ResetCalibration(ADC1);
  while(ADC_GetResetCalibrationStatus(ADC1));
  ADC_StartCalibration(ADC1);
  while(ADC_GetCalibrationStatus(ADC1));
  ADC_SoftwareStartConvCmd(ADC1, ENABLE);
}

void  TIM1_Configuration(void){
    TIM_TimeBaseInitTypeDef TIM_BaseInitStructure; 
    TIM_OCInitTypeDef TIM_OCInitStructure; 

    //TIM1基本计数器设置（设置PWM频率） 
    //频率=TIM1_CLK/(ARR+1) 
    TIM_BaseInitStructure.TIM_Period =1000; 
    TIM_BaseInitStructure.TIM_Prescaler =71; 
    TIM_BaseInitStructure.TIM_ClockDivision = 0; 
    TIM_BaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up; 
    TIM_BaseInitStructure.TIM_RepetitionCounter = 0; 
    TIM_TimeBaseInit(TIM1, &TIM_BaseInitStructure); 
    //启用ARR的影子寄存器（直到产生更新事件才更改设置） 
    TIM_ARRPreloadConfig(TIM1, ENABLE); 


    //TIM1_OC1模块设置（设置1通道占空比） 
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; 
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; 
    TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; 
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; 
    TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; 
    TIM_OCInitStructure.TIM_Pulse =CCR1_Val; 
    TIM_OC3Init(TIM1, &TIM_OCInitStructure); 
    //启用CCR1寄存器的影子寄存器（直到产生更新事件才更改设置） 
    TIM_OC3PreloadConfig(TIM1, TIM_OCPreload_Enable); 
    //TIM2_OC2模块设置（设置2通道占空比） 
    TIM_OCInitStructure.TIM_Pulse =CCR1_Val; 
    TIM_OC4Init(TIM1, &TIM_OCInitStructure); 
    //启用CCR2寄存器的影子寄存器（直到产生更新事件才更改设置） 
    TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable); 
    
    //TIM1开启 
    TIM_Cmd(TIM1, ENABLE); 
    //TIM1_OC通道输出PWM（一定要加） 
    TIM_CtrlPWMOutputs(TIM1, ENABLE); 

} 

void SysTick_Configuration(void){
	SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK);
	NVIC_SystemHandlerPriorityConfig(SystemHandler_SysTick, 3, 0);
	SysTick_SetReload(72000);
	SysTick_ITConfig(ENABLE);
}


void Delay_ms(u32 nTime){
	SysTick_CounterCmd(SysTick_Counter_Enable);
	TimingDelay = nTime;
	while(TimingDelay != 0); //等待计数至0
	SysTick_CounterCmd(SysTick_Counter_Disable);
	SysTick_CounterCmd(SysTick_Counter_Clear);
}

void ADC_result(void){
	unsigned int result[2]={0,0};
    unsigned char i; 
    for(i=0;i<15;i=i+2)
		result[0]+=*(AD_Value+i);
	for(i=1;i<16;i=i+2)
		result[1]+=*(AD_Value+i);		
	AccleX=(result[0]>>3)-2098;				//AccleX=(result[0]*3.3/(4096*4));
	Gyro=(result[1]>>3)-1510;				    //Gyro=(result[1]*3.3/(4096*4)); 
	acceler=(float)(AccleX/409.6);
	if(acceler>1)
		acceler=1;
	else if(acceler<-1)
		acceler=-1;
    acceler=asin(acceler)*57.296;                    //弧度转换 180/3.14
    gyro=Gyro*0.0644;								  //3.3/（4096*25mv）
	Kalman_Filter(acceler,gyro);
}

//kalman filter

static float angle, angle_dot; 		//外部需要引用的变量
static float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;   //注意：dt的取值为kalman滤波器采样时间;
static float P[2][2] = {	{ 1, 0 },
                	{ 0, 1 }
						};	
static float Pdot[4] ={0,0,0,0};
static char C_0 = 1;
static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//-------------------------------------------------------

void Kalman_Filter(float angle_m,float gyro_m)			//gyro_m:gyro_measure
{
	angle+=(gyro_m-q_bias) * dt;//先验估计
	
	Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分
	Pdot[1]=- P[1][1];
	Pdot[2]=- P[1][1];
	Pdot[3]=Q_gyro;
	
	P[0][0] += Pdot[0] * dt;// Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差
	P[0][1] += Pdot[1] * dt;
	P[1][0] += Pdot[2] * dt;
	P[1][1] += Pdot[3] * dt;
	
	
	angle_err = angle_m - angle;//zk-先验估计
	
	
	PCt_0 = C_0 * P[0][0];
	PCt_1 = C_0 * P[1][0];
	
	E = R_angle + C_0 * PCt_0;
	
	K_0 = PCt_0 / E;//Kk
	K_1 = PCt_1 / E;
	
	t_0 = PCt_0;
	t_1 = C_0 * P[0][1];

	P[0][0] -= K_0 * t_0;//后验估计误差协方差
	P[0][1] -= K_0 * t_1;
	P[1][0] -= K_1 * t_0;
	P[1][1] -= K_1 * t_1;
	
	
	angle	+= K_0 * angle_err;//后验估计
	q_bias	+= K_1 * angle_err;//后验估计
	angle_dot = gyro_m-q_bias;//输出值（后验估计）的微分 = 角速度
}


//====================电机方向控制=====================
void  FORWARD_Init(void) {
   GPIO_SetBits(GPIOD, GPIO_Pin_0);	//亮	
   GPIO_ResetBits(GPIOD, GPIO_Pin_1);	//灭 
   GPIO_SetBits(GPIOD, GPIO_Pin_14);	//亮	
   GPIO_ResetBits(GPIOD, GPIO_Pin_15);	//D2灭 
}

void  BACKWARD_Init(void){
   GPIO_ResetBits(GPIOD, GPIO_Pin_0);	//亮	
   GPIO_SetBits(GPIOD, GPIO_Pin_1);	//灭 
   GPIO_ResetBits(GPIOD, GPIO_Pin_14);	//亮	
   GPIO_SetBits(GPIOD, GPIO_Pin_15);	//D2灭
}

void  STOP_Init(void){                              
   GPIO_ResetBits(GPIOD, GPIO_Pin_0);	//亮	
   GPIO_ResetBits(GPIOD, GPIO_Pin_1);	//灭 
   GPIO_ResetBits(GPIOD, GPIO_Pin_14);	//亮	
   GPIO_ResetBits(GPIOD, GPIO_Pin_15);	//D2灭
}
//======================================================


//===================电机驱动===========================
void  active(void) {  
	TIM_OCInitTypeDef TIM_OCInitStructure; 
	s16		pwm;

	if(angle>2) 					FORWARD_Init();      
    if(angle<-2) 					BACKWARD_Init();
    if((angle<=2)&&(angle>=-2))	    STOP_Init();

	pwm=angle*Kp+angle_dot*Kd;
	if(pwm<0)		CCR1_Val=0-pwm;
	else			CCR1_Val=pwm;
	CCR1_Val+=30;                     //最终输出
	if(angle>20||angle<-20)		CCR1_Val=999;
	if(CCR1_Val>999)	 		CCR1_Val=999;

	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; 
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; 
    TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; 
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; 
    TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; 
    TIM_OCInitStructure.TIM_Pulse =CCR1_Val; 
    TIM_OC3Init(TIM1, &TIM_OCInitStructure);

	TIM_OCInitStructure.TIM_Pulse =CCR1_Val; 
    TIM_OC4Init(TIM1, &TIM_OCInitStructure); 
}
//======================================================