麦轮控制32f4_麦轮控制代码

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205