電機(jī)編碼器解碼

1、/*功能名稱IM3_PWM_Init(u16 arr,u16 psc) 描述 TIM3產(chǎn)生四路PWM*/void TIM3_PWM_Init(u16 arr,u16 psc) GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Perip

2、h_GPIOC | RCC_APB2Periph_AFIO, ENABLE); /使能GPIO外設(shè)和AFIO復(fù)用功能模塊時鐘使能 GPIO_PinRemapConfig(GPIO_FullRemap_TIM3, ENABLE); /Timer3全映射 GPIOC-> 6,7,8,9 /用于TIM3的CH2輸出的PWM通過該LED顯示 /設(shè)置該引腳為復(fù)用輸出功能,輸出TIM3 CH1 CH2 CH3 CH4 的PWM脈沖波形 GPIO_InitStructure.GPIO_Pin =GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9; /初始

3、化GPIO GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; /復(fù)用推挽輸出 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOC, &GPIO_InitStructure); GPIO_ResetBits(GPIOC,GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9);/默認(rèn)電機(jī)使能端狀態(tài)：不使能 TIM_TimeBaseStructure.TIM_Period = arr; /設(shè)置在下一個更新事件裝入活動的自動

4、重裝載寄存器周期的值 TIM_TimeBaseStructure.TIM_Prescaler =psc; /設(shè)置用來作為TIMx時鐘頻率除數(shù)的預(yù)分頻值 這里是72分頻，那么時鐘頻率就是1M TIM_TimeBaseStructure.TIM_ClockDivision = 0; /設(shè)置時鐘分割:TDTS = Tck_tim TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; /TIM向上計(jì)數(shù)模式 TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); /根據(jù)TIM_TimeBas

5、eInitStruct中指定的參數(shù)初始化TIMx的時間基數(shù)單位 TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; /選擇定時器模式:TIM脈沖寬度調(diào)制模式1 TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; /比較輸出使能 TIM_OCInitStructure.TIM_Pulse = 0; /設(shè)置待裝入捕獲比較寄存器的脈沖值 TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; /輸出極性:TIM輸出比較極性

6、高 TIM_OC1Init(TIM3, &TIM_OCInitStructure); /根據(jù)TIM_OCInitStruct中指定的參數(shù)初始化外設(shè)TIMx TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable); /使能TIMx在CCR1上的預(yù)裝載寄存器 TIM_OC2Init(TIM3, &TIM_OCInitStructure); /根據(jù)TIM_OCInitStruct中指定的參數(shù)初始化外設(shè)TIMx TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable); /使能TIMx在CCR2上的預(yù)裝載

7、寄存器 TIM_OC3Init(TIM3, &TIM_OCInitStructure); /根據(jù)TIM_OCInitStruct中指定的參數(shù)初始化外設(shè)TIMx TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable); /使能TIMx在CCR3上的預(yù)裝載寄存器 TIM_OC4Init(TIM3, &TIM_OCInitStructure); /根據(jù)TIM_OCInitStruct中指定的參數(shù)初始化外設(shè)TIMx TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable); /使能TIMx在CCR4上的預(yù)

8、裝載寄存器 TIM_ARRPreloadConfig(TIM3, ENABLE); /使能TIMx在ARR上的預(yù)裝載寄存器 TIM_Cmd(TIM3, ENABLE); /使能TIMx外設(shè) /*功能名稱TIM4_PWMINPUT_INIT(u16 arr,u16 psc) 描述 PWM輸入初始化*/void TIM4_PWMINPUT_INIT(u16 arr,u16 psc) TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; /TIM的初始化結(jié)構(gòu)體 NVIC_InitTypeDef NVIC_InitStructure; /中斷配置 TIM_ICI

9、nitTypeDef TIM4_ICInitStructure; /TIM4 PWM配置結(jié)構(gòu)體 GPIO_InitTypeDef GPIO_InitStructure; /IO口配置結(jié)構(gòu)體 RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); /Open TIM4 clock RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); /open gpioB clock GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7; /GPIO 7 GPIO_InitStru

10、cture.GPIO_Mode = GPIO_Mode_IPU; /上拉輸入 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOB, &GPIO_InitStructure); TIM_TimeBaseStructure.TIM_Period = arr; /設(shè)置在下一個更新事件裝入活動的自動重裝載寄存器周期的值 TIM_TimeBaseStructure.TIM_Prescaler =psc; /設(shè)置用來作為TIMx時鐘頻率除數(shù)的預(yù)分頻值 TIM_TimeBaseStructure.TIM_ClockDiv

11、ision = 0; /設(shè)置時鐘分割:TDTS = Tck_tim TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; /TIM向上計(jì)數(shù)模式 TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); /根據(jù)TIM_TimeBaseInitStruct中指定的參數(shù)初始化TIMx的時間基數(shù)單位 /*配置中斷優(yōu)先級*/ NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn; NVIC_InitStructure.NVIC_IRQChannelPre

12、emptionPriority = 1; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); TIM4_ICInitStructure.TIM_Channel = TIM_Channel_2; TIM4_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; TIM4_ICInitStructure.TIM_ICSelect

13、ion = TIM_ICSelection_DirectTI; TIM4_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; TIM4_ICInitStructure.TIM_ICFilter = 0x3; /Filter:過濾 TIM_PWMIConfig(TIM4, &TIM4_ICInitStructure); /PWM輸入配置 TIM_SelectInputTrigger(TIM4, TIM_TS_TI2FP2); /選擇有效輸入端 TIM_SelectSlaveMode(TIM4, TIM_SlaveMode_Reset); /

14、配置為主從復(fù)位模式 TIM_SelectMasterSlaveMode(TIM4, TIM_MasterSlaveMode_Enable);/啟動定時器的被動觸發(fā) TIM_ITConfig(TIM4, TIM_IT_CC2|TIM_IT_Update, ENABLE); /中斷配置 TIM_ClearITPendingBit(TIM4, TIM_IT_CC2|TIM_IT_Update); /清除中斷標(biāo)志位 TIM_Cmd(TIM4, ENABLE); void TIM4_IRQHandler(void) if (TIM_GetITStatus(TIM4, TIM_IT_CC2) != RES

15、ET)/捕獲1發(fā)生捕獲事件 duty_TIM4 = TIM_GetCapture1(TIM4); /采集占空比 if (TIM_GetCapture2(TIM4)>600) period_TIM4 = TIM_GetCapture2(TIM4);/簡單的處理 CollectFlag_TIM4 = 0; TIM_ClearITPendingBit(TIM4, TIM_IT_CC2|TIM_IT_Update); /清除中斷標(biāo)志位/*功能名稱TIM1_PWMINPUT_INIT(u16 arr,u16 psc) 描述 PWM輸入初始化*/ void TIM1_PWMINPUT_INIT(u1

16、6 arr,u16 psc) TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; /TIM的初始化結(jié)構(gòu)體 NVIC_InitTypeDef NVIC_InitStructure; /中斷配置 TIM_ICInitTypeDef TIM1_ICInitStructure; /PWM配置結(jié)構(gòu)體 GPIO_InitTypeDef GPIO_InitStructure; /IO口配置結(jié)構(gòu)體 RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); /Open TIM1 clock RCC_APB2PeriphCl

17、ockCmd(RCC_APB2Periph_GPIOE, ENABLE); /open gpioE clock GPIO_PinRemapConfig(GPIO_FullRemap_TIM1, ENABLE); /Timer1完全重映射 TIM1_CH2->PE11 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11; /GPIO 11 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; /上拉輸入 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init

18、(GPIOE, &GPIO_InitStructure); TIM_TimeBaseStructure.TIM_Period = arr; /設(shè)置在下一個更新事件裝入活動的自動重裝載寄存器周期的值 TIM_TimeBaseStructure.TIM_Prescaler =psc; /設(shè)置用來作為TIMx時鐘頻率除數(shù)的預(yù)分頻值 TIM_TimeBaseStructure.TIM_ClockDivision = 0; /設(shè)置時鐘分割:TDTS = Tck_tim TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; /TIM

19、向上計(jì)數(shù)模式 TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure); /根據(jù)TIM_TimeBaseInitStruct中指定的參數(shù)初始化TIMx的時間基數(shù)單位 /*配置中斷優(yōu)先級*/ NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn; /TIM1捕獲中斷 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; NVIC_InitStru

20、cture.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); TIM1_ICInitStructure.TIM_Channel = TIM_Channel_2; TIM1_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; TIM1_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; TIM1_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1

21、; TIM1_ICInitStructure.TIM_ICFilter = 0x03; /Filter:過濾 TIM_PWMIConfig(TIM1, &TIM1_ICInitStructure); /PWM輸入配置 TIM_SelectInputTrigger(TIM1, TIM_TS_TI2FP2); /選擇有效輸入端 TIM_SelectSlaveMode(TIM1, TIM_SlaveMode_Reset); /配置為主從復(fù)位模式 TIM_SelectMasterSlaveMode(TIM1, TIM_MasterSlaveMode_Enable);/啟動定時器的被動觸發(fā) /

22、TIM_ITConfig(TIM1, TIM_IT_CC2|TIM_IT_Update, ENABLE); /中斷配置 TIM_ITConfig(TIM1, TIM_IT_CC2, ENABLE); /通道2 捕獲中斷打開 /TIM_ClearITPendingBit(TIM1, TIM_IT_CC2|TIM_IT_Update); /清除中斷標(biāo)志位 TIM_Cmd(TIM1, ENABLE); void TIM1_CC_IRQHandler(void) if (TIM_GetITStatus(TIM1, TIM_IT_CC2) != RESET)/捕獲1發(fā)生捕獲事件 duty_TIM1 =

23、TIM_GetCapture1(TIM1); /采集占空比 if (TIM_GetCapture2(TIM1)>600) period_TIM1 = TIM_GetCapture2(TIM1); CollectFlag_TIM1 = 0; TIM_ClearITPendingBit(TIM1, TIM_IT_CC2|TIM_IT_Update); /清除中斷標(biāo)志位 /*功能名稱TIM2_PWMINPUT_INIT(u16 arr,u16 psc) 描述 PWM輸入初始化*/void TIM2_PWMINPUT_INIT(u16 arr,u16 psc) TIM_TimeBaseInitT

24、ypeDef TIM_TimeBaseStructure; /TIM的初始化結(jié)構(gòu)體 NVIC_InitTypeDef NVIC_InitStructure; /中斷配置 TIM_ICInitTypeDef TIM2_ICInitStructure; /TIM2 PWM配置結(jié)構(gòu)體 GPIO_InitTypeDef GPIO_InitStructure; /IO口配置結(jié)構(gòu)體 RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); /Open TIM2 clock / RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPI

25、OB, ENABLE); /open gpioB clock RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE); /使能GPIO外設(shè)和AFIO復(fù)用功能模塊時鐘 GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE); /關(guān)閉JTAG GPIO_PinRemapConfig(GPIO_FullRemap_TIM2, ENABLE); /Timer2完全重映射 TIM2_CH2->PB3 GPIO_InitStructure.GPIO_Pi

26、n = GPIO_Pin_3; /GPIO 3 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; /浮空輸入 上拉輸入 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOB, &GPIO_InitStructure); TIM_TimeBaseStructure.TIM_Period = arr; /設(shè)置在下一個更新事件裝入活動的自動重裝載寄存器周期的值 TIM_TimeBaseStructure.TIM_Prescaler =psc; /設(shè)置用來作為TIMx時鐘頻率

27、除數(shù)的預(yù)分頻值 TIM_TimeBaseStructure.TIM_ClockDivision = 0; /設(shè)置時鐘分割:TDTS = Tck_tim TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; /TIM向上計(jì)數(shù)模式 TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); /根據(jù)TIM_TimeBaseInitStruct中指定的參數(shù)初始化TIMx的時間基數(shù)單位 /*配置中斷優(yōu)先級*/ NVIC_InitStructure.NVIC_IRQChannel = TIM2_I

28、RQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); TIM2_ICInitStructure.TIM_Channel = TIM_Channel_2; TIM2_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity

29、_Rising; TIM2_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; TIM2_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; TIM2_ICInitStructure.TIM_ICFilter = 0x3; /Filter:過濾 TIM_PWMIConfig(TIM2, &TIM2_ICInitStructure); /PWM輸入配置 TIM_SelectInputTrigger(TIM2, TIM_TS_TI2FP2); /選擇有效輸入端 TIM_Sel

30、ectSlaveMode(TIM2, TIM_SlaveMode_Reset); /配置為主從復(fù)位模式 TIM_SelectMasterSlaveMode(TIM2, TIM_MasterSlaveMode_Enable);/啟動定時器的被動觸發(fā) TIM_ITConfig(TIM2, TIM_IT_CC2|TIM_IT_Update, ENABLE); /中斷配置 TIM_ClearITPendingBit(TIM2, TIM_IT_CC2|TIM_IT_Update); /清除中斷標(biāo)志位 TIM_Cmd(TIM2, ENABLE); void TIM2_IRQHandler(void) if

31、 (TIM_GetITStatus(TIM2, TIM_IT_CC2) != RESET)/捕獲1發(fā)生捕獲事件 duty_TIM2 = TIM_GetCapture1(TIM2); /采集占空比 if (TIM_GetCapture2(TIM2)>600) period_TIM2 = TIM_GetCapture2(TIM2); CollectFlag_TIM2 = 0; TIM_ClearITPendingBit(TIM2, TIM_IT_CC2|TIM_IT_Update); /清除中斷標(biāo)志位 /*功能名稱TIM5_PWMINPUT_INIT(u16 arr,u16 psc) 描述

32、PWM輸入初始化*/void TIM5_PWMINPUT_INIT(u16 arr,u16 psc) TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; /TIM的初始化結(jié)構(gòu)體 NVIC_InitTypeDef NVIC_InitStructure; /中斷配置 TIM_ICInitTypeDef TIM5_ICInitStructure; /TIM4 PWM配置結(jié)構(gòu)體 GPIO_InitTypeDef GPIO_InitStructure; /IO口配置結(jié)構(gòu)體 RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, E

33、NABLE); /Open TIM4 clock RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); /open gpioB clock GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1; /GPIO 1 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; /浮空輸入 上拉輸入 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOA, &GPIO_InitStructure); TIM

34、_TimeBaseStructure.TIM_Period = arr; /設(shè)置在下一個更新事件裝入活動的自動重裝載寄存器周期的值 TIM_TimeBaseStructure.TIM_Prescaler =psc; /設(shè)置用來作為TIMx時鐘頻率除數(shù)的預(yù)分頻值 TIM_TimeBaseStructure.TIM_ClockDivision = 0; /設(shè)置時鐘分割:TDTS = Tck_tim TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; /TIM向上計(jì)數(shù)模式 TIM_TimeBaseInit(TIM5, &TI

35、M_TimeBaseStructure); /根據(jù)TIM_TimeBaseInitStruct中指定的參數(shù)初始化TIMx的時間基數(shù)單位 /*配置中斷優(yōu)先級*/ NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&

36、NVIC_InitStructure); TIM5_ICInitStructure.TIM_Channel = TIM_Channel_2; TIM5_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; TIM5_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; TIM5_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; TIM5_ICInitStructure.TIM_ICFilter = 0x3; /Filter

37、:過濾 TIM_PWMIConfig(TIM5, &TIM5_ICInitStructure); /PWM輸入配置 TIM_SelectInputTrigger(TIM5, TIM_TS_TI2FP2); /選擇有效輸入端 TIM_SelectSlaveMode(TIM5, TIM_SlaveMode_Reset); /配置為主從復(fù)位模式 TIM_SelectMasterSlaveMode(TIM5, TIM_MasterSlaveMode_Enable);/啟動定時器的被動觸發(fā) TIM_ITConfig(TIM5, TIM_IT_CC2|TIM_IT_Update, ENABLE);

38、 /中斷配置 TIM_ClearITPendingBit(TIM5, TIM_IT_CC2|TIM_IT_Update); /清除中斷標(biāo)志位 TIM_Cmd(TIM5, ENABLE); void TIM5_IRQHandler(void) if (TIM_GetITStatus(TIM5, TIM_IT_CC2) != RESET)/捕獲1發(fā)生捕獲事件 duty_TIM5 = TIM_GetCapture1(TIM5); /采集占空比 if (TIM_GetCapture2(TIM5)>600) period_TIM5 = TIM_GetCapture2(TIM5); CollectF

39、lag_TIM5 = 0; TIM_ClearITPendingBit(TIM5, TIM_IT_CC2|TIM_IT_Update); /清除中斷標(biāo)志位PID部分typedef struct int setpoint;/設(shè)定目標(biāo) int sum_error;/誤差累計(jì) float proportion ;/比例常數(shù) float integral ;/積分常數(shù) float derivative;/微分常數(shù) int last_error;/e-1 int prev_error;/e-2PIDtypedef;float Kp = 0.32 ; /比例常數(shù)float Ti = 0.09 ; /積分時

40、間常數(shù)float Td = 0.0028 ; /微分時間常數(shù)#define T 0.02 /采樣周期#define Ki Kp*(T/Ti) / Kp Ki Kd 三個主要參數(shù)#define Kd Kp*(Td/T)PID.Hvoid PIDperiodinit(u16 arr,u16 psc); /PID 采樣定時器設(shè)定void incPIDinit(void); /初始化，參數(shù)清零清零int incPIDcalc(PIDtypedef*PIDx,u16 nextpoint); /PID計(jì)算void PID_setpoint(PIDtypedef*PIDx,u16 setvalue); /設(shè)定 PID預(yù)期值void PID_set(float pp,float ii,float dd);/設(shè)定PID kp ki kd三個參數(shù)void set_speed(float W1,float W2,float W3,float W4);/設(shè)定四個電機(jī)的目標(biāo)轉(zhuǎn)速int incPIDcalc(PIDtypedef *PIDx,u16 nextpoint) int iError,iincpid; iError=PIDx->se