作者:

平衡计算过程分析 翻译

https://sites.google.com/site/onewheeledselfbalancing/Home/links-to-other-self-balancing-projects/self-balancing-software

//手动测试获取一个平衡点的值. 设置这个值让她保持平衡
balance_point = 520; // 它的取值范围为 0 – 1023 需要根据自己的做的设备找个平衡值

加速度计信号处理:

  / *减去偏移* /

 x_acc=(float) (accsum/20) – Balance_point;  // 采集20个加速度值样本,求平均值  再减去我们上面测试出的平衡点, 这样x_acc的平衡点就是0

 if (x_acc<-250) x_acc=-250;  //让加速度值保持在 [-250, 250] 超过的就已经没有意义了. 直接取最大值

  if (x_acc>250) x_acc=250;

/* 测试一个合理的量,我们这里以3.45个单位量等于1度 ( 其实就是吧0-1023区间转换成了 0-360, 这样就可以很清晰的被我们理解, 这样x_acc就变成度数了 ) */

 x_accdeg= (float) x_acc/-3.45; // 这里的这个负号 只是改变下加速度方向

陀螺仪信号处理:

 

  / *减去偏移:传感器读数是1023,因此将是“平衡点”,即我所需的零点,读数减去512 * /

//gangleratedeg 这是角度变化率, 这基本和加数据的处理方法一样.

gangleratedeg=(float)(gyrosum/20 – 508)/4.096;

//限制下取值范围

if (gangleratedeg < -92) gangleratedeg=-92;

if (gangleratedeg >92) gangleratedeg=92;

/*

接下来需要测量下程序运行周期, 测试的时候可以在程序中写入

 PORTB &= (0<<PB2);

然后将示波器接在B2口上这样主程序每次循环的时候都会变化, 这样就能检测出程序的周期了

*/

// 接下来的cycle_time 就是程序的运行周期了, ti 是一个缩放因子 

gyroangledt = (float)ti*cycle_time*gangleratedeg;

// 1度=π/180弧度( ≈0.017453弧度 )  这里是将角度转换为弧度

gangleraterads=(float)gangleratedeg*0.017453;

aa=0.01; // 这个是指在一个周期中, 使用多少加速度去修正上一周期角度, aa 是一个比例值

angle = (float)((1-aa) * (angle+gyroangledt)) + (aa * x_accdeg);

 anglerads=(float)angle*0.017453; //转换为弧度

//定义水平值在-1 与 +1 ,这就代表电机占空, 应该可以理解为平衡

//下面 机器平衡与移动 所要使用的公式:

//level=(float)((k1*anglerads)*overallgain

// 机器的角度乘以 扭矩     陀螺仪测量角度乘以功率  计算出转矩

balance_torque=(float)(4.5*anglerads) + (0.5*gangleraterads);

// 其中 anglerads 这个值包含了陀螺仪的值, 相当于实际的倾斜角, 并且这个值是经过长时间的纠正了的.  gangleraterads反应了陀螺仪长期的值….. 这样做实际上用到P(比例),I(积分)和D(微分)计算上面的公式

// cur_speed 这是一个稳定移动过程中速度的值,移动过程中角度可能会发生一些变化希望能保持一定值, cur_speed 是一个平衡值,  在后面乘以0.999 防止在前进过程中,突然向导致移动突然停止

cur_speed = (float)(cur_speed + (Throttle_pedal * balance_torque * cycle_time)) * 0.999;

// overallgain 这个是启动速度调整

 level = (balance_torque + cur_speed) * overallgain;

//开始做写安全方面的判断

//当电机已经达到最大马力时, 如果还继续向前倾斜, 这将是相当危险的, 所以我们就必须要做个提示器,避免危险发生, 在B1口安装一个LED灯提示或者蜂鸣器, 提示是否走的太快了.

  if (level<-0.7 || level>0.7) {

   PORTB |= (1<<PB1);    //i.e. turn on a warning buzzer

                   }

  else {

   PORTB &= (0<<PB1);

                   }

 //开始启动过程, 每个周期添加0.001 是为了让这启动过程像加速度, 避免突然启动.

  softstart = (float) softstart+0.001;

  if (softstart>1.0) softstart=1.0;

 

刚刚开始测试的时候, 尽量在有护的地方进行. 避免事故

发表评论

评论