赞
踩
#include "motor.h" double CNT_10ms[4]={0};//每5ms脉冲数 double CNT_10ms_old[4]={0}; static void Dis_Gpio_Init(void) { GPIO_InitTypeDef GPIO_InitStructure; RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7|GPIO_Pin_8|GPIO_Pin_10|GPIO_Pin_12; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;//普通输出模式 GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;//推挽输出 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100MHz GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;//上拉 GPIO_Init(GPIOE, &GPIO_InitStructure);//初始化GPIO GPIO_SetBits(GPIOE,GPIO_Pin_7|GPIO_Pin_8|GPIO_Pin_10|GPIO_Pin_12); } static void pwm_init(void)//GPIO_AF_TIM1,,pwm初始化 pe { GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; RCC_AHB1PeriphClockCmd (RCC_AHB1Periph_GPIOE, ENABLE); //开启引脚时钟 GPIO_PinAFConfig(GPIOE,GPIO_PinSource9,GPIO_AF_TIM1); GPIO_PinAFConfig(GPIOE,GPIO_PinSource11,GPIO_AF_TIM1); GPIO_PinAFConfig(GPIOE,GPIO_PinSource13,GPIO_AF_TIM1); GPIO_PinAFConfig(GPIOE,GPIO_PinSource14,GPIO_AF_TIM1); //复用引脚 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9|GPIO_Pin_11|GPIO_Pin_13|GPIO_Pin_14; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; GPIO_Init(GPIOE, &GPIO_InitStructure); //初始化引脚 RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); //开启定时器时钟 TIM_TimeBaseStructure.TIM_Period = 1200-1; TIM_TimeBaseStructure.TIM_Prescaler = 14-1; TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; TIM_TimeBaseStructure.TIM_RepetitionCounter=0; TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure); //初始化定时器 TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse = 600-1; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; TIM_OC1Init(TIM1, &TIM_OCInitStructure); TIM_OC2Init(TIM1, &TIM_OCInitStructure); TIM_OC3Init(TIM1, &TIM_OCInitStructure); TIM_OC4Init(TIM1, &TIM_OCInitStructure); //配置pwm TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable); TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Enable); TIM_OC3PreloadConfig(TIM1, TIM_OCPreload_Enable); TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable); //使能通道重载 TIM_Cmd(TIM1, ENABLE); //开启定时器 TIM_CtrlPWMOutputs(TIM1, ENABLE); } void Encoder_init(void)//正交解码初始化 { GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_ICInitTypeDef TIM_ICInitStructure; RCC_AHB1PeriphClockCmd (RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB|RCC_AHB1Periph_GPIOC|RCC_AHB1Periph_GPIOD, ENABLE); //开启引脚时钟 GPIO_PinAFConfig(GPIOA,GPIO_PinSource15,GPIO_AF_TIM2); GPIO_PinAFConfig(GPIOB,GPIO_PinSource3,GPIO_AF_TIM2); GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM8); GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_TIM8); GPIO_PinAFConfig(GPIOD,GPIO_PinSource12,GPIO_AF_TIM4); GPIO_PinAFConfig(GPIOD,GPIO_PinSource13,GPIO_AF_TIM4); GPIO_PinAFConfig(GPIOA,GPIO_PinSource0,GPIO_AF_TIM5); GPIO_PinAFConfig(GPIOA,GPIO_PinSource1,GPIO_AF_TIM5); //复用引脚 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_15; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; GPIO_Init(GPIOA, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3; GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; GPIO_Init(GPIOC, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13; GPIO_Init(GPIOD, &GPIO_InitStructure); //初始化引脚 RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2|RCC_APB1Periph_TIM4|RCC_APB1Periph_TIM5, ENABLE); //开启定时器时钟 TIM_TimeBaseStructure.TIM_Period = 0xffff; TIM_TimeBaseStructure.TIM_Prescaler = 1-1; TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure); TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure); //初始化定时器 TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_BothEdge ,TIM_ICPolarity_BothEdge); TIM_EncoderInterfaceConfig(TIM8, TIM_EncoderMode_TI12, TIM_ICPolarity_BothEdge ,TIM_ICPolarity_BothEdge); TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_BothEdge ,TIM_ICPolarity_BothEdge); TIM_EncoderInterfaceConfig(TIM5, TIM_EncoderMode_TI12, TIM_ICPolarity_BothEdge ,TIM_ICPolarity_BothEdge); TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICInitStructure.TIM_ICFilter = 0; TIM_ICInit(TIM2,&TIM_ICInitStructure); TIM_ICInit(TIM8,&TIM_ICInitStructure); TIM_ICInit(TIM4,&TIM_ICInitStructure); TIM_ICInit(TIM5,&TIM_ICInitStructure); //配置正交解码 TIM_Cmd(TIM2, ENABLE); TIM_Cmd(TIM8, ENABLE); TIM_Cmd(TIM4, ENABLE); TIM_Cmd(TIM5, ENABLE); //开启定时器 TIM2->CNT=0; TIM8->CNT=0; TIM4->CNT=0; TIM5->CNT=0; } void Get_Encoder(void) { s16 temp; temp=(s16)((u16)(TIM2->CNT)); CNT_10ms[0]=(double)temp*0.95+CNT_10ms_old[0]*0.05; CNT_10ms_old[0]=CNT_10ms[0]; TIM2->CNT=0; temp=-(s16)((u16)(TIM8->CNT)); CNT_10ms[1]=(double)temp*0.95+CNT_10ms_old[1]*0.05; CNT_10ms_old[1]=CNT_10ms[1]; TIM8->CNT=0; temp=-(s16)((u16)(TIM4->CNT)); CNT_10ms[2]=(double)temp*0.95+CNT_10ms_old[2]*0.05; CNT_10ms_old[2]=CNT_10ms[2]; TIM4->CNT=0; temp=(s16)((u16)(TIM5->CNT)); CNT_10ms[3]=(double)temp*0.95+CNT_10ms_old[3]*0.05; CNT_10ms_old[3]=CNT_10ms[3]; TIM5->CNT=0; } void motor_control(void)//电机控制 { TIM1->CCR3=motor_1.motor_uk+600; TIM1->CCR4=-motor_2.motor_uk+600; TIM1->CCR2=-motor_3.motor_uk+600; TIM1->CCR1=motor_4.motor_uk+600; } void Enable_Motor(void) { GPIO_ResetBits(GPIOE,GPIO_Pin_7|GPIO_Pin_8|GPIO_Pin_10|GPIO_Pin_12); } void Disenable_Motor(void) { GPIO_SetBits(GPIOE,GPIO_Pin_7|GPIO_Pin_8|GPIO_Pin_10|GPIO_Pin_12); } void Motor_Init(void) { Dis_Gpio_Init(); pwm_init(); Encoder_init(); } void Mecanum_Wheel_Motor_Calculate(float front_back,float left_right,float clockwise_anticlockwise) { float duty[4]={0}; duty[0]+=front_back; duty[1]+=front_back; duty[2]+=front_back; duty[3]+=front_back; duty[0]+=left_right; duty[2]+=left_right; duty[1]-=left_right; duty[3]-=left_right; duty[0]+=clockwise_anticlockwise; duty[3]+=clockwise_anticlockwise; duty[1]-=clockwise_anticlockwise; duty[2]-=clockwise_anticlockwise; motor_1.motor_point=duty[0]; motor_2.motor_point=duty[1]; motor_3.motor_point=duty[2]; motor_4.motor_point=duty[3]; }
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。