一、计算倾斜角度与角速率
csharp
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值得到乘以系数
二、计算平衡值
csharp
Balance_moment = Angle_Rate*(long)(config.s.Dangle) + (long)(config.s.Pangle)*(Tilt_ANGLE) + Protect;
倾斜角度计算使用P参数,角速率计算使用D参数,全车调试主要针对P参数与D参数调整
csharp
Drivespeed = Drive_sum /240 + Balance_moment / 225 + slope_add;
加入slope_add校正,计算电机需要保持平衡的PWM值
三、计算两轮电机的轮速差值
csharp
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为调整系数
四、计算转向摇杆数值
csharp
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数值
csharp
Drive_A = Drivespeed + Steeringsignal - DiffAdjust;
Drive_B = Drivespeed - Steeringsignal + DiffAdjust;
Drivespeed 为陀螺仪与加计计算出来电机驱动值
Steeringsignal 为摇杆计算出来电机驱动值
DiffAdjust 为两电机编码器差值补偿值
Drive_A 与 Drive_B 输出到电机实际PWM值