//手动测试获取一个平衡点的值. 设置这个值让她保持平衡
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;
刚刚开始测试的时候, 尽量在有护的地方进行. 避免事故