平衡车原理

先看一下成品是什么样子的

6lGgN8.jpg

准备就做成这个样子的。

想象在电机不供电不动的情况下，将车放在地上下松手后，车子肯定会向前或向后倒，就像人站在一根细的杆子上，大脑不控制肯定是站不稳的，因为重心不在适当的点，人通过大脑完成调节，而平衡车调节靠STM32控制电机的转速，转向来使车平衡不倒。

材料购买

电机带编码器x2，一般直接购买底盘；

电池座，18650电池x3；

M3铜柱、螺丝、螺母，多买一些备用，容易弄丢；

蓝牙模块；

亚克力板可以直接购买成品，也可以定制，会CAD用CAD画，不会用AD画，导出DXF格式，去某宝定制；

其他可能会用到的：2.4g模块，超声波模块，红外循迹模块，电机驱动模块，红外模块，MPU6050模块，STM32C8T6最小系统，MPU6050和STM32C8T6可以直接集成在板子上。

所需电子器件等硬件设计完成再购买。

硬件分析设计

原理图设计

电源设计

因为使用3个18650电池串联，所以需要先降压，可以使用7805稳压芯片先降到5V，然后再通过AMS11173.3,将5V降到3.3v

68Fqkn.jpg

68Fx6U.jpg

STM32主控设计

因为用不到RTC，所以省略32.768KHz的晶振，只用8M的高速外部晶振

68kpm4.jpg

MPU6050模块设计

68knne.jpg

电机驱动模块设计

68kG1f.jpg

蓝牙模块

蓝牙模块使用购买的模块直插，所以只要添加对应管脚数的排母

68kYjS.jpg

PCB设计

TOP层：

68kXEd.jpg

BOTTOM层：

68AAEj.jpg

BOM表整理配单

Comment Description Designator Footprint Value Quantity
Cap2 引线型铝电解电容 C1, C2 RADIAL,6X12MM 16v,330uF 2
Cap 0603贴片电容 C3, C6, C7, C9, C10, C12, C13, C14, C16, C17, C18, C19, C20 0603C 100nF 13
Cap2 引线型铝电解电容 C4, C5 RADIAL,6X12MM 10v,470uF 2
Cap 0603贴片电容 C8 0603C 10nF 1
Cap2 引线型铝电解电容 C11 RADIAL,5X11MM 10uF,25V 1
Cap 0603贴片电容 C15 0603C 100pF 1
Cap 0603贴片电容 C22, C24 0603C 22pF 2
LED0 Red、Blue D1, D2, D3, D4 0603LED 4
MF-NSMF050 保险丝 F1 1206F 1
Header 20P 20p排针 H1, H2 HDR1X20 2
CDS-2P 2p电源插座 J1 CDS-2P 1
BLUE 蓝牙模块 J2 BLUE 1
MSS22D18 贴片6脚开关 KG1 MSS22D18 1
PH-6P PH2.0 6p插座 M1, M2 PH2.0 6P 2
Header 4 4p排针 P1 HDR1X4 1
Header 3X2 3x2p排针 P2 HDR2X3 1
Res2 0603贴片电阻 R1, R2, R4, R5, R6, R9 0603R 10K 6
Res2 0603贴片电阻 R3 0603R 3.3K 1
Res2 0603贴片电阻 R7, R8 0603R 100K 2
Res2 0603贴片电阻 R10, R11 0603R 680R 2
SW1 轻触按键开关 S2 KEY_4(SOP) 1
78M05 DCDC芯片 U1 TO-252-2-180 1
MPU-6050 六轴传感器 U2 QFN-24_4x4x05P 1
TB6612FNG 电机驱动芯片 U3 LC-SSOP-24_208mil 1
STM32F103C8 STM32 MCU U4 LQFP-48_7x7x05P 1
AMS1117-3.3 低压差线性稳压(LDO) VR1 SOT-23(SOT-23-3) 1
XTAL 直插式晶振HC-49S Y2 XTAL_US 8M 1

拿着上面BOM表就可以配单了

软件设计

先实现基本的直立控制，再实现通过上位机，遥控等方式控制小车行驶，因为控制小车的基础是基本的直立。两种PID控制方法，串级和并级，关于串并PID可以参考这里，两种PID参数是完全不相同的，需要分别调参。

串级PID

主函数

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
#include "stm32f10x.h"
#include "timer.h"
#include "motor.h"
#include "usart.h"
#include "led.h"
#include "I2C.h"
#include "systick.h"
#include "car.h"
#include "mpu6050.h"

int main(void)
{	
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置优先级分组2
	USART1_Config();//串口1初始化，用于下载程序和打印MPU6050相关的日志
	TIM2_PWM_Init();//定时器2初始化用于产生PWM波控制电机
	TIM3_Encoder_Init(); //定时器3初始化，编码器模式用于测车速
	TIM4_Encoder_Init();//定时器4初始化，编码器模式用于测车速
	MOTOR_GPIO_Config();//电机GPIO初始化
	LED_GPIO_Config();//LED初始化，用于显示MPU6050状态，异常亮，正常灭
	i2cInit();//I2C初始化
	delay_nms(10);
	MPU6050_Init();	//MPU6050初始化
	CarUpstandInit();//直立参数初始化
	SysTick_Init();//Systick初始化，使用systick中断控制整个系统的控制，用于整个系统运行
	while(1)
	{
		MPU6050_Pose();//不断获取姿态
	}
}

延时函数使用粗略的延时，因为Systick被用于系统控制了，也可以开一个定时器延时

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
#include "stm32f10x.h"
#include "delay.h"

void delay_us(u32 n)
{
	u8 j;
	while(n--)
	for(j=0;j<10;j++);
}
void delay_ms(u32 n)
{
	while(n--)
	delay_us(1000);
}
void get_ms(unsigned long *time)
{

}

void delay_nms(u16 time)
{    
   u16 i=0;  
   while(time--)
   {
      i=12000;
      while(i--) ;    
   }
}

串口配置

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
#include "usart.h"
#include <stdarg.h>
#include "misc.h"
#include "stm32f10x_usart.h"

void USART1_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;//定义初始化结构体
	USART_InitTypeDef USART_InitStructure;//定义初始化结构体
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1 | RCC_APB2Periph_GPIOA, ENABLE);//使能GPIO和串口时钟
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIO PA9
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIO PA10
	
	USART_InitStructure.USART_BaudRate = 115200;//设置波特率115200
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;//8位数据位
	USART_InitStructure.USART_StopBits = USART_StopBits_1;//1位停止位
	USART_InitStructure.USART_Parity = USART_Parity_No ;//无奇偶校验位
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//不使用硬件流控制
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;//发送接收模式
	USART_Init(USART1, &USART_InitStructure); //初始化串口1
	USART_Cmd(USART1, ENABLE);//使能串口1
}

int fputc(int ch, FILE *f)
{
	USART_SendData(USART1, (unsigned char) ch);//将Printf内容发往串口
	while(USART_GetFlagStatus(USART1,USART_FLAG_TC)!= SET);	
	return (ch);
}

void PrintChar(char *s)//输出字符串到串口1
{
	char *p;
	p=s;
	while(*p != '\0')
	{
		USART_SendData(USART1, *p);        //通过库函数  发送数据
    while( USART_GetFlagStatus(USART1,USART_FLAG_TC)!= SET);//等待发送完成。   检测 USART_FLAG_TC 是否置1；    //见库函数 P359 介绍
		p++;
	}
}

定时器配置

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
#include "timer.h"
#include "sys.h"

void TIM2_PWM_Init(void)
{
	u16 CCR3_Val = 0;
	u16 CCR4_Val = 0;
	
	GPIO_InitTypeDef GPIO_InitStructure;//定义初始化结构体
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;//定义初始化结构体
	TIM_OCInitTypeDef  TIM_OCInitStructure;//定义初始化结构体

	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //使能定时器2时钟，PCLK1经过2倍频后作为TIM2的时钟源等于72MHz
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //使能GPIOA时钟

	GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_2 | GPIO_Pin_3;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

	GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIO

    TIM_TimeBaseStructure.TIM_Period = 999 ;//ARR 当定时器从0计数到999，即为1000次，为一个定时周期
    TIM_TimeBaseStructure.TIM_Prescaler = 0;//设置预分频：不预分频，即为72MHz
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1 ;//设置时钟分频系数：不分频
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;//向上计数模式

    TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);

    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;//配置为PWM模式1
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;//比较输出使能
    TIM_OCInitStructure.TIM_Pulse = CCR3_Val;//设置跳变值，当计数器计数到这个值时，电平发生跳变
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;//当定时器计数值小于CCR1_Val时为高电平

    TIM_OC3Init(TIM2, &TIM_OCInitStructure);//使能通道3
    TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);

    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = CCR4_Val;//设置通道2的电平跳变值，输出另外一个占空比的PWM
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;  //当定时器计数值小于CCR1_Val时为高电平

    TIM_OC4Init(TIM2, &TIM_OCInitStructure);//使能通道4
    TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);

    TIM_ARRPreloadConfig(TIM2, ENABLE);// 使能TIM3重载寄存器ARR
    TIM_Cmd(TIM2, ENABLE);//使能定时器2	
}

void TIM3_Encoder_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    TIM_ICInitTypeDef TIM_ICInitStructure;
    NVIC_InitTypeDef NVIC_InitStructure;

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);//使能定时器3时钟
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);//使能GPIO时钟

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIO PA6 PA7作为编码器输入引脚

    NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority =1;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);//定时器3中断优先级初始化

    TIM_DeInit(TIM3);//重新初始化定时器3

    TIM_TimeBaseStructure.TIM_Prescaler = 0x0;
    TIM_TimeBaseStructure.TIM_Period = 0xFFFF;  
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;//向上计数模式 
    TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);

    TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_BothEdge, TIM_ICPolarity_BothEdge); //TIM_ICPolarity_Rising上升沿捕获
    TIM_ICStructInit(&TIM_ICInitStructure);
    TIM_ICInitStructure.TIM_ICFilter = 6; //无滤波器
    TIM_ICInit(TIM3, &TIM_ICInitStructure);

    TIM_ClearFlag(TIM3, TIM_FLAG_Update);
    TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);//使能中断

    TIM_Cmd(TIM3, ENABLE);//使能定时器3
}

void TIM4_Encoder_Init(void)//和TIM3一样
{
	GPIO_InitTypeDef GPIO_InitStructure;
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    TIM_ICInitTypeDef TIM_ICInitStructure;
    NVIC_InitTypeDef NVIC_InitStructure; 

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);

    GPIO_StructInit(&GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOB, &GPIO_InitStructure);

    NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    TIM_DeInit(TIM4);

    TIM_TimeBaseStructure.TIM_Prescaler = 0x0;  // No prescaling 
    TIM_TimeBaseStructure.TIM_Period = 0xFFFF;  
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //向上计数模式 
    TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);

    TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_BothEdge, TIM_ICPolarity_BothEdge); //TIM_ICPolarity_Rising上升沿捕获
    TIM_ICStructInit(&TIM_ICInitStructure);
    TIM_ICInitStructure.TIM_ICFilter = 6; //无滤波器
    TIM_ICInit(TIM4, &TIM_ICInitStructure);

    TIM_ClearFlag(TIM4, TIM_FLAG_Update);
    TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);	  //使能中断

    TIM_Cmd(TIM4, ENABLE);  	   //使能定时器3
}


void TIM3_IRQHandler(void)
{ 	
	if(TIM_GetITStatus(TIM3,TIM_IT_Update)!=0)
	{
		TIM_ClearITPendingBit(TIM3,TIM_IT_Update);//清除中断标志位
	}
}

void TIM4_IRQHandler(void)
{ 		    		  			    
	if(TIM_GetITStatus(TIM4,TIM_IT_Update)!=0)
	{
		TIM_ClearITPendingBit(TIM4,TIM_IT_Update);//清除中断标志位
	}
}

电机配置

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
#include "motor.h"
#include "stm32f10x.h"

void MOTOR_GPIO_Config()
{
	GPIO_InitTypeDef GPIO_InitStructure;//定义初始化结构体
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);//使能GPIO时钟

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;//选择要控制的GPIOB引脚	
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //设置引脚模式为通用推挽输出  
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//设置引脚速率为50MHz
    GPIO_Init(GPIOB, &GPIO_InitStructure);//初始化GPIO	  
    GPIO_SetBits(GPIOB, GPIO_Pin_13 | GPIO_Pin_14|GPIO_Pin_12 | GPIO_Pin_15);//全部初始化为高电平，制动状态
}

LED配置

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
#include "led.h"
#include "stm32f10x.h"

void LED_GPIO_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;//定义初始化结构体

	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC| RCC_APB2Periph_AFIO,ENABLE);//使能GPIOB和AFIO的外设时钟
	
    GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable , ENABLE);//改变指定管脚的映射 GPIO_Remap_SWJ_JTAGDisable ，JTAG-DP 禁用 + SW-DP 使能													   
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;//选择要控制的GPIOB引脚
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;//设置引脚速率为50MHz
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;//设置引脚模式为通用推挽输出
	
  	GPIO_Init(GPIOC, &GPIO_InitStructure);//初始化GPIOB	 
	
	GPIO_SetBits(GPIOC, GPIO_Pin_13);//打开led灯
}

软件I2C

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
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
#include "I2C.h"

#define SCL_H         GPIOB->BSRR = GPIO_Pin_8
#define SCL_L         GPIOB->BRR  = GPIO_Pin_8 

#define SDA_H         GPIOB->BSRR = GPIO_Pin_9
#define SDA_L         GPIOB->BRR  = GPIO_Pin_9 

#define SCL_read      GPIOB->IDR  & GPIO_Pin_8
#define SDA_read      GPIOB->IDR  & GPIO_Pin_9 

static void I2C_delay(void)
{
    volatile int i = 7;
    while (i)
        i--;
}

static bool I2C_Start(void)
{
    SDA_H;
    SCL_H;
    I2C_delay();
    if (!SDA_read)
        return false;
    SDA_L;
    I2C_delay();
    if (SDA_read)
        return false;
    SDA_L;
    I2C_delay();
    return true;
}

static void I2C_Stop(void)
{
    SCL_L;
    I2C_delay();
    SDA_L;
    I2C_delay();
    SCL_H;
    I2C_delay();
    SDA_H;
    I2C_delay();
}

static void I2C_Ack(void)
{
    SCL_L;
    I2C_delay();
    SDA_L;
    I2C_delay();
    SCL_H;
    I2C_delay();
    SCL_L;
    I2C_delay();
}

static void I2C_NoAck(void)
{
    SCL_L;
    I2C_delay();
    SDA_H;
    I2C_delay();
    SCL_H;
    I2C_delay();
    SCL_L;
    I2C_delay();
}

static bool I2C_WaitAck(void)
{
    SCL_L;
    I2C_delay();
    SDA_H;
    I2C_delay();
    SCL_H;
    I2C_delay();
    if (SDA_read) {
        SCL_L;
        return false;
    }
    SCL_L;
    return true;
}

static void I2C_SendByte(uint8_t byte)
{
    uint8_t i = 8;
    while (i--) {
        SCL_L;
        I2C_delay();
        if (byte & 0x80)
            SDA_H;
        else
            SDA_L;
        byte <<= 1;
        I2C_delay();
        SCL_H;
        I2C_delay();
    }
    SCL_L;
}

static uint8_t I2C_ReceiveByte(void)
{
    uint8_t i = 8;
    uint8_t byte = 0;

    SDA_H;
    while (i--) {
        byte <<= 1;
        SCL_L;
        I2C_delay();
        SCL_H;
        I2C_delay();
        if (SDA_read) {
            byte |= 0x01;
        }
    }
    SCL_L;
    return byte;
}

void i2cInit(void)
{
    GPIO_InitTypeDef gpio;
    
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB , ENABLE); 
    
    gpio.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9;
    gpio.GPIO_Speed = GPIO_Speed_2MHz;
    gpio.GPIO_Mode = GPIO_Mode_Out_OD;
    GPIO_Init(GPIOB, &gpio);

}

bool i2cWriteBuffer(uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data)
{
    int i;
    if (!I2C_Start())
        return false;
    I2C_SendByte(addr << 1 | I2C_Direction_Transmitter);
    if (!I2C_WaitAck()) {
        I2C_Stop();
        return false;
    }
    I2C_SendByte(reg);
    I2C_WaitAck();
    for (i = 0; i < len; i++) {
        I2C_SendByte(data[i]);
        if (!I2C_WaitAck()) {
            I2C_Stop();
            return false;
        }
    }
    I2C_Stop();
    return true;
}
/////////////////////////////////////////////////////////////////////////////////
int8_t i2cwrite(uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data)
{
	if(i2cWriteBuffer(addr,reg,len,data))
	{
		return TRUE;
	}
	else
	{
		return FALSE;
	}
	//return FALSE;
}
int8_t i2cread(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
{
	if(i2cRead(addr,reg,len,buf))
	{
		return TRUE;
	}
	else
	{
		return FALSE;
	}
	//return FALSE;
}
//////////////////////////////////////////////////////////////////////////////////
bool i2cWrite(uint8_t addr, uint8_t reg, uint8_t data)
{
    if (!I2C_Start())
        return false;
    I2C_SendByte(addr << 1 | I2C_Direction_Transmitter);
    if (!I2C_WaitAck()) {
        I2C_Stop();
        return false;
    }
    I2C_SendByte(reg);
    I2C_WaitAck();
    I2C_SendByte(data);
    I2C_WaitAck();
    I2C_Stop();
    return true;
}

bool i2cRead(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
{
    if (!I2C_Start())
        return false;
    I2C_SendByte(addr << 1 | I2C_Direction_Transmitter);
    if (!I2C_WaitAck()) {
        I2C_Stop();
        return false;
    }
    I2C_SendByte(reg);
    I2C_WaitAck();
    I2C_Start();
    I2C_SendByte(addr << 1 | I2C_Direction_Receiver);
    I2C_WaitAck();
    while (len) {
        *buf = I2C_ReceiveByte();
        if (len == 1)
            I2C_NoAck();
        else
            I2C_Ack();
        buf++;
        len--;
    }
    I2C_Stop();
    return true;
}

uint16_t i2cGetErrorCounter(void)
{
    // TODO maybe fix this, but since this is test code, doesn't matter.
    return 0;
}

Systick配置

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
#include "systick.h"
#include "stm32f10x.h"
#include "car.h"

u16 TimeDelay=0;

extern void SysTick_Init(void)
{
	if (SysTick_Config(SystemCoreClock / 200))// 1s/x= a ms  即5ms产生一次systick中断
 	{ 
		while(1);
	}
	SysTick->CTRL |=  SysTick_CTRL_ENABLE_Msk;
}

void SysTick_Handler(void)//5ms定时器
{  
	GetMotorPulse();//脉冲计算
	SpeedControl();//速度环
	TurnControl(); //转向环
	AngleControl();//角度环
	MotorOutput();//小车总PWM输出  	
}

小车平衡控制

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
#include "car.h"
#include "stm32f10x.h"
#include "mpu6050.h"
#include "timer.h"

float BST_fCarSpeed;//车子当前速度值
float BST_fCarSpeedOld;//车子上一次速度值
float BST_fCarPosition;//车子位置
float BST_fCarAngle;//车子角度
float BST_fAngleControlOut;//角度控制PWM值
float BST_fSpeedControlOut;//速度控制PWM值
float BST_fTurnControlOut;//转向环控制PWM值
float BST_fLeftMotorOut;//左电机输出PWM值
float BST_fRightMotorOut;//右电机输出PWM值

/******电机控制参数******/
float BST_fSpeedControlOut;//速度控制PWM
float BST_fSpeedControlOutOld;
float BST_fSpeedControlOutNew;
float BST_fAngleControlOut;//角度控制PWM
float BST_fTurnControlOut;//转向控制PWM
float BST_fLeftMotorOut;//左电机输出PWM
float BST_fRightMotorOut;//右电机输出PWM

/*-----角度环和速度环PID控制参数-----*///以下参考为重点调试参考，同电池电压有关，建议充好电再调试
float  BST_fCarAngle_P =84;//角度环P
float  BST_fCarAngle_D =0.3;//角度环D

float  BST_fCarSpeed_P=-0.4;//速度环P
float  BST_fCarSpeed_I=-0.002;//速度环I

float BST_fCarTurn=-0.28;//转向环

/******速度控制参数******/
s16   BST_s16LeftMotorPulse; //左电机脉冲数
s16	  BST_s16RightMotorPulse;//右电机脉冲数

s32   BST_s32LeftMotorPulseOld;
s32   BST_s32RightMotorPulseOld;
s32   BST_s32LeftMotorPulseSigma;//50ms左电机叠加值
s32   BST_s32RightMotorPulseSigma;//50ms右电机叠加值

float BST_fCarSpeed;//测速码盘得出的车速
float BST_fCarSpeedOld;

float BST_fCarPosition;//测速码盘通过计算得到的小车位移

/*-----悬停参数-----*/
int leftstop=0;
int rightstop=0;
int stopflag=0;

void CarUpstandInit(void)
{
	BST_s16LeftMotorPulse = BST_s16RightMotorPulse = 0;//左右脉冲值初始化
	BST_s32LeftMotorPulseSigma = BST_s32RightMotorPulseSigma = 0;//叠加脉冲数初始化

	BST_fCarSpeed = BST_fCarSpeedOld = 0;//平衡小车车速	初始化
	BST_fCarPosition = 0;//平衡小车位移量	初始化
	BST_fCarAngle= 0;//平衡小车车速	初始化

	BST_fAngleControlOut = BST_fSpeedControlOut=0;//角度PWM、车速PWM、蓝牙控制PWM初始化
	BST_fLeftMotorOut= BST_fRightMotorOut   = 0;//左右车轮PWM输出值初始化
}

void GetMotorPulse(void)//采集电机速度脉冲
{ 
	uint16_t u16TempLeft;
	uint16_t u16TempRight;
	
	u16TempLeft = TIM_GetCounter(TIM3);//TIM3定时器计算调用
 	u16TempRight= TIM_GetCounter(TIM4);	//TIM4定时器计算调用
	leftstop=u16TempLeft;
	rightstop=u16TempRight;
	TIM_SetCounter(TIM3,0);//TIM3->CNT = 0;
	TIM_SetCounter(TIM4,0);//TIM4->CNT = 0;//清零
	BST_s16LeftMotorPulse=u16TempLeft;
	BST_s16RightMotorPulse=(-u16TempRight);
		
	BST_s32LeftMotorPulseSigma  +=BST_s16LeftMotorPulse;//脉冲值叠加 40ms叠加值
	BST_s32RightMotorPulseSigma +=BST_s16RightMotorPulse;//脉冲值叠加 40ms叠加值
}

void AngleControl(void)	 
{
	if(Pitch==0||Pitch<-20||Pitch>20)//MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
	{
		  //Pitch=1;
	  GPIO_ResetBits(GPIOC, GPIO_Pin_13);//MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
	}
	else 
	{GPIO_SetBits(GPIOC, GPIO_Pin_13);}//MPU6050状态正常时LED灯熄灭
	
	BST_fCarAngle = Roll- CAR_ZERO_ANGLE;//DMP ROLL滚动方向角度与预设小车倾斜角度值的差得出角度   
	BST_fAngleControlOut =  (BST_fCarAngle-BST_fSpeedControlOutNew) * BST_fCarAngle_P + gyro[0] * BST_fCarAngle_D ;//角度PD控制							   
}

void TurnControl(void)
{
	BST_fTurnControlOut=BST_fCarTurn*gyro[2];
}
void SpeedControl(void)
{
	BST_fCarSpeed = (BST_s32LeftMotorPulseSigma  + BST_s32RightMotorPulseSigma );// * 0.5 ;//左右电机脉冲数平均值作为小车当前车速
	BST_s32LeftMotorPulseSigma =BST_s32RightMotorPulseSigma = 0;	  //全局变量 注意及时清零		
	BST_fCarSpeedOld *= 0.7;
	BST_fCarSpeedOld +=BST_fCarSpeed*0.3;
	BST_fCarPosition += BST_fCarSpeedOld;//路程  即速度积分	   1/11 3:03
	if(stopflag==1)
	{
		BST_fCarPosition=0;
	}
	
	//积分上限设限//
	if((s32)BST_fCarPosition > CAR_POSITION_MAX)    BST_fCarPosition = CAR_POSITION_MAX;
	if((s32)BST_fCarPosition < CAR_POSITION_MIN)    BST_fCarPosition = CAR_POSITION_MIN;
																								  
	BST_fSpeedControlOutNew = (BST_fCarSpeedOld-CAR_SPEED_SET ) * BST_fCarSpeed_P + (BST_fCarPosition - CAR_POSITION_SET ) * BST_fCarSpeed_I; //速度PI算法 速度*P +位移*I=速度PWM输出
}

void MotorOutput(void)																					  //电机PWM输出函数
{	   
			//右电机转向PWM控制融合平衡角度、速度输出

	BST_fLeftMotorOut  = BST_fAngleControlOut-BST_fTurnControlOut;//左电机转向PWM控制融合平衡角度、速度输出	
  BST_fRightMotorOut = BST_fAngleControlOut+BST_fTurnControlOut;//右电机转向PWM控制融合平衡角度、速度输出

	
	if((s16)BST_fLeftMotorOut  > MOTOR_OUT_MAX)	BST_fLeftMotorOut  = MOTOR_OUT_MAX;
	if((s16)BST_fLeftMotorOut  < MOTOR_OUT_MIN)	BST_fLeftMotorOut  = MOTOR_OUT_MIN;
	if((s16)BST_fRightMotorOut > MOTOR_OUT_MAX)	BST_fRightMotorOut = MOTOR_OUT_MAX;
	if((s16)BST_fRightMotorOut < MOTOR_OUT_MIN)	BST_fRightMotorOut = MOTOR_OUT_MIN;
	
  SetMotorVoltageAndDirection((s16)BST_fLeftMotorOut,(s16)BST_fRightMotorOut); 
}

void SetMotorVoltageAndDirection(s16 s16LeftVoltage,s16 s16RightVoltage)
{
	  u16 u16LeftMotorValue;
	  u16 u16RightMotorValue;
	
    if(s16LeftVoltage<0)//当左电机PWM输出为负时 PB14设为正 PB15设为负 （PB14 15 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
    {	
	    GPIO_SetBits(GPIOB, GPIO_Pin_14 );				    
      GPIO_ResetBits(GPIOB, GPIO_Pin_15 );
      s16LeftVoltage = (-s16LeftVoltage);
    }
    else 
    {	
      GPIO_SetBits(GPIOB, GPIO_Pin_15 );//当左电机PWM输出为正时 PB14设为负 PB15设为正 （PB14 15 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
      GPIO_ResetBits(GPIOB, GPIO_Pin_14 ); 
      s16LeftVoltage = s16LeftVoltage;
    }

    if(s16RightVoltage<0)
    {
      //当右电机PWM输出为负时 PB12设为正 PB13设为负 （PB12 13 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
      GPIO_SetBits(GPIOB, GPIO_Pin_13 );				    
      GPIO_ResetBits(GPIOB, GPIO_Pin_12 );
      s16RightVoltage = (-s16RightVoltage);
    }
    else //当右电机PWM输出为正时 PB12设为负 PB13设为正 （PB12 13 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
    {
	    GPIO_SetBits(GPIOB, GPIO_Pin_12 );				    
      GPIO_ResetBits(GPIOB, GPIO_Pin_13 );	
     
      s16RightVoltage = s16RightVoltage;
    }
		
	u16RightMotorValue= (u16)s16RightVoltage;
	u16LeftMotorValue = (u16)s16LeftVoltage;
		
	TIM_SetCompare3(TIM2,u16LeftMotorValue);//TIM2与 u16RightMotorValue对比，不相同则翻转波形，调节PWM占空比
	TIM_SetCompare4(TIM2,u16RightMotorValue);//TIM3与 u16LeftMotorValue对比，不相同则翻转波形，调节PWM占空比

#if 1	 /*判断车辆 是否悬停或跌倒*/
		
  if(Pitch>40||Pitch<-40)
	{		
		TIM_SetCompare3(TIM2,0);
		TIM_SetCompare4(TIM2,0);
		stopflag=1;		
	}
	else stopflag=0;
	
	if(BST_fCarAngle > 50 || BST_fCarAngle < (-50))
	{
		TIM_SetCompare3(TIM2,0);
		TIM_SetCompare4(TIM2,0);  
		stopflag=1;	
	}
	else stopflag=0;

#endif
}

MPU6050

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

#include "mpu6050.h"
#include "i2c.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "usart.h"
#include "math.h"

static signed char gyro_orientation[9] = {-1, 0, 0,
                                           0,-1, 0,
                                           0, 0, 1};
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
float Pitch,Roll,Yaw;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];

void MPU6050_Init(void)
{
	int result=0;
	//IIC_Init();
	result=mpu_init();
	if(!result)
	{	 		 
	
		PrintChar("mpu initialization complete......\n ");		//mpu initialization complete	 	  

		if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))		//mpu_set_sensor
			PrintChar("mpu_set_sensor complete ......\n");
		else
			PrintChar("mpu_set_sensor come across error ......\n");

		if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))	//mpu_configure_fifo
			PrintChar("mpu_configure_fifo complete ......\n");
		else
			PrintChar("mpu_configure_fifo come across error ......\n");

		if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))	   	  		//mpu_set_sample_rate
		 PrintChar("mpu_set_sample_rate complete ......\n");
		else
		 	PrintChar("mpu_set_sample_rate error ......\n");

		if(!dmp_load_motion_driver_firmware())   	  			//dmp_load_motion_driver_firmvare
			PrintChar("dmp_load_motion_driver_firmware complete ......\n");
		else
			PrintChar("dmp_load_motion_driver_firmware come across error ......\n");

		if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) 	  //dmp_set_orientation
		 	PrintChar("dmp_set_orientation complete ......\n");
		else
		 	PrintChar("dmp_set_orientation come across error ......\n");

		if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
		    DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
		    DMP_FEATURE_GYRO_CAL))		   	 					 //dmp_enable_feature
		 	PrintChar("dmp_enable_feature complete ......\n");
		else
		 	PrintChar("dmp_enable_feature come across error ......\n");

		if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))   	 			 //dmp_set_fifo_rate
		 	PrintChar("dmp_set_fifo_rate complete ......\n");
		else
		 	PrintChar("dmp_set_fifo_rate come across error ......\n");

		run_self_test();		//自检

		if(!mpu_set_dmp_state(1))
		 	PrintChar("mpu_set_dmp_state complete ......\n");
		else
		 	PrintChar("mpu_set_dmp_state come across error ......\n");
	}
	else												 //MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
	 {
		GPIO_ResetBits(GPIOC, GPIO_Pin_13);				//MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
		while(1);
	 }
	 
}
void MPU6050_Pose(void)
{
	dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);	 
	/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
	 * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
	**/
	/*if (sensors & INV_XYZ_GYRO )
	send_packet(PACKET_TYPE_GYRO, gyro);
	if (sensors & INV_XYZ_ACCEL)
	send_packet(PACKET_TYPE_ACCEL, accel); */
	/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
	 * The orientation is set by the scalar passed to dmp_set_orientation during initialization. 
	**/
	
	
	if(sensors & INV_WXYZ_QUAT )
	{
		q0 = quat[0] / q30;	
		q1 = quat[1] / q30;
		q2 = quat[2] / q30;
		q3 = quat[3] / q30;

		Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	// pitch
		Roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)*57.3;	// roll
		Yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw	
	}
}

并级PID

并级PID和串级PID只有Systick中断服务函数和小车控制部分不一样

Systick

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"
#include "stm32f10x.h"
#include "car.h"

u16 TimeDelay=0;

extern void SysTick_Init(void)//1ms定时时钟
{
	if (SysTick_Config(SystemCoreClock / 200))	// ST3.5.0库版本   1s/x= a ms  即5ms产生一次systick中断
 	{ 
		while(1);
	}
	SysTick->CTRL |=  SysTick_CTRL_ENABLE_Msk;
}

void SysTick_Handler(void)//5ms定时器
{  
	BST_u8SpeedControlCount++;
	GetMotorPulse();//脉冲计算函数

	AngleControl();//角度PD控制PWNM输出
	TurnControl();
	MotorOutput();//小车总PWM输出  	

  if(BST_u8SpeedControlCount>=8)//当计数值8时，即总系统运行40ms时候(每10个角度PWM输出中融入1个速度PWM输出，这样能保持速度PID输出不干扰角度PID输出，从而影响小车平衡)
	{	
		SpeedControl(); //车模速度控制函数   每40ms调用一次
		BST_u8SpeedControlCount=0;//小车速度控制调用计数值清零
		BST_u8SpeedControlPeriod=0;//平滑输出比例值清零
	}
}

小车平衡控制

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
216
217
#include "car.h"
#include "stm32f10x.h"
#include "mpu6050.h"
#include "timer.h"

float BST_fCarSpeed;//车子当前速度值
float BST_fCarSpeedOld;//车子上一次速度值
float BST_fCarPosition;//车子位置
float BST_fCarAngle;//车子角度
float BST_fAngleControlOut;//角度控制PWM值
float BST_fSpeedControlOut;//速度控制PWM值
float BST_fLeftMotorOut;//左电机输出PWM值
float BST_fRightMotorOut;//右电机输出PWM值

float BST_u8MainEventCount=0;//用于5ms定时器子程序SysTick_Handler(void)中总中断计数位
float BST_u8SpeedControlCount=0;//用于5ms定时器子程序SysTick_Handler(void)中50ms速度平衡融入计数位
float BST_u8SpeedControlPeriod=0;//用于5ms定时器子程序SysTick_Handler(void)中50ms速度平衡融入计数位


u8 BST_u8DirectionControlPeriod;
u8 BST_u8DirectionControlCount;//转向控制循环计数  在SysTick_Handler(void)中使用 每5ms加1 


/******电机控制参数******/
float BST_fSpeedControlOut;//速度控制PWM
float BST_fSpeedControlOutOld;
float BST_fSpeedControlOutNew;
float BST_fAngleControlOut;
float BST_fTurnControlOut;
float BST_fLeftMotorOut;
float BST_fRightMotorOut;

float BST_fCarAngle;//角度控制PWM
float gyro_z;
float gyrx;
float gy0;

/*-----角度环和速度环PID控制参数-----*///以下参考为重点调试参考，同电池电压有关，建议充好电再调试
float  BST_fCarAngle_P =91.3;//	91.3 //调大小时会左右摆，调大时会振动  请调到基本能够站立 P=91.3是用于给小车在运动过程使用
float  BST_fCarAngle_D =0.21;	// 0.001 0.002 0.004 0.008 0.0010 0.011	 调小时反应慢，调大时会干扰

float  BST_fCarSpeed_P=5.1;
float  BST_fCarSpeed_I=0.10;

float BST_fCarTurn=0.4;

const double PID_Original[4] ={91.3, 0.21, 5.1, 0.10}; 
char  alldata[80];
char *iap;

/******速度控制参数******/
s16   BST_s16LeftMotorPulse;//左电机脉冲数
s16	  BST_s16RightMotorPulse;//右电机脉冲数

s32   BST_s32LeftMotorPulseOld;
s32   BST_s32RightMotorPulseOld;
s32   BST_s32LeftMotorPulseSigma;//50ms左电机叠加值
s32   BST_s32RightMotorPulseSigma;//50ms右电机叠加值

float BST_fCarSpeed;//测速码盘得出的车速
float BST_fCarSpeedOld;

float BST_fCarPosition;//测速码盘通过计算得到的小车位移

/*-----悬停参数-----*/
int leftstop=0;
int rightstop=0;
int stopflag=0;

void CarUpstandInit(void)
{
	BST_s16LeftMotorPulse = BST_s16RightMotorPulse = 0;//左右脉冲值初始化
	BST_s32LeftMotorPulseSigma = BST_s32RightMotorPulseSigma = 0;//叠加脉冲数初始化

	BST_fCarSpeed = BST_fCarSpeedOld = 0;//平衡小车车速	初始化
	BST_fCarPosition = 0;//平衡小车位移量	初始化
	BST_fCarAngle= 0;//平衡小车车速	初始化

	BST_fAngleControlOut = BST_fSpeedControlOut=0;//角度PWM、车速PWM、蓝牙控制PWM初始化
	BST_fLeftMotorOut= BST_fRightMotorOut   = 0;//左右车轮PWM输出值初始化
	
  BST_u8MainEventCount=0;//用于5ms定时器子程序SysTick_Handler(void)中总中断计数位
	BST_u8SpeedControlCount=0;//用于5ms定时器子程序SysTick_Handler(void)中50ms速度平衡融入计数位
  BST_u8SpeedControlPeriod=0;//用于5ms定时器子程序SysTick_Handler(void)中50ms速度平衡融入计数位
}

void GetMotorPulse(void)//采集电机速度脉冲
{ 
	uint16_t u16TempLeft;
	uint16_t u16TempRight;
	
	u16TempLeft = TIM_GetCounter(TIM3);//TIM3定时器计算调用
 	u16TempRight= TIM_GetCounter(TIM4);	//TIM4定时器计算调用
	leftstop=u16TempLeft;
	rightstop=u16TempRight;
	TIM_SetCounter(TIM3,0);//TIM3->CNT = 0;
	TIM_SetCounter(TIM4,0);//TIM4->CNT = 0;//清零
	BST_s16LeftMotorPulse=u16TempLeft;
	BST_s16RightMotorPulse=(-u16TempRight);
		
	BST_s32LeftMotorPulseSigma  +=BST_s16LeftMotorPulse;//脉冲值叠加 40ms叠加值
	BST_s32RightMotorPulseSigma +=BST_s16RightMotorPulse;//脉冲值叠加 40ms叠加值
}

void AngleControl(void)	 
{
	if(Pitch==0||Pitch<-20||Pitch>20)//MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
	{
		  //Pitch=1;
	  GPIO_ResetBits(GPIOC, GPIO_Pin_13);//MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
	}
	else 
	{GPIO_SetBits(GPIOC, GPIO_Pin_13);}//MPU6050状态正常时LED灯熄灭
	
	BST_fCarAngle = Roll- CAR_ZERO_ANGLE;//DMP ROLL滚动方向角度与预设小车倾斜角度值的差得出角度   
	BST_fAngleControlOut =  BST_fCarAngle * BST_fCarAngle_P + gyro[0] * BST_fCarAngle_D ;	  //角度PD控制							   
}

void TurnControl(void)
{
	BST_fTurnControlOut=BST_fCarTurn*gyro[2];
}

void SpeedControl(void)
{
	BST_fCarSpeed = (BST_s32LeftMotorPulseSigma  + BST_s32RightMotorPulseSigma );// * 0.5 ;//左右电机脉冲数平均值作为小车当前车速
	BST_s32LeftMotorPulseSigma =BST_s32RightMotorPulseSigma = 0;//全局变量 注意及时清零		
	BST_fCarSpeedOld *= 0.7;
	BST_fCarSpeedOld +=BST_fCarSpeed*0.3;
	BST_fCarPosition += BST_fCarSpeedOld;//路程  即速度积分	   1/11 3:03
	if(stopflag==1)
	{
		BST_fCarPosition=0;
		
	}
	
	//积分上限设限//
	if((s32)BST_fCarPosition > CAR_POSITION_MAX)    BST_fCarPosition = CAR_POSITION_MAX;
	if((s32)BST_fCarPosition < CAR_POSITION_MIN)    BST_fCarPosition = CAR_POSITION_MIN;
																								  
	BST_fSpeedControlOutNew = (BST_fCarSpeedOld -CAR_SPEED_SET ) * BST_fCarSpeed_P + (BST_fCarPosition - CAR_POSITION_SET ) * BST_fCarSpeed_I; //速度PI算法 速度*P +位移*I=速度PWM输出
}

void MotorOutput(void) //电机PWM输出函数
{	   
	BST_fLeftMotorOut  = BST_fAngleControlOut +BST_fSpeedControlOutNew+BST_fTurnControlOut;//左电机PWM控制输出	
  BST_fRightMotorOut = BST_fAngleControlOut +BST_fSpeedControlOutNew-BST_fTurnControlOut;//右电机PWM控输出

		
	if((s16)BST_fLeftMotorOut  > MOTOR_OUT_MAX)	BST_fLeftMotorOut  = MOTOR_OUT_MAX;
	if((s16)BST_fLeftMotorOut  < MOTOR_OUT_MIN)	BST_fLeftMotorOut  = MOTOR_OUT_MIN;
	if((s16)BST_fRightMotorOut > MOTOR_OUT_MAX)	BST_fRightMotorOut = MOTOR_OUT_MAX;
	if((s16)BST_fRightMotorOut < MOTOR_OUT_MIN)	BST_fRightMotorOut = MOTOR_OUT_MIN;
	
  SetMotorVoltageAndDirection((s16)BST_fLeftMotorOut,(s16)BST_fRightMotorOut);
    
}

void SetMotorVoltageAndDirection(s16 s16LeftVoltage,s16 s16RightVoltage)
{
	  u16 u16LeftMotorValue;
	  u16 u16RightMotorValue;
	
    if(s16LeftVoltage<0)//当左电机PWM输出为负时 PB14设为正 PB15设为负 （PB14 15 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
    {	
	    GPIO_SetBits(GPIOB, GPIO_Pin_14 );				    
      GPIO_ResetBits(GPIOB, GPIO_Pin_15 );
      s16LeftVoltage = (-s16LeftVoltage);
    }
    else 
    {	
      GPIO_SetBits(GPIOB, GPIO_Pin_15 );//当左电机PWM输出为正时 PB14设为负 PB15设为正 （PB14 15 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
      GPIO_ResetBits(GPIOB, GPIO_Pin_14 ); 
      s16LeftVoltage = s16LeftVoltage;
    }

    if(s16RightVoltage<0)
    {//当右电机PWM输出为负时 PB12设为正 PB13设为负 （PB12 13 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
      GPIO_SetBits(GPIOB, GPIO_Pin_13 );				    
      GPIO_ResetBits(GPIOB, GPIO_Pin_12 );
      s16RightVoltage = (-s16RightVoltage);
    }
    else //当右电机PWM输出为正时 PB12设为负 PB13设为正 （PB12 13 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
    {
	    GPIO_SetBits(GPIOB, GPIO_Pin_12 );				    
      GPIO_ResetBits(GPIOB, GPIO_Pin_13 );	
     
      s16RightVoltage = s16RightVoltage;
    }
		
	   u16RightMotorValue= (u16)s16RightVoltage;
	   u16LeftMotorValue = (u16)s16LeftVoltage;


	TIM_SetCompare3(TIM2,u16LeftMotorValue);//TIM2与 u16RightMotorValue对比，不相同则翻转波形，调节PWM占空比
	TIM_SetCompare4(TIM2,u16RightMotorValue);//TIM3与 u16LeftMotorValue对比，不相同则翻转波形，调节PWM占空比

#if 1	 /*判断车辆 是否悬停或跌倒*/
		
  if(Pitch>40||Pitch<-40)
	{		
		TIM_SetCompare3(TIM2,0);
		TIM_SetCompare4(TIM2,0);
		stopflag=1;		
	}
	else stopflag=0;
	
	if(BST_fCarAngle > 50 || BST_fCarAngle < (-50))
	{
		TIM_SetCompare3(TIM2,0);
		TIM_SetCompare4(TIM2,0);  
		stopflag=1;	
	}
	else stopflag=0;

#endif
}

PID调参

步骤：

  1. 把所有Kp和Ki设置为0；
  2. 从角度环开始调，先调Kp，确定Kp极性，现将Kp设定一个正值，如果轮子走向和倾斜的方向相同，则极性正确，Kp为正值，否则极性错误，Kp为负值，Kp绝对值从小到大开始调，直到车子出现明显抖动，之后调节Kd值，Kd值得极性确定，将Kp设置为0，将Kd设置为一个正值，将一个轮子用手转动，另一个轮子同向转动，则极性正确，Kd为正值，否则极性错误，Kd为负值，然后将刚刚调的Kp再填上，将Kd的绝对值从小到大调，直到车子出现高频抖动，之后将Kp，Kd的值 * 0.6作为最终Kp，Kd值；
  3. 然后调节速度环，ki=1/200Kp，极性和Kp相同，设置Kp同时设置Ki，确定Kp的极性，将角度环的Kp，Kd设为0，速度环Ki设为0，Kp设置为一个正值，将一个轮子用手转动，另一个轮子同向转动，则极性正确，Kp为正值，否则极性错误，Kp为负值，将Kp绝对值从小到大调，直到小车速度接近0，回位效果好，将此时的Kp，Ki作为最终Kp，Ki；
  4. 然后调转向环，将其他PID参数设置为0，将转向参数设置为一个正值，将车子往一个方向转动，如果车子是顺着转的方向转，则转向参数极性错误，转向参数应为负值，否则为正值，增大转向参数绝对值，直到车子能不往一个方向转动切没有抖动现象。

实现蓝牙控制方向

Remote.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
#include "Remote.h"
#include "NRF24L01.h"
#include "stm32f10x.h"
#include "car.h"
#include "usart.h"
float BST_BluetoothSpeed=0;
void Remote_Control(void)
{
	if(Fore==0&&Back==0&&Left==0&&Right==0)
	{
		if(BST_BluetoothSpeed<=2.5&&BST_BluetoothSpeed>=-2.5)
		{
			BST_BluetoothSpeed=0;
		}
		if(BST_BluetoothSpeed>5)
		{
			BST_BluetoothSpeed-=5;
		}
		if(BST_BluetoothSpeed<-5)
		{
			BST_BluetoothSpeed+=5;
		}
		if(BST_BluetoothSpeed==0&&BST_fCarSpeed>=-2.5&&BST_fCarSpeed<=2.5)
		{
			BST_fCarTurn_Kd=0.24;
		}
		BST_fCarTurn_Kp=0;
	}
	else if(Fore==1)
	{
		//BST_fCarTurn_Kd=0;
		BST_fCarTurn_Kd=0.24;
		BST_BluetoothSpeed+=5;
	}
	else if(Back==1)
	{
		//BST_fCarTurn_Kd=0;
		BST_fCarTurn_Kd=0.24;
		BST_BluetoothSpeed-=5;
	}
	
//	
//	if(Left==0&&Right==0)
//	{
//		BST_fCarTurn_Kd=0.25;
//		BST_fCarTurn_Kp=0;
//	}
	else if(Left==1)
	{
		BST_fCarTurn_Kd=0;
		BST_fCarTurn_Kp=1;
		BST_TurnSpeed+=20;
	}
	else if(Right==1)
	{
		BST_fCarTurn_Kd=0;
		BST_fCarTurn_Kp=1;
		BST_TurnSpeed-=20;
	}
	BST_BluetoothSpeed=BST_BluetoothSpeed>300?300:(BST_BluetoothSpeed<-300?-300:BST_BluetoothSpeed);
	BST_TurnSpeed=BST_TurnSpeed>500?500:(BST_TurnSpeed<-500?-500:BST_TurnSpeed);
}

Car.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
216
217
218
219
220
221
222
223
224
225
226
227
#include "car.h"
#include "stm32f10x.h"
#include "mpu6050.h"
#include "timer.h"
#include "Remote.h"

float CAR_SPEED_SET=0;
float CAR_ZERO_ANGLE=0;

float BST_fCarSpeed;//车子当前速度值
float BST_fCarSpeedOld;//车子上一次速度值
float BST_TurnSpeed=0;

float BST_fCarPosition;//车子位置
float BST_fCarAngle;//车子角度
float BST_fAngleControlOut;//角度控制PWM值
float BST_fSpeedControlOut;//速度控制PWM值
float BST_fLeftMotorOut;//左电机输出PWM值
float BST_fRightMotorOut;//右电机输出PWM值

float BST_u8MainEventCount=0;//用于5ms定时器子程序SysTick_Handler(void)中总中断计数位
float BST_u8SpeedControlCount=0;//用于5ms定时器子程序SysTick_Handler(void)中50ms速度平衡融入计数位
float BST_u8SpeedControlPeriod=0;//用于5ms定时器子程序SysTick_Handler(void)中50ms速度平衡融入计数位


u8 BST_u8DirectionControlPeriod;
u8 BST_u8DirectionControlCount;//转向控制循环计数  在SysTick_Handler(void)中使用 每5ms加1 


/******电机控制参数******/
float BST_fSpeedControlOut;//速度控制PWM
float BST_fSpeedControlOutOld;
float BST_fSpeedControlOutNew;
float BST_fAngleControlOut;
float BST_fTurnControlOut;
float BST_fLeftMotorOut;
float BST_fRightMotorOut;

float BST_fCarAngle;//角度控制PWM
float gyro_z;
float gyrx;
float gy0;

/*-----角度环和速度环PID控制参数-----*///以下参考为重点调试参考，同电池电压有关，建议充好电再调试
float  BST_fCarAngle_P =91.3;//	91.3 //调大小时会左右摆，调大时会振动  请调到基本能够站立 P=91.3是用于给小车在运动过程使用
float  BST_fCarAngle_D =0.22;	// 0.001 0.002 0.004 0.008 0.0010 0.011	 调小时反应慢，调大时会干扰

float  BST_fCarSpeed_P=5.1;
float  BST_fCarSpeed_I=0.10;

float BST_fCarTurn_Kd=0.24;
float BST_fCarTurn_Kp=0;

const double PID_Original[4] ={91.3, 0.21, 5.1, 0.10}; 
char  alldata[80];
char *iap;

/******速度控制参数******/
s16   BST_s16LeftMotorPulse;//左电机脉冲数
s16	  BST_s16RightMotorPulse;//右电机脉冲数

s32   BST_s32LeftMotorPulseOld;
s32   BST_s32RightMotorPulseOld;
s32   BST_s32LeftMotorPulseSigma;//50ms左电机叠加值
s32   BST_s32RightMotorPulseSigma;//50ms右电机叠加值

float BST_fCarSpeed;//测速码盘得出的车速
float BST_fCarSpeedOld;

float BST_fCarPosition;//测速码盘通过计算得到的小车位移

/*-----悬停参数-----*/
int leftstop=0;
int rightstop=0;
int stopflag=0;

void CarUpstandInit(void)
{
	BST_s16LeftMotorPulse = BST_s16RightMotorPulse = 0;//左右脉冲值初始化
	BST_s32LeftMotorPulseSigma = BST_s32RightMotorPulseSigma = 0;//叠加脉冲数初始化

	BST_fCarSpeed = BST_fCarSpeedOld = 0;//平衡小车车速	初始化
	BST_fCarPosition = 0;//平衡小车位移量	初始化
	BST_fCarAngle= 0;//平衡小车车速	初始化

	BST_fAngleControlOut = BST_fSpeedControlOut=0;//角度PWM、车速PWM、蓝牙控制PWM初始化
	BST_fLeftMotorOut= BST_fRightMotorOut   = 0;//左右车轮PWM输出值初始化
	
  BST_u8MainEventCount=0;//用于5ms定时器子程序SysTick_Handler(void)中总中断计数位
	BST_u8SpeedControlCount=0;//用于5ms定时器子程序SysTick_Handler(void)中50ms速度平衡融入计数位
  BST_u8SpeedControlPeriod=0;//用于5ms定时器子程序SysTick_Handler(void)中50ms速度平衡融入计数位
}

void GetMotorPulse(void)//采集电机速度脉冲
{ 
	uint16_t u16TempLeft;
	uint16_t u16TempRight;
	
	u16TempLeft = TIM_GetCounter(TIM3);//TIM3定时器计算调用
 	u16TempRight= TIM_GetCounter(TIM4);	//TIM4定时器计算调用
	leftstop=u16TempLeft;
	rightstop=u16TempRight;
	TIM_SetCounter(TIM3,0);//TIM3->CNT = 0;
	TIM_SetCounter(TIM4,0);//TIM4->CNT = 0;//清零
	BST_s16LeftMotorPulse=u16TempLeft;
	BST_s16RightMotorPulse=(-u16TempRight);
		
	BST_s32LeftMotorPulseSigma  +=BST_s16LeftMotorPulse;//脉冲值叠加 40ms叠加值
	BST_s32RightMotorPulseSigma +=BST_s16RightMotorPulse;//脉冲值叠加 40ms叠加值
}

void AngleControl(void)	 
{
	if(Pitch==0||Pitch<-20||Pitch>20)//MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
	{
		  //Pitch=1;
	  GPIO_ResetBits(GPIOC, GPIO_Pin_13);//MPU6050状态指示灯 STM32核心板 PC13 绿色灯亮起为不正常
	}
	else 
	{GPIO_SetBits(GPIOC, GPIO_Pin_13);}//MPU6050状态正常时LED灯熄灭
	
	BST_fCarAngle = Roll- CAR_ZERO_ANGLE;//DMP ROLL滚动方向角度与预设小车倾斜角度值的差得出角度   
	BST_fAngleControlOut =  BST_fCarAngle * BST_fCarAngle_P + gyro[0] * BST_fCarAngle_D ;	  //角度PD控制							   
}

void TurnControl(void)
{
	BST_fTurnControlOut=BST_fCarTurn_Kd*gyro[2]+BST_fCarTurn_Kp*BST_TurnSpeed;
}

void SpeedControl(void)
{
	BST_fCarSpeed = (BST_s32LeftMotorPulseSigma  + BST_s32RightMotorPulseSigma );// * 0.5 ;//左右电机脉冲数平均值作为小车当前车速
	BST_s32LeftMotorPulseSigma =BST_s32RightMotorPulseSigma = 0;//全局变量 注意及时清零		
	BST_fCarSpeedOld *= 0.7;
	BST_fCarSpeedOld +=BST_fCarSpeed*0.3;
	BST_fCarPosition += BST_fCarSpeedOld;//路程  即速度积分	   1/11 3:03
	BST_fCarPosition += BST_BluetoothSpeed;
	if(stopflag==1)
	{
		BST_fCarPosition=0;
		
	}
	
	//积分上限设限//
	if((s32)BST_fCarPosition > CAR_POSITION_MAX)    BST_fCarPosition = CAR_POSITION_MAX;
	if((s32)BST_fCarPosition < CAR_POSITION_MIN)    BST_fCarPosition = CAR_POSITION_MIN;
																								  
	BST_fSpeedControlOutNew = (BST_fCarSpeedOld -CAR_SPEED_SET ) * BST_fCarSpeed_P + (BST_fCarPosition - CAR_POSITION_SET ) * BST_fCarSpeed_I; //速度PI算法 速度*P +位移*I=速度PWM输出
}

void MotorOutput(void) //电机PWM输出函数
{	   
			//右电机转向PWM控制融合平衡角度、速度输出

	BST_fLeftMotorOut  = BST_fAngleControlOut +BST_fSpeedControlOutNew+BST_fTurnControlOut+BST_BluetoothSpeed;//+directionl - BST_fBluetoothDirectionNew;			//左电机转向PWM控制融合平衡角度、速度输出	
  BST_fRightMotorOut = BST_fAngleControlOut +BST_fSpeedControlOutNew-BST_fTurnControlOut+BST_BluetoothSpeed;//-directionl+ BST_fBluetoothDirectionNew;			//右电机转向PWM控制融合平衡角度、速度输出

		
	if((s16)BST_fLeftMotorOut  > MOTOR_OUT_MAX)	BST_fLeftMotorOut  = MOTOR_OUT_MAX;
	if((s16)BST_fLeftMotorOut  < MOTOR_OUT_MIN)	BST_fLeftMotorOut  = MOTOR_OUT_MIN;
	if((s16)BST_fRightMotorOut > MOTOR_OUT_MAX)	BST_fRightMotorOut = MOTOR_OUT_MAX;
	if((s16)BST_fRightMotorOut < MOTOR_OUT_MIN)	BST_fRightMotorOut = MOTOR_OUT_MIN;
	
  SetMotorVoltageAndDirection((s16)BST_fLeftMotorOut,(s16)BST_fRightMotorOut);
    
}

void SetMotorVoltageAndDirection(s16 s16LeftVoltage,s16 s16RightVoltage)
{
	  u16 u16LeftMotorValue;
	  u16 u16RightMotorValue;
	
    if(s16LeftVoltage<0)//当左电机PWM输出为负时 PB14设为正 PB15设为负 （PB14 15 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
    {	
	    GPIO_SetBits(GPIOB, GPIO_Pin_14 );				    
      GPIO_ResetBits(GPIOB, GPIO_Pin_15 );
      s16LeftVoltage = (-s16LeftVoltage);
    }
    else 
    {	
      GPIO_SetBits(GPIOB, GPIO_Pin_15 );//当左电机PWM输出为正时 PB14设为负 PB15设为正 （PB14 15 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
      GPIO_ResetBits(GPIOB, GPIO_Pin_14 ); 
      s16LeftVoltage = s16LeftVoltage;
    }

    if(s16RightVoltage<0)
    {															 //当右电机PWM输出为负时 PB12设为正 PB13设为负 （PB12 13 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
      GPIO_SetBits(GPIOB, GPIO_Pin_13 );				    
      GPIO_ResetBits(GPIOB, GPIO_Pin_12 );
      s16RightVoltage = (-s16RightVoltage);
    }
    else														//当右电机PWM输出为正时 PB12设为负 PB13设为正 （PB12 13 分别控制TB6612fng驱动芯片，逻辑0 1可控制左电机正转反转）
    {
	    GPIO_SetBits(GPIOB, GPIO_Pin_12 );				    
      GPIO_ResetBits(GPIOB, GPIO_Pin_13 );	
     
      s16RightVoltage = s16RightVoltage;
    }
		
	   u16RightMotorValue= (u16)s16RightVoltage;
	   u16LeftMotorValue = (u16)s16LeftVoltage;


	TIM_SetCompare3(TIM2,u16LeftMotorValue);//TIM2与 u16RightMotorValue对比，不相同则翻转波形，调节PWM占空比
	TIM_SetCompare4(TIM2,u16RightMotorValue);//TIM3与 u16LeftMotorValue对比，不相同则翻转波形，调节PWM占空比

#if 1	 /*判断车辆 是否悬停或跌倒，调试用*/
		
  if(Pitch>40||Pitch<-40)
	{		
		TIM_SetCompare3(TIM2,0);
		TIM_SetCompare4(TIM2,0);
		stopflag=1;		
	}
	else stopflag=0;
	
	if(BST_fCarAngle > 50 || BST_fCarAngle < (-50))
	{
		TIM_SetCompare3(TIM2,0);
		TIM_SetCompare4(TIM2,0);  
		stopflag=1;	
	}
	else stopflag=0;

#endif
}