两轮平衡电动车原理简单叙述

一、计算倾斜角度与角速率

void Get_Tiltangle( void )				//¼Ƌ㽇˙Š¼°ǣб½Ƕȍ
{
	float buf;
	static uint32_t time;
    buf = total_ADXL_GYRO / total_looptime;
    total_ADXL_GYRO -= buf;
	total_ADXL_GYRO *=0.918;

	if( TIMER_MS>time+100 )
	{
		time = TIMER_MS;
		if( CarStus.limit_run )
		{
			if( CarStus.limit_angle <100 )
				CarStus.limit_angle++;
				
		}
		else
			CarStus.limit_angle=0;
	}
    // ADXL part
    total_ADXL_GYRO += (float)(ACCEL_X()-(config.s.ACCEL_X_M-CarStus.limit_angle));
    
    // Gyro part
    buf = Average_Gyro/total_looptime;
    Average_Gyro -= buf;
   	Average_Gyro += (float)GYRO_X();

    buf = Average_Gyro/(total_looptime);
	buf =  buf - (float)GYRO_X();
    buf *= 3;
    Angle_Rate = buf;

    total_ADXL_GYRO += Angle_Rate;
    Tilt_ANGLE = total_ADXL_GYRO/(total_looptime/10);
}
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

total_ADXL_GYRO *=0.918;为一个高通滤波，叠加角度数值与角速率值，除采样次数得到当前倾斜角度
Angle_Rate，利用滑动平均值滤波，叠加采样AD值得到乘以系数

二、计算平衡值

Balance_moment =  Angle_Rate*(long)(config.s.Dangle) + (long)(config.s.Pangle)*(Tilt_ANGLE) + Protect;
1

倾斜角度计算使用P参数，角速率计算使用D参数，全车调试主要针对P参数与D参数调整

Drivespeed = Drive_sum /240  + Balance_moment / 225 + slope_add;
1

加入slope_add校正，计算电机需要保持平衡的PWM值

三、计算两轮电机的轮速差值

if( EncoderData )
		{
			EncoderData = 0;
			
			if (SpeedDiff>50)         // ±«ƷϠ²0Ҕɏ£¬½øА±£»¤
				SpeedDiff=50;
			else if (SpeedDiff<-50)
				SpeedDiff=-50;

			if( SpeedDiff>5 )         // ±«ƷϠ²ԉϲŽøА¾Àƫ
			{
				if( DiffAdjust<10 )      // Ӄ׮´󲵵ĵ绺ֵÀ´¾Àƫ
					DiffAdjust+=(float)SpeedDiff*0.08;   // ¾Àƫµ绺ֵÿ5ms׮С0.8£¬׮´󸍊				else
					DiffAdjust = 10;
			}
			else if( SpeedDiff<-5 )
			{
				if( DiffAdjust>-10 )
					DiffAdjust+=(float)SpeedDiff*0.08;
				else
					DiffAdjust = -10;
			}
		}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

0.08为调整系数

四、计算转向摇杆数值

if( turn<config.s.RCOKER_M-ROCKER_DEADBAND )	//ҡ¸˗󗪍
		{
			if( LeftFlag == 0 )
			{
				#if MODEL_BW
				Steeringsignal = -12;     //BW -12, ST -8
				#elif MODEL_ST
				Steeringsignal = -8;     //BW -12, ST -8
				#else
				Steeringsignal = -8;     //BW -12, ST -8
				#endif
				if( fabs(Drivespeed)<3 && fabs(CarStus.Speed)>360)
					Steeringsignal = 0;
			}
			if( RightFlag )
			{
				RightFlag = 0;
				Steeringsignal = 0;
			}
			#if FIRMWARE15
			if (TurnSpeed > (700/(fabs(CarStus.Speed)/500+5))*(-1) )
			#elif FIRMWARE10
			if (TurnSpeed > (420/(fabs(CarStus.Speed)/300+5))*(-1) )
			#else
			if (TurnSpeed > (700/(fabs(CarStus.Speed)/500+5))*(-1) )
			#endif
			{
				#if MODEL_BW
				Steeringsignal -= 0.25;    //BW 0.15, ST 0.1
				#elif MODEL_ST
				Steeringsignal -= 0.2;    //BW 0.15, ST 0.1
				#else
				Steeringsignal -= 0.2;    //BW 0.15, ST 0.1
				#endif
				if( fabs(Drivespeed)<1.1 )
					Steeringsignal -= 0.04;
			}
			/*#if FIRMWARE15
			else if (TurnSpeed < (800/(fabs(CarStus.Speed)/500+5))*(-1) )
			#elif FIRMWARE10
			else if (TurnSpeed < (500/(fabs(CarStus.Speed)/300+5))*(-1) )
			#else
			else if (TurnSpeed < (800/(fabs(CarStus.Speed)/500+5))*(-1) )
			#endif
			{
				#if MODEL_BW
				Steeringsignal += 0.25;  
				#elif MODEL_ST
				Steeringsignal += 0.2; 
				#else
				Steeringsignal += 0.2; 
				#endif
				if( fabs(Drivespeed)<1.1 )
					Steeringsignal += 0.04;
			}*/
			LeftFlag = 1;
		}
        else if( turn>config.s.RCOKER_M+ROCKER_DEADBAND )	//Ӓת
		{
			if( RightFlag == 0 )
			{
				#if MODEL_BW
				Steeringsignal = 12;       //BW 12, ST 8
				#elif MODEL_ST
				Steeringsignal = 8;       //BW 12, ST 8
				#else
				Steeringsignal = 8;       //BW 12, ST 8
				#endif
				if( fabs(Drivespeed)<5 && fabs(CarStus.Speed)>360)
					Steeringsignal = 0;
			}
			if( LeftFlag )
			{
				LeftFlag = 0;
				Steeringsignal = 0;
			}
			#if FIRMWARE15
			if (TurnSpeed < (700/(fabs(CarStus.Speed)/500+5)) )
			#elif FIRMWARE10
			if (TurnSpeed < (420/(fabs(CarStus.Speed)/300+5)) )
			#else 
			if (TurnSpeed < (700/(fabs(CarStus.Speed)/500+5)) )
			#endif
			{
				#if MODEL_BW
				Steeringsignal += 0.25; 
				#elif MODEL_ST
				Steeringsignal += 0.2; 
				#else
				Steeringsignal += 0.2;
				#endif
				if( fabs(Drivespeed)<1.1 )
					Steeringsignal += 0.04;
			}
/*			#if FIRMWARE15
			else if (TurnSpeed > (800/(fabs(CarStus.Speed)/500+5)) )
			#elif FIRMWARE10
			else if (TurnSpeed > (500/(fabs(CarStus.Speed)/300+5)) )
			#else 
			else if (TurnSpeed > (800/(fabs(CarStus.Speed)/500+5)) )
			#endif
			{
				#if MODEL_BW
				Steeringsignal -= 0.25;
				#elif MODEL_ST
				Steeringsignal -= 0.2;
				#else
				Steeringsignal -= 0.2;
				#endif
				if( fabs(Drivespeed)<1.1 )
					Steeringsignal -= 0.04;
			}*/
			RightFlag = 1;
		}
        else								//»ָ´µ½֐µ㍊		{
            if (Steeringsignal > 0)
			{
				Steeringsignal -= 1;
				if( Steeringsignal<0.5 )
				{
					Steeringsignal = 0;
					RightFlag = 0;
				}
			}
			else if (Steeringsignal < 0)
			{
				Steeringsignal += 1;
				if( Steeringsignal>-0.5 )
				{
					Steeringsignal = 0;
					LeftFlag = 0;
				}
			}
		}
		//Ϟֆתϲµķù¶ȍ
		if( Steeringsignal>70 )
			Steeringsignal = 70;
		else if( Steeringsignal<-70 )
			Steeringsignal = -70;
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

Steeringsignal 通过编码器不同速度条件下计算的转向PWM值

五、计算实际输出电机PWM数值

Drive_A = Drivespeed + Steeringsignal - DiffAdjust;
Drive_B = Drivespeed - Steeringsignal + DiffAdjust;
1
2

Drivespeed 为陀螺仪与加计计算出来电机驱动值
Steeringsignal 为摇杆计算出来电机驱动值
DiffAdjust 为两电机编码器差值补偿值
Drive_A 与 Drive_B 输出到电机实际PWM值