独轮车成功站立

真是废了不少力

卡了这么久首要原因就是过于青睐串级PID,串级PID可以自主寻找机械中位的特性实在是太优雅了,但动量轮这种对即使性要求极高的系统似乎不能用串级PID实现。昨天沉下心把串级PID推掉换成并联，波形一下就朝着正常的方向改变，再稍加调参那么三四个小时动量轮就很稳了。

没想到的是减速电机居然比动量轮还难调，O车模整车太重1：19的减速比就不太够用。调来调去电机都只有猛抽一下和静止两种状态，最终虽然平衡了但总体的静态效果还是不太可观。好在比赛只要保持稳定前进就行，静态不稳应该问题不大。

CSDN不支持发视频，那就把平衡代码发出来给大家参考参考吧。

虽然代码可读性和移植性都比较不错，但不建议大家直接拿去用，硬件差异很大。

我的硬件完全是自己搭的，陀螺仪MPU6050，减速电机电压7.4V，减速电机驱动集成H桥A4950，编码器也是自制的霍尔编码器一圈大概340线。

由此也证明了，龙邱逐飞为了利润把智能车市场拉太高了，明明六块钱的软件I2C陀螺仪就够用了他要推六十块的SPI陀螺仪，两块钱的集成H桥也够用了他要推MOS管搭的板子，我自制编码器全程投入不超过五十块而且效果优雅，他总推荐大家用一百多一个的高精度编码器。骄奢淫逸，把智能车搞得愈发商业化。

//X编码器积分限幅
static int range_x_enco(int value)
{
  static const int range_max = 10000;
  //略
}
float x_target_ang = 0;   //x轴目标角度(机械中位)

//X轴角度环PD
static void x_balance_cont(void)
{
  const float bal_kp = 90,bal_kd = 1100;          //PD参数
  int pwm_balance;                //临时存放输出量
  static float last_value;        //上一次的偏差量
  float ang_value = x_target_ang - atti.pitch;    //获取偏差量
  pwm_balance = bal_kp*ang_value + bal_kd*(ang_value - last_value);   //角度环PD
  speed.x1 = pwm_balance;   //输出PWM X1
  speed.x2 = pwm_balance;   //输出PWM X2
  last_value = ang_value;   //保存本次偏差用于下次微分
}
//X轴速度环PI
static void x_speed_cont(void)
{
  const float sp_kp = 9, sp_ki = 3, sp_kd = 12;   //PID参数
  int pwm_speed;                      //临时存放输出量
  int enco_value = enco.x1+enco.x2;   //两个动量轮编码器相加
  static int last_value = 0;
  static long enco_d_value;           //编码器积分
  enco_d_value += enco_value;         //累加出积分
  enco_d_value = range_x_enco(enco_d_value);   //编码器积分限幅
  pwm_speed = -(sp_kp*enco_value + sp_ki*enco_d_value/100 + sp_kd*(enco_value-last_value));    //速度环PID
  speed.x1 += pwm_speed;        //并联直接累加
  speed.x2 += pwm_speed;
  last_value = enco_value;      //记录本次差值用于微分
}

//Y轴角度积分限幅
int range_y_ang(int value)
{
  static const int value_max = 80;
  //略
}
//Y轴编码器积分限幅
int range_y_sp(int value)
{
  static const int value_max = 200;
  //略
}

float y_target_ang = 2.4;   //初始机械中位
float y_target_sp = 0;      //目标速度,要实现前进后退就改这个
//Y轴平衡环
static void y_balance_cont(void)
{
  const float bal_kp = 50,bal_ki = 6, bal_kd = 320;   //平衡环PD参数
  int pwm_balance;                        //临时存储输出量
  static float last_value = 0;            //记录偏差量,微分用
  static int ang_d_value = 0;             //角度积分量
  float ang_value = (y_target_ang - atti.roll);   //获取偏差量
  ang_d_value += ang_value;                       //积分
  ang_d_value = range_y_ang(ang_d_value);         //积分限幅
  if(ang_value > 0)                               //根据输出正负添加电机死区补偿
  {
    pwm_balance = (bal_kp*ang_value) + bal_kd*(ang_value-last_value)+40;   //角度环PD
  }  
  else if(ang_value < 0)
  {
    pwm_balance = (bal_kp*ang_value) + bal_ki*ang_d_value/100 + bal_kd*(ang_value-last_value)-40;
  }
  speed.y = pwm_balance;
  last_value = ang_value;                         //记录偏差量下次微分用
}
//Y轴速度环
static void y_speed_cont(void)
{
  const float sp_kp = 0.015, sp_kd = 0.09;    //速度环PD参数
  static int last_value;                 //记录上次的偏差量
  int sp_value = y_target_sp - enco.y;      //速度环偏差量
  y_target_ang -= (sp_kp*sp_value + sp_kd*(sp_value-last_value));    //速度环PD
  last_value = sp_value;  //保存偏差量用于下次微分
}

//定时器中断任务调度
//10ms定时器中断内调用
void tim_it_loop(void) 
{
  const int MAX_ANG = 20;   //安全角度
  glo_time+=10;             //全局时间
  //10ms
  {
    if(atti.pitch<MAX_ANG&&atti.pitch>-MAX_ANG&&atti.roll<MAX_ANG&&atti.roll>-MAX_ANG&&atti.work==1)    
    {//安全角度  MAX_ANG
      x_balance_cont();   //X平衡环
      y_balance_cont();   //Y平衡环
      get_enco_x();       //获取X编码器
      x_speed_cont();     //X速度环
      set_speed(speed.x1, speed.x2, speed.y);   //赋值PWM
    }
    else
    {//超过安全角度停止工作
      set_speed(0, 0, 0);
    }
  }
  //50ms
  if(glo_time%50==0)
  {
    get_enco_y();     //获取Y编码器
    y_speed_cont();   //Y速度环
  }
}