|
|
/*****************************************************
函数 : Get_Tiltangle()
计算的倾斜角度与手工制作的环路滤波器,
计算角速度
*****************************************************/
void Get_Tiltangle()
{
buf = total_ADXL_GYRO / total_looptime;
total_ADXL_GYRO -= buf;
// 加速度输入
buf = AD_ADXL- ADXL_ZERO+ ADXL_offset;
total_ADXL_GYRO += buf;
// 陀螺仪输入
buf1 = Average_Gyro / total_looptime;
Average_Gyro -= buf1;
Average_Gyro += AD_GYRO;
buf1 = Average_Gyro / (total_looptime / 10);
// 计算角速度
buf1 = buf1 - AD_GYRO * 10;
buf1 *= 35;
buf1 /= 100;
Angle_Rate = buf1;
// 计算倾斜角
total_ADXL_GYRO += Angle_Rate;
Tilt_ANGLE = total_ADXL_GYRO / (total_looptime / 10);
}
/************************************************************************/
/*****************************************************
函数 : Algorythmus()
返回限制最高速度的倾斜角，计算行车速度
*****************************************************/
void Algorythmus()
{
Balance_moment = Angle_Rate*4 + 5 * (Tilt_ANGLE + Anglecorrection); // 平衡力矩计算
Overspeed = fmax(0, Drive_sum - Overspeedlimit); // 恢复速度
if (Overspeed > 0)
{
if(MODE == Run)Overspeed_flag = 1;
Overspeed_integral = fmin(1000, Overspeed_integral + fmin(500, Overspeed + 125)); // 计算速度积分
}
else
{
Overspeed_flag = 0;
Overspeed_integral = fmax(0, Overspeed_integral - 100); // 计算速度积分
}
Anglecorrection = Overspeed / 200 + Overspeed_integral / 125; // 计算角度校正
Drive_sum += Balance_moment; // 计算 Drive_sum
if(Drive_sum > Drivesumlimit)Drive_sum = Drivesumlimit;
if(Drive_sum <- Drivesumlimit)Drive_sum =-Drivesumlimit;
Drivespeed = Drive_sum / 250 + Balance_moment / 40; // 计算速度
}
/*****************************************************
函数 : Signal_Processing()
生成启动条件，计算左/右转向信号
*****************************************************/
void Signal_Processing()
{
if(MODE == Standby) // 待命模式
{
PWM_output(0,0);
Drivespeed=0;
Anglecorrection=0;
Overspeed=0;
Overspeed_integral = 0;
Drive_sum = 0;
total_ADXL_GYRO = 0;
Tilt_ANGLE = 0;
Average_Gyro = total_looptime * GYRO_ZERO;
buf2 = AD_ADXL - ADXL_ZERO + ADXL_offset;
if(buf2 == 0&&(Footswitch==0)) // 坚持是开始位置的初始运行系统
{
MODE = Run;
}
}
else // 启动模式
{
Steeringsignal =AD_ZHUANWAN - ZHUAN_WAN_ZERO ; // 得到转向信号
// 检查 死区
if(Steeringsignal <- ROCKER_DEADBAND)
{
Steeringsignal += ROCKER_DEADBAND;
}
else
{
if(Steeringsignal > ROCKER_DEADBAND)
{
Steeringsignal -= ROCKER_DEADBAND;
}
else
{
Steeringsignal = 0;
}
}
// 计算减少信号的差速转向
Steeringsignal *= 7;
buf2 = Drivespeed;
if(buf2 < 0)buf2 *= -1;
buf2 /= 5;
Steeringsignal /= (buf2 + 7);
speed_output_LH= Drivespeed + Steeringsignal; // 差速转向
speed_output_RH= Drivespeed - Steeringsignal; // 差速转向
PWM_output (speed_output_LH,speed_output_RH);
}
}
求解德国平衡车程序核心的平衡算法 用到的环路滤波是怎么实现的，哪位有环路滤波的资料，谢谢 |
|
发表于 2014-6-6 10:25:21
