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

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

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);
}

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

二、计算平衡值

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

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

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

加入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;
			}
		}

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;

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

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

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

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