倒立摆系统设计

本文最后更新于：2022年5月29日 上午

一、前言

倒立摆系统（Inverted Pendulum System, IPS）是一个典型的复杂、不稳定、非线性、多输入多输出（MIMO）系统，是进行控制理论研究的理想实验平台，常用来作为检验某种控制理论或方法是否合理的典型方案。

一阶倒立摆系统能用多种理论和方法来实现其稳定控制，如ＰＩＤ、自适应、状态反馈、模糊控制及人工神经元网络等多种理论和方法都能在倒立摆系统控制上得到实现。

对倒立摆系统的研究能有效地反映控制中的许多基本问题：如非线性问题、鲁棒性问题、稳定化系统的镇定问题、随动问题以及跟踪问题。

二、系统构成及工作原理

1，系统构成

下图为一阶旋转倒立摆结构示意图。直流电机作为唯一的动力装置，与旋臂保持刚性连接，带动旋臂在水平面内旋转，旋臂的一端通过转轴（本系统选用编码器角度传感器）与摆杆连接，摆杆可做垂直于旋臂的圆周运动。在自然状态下，摆杆为竖直下垂状态。倒立摆控制的目的是通过控制直流电动机的运动状态，使摆杆保持倒立状态。

MbdUXR.jpg

2，系统工作原理

摆杆摆动时，角度传感器检测摆杆的角度，当前测量值与期望的值进行比较产生偏差，通过单片机对该偏差进行处理，即ＰＩＤ控制运算，根据运算结果产生控制信号控制电机和旋臂的转动，使摆杆的角度与期望的角度更接近。

三、程序模块化设计

1，电机驱动模块

由于TB6612相对于传统的L298N效率上提高很多,体积上也大幅度减少，在额定范围内，芯片基本不发热，所以我们设计的时候选择了这款芯片。

MbQwwj.png

（1）motor.h
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
#ifndef __MOTOR_H
#define	__MOTOR_H

#include "stm32f10x.h"


//Pin definition
/*******************************************************/
#define MOTOR_OCPWM_GPIO_CLK			RCC_APB2Periph_GPIOA
#define MOTOR_OCPWM_GPIO_AF				RCC_APB2Periph_AFIO

#define MOTOR_OC1PWM_GPIO_PORT			GPIOA
#define MOTOR_OC1PWM_PIN				GPIO_Pin_0
#define MOTOR_OC1PWM_PINSOURCE			GPIO_PinSource0


#define MOTOR_TIM						TIM2
#define MOTOR_TIM_CLK					RCC_APB1Periph_TIM2

#define MOTOR_TIM_IRQn					TIM2_IRQn
#define MOTOR_TIM_IRQHandler			TIM2_IRQHandler


#define MOTOR_GPIO_CLK					RCC_APB2Periph_GPIOA
#define MOTOR_GPIO_PORT					GPIOA
 
#define MOTOR_IN1_PIN					GPIO_Pin_4
#define MOTOR_IN2_PIN					GPIO_Pin_5

/*******************************************************/

#define MOTOR_IN1_1()	GPIO_SetBits(MOTOR_GPIO_PORT,MOTOR_IN1_PIN)		/* IN1 = 1 */
#define MOTOR_IN1_0()	GPIO_ResetBits(MOTOR_GPIO_PORT,MOTOR_IN1_PIN)	/* IN1 = 0 */

#define MOTOR_IN2_1()	GPIO_SetBits(MOTOR_GPIO_PORT,MOTOR_IN2_PIN)		/* IN2 = 1 */
#define MOTOR_IN2_0()	GPIO_ResetBits(MOTOR_GPIO_PORT,MOTOR_IN2_PIN)	/* IN2 = 0 */

/*******************************************************/

void MOTOR_Init(void);
void MOTOR_TIM_SetPWM_Num(uint8_t chx,uint16_t value);
uint16_t Position_PID(float currentAngle,float Target);
void SpeedRegulation(float currentAngle,float targetAngle);

/********* Direction control function *********/

void Forward_Go(void);
void Backward_Go(void);
void Motor_Stop(void);

/**********************************************/

#endif	/* __MOTOR_H */

（2）motor.c
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
206
207
208
209
210
211
212
213
214
215
/**
  ******************************************************************************
  * @file    motor.c
  * @author  Soso
  * @date    2019-08-19
  * @brief   General purpose timer multi-channel PWM output configuration.
  * 		 DC motor pin GPIO mode configuration and Direction control configuration.
  *
  ******************************************************************************
  */

#include "motor.h"

extern float Kp,Ki,Kd,Pwm,error,lastErr,errSum,dErr,sumerror;
uint8_t derection = 0;

/**
  * @brief  Configuring motor pin IN1~IN8 GPIO mode.
  * @param  None
  * @retval None
  */
static void Motor_GPIO_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	
	/* Enable GPIO port clock */
	RCC_APB2PeriphClockCmd(MOTOR_GPIO_CLK,ENABLE);
	
	// GPIO configuration
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	
	// IN1 pin initialzation
	GPIO_InitStructure.GPIO_Pin = MOTOR_IN1_PIN;
	GPIO_Init(MOTOR_GPIO_PORT,&GPIO_InitStructure);
	GPIO_ResetBits(MOTOR_GPIO_PORT,MOTOR_IN1_PIN);
	// IN2 pin initialzation
	GPIO_InitStructure.GPIO_Pin = MOTOR_IN2_PIN;
	GPIO_Init(MOTOR_GPIO_PORT,&GPIO_InitStructure);
	GPIO_ResetBits(MOTOR_GPIO_PORT,MOTOR_IN2_PIN);

}


/**
  * @brief  Configure the I/O used when the TIM multiplexes the output PWM.
  * @param  None
  * @retval None
  */
static void TIM2_GPIO_Config(void) 
{
	GPIO_InitTypeDef GPIO_InitStructure;

	/* Enable the relevant GPIO peripheral clock */
	RCC_APB2PeriphClockCmd(MOTOR_OCPWM_GPIO_CLK|MOTOR_OCPWM_GPIO_AF, ENABLE); 
	
	/* Timer channel pin configuration */
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	
	/* Channel 1 initialization */
	GPIO_InitStructure.GPIO_Pin = MOTOR_OC1PWM_PIN;
	GPIO_Init(MOTOR_OC1PWM_GPIO_PORT, &GPIO_InitStructure);
}


/**
  * @brief  Timer PWM output mode configuartion.
  * @param  None
  * @retval None
  */
static void TIM2_PWM_Config_Init(void)
{
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_OCInitTypeDef  TIM_OCInitStructure;

	/* Enable TIMx_CLK */
	RCC_APB1PeriphClockCmd(MOTOR_TIM_CLK, ENABLE); 

/*  1, PSC(prescaler) = 72-1, 
	   timer frequency = 72M/(PSC + 1) = 1,000KHZ, 
	   calculate the time of one pulse = 1 / 1,000,000 s
	2, ARR (automatic reload register) = 100-1, counted from 0 to 99, then counted 100 times.
	3, Time=100/1,000,000=0.0001s=0.1ms */ 
	TIM_TimeBaseStructure.TIM_Period = 100-1;
	TIM_TimeBaseStructure.TIM_Prescaler = 72-1;
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;

	// Initialization TIMx
	TIM_TimeBaseInit(MOTOR_TIM, &TIM_TimeBaseStructure);

	/* PWM mode configuration */
	/* PWM1 Mode configuration */
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;	    // Configured as PWM mode 1
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;	
	TIM_OCInitStructure.TIM_Pulse = 1-1;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;  	  // High level when the timer count value is less than CCR1_Val
	
	/*	Enable channel 1 */
	TIM_OC1Init(MOTOR_TIM, &TIM_OCInitStructure);

	/* Enable channel 1 overload */
	TIM_OC1PreloadConfig(MOTOR_TIM, TIM_OCPreload_Enable);

	/* Enable timer */
	TIM_Cmd(MOTOR_TIM, ENABLE);
}


/**
  * @brief  Change the timer channel duty cycle.
  * @param  chx: where x can be  1
  * @param  value: new value of the TIMx channelx
  * @retval None
  */
void MOTOR_TIM_SetPWM_Num(uint8_t chx,uint16_t value)
{
	if (chx==1)
	{
		TIM_SetCompare1(MOTOR_TIM,value);
	}
}


void MOTOR_Init(void)
{
	Motor_GPIO_Config();
	TIM2_GPIO_Config();
	TIM2_PWM_Config_Init();
}


/*********************Direction control function*********************/

// 逆：IN1: 1	IN2: 0
// 顺：IN1: 0 	IN2: 1
// STOP: IN1: 0 IN2: 0

void Forward_Go(void)
{
	MOTOR_IN1_0();
	MOTOR_IN2_1();
}

void Backward_Go(void)
{
	MOTOR_IN1_1();
	MOTOR_IN2_0();
}

void Motor_Stop(void)
{
	MOTOR_IN1_0();
	MOTOR_IN2_0();
}



uint16_t Position_PID(float currentAngle,float Target)
{
	uint16_t currentAngle_int = currentAngle / 1;
	uint16_t Target_int = Target / 1,timeChange = 1;
	
	
	error = Target_int - currentAngle_int;    //正 顺时针转，负 逆时针转
	if(error>1 || error <-1)
		errSum +=(error * timeChange);
	
	if(errSum > sumerror)
		errSum = sumerror;
	else if(errSum < -sumerror)
		errSum = -sumerror;
	
	dErr = (error -lastErr)/timeChange;
	
	Pwm = Kp * error + Ki * errSum + Kd * dErr;
	
	lastErr = error;
	
	if(Pwm < 0)
	{
		derection = 0 ;
		Pwm = 0 - Pwm;
	}
	else
		derection = 1 ;
		
	if(Pwm != 0)
		Pwm = Pwm*0.69+6;
	
	if(Pwm >= 75)	Pwm = 75;
	
	return Pwm;					//增量输出
}

void SpeedRegulation(float currentAngle,float targetAngle)
{
	uint16_t pwm = 0;
	
	pwm = Position_PID(currentAngle,targetAngle);
	// Change duty cycle
	MOTOR_TIM_SetPWM_Num(1,pwm);
	
	// Change direction
	if(derection == 0)
		Forward_Go();
	else if(derection == 1)
		Backward_Go();
	if(pwm == 0)
	    Motor_Stop();
}

/*********************************************END OF FILE*********************************************/

2，编码器模块

（1）encoder.h
1
2
3
4
5
6
7
8
9
#ifndef __ENCODER_H
#define __ENCODER_H

#include "stm32f10x.h"
void TIM4_Init(void);
int Read_Encoder(u8 TIMX);

#endif	/* __ENCODER_H */

（1）encoder.c
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
/**
  ******************************************************************************
  * @file    encoder.c
  * @author  Soso
  * @date    2019-08-19
  * @brief   Initialize TIM2 to encoder interface mode.
  *
  ******************************************************************************
  */

#include "encoder.h"

/**************************************************************************
函数功能：把TIM2初始化为编码器接口模式
入口参数：无
返回  值：无
**************************************************************************/
extern u16	Parameter;	//输入编码器线数


static void TIM4_Mode_Config(void)
{
	SystemInit();
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_ICInitTypeDef TIM_ICInitStructure;

	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
	
	/*- 正交编码器输入引脚 PB->6   PB->7 -*/
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);

	/*- TIM4编码器模式配置 -*/
	TIM_DeInit(TIM4);
	TIM_TimeBaseStructure.TIM_Period = Parameter;
	TIM_TimeBaseStructure.TIM_Prescaler = 0;
	TIM_TimeBaseStructure.TIM_ClockDivision =TIM_CKD_DIV1 ; 
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
                 
	TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI2, TIM_ICPolarity_Rising ,TIM_ICPolarity_Rising);	//配置编码器模式触发源和极性
	
	TIM_ICStructInit(&TIM_ICInitStructure);																			 															//配置滤波器TIM_ICPolarity_Rising
	TIM_ICInitStructure.TIM_ICFilter = 6;
	TIM_ICInit(TIM4, &TIM_ICInitStructure);
		
	TIM_SetCounter(TIM4,0);
  //TIM4 -> CNT = 0;
	TIM_Cmd(TIM4, ENABLE);   //启动TIM4定时器 
}


int Read_Encoder(u8 TIMX)
{
	int Encoder_TIM;
	switch(TIMX)
	{
		case 2:  Encoder_TIM= (short)TIM2 -> CNT;  break;
		case 3:  Encoder_TIM= (short)TIM3 -> CNT;  break;	
		case 4:  Encoder_TIM= (short)TIM4 -> CNT;  break;	
		default: Encoder_TIM=0;
	}
	return Encoder_TIM;
}



void TIM4_Init(void)
{
  TIM4_Mode_Config();
}

3，按键模块

（1）key.h
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
#ifndef __KEY_H
#define __KEY_H

#include "stm32f10x.h"


// Pin definition
/*******************************************************/
#define KEY1_INT_GPIO_PORT					GPIOA
#define KEY1_INT_GPIO_CLK					RCC_APB2Periph_GPIOA
#define KEY1_INT_GPIO_PIN					GPIO_Pin_6
#define KEY1_INT_EXTI_PORTSOURCE			GPIO_PortSourceGPIOA
#define KEY1_INT_EXTI_PINSOURCE				GPIO_PinSource6
#define KEY1_INT_EXTI_LINE					EXTI_Line6
#define KEY1_INT_EXTI_IRQ					EXTI9_5_IRQn

#define KEY2_INT_GPIO_PORT					GPIOA
#define KEY2_INT_GPIO_CLK					RCC_APB2Periph_GPIOA
#define KEY2_INT_GPIO_PIN					GPIO_Pin_7
#define KEY2_INT_EXTI_PORTSOURCE			GPIO_PortSourceGPIOA
#define KEY2_INT_EXTI_PINSOURCE				GPIO_PinSource7
#define KEY2_INT_EXTI_LINE					EXTI_Line7
#define KEY2_INT_EXTI_IRQ					EXTI9_5_IRQn

#define KEY3_INT_GPIO_PORT					GPIOA
#define KEY3_INT_GPIO_CLK					RCC_APB2Periph_GPIOA
#define KEY3_INT_GPIO_PIN					GPIO_Pin_8
#define KEY3_INT_EXTI_PORTSOURCE			GPIO_PortSourceGPIOA
#define KEY3_INT_EXTI_PINSOURCE				GPIO_PinSource8
#define KEY3_INT_EXTI_LINE					EXTI_Line8
#define KEY3_INT_EXTI_IRQ					EXTI9_5_IRQn

#define KEY_IRQHandler						EXTI9_5_IRQHandler

/*******************************************************/

void EXTI_Key_Config(void);

#endif	/* __KEY_H */

（2）key.c
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
/**
  ******************************************************************************
  * @file    key.c
  * @date    2019-08-19
  * @brief   I/O Line interrupt Application.
  *
  ******************************************************************************
  */

#include "key.h"

// Configuring nested vectored interrupt controller(NVIC)
static void EXTI_NVIC_Config(void)
{
	NVIC_InitTypeDef NVIC_InitStructure;
	/* Configure NVIC as a priority group 1 */
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
	
	/* Configuration preemption priority： 1 */
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
	/* Configure sub-priority： 1 */
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
	/* Enable interrupt channel */
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	
	/* Configure interrupt source */
	NVIC_InitStructure.NVIC_IRQChannel = KEY1_INT_EXTI_IRQ;
	NVIC_Init(&NVIC_InitStructure);
	NVIC_InitStructure.NVIC_IRQChannel = KEY2_INT_EXTI_IRQ;
	NVIC_Init(&NVIC_InitStructure);
	NVIC_InitStructure.NVIC_IRQChannel = KEY3_INT_EXTI_IRQ;
	NVIC_Init(&NVIC_InitStructure);
}


void EXTI_Key_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	EXTI_InitTypeDef EXTI_InitStructure;
	
	/* Enable KeyGPIO port clock */
	RCC_APB2PeriphClockCmd(KEY1_INT_GPIO_CLK|KEY2_INT_GPIO_CLK|KEY3_INT_GPIO_CLK,ENABLE);
	/* Key GPIO Configuration */
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	
	GPIO_InitStructure.GPIO_Pin = KEY1_INT_GPIO_PIN;
	GPIO_Init(KEY1_INT_GPIO_PORT,&GPIO_InitStructure);
	GPIO_InitStructure.GPIO_Pin = KEY2_INT_GPIO_PIN;
	GPIO_Init(KEY2_INT_GPIO_PORT,&GPIO_InitStructure);
	GPIO_InitStructure.GPIO_Pin = KEY3_INT_GPIO_PIN;
	GPIO_Init(KEY3_INT_GPIO_PORT,&GPIO_InitStructure);
	
	/* Connect the exti interrupt source to the key pin */
	GPIO_EXTILineConfig(KEY1_INT_EXTI_PORTSOURCE,KEY1_INT_EXTI_PINSOURCE);
	GPIO_EXTILineConfig(KEY2_INT_EXTI_PORTSOURCE,KEY2_INT_EXTI_PINSOURCE);
	GPIO_EXTILineConfig(KEY3_INT_EXTI_PORTSOURCE,KEY3_INT_EXTI_PINSOURCE);

	/* Interrupt mode */
	EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
	/* Falling edge trigger */
	EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
	/* Enable interruptions/event lines */
	EXTI_InitStructure.EXTI_LineCmd = ENABLE;
	
	/* Select the exti interrupt source */
	EXTI_InitStructure.EXTI_Line = KEY1_INT_EXTI_LINE;
	EXTI_Init(&EXTI_InitStructure);
	
	/* Select the exti interrupt source */
	EXTI_InitStructure.EXTI_Line = KEY2_INT_EXTI_LINE;
	EXTI_Init(&EXTI_InitStructure);
	
	/* Select the exti interrupt source */
	EXTI_InitStructure.EXTI_Line = KEY3_INT_EXTI_LINE;
	EXTI_Init(&EXTI_InitStructure);
	
	/* Configure NVIC */
	EXTI_NVIC_Config();
}

/*********************************************END OF FILE*********************************************/

4，定时器计时模块

（1）tim.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
#ifndef __TIM_H
#define	__TIM_H

#include "stm32f10x.h"

#define COUNTER_TIM						TIM3
#define COUNTER_TIM_CLK					RCC_APB1Periph_TIM3

#define COUNTER_TIM_IRQn				TIM3_IRQn
#define COUNTER_TIM_IRQHandler			TIM3_IRQHandler

void TIM3_Config(void);

#endif	/* __TIM_H */

（2）tim.c
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
/**
  ******************************************************************************
  * @file    tim.c
  * @author  Soso
  * @date    2019-08-19
  * @brief   Configure timer 3 as the count mode.
  *
  ******************************************************************************
  */
  
#include "tim.h"


// Configuring nested vectored interrupt controller(NVIC)
static void TIM3_NVIC_Config(void)
{
	NVIC_InitTypeDef NVIC_InitStructure;
	/* Configure NVIC as a priority group 1 */
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
	
	/* Configure interrupt source*/
	NVIC_InitStructure.NVIC_IRQChannel = COUNTER_TIM_IRQn;
	/* Configuration preemption priority： 1 */
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
	/* Configure sub-priority： 1 */
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
	/* Enable interrupt channel */
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);
}


void TIM3_Config(void)
{
	SystemInit();
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
	// Enable TIM3_CLK 
	RCC_APB1PeriphClockCmd(COUNTER_TIM_CLK,ENABLE);
	
/*  1, PSC(prescaler) = 720-1
	   timer frequency = 72M/(PSC + 1) = 100KHZ
	   calculate the time of one pulse = 1/1000,000 s
	2, ARR (automatic reload register) = 100-1, counted from 0 to 99, then counted 100 times.
	3, Time = 100/100,000 = 0.001s = 10ms */ 
	TIM_TimeBaseStructure.TIM_Period = 100-1;
	TIM_TimeBaseStructure.TIM_Prescaler = 720-1;
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	// Initialization TIM3
	TIM_TimeBaseInit(COUNTER_TIM,&TIM_TimeBaseStructure);
	
	// Clear timer update interrupt flag
	TIM_ClearITPendingBit(COUNTER_TIM,TIM_IT_Update);
	// Enable timer update interrupt
	TIM_ITConfig(COUNTER_TIM,TIM_IT_Update,ENABLE);
	// Initialization disable timer
	TIM_Cmd(COUNTER_TIM,DISABLE);
	
	TIM3_NVIC_Config();
	
}
/*********************************************END OF FILE*********************************************/

5，串口调试模块

（1）usart.h
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
#ifndef __USART_H
#define __USART_H

#include "stm32f10x.h"
#include "stdio.h"

// Pin definition
/*******************************************************/
#define DEBUG_USART                             USART3
#define DEBUG_USART_CLK                         RCC_APB1Periph_USART3
#define DEBUG_USART_AF                       	RCC_APB2Periph_AFIO
#define DEBUG_USART_GPIO_CLK                 	RCC_APB2Periph_GPIOB
#define DEBUG_USART_BAUDRATE                    256000

#define DEBUG_USART_RX_GPIO_PORT                GPIOB
#define DEBUG_USART_RX_PIN                      GPIO_Pin_11

#define DEBUG_USART_TX_GPIO_PORT                GPIOB
#define DEBUG_USART_TX_PIN                      GPIO_Pin_10


#define DEBUG_USART_IRQ                 		USART3_IRQn
#define DEBUG_USART_IRQHandler					USART3_IRQHandler

/************************************************************/

void USART_Config(void);
void Usart_SendByte( USART_TypeDef * pUSARTx, uint8_t ch);
void Usart_SendString( USART_TypeDef * pUSARTx, char *str);

void Usart_SendHalfWord( USART_TypeDef * pUSARTx, uint16_t ch);

#endif	/* __USART_H */

（2）usart.c
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
/**
  ******************************************************************************
  * @file    uasrt.c
  * @author  Soso
  * @date    2019-08-19
  * @brief   Redirect c library printf function to usart port, interrupt receiving mode.
  ******************************************************************************
  */

#include "usart.h"


/* Configuring nested vectored interrupt controller NVIC */
static void NVIC_Configuration(void)
{
	NVIC_InitTypeDef NVIC_InitStructure;

	/* Nested vector interrupt controller group selection */
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);

	/* Preemptio Priority is 1 */
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
	/* Sub Priority is 1 */
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
	/* Enable interrupt */
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	
	/* Configure USART as the interrupt source */
	NVIC_InitStructure.NVIC_IRQChannel = DEBUG_USART_IRQ;
	/* Initialize configuration NVIC */
	NVIC_Init(&NVIC_InitStructure);
}

void USART_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;

	/* Enable port alternate function and enable USART GPIO clock */
	RCC_APB2PeriphClockCmd(DEBUG_USART_AF|DEBUG_USART_GPIO_CLK,ENABLE);
	/* Enable USART Clock */
	RCC_APB1PeriphClockCmd(DEBUG_USART_CLK,ENABLE);
	
	/* Configure the Tx pin for alternate function  */
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;

	GPIO_InitStructure.GPIO_Pin = DEBUG_USART_TX_PIN;
	GPIO_Init(DEBUG_USART_TX_GPIO_PORT, &GPIO_InitStructure);
	
	/* Configure the Rx pin for the alternate function */
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	
	GPIO_InitStructure.GPIO_Pin = DEBUG_USART_RX_PIN;
	GPIO_Init(DEBUG_USART_RX_GPIO_PORT, &GPIO_InitStructure);
	
	/* Configuration USART mode */
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
	USART_InitStructure.USART_StopBits = USART_StopBits_1;
	USART_InitStructure.USART_Parity = USART_Parity_No;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	/* USART mode control: simultaneous enable send and receive */
	USART_InitStructure.USART_Mode = USART_Mode_Tx|USART_Mode_Rx;
	
	USART_InitStructure.USART_BaudRate = DEBUG_USART_BAUDRATE;
	USART_Init(DEBUG_USART, &USART_InitStructure);
	
	/* Configure the serial port to receive interrupts. */
	NVIC_Configuration();
	
	/* Enable serial port receive interrupt */
	USART_ITConfig(DEBUG_USART, USART_IT_RXNE, ENABLE);
	
	/* Enable serial */
	USART_Cmd(DEBUG_USART, ENABLE);
}


/*****************  Send a character **********************/
void Usart_SendByte( USART_TypeDef * pUSARTx, uint8_t ch)
{
	/* Send a byte of data to the USART */
	USART_SendData(pUSARTx,ch);
		
	/* Waiting for the transmit data register to be empty */
	while (USART_GetFlagStatus(pUSARTx, USART_FLAG_TXE) == RESET);	
}

/*****************  Send string **********************/
void Usart_SendString( USART_TypeDef * pUSARTx, char *str)
{
	unsigned int k=0;
  do 
  {
      Usart_SendByte( pUSARTx, *(str + k) );
      k++;
  } while(*(str + k)!='\0');
  
  /* Waiting for transmission to complete */
  while(USART_GetFlagStatus(pUSARTx,USART_FLAG_TC)==RESET)
  {}
}

/*****************  Send a 16-digit number **********************/
void Usart_SendHalfWord( USART_TypeDef * pUSARTx, uint16_t ch)
{
	uint8_t temp_h, temp_l;
	
	/* Take out the high eight */
	temp_h = (ch&0XFF00)>>8;
	/* Take the lower eight */
	temp_l = ch&0XFF;
	
	/* Send high eight */
	USART_SendData(pUSARTx,temp_h);	
	while (USART_GetFlagStatus(pUSARTx, USART_FLAG_TXE) == RESET);
	
	/* Send the lower eight digits */
	USART_SendData(pUSARTx,temp_l);	
	while (USART_GetFlagStatus(pUSARTx, USART_FLAG_TXE) == RESET);	
}

/* Redirect the c library function printf to the serial port, 
   use the printf function after redirection.	*/
int fputc(int ch, FILE *f)
{
	/* Send a byte of data to the serial port */
	USART_SendData(DEBUG_USART, (uint8_t) ch);

	/* Waiting to send */
	while (USART_GetFlagStatus(DEBUG_USART, USART_FLAG_TXE) == RESET);		

	return (ch);
}

/* Redirect the c library function scanf to the serial port, 
   rewrite backwards can use scanf, getchar and other functions. */
int fgetc(FILE *f)
{
	/* Waiting for serial input data */
	while (USART_GetFlagStatus(DEBUG_USART, USART_FLAG_RXNE) == RESET);

	return (int)USART_ReceiveData(DEBUG_USART);
}
/*********************************************END OF FILE*********************************************/

6，延时函数配置模块

（1）systick.h
1
2
3
4
5
6
7
8
9
10
11
#ifndef __SYSTICK_H
#define __SYSTICK_H

#include"stm32f10x.h"

void delay_us(u32 i);
void delay_ms(u32 i);


#endif	/* __SYSTICK_DELAY_H */

（2）systick.c
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
#include "systick.h"

void delay_us(u32 i)
{
	u32 temp;
	SysTick->LOAD=9*i;		 //Set the reload value, at 72MHZ
	SysTick->CTRL=0X01;		 //Enable, reduce to zero is no action, use external clock source
	SysTick->VAL=0;		   	 //Clear counter
	do
	{
		temp=SysTick->CTRL;		//Read current countdown value
	}
	while((temp&0x01)&&(!(temp&(1<<16))));	 //Waiting time to arrive
	SysTick->CTRL=0;	//Close counter
	SysTick->VAL=0;		//Empty counter
}
void delay_ms(u32 i)
{
	u32 temp;
	SysTick->LOAD=9000*i;	//Set the reload value, at 72MHZ
	SysTick->CTRL=0X01;		//Enable, reduce to zero is no action, use external clock source
	SysTick->VAL=0;			//Clear counter
	do
	{
		temp=SysTick->CTRL;	   //Read current countdown value
	}
	while((temp&0x01)&&(!(temp&(1<<16))));	//Waiting time to arrive
	SysTick->CTRL=0;	//Close counter
	SysTick->VAL=0;		//Empty counter
}

7，中断配置模块

把下面的代码添加到 stm32f10x_it.c 文件中。

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
#include "stm32f10x_it.h"
#include "usart.h"
#include "tim.h"
#include "motor.h"
#include "encoder.h"
#include "key.h"
#include "systick.h"


extern uint32_t ms_TimCnt;
/**
  * @brief  This function handles TIM3_IRQHandler interrupt request.
  * @param  None
  * @retval None
  */
void COUNTER_TIM_IRQHandler(void)
{
	ms_TimCnt++;
	TIM_ClearITPendingBit(COUNTER_TIM,TIM_IT_Update);
}

extern uint8_t Modeflag;
/**
  * @brief  This function handles EXTI9_5_IRQHandler interrupt request.
  * @param  None
  * @retval None
  */
void KEY_IRQHandler(void)
{
	if(EXTI_GetITStatus(KEY1_INT_EXTI_LINE) == SET)
	{
		if(GPIO_ReadInputDataBit(KEY1_INT_GPIO_PORT,KEY1_INT_GPIO_PIN) == 0)
		{
			delay_ms(10);
			if(GPIO_ReadInputDataBit(KEY1_INT_GPIO_PORT,KEY1_INT_GPIO_PIN) == 0)
			{
				while(!GPIO_ReadInputDataBit(KEY1_INT_GPIO_PORT,KEY1_INT_GPIO_PIN));
				Modeflag = 0;
			}
		}
	}
	else if(EXTI_GetITStatus(KEY2_INT_EXTI_LINE) == SET)
	{
		if(GPIO_ReadInputDataBit(KEY2_INT_GPIO_PORT,KEY2_INT_GPIO_PIN) == 0)
		{
			delay_ms(10);
			if(GPIO_ReadInputDataBit(KEY2_INT_GPIO_PORT,KEY2_INT_GPIO_PIN) == 0)
			{
				while(!GPIO_ReadInputDataBit(KEY2_INT_GPIO_PORT,KEY2_INT_GPIO_PIN));
				Modeflag = 1;
			}
		}
	}
	else if(EXTI_GetITStatus(KEY3_INT_EXTI_LINE) == SET)
	{
		if(GPIO_ReadInputDataBit(KEY3_INT_GPIO_PORT,KEY3_INT_GPIO_PIN) == 0)
		{
			delay_ms(10);
			if(GPIO_ReadInputDataBit(KEY3_INT_GPIO_PORT,KEY3_INT_GPIO_PIN) == 0)
			{
				while(!GPIO_ReadInputDataBit(KEY3_INT_GPIO_PORT,KEY3_INT_GPIO_PIN));
			}
		}
	}
	
	EXTI_ClearITPendingBit(KEY1_INT_EXTI_LINE);
	EXTI_ClearITPendingBit(KEY2_INT_EXTI_LINE);
	EXTI_ClearITPendingBit(KEY3_INT_EXTI_LINE);
}


extern float Kp,Ki,Kd,errSum; 
/**
  * @brief  This function handles USART3_IRQHandler interrupt request.
  * @param  None
  * @retval None
  */
void DEBUG_USART_IRQHandler(void)
{
	uint8_t RxTemp = 0;
	
	if(USART_GetITStatus(DEBUG_USART,USART_IT_RXNE) != RESET)
	{
		RxTemp = USART_ReceiveData(DEBUG_USART);
		USART_SendData(DEBUG_USART,RxTemp);
		
		if(RxTemp == 'q')
			Kp += 1;
		else if(RxTemp == 'w')
			Kp -= 1;
		else if(RxTemp == 'a')
			Kp += 0.1;
		else if(RxTemp == 's')
			Kp -= 0.1;
		else if(RxTemp == 'z')
			Kp += 0.01;
		else if(RxTemp == 'x')
			Kp -= 0.01;
		else if(RxTemp == 'e')
			Ki += 0.0001;
		else if(RxTemp == 'r')
			Ki -= 0.0001;
		else if(RxTemp == 'd')
			Ki += 0.000000001;
		else if(RxTemp == 'f')
			Ki -= 0.000000001;
		else if(RxTemp == 't')
			Kd += 1;
		else if(RxTemp == 'y')
			Kd -= 1;
		else if(RxTemp == 'g')
			Kd += 0.1;
		else if(RxTemp == 'h')
			Kd -= 0.1;
		else if(RxTemp == 'b')
			Kd += 1;
		else if(RxTemp == 'n')
			Kd -= 1;
		else if(RxTemp == 'm')
			Kd += 10;
		else if(RxTemp == ',')
			Kd -= 10;
		else if(RxTemp == 'u')
			Kp = 0;
		else if(RxTemp == 'i')
			Ki = 0;
		else if(RxTemp == 'o')
			Kd = 0;
		else if(RxTemp == 'l')
			errSum = 0;
	}
}

8，main.c

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
/**
  ******************************************************************************
  * @file    main.c
  * @author  Soso
  * @date    2019-08-19
  * @brief   Inverted Pendulum DC geared motor Control.
  *
  ******************************************************************************
  */
  
#include "stm32f10x.h"
#include "motor.h"
#include "systick.h"
#include "usart.h"
#include "encoder.h"
#include "tim.h"
#include "key.h"

uint8_t Angle_flag = 0,Modeflag = 3;
u16	Parameter = 1200;				//输入编码器线数
float CurrentAngle = 0,TargetAngle = 180,Old_CurrentAngle = 0,sumerror = 6000;
float Kp = 3.6,Ki = 0.045,Kd = 3.9,Pwm = 0,error = 0,lastErr = 0,errSum = 0,dErr = 0;
uint32_t ms_TimCnt = 0;

int main(void)
{
	MOTOR_Init();
	USART_Config();
	TIM3_Config();
	TIM4_Init();
	EXTI_Key_Config();
	
	uint8_t time_flag = 0;
	
	printf("Initialization successful.\r\n");
	
	
	while(1)
	{
		Old_CurrentAngle = CurrentAngle;		//获取先前的计数值
		CurrentAngle = Read_Encoder(4)*0.3;		//获取编码器当前数值
		if(CurrentAngle == 360)
            CurrentAngle = 0;
		
		if(Modeflag == 1)
		{
			if(time_flag == 0)
			{
				ms_TimCnt = 0;
				TIM_Cmd(COUNTER_TIM,ENABLE);
				time_flag = 1;
			}
			else if(time_flag == 1)
			{
				MOTOR_TIM_SetPWM_Num(1,20);
				Backward_Go();
				if(ms_TimCnt == 50)
				{
					Motor_Stop();
					MOTOR_TIM_SetPWM_Num(1,0);
					time_flag = 2;
					ms_TimCnt = 0;
				}
			}
			else if(time_flag == 2)
			{
				if(( CurrentAngle >= 358 )|| ( CurrentAngle <=2 ))
				{
					time_flag = 3;		
					ms_TimCnt = 0;
				}
					
			}
			else if(time_flag == 3)
			{
				MOTOR_TIM_SetPWM_Num(1,20);
				Forward_Go();
				if(ms_TimCnt == 50)
				{
					time_flag = 4;		
					ms_TimCnt = 0;
				}
			}
			else if(time_flag == 4)
			{
				Motor_Stop();
				MOTOR_TIM_SetPWM_Num(1,0);
				if(( CurrentAngle >= 358 )|| ( CurrentAngle <=2 ))
				{
					time_flag = 0;		
					ms_TimCnt = 0;
				}
			}
		}
		else if(Modeflag == 2)	//model 2
		{
			if(time_flag == 0)
			{
				ms_TimCnt = 0;
				TIM_Cmd(COUNTER_TIM,ENABLE);
				time_flag = 1;
			}
			else if(time_flag == 1)
			{
				MOTOR_TIM_SetPWM_Num(1,40);
				Forward_Go ();
				if(ms_TimCnt == 30)
				{
					Motor_Stop();
					MOTOR_TIM_SetPWM_Num(1,0);
					time_flag = 2;
					ms_TimCnt = 0;
				}			
			}
			else if(time_flag == 2)
			{
				if((CurrentAngle >= 358)|| (CurrentAngle <= 2))
				{
					time_flag = 3;		
					ms_TimCnt = 0;
				}
			}
			else if(time_flag == 3)
			{
				MOTOR_TIM_SetPWM_Num(1,70);
				Backward_Go();
				if(ms_TimCnt == 30)
				{
					Motor_Stop();
					MOTOR_TIM_SetPWM_Num(1,0);
					time_flag = 4;
					ms_TimCnt = 0;
				}		
			}
		}
		else if(Modeflag == 3)	//model 3
		{
			if(CurrentAngle >= 155 && CurrentAngle <= 205)
				SpeedRegulation(CurrentAngle,TargetAngle);
			else 
				Motor_Stop();
		}
			
		printf("Kp: %0.2f  Ki: %0.9f  Kd: %0.2f  error:%0.1f  Pwm: %0.2f CurrentAngle: %0.2f errSum: %0.2f\r\n",Kp,Ki,Kd,error,Pwm,CurrentAngle,errSum);
		
	}
}




/*********************************************END OF FILE*********************************************/

四、元器件接线说明

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
【*】 引脚分配
			
			STM32	    Encoder
			PB6    ->     A
			PB7    ->     B
			+5V    ->    VCC
			GND    ->    GND
			
			STM32	     TB6612FNG
			PA0     ->     PWMA
			PA4     ->     AIN1
			PA5     ->     AIN2
			+3.3V	->     VCC
			GND     ->     GND
	
			电源		TB6612FNG
			12V     ->     VM
			GND     ->     GND
			
            DC Geared motor	    TB6612FNG
			+     ->     A01
			-     ->     A02
			
			STM32		串口模块
			PB11    ->     Tx
			PB10    ->     Rx
			+3.3V   ->     +3.3V
			GND     ->     GND
			波特率：115200
			
			STM32		 Button
			PA6    ->     key1
			PA7    ->     key2
			PA8    ->     key3
			+3.3V  ->     +3.3V
			GND    ->     GND
嵌入式 > STM32
#竞赛 #STM32 #倒立摆
倒立摆系统设计
https://kevinloongc.github.io/posts/6c941452.html
作者
Kevin Loongc
发布于
2019年8月19日
许可协议