linyili1989 发表于 2014-6-6 10:25:21

求解德国平衡车程序


/*****************************************************
函数 : 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);
    }
}
求解德国平衡车程序核心的平衡算法 用到的环路滤波是怎么实现的，哪位有环路滤波的资料，谢谢

huangtao 发表于 2014-10-21 11:36:27

{:biggrin:}

huangguimina4 发表于 2014-10-21 11:43:46

mark一下

qxc16b 发表于 2014-10-21 11:47:45

顶一吓。。。。。。。。。。。。。。。。。。

ledao_rj 发表于 2014-10-21 11:59:20

哦。。。。

glacier1 发表于 2014-10-21 13:23:06

这手伸的。。。

qxc16b 发表于 2014-10-21 15:12:26

{:biggrin:}

tj7856 发表于 2014-10-21 15:19:27

{:smile:}{:smile:}{:smile:}

daienming 发表于 2014-10-22 09:26:11

这种程序哪里找来的？我也想找找，苦于没有来源，楼主能不能提供一下？

ssclark1988 发表于 2014-10-22 09:27:05

程序哪来的？

jiang739707231 发表于 2014-12-1 14:54:43

楼主有没有研究透彻呢    透彻了也给我们也分享一下呗{:lol:}

xinxinyu2013 发表于 2014-12-1 15:17:06

梦幻之旅 发表于 2015-10-29 17:30:10

求解德国平衡车程序

jimmyxumotor 发表于 2015-10-31 17:13:51

开源的这么多，到底指标是什么
页: [1]
查看完整版本: 求解德国平衡车程序