第1页 / 共8页
第2页 / 共8页
第3页 / 共8页
第4页 / 共8页
第5页 / 共8页
第6页 / 共8页
第7页 / 共8页
第8页 / 共8页
STM32 小车程序（最好的）.docx
#include "stm32f10x_lib.h"
#include
#define Kp
#define Ki
#define Kd
18
0
0.5
// 比例常数 Proportional Const
// 积分常数 Integral Const
// 微分常数 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
void Delay_ms(u32 nTime);
active(void);
void ADC_result(void);
u16
u32
u16
AD_Value[16];
TimingDelay;
CCR1_Val =500;
float gyro;
float
acceler;
s16
s16
Gyro;
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_HSEConfig(RCC_HSE_ON);
HSEStartUpStatus=RCC_WaitForHSEStartUp();
if(HSEStartUpStatus==SUCCESS)
{
//RCC 复位
// 开启 HSE 振荡器
//等待 HSE 完成启动
//配置 AHB,APB2,APB1 的时钟
RCC_HCLKConfig(RCC_SYSCLK_Div1);
RCC_PCLK2Config(RCC_HCLK_Div1);
RCC_PCLK1Config(RCC_HCLK_Div2);
FLASH_SetLatency(FLASH_Latency_2);
FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); //使能预取
RCC_PLLConfig(RCC_PLLSource_HSE_Div1,RCC_PLLMul_9);
RCC_PLLCmd(ENABLE);
while(RCC_GetFlagStatus(RCC_FLAG_PLLRDY)==RESET)
{
}
RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);//设置 PLL 为系统时钟源
while(RCC_GetSYSCLKSource()!=0x08)
{
}
//PLL 是否就绪
//使能 PLL
//PLLCLK=72Mhz
}
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;
Gyro=(result[1]>>3)-1510;
acceler=(float)(AccleX/409.6);
if(acceler>1)
acceler=1;
else if(acceler<-1)
acceler=-1;
acceler=asin(acceler)*57.296;
gyro=Gyro*0.0644;
Kalman_Filter(acceler,gyro);
}
//kalman filter
//AccleX=(result[0]*3.3/(4096*4));
//Gyro=(result[1]*3.3/(4096*4));
//弧度转换 180/3.14
//3.3/（4096*25mv）
//注意：dt 的取值为 kalman
//外部需要引用的变量
static float angle, angle_dot;
static float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;
滤波器采样时间;
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)
{
angle+=(gyro_m-q_bias) * dt;//先验估计
//gyro_m:gyro_measure
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;
+= K_0 * angle_err;//后验估计
+= K_1 * angle_err;//后验估计
angle
q_bias
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)
if(angle<-2)
if((angle<=2)&&(angle>=-2))
FORWARD_Init();
BACKWARD_Init();
STOP_Init();
CCR1_Val=0-pwm;
pwm=angle*Kp+angle_dot*Kd;
if(pwm<0)
else
CCR1_Val+=30;
if(angle>20||angle<-20)
if(CCR1_Val>999)
CCR1_Val=pwm;
//最终输出
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);
}
//======================================================
相关推荐
2023年江西萍乡中考道德与法治真题及答案.doc
2012年重庆南川中考生物真题及答案.doc
2013年江西师范大学地理学综合及文艺理论基础考研真题.doc
2020年四川甘孜小升初语文真题及答案I卷.doc
2020年注册岩土工程师专业基础考试真题及答案.doc
2023-2024学年福建省厦门市九年级上学期数学月考试题及答案.doc
2021-2022学年辽宁省沈阳市大东区九年级上学期语文期末试题及答案.doc
2022-2023学年北京东城区初三第一学期物理期末试卷及答案.doc
2018上半年江西教师资格初中地理学科知识与教学能力真题及答案.doc
2012年河北国家公务员申论考试真题及答案-省级.doc
2020-2021学年江苏省扬州市江都区邵樊片九年级上学期数学第一次质量检测试题及答案.doc
2022下半年黑龙江教师资格证中学综合素质真题及答案.doc
