matlab模拟的PID算法:
kp=-50;%比例常数
td=0.2;%微分时间常数
ti=-1/300;%积分时间常数

propertion=( gyro(i) - 0 ) * kp;%角速度就是PID中的偏移量e。gyro(i)指的是当前角速度。

speed=( gyro(i) - gyro(i-1) )/0.001;%这里的speed指的是角速度变化的速度即角加速度，控制速度是每一毫秒控制一次所以要除以0.001求得角加速度。
diff=td*speed;%微分项。微分项主要是为了消除比例控制中由于延迟造成的自激振荡，就像弹簧挂着重物上下晃动，弹力就是比例项，空气阻力就是微分项，晃动是自激振荡，而空气阻力会将晃动消减掉。

integra=0;
int_num=0;
    for j=1:integra_num%循环integra_num次。因为我是要稳定频率高于2Hz的抖动，所以只用积分最近一段时间内的数据。
        if ( gyro(i-j) / gyro(i-j) == gyro(i) / gyro(i) )%判断抖动方向是否跟当前一样。实验发现这句要不要影响都不大。
          integra=integra+gyro(i-j);
        end
    end
integra=integra/ti;%求得积分项

angle_acc(i) = propertion + diff + integra;%装置要产生的角角速度等于三项的和。
    if angle_acc(i)>40  %电机输出力矩也是有限制的，所以限制电机产生的角加速度值
       angle_acc(i)=40;
    end
    if angle_acc(i)<-40
       angle_acc(i)=-40;
    end


就是这些。感觉PID控制的鲁棒性不太好可能不适合干这样的工作。
参考：
PID控制器：XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
鲁棒：XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

曾想过用皮带，但我认为皮带的弹性会造成力矩滞后和抖动所以没有采用；另外小皮带轮不好做。
另外推荐xv3500和xv3700这两个陀螺，温漂真的很低。