斯图尔特平台研究一：自平衡代码

从网上找到的几个例子来看（以舵机控制平台为例），自平衡的核心程序就是这一个函数

x = map(num,fromMAX,fromMIN,toMAX,forMIN);

首先看一下它的参数：map(num，旧区间初值，旧区间终值，新区间初值，新区间终值)；意思就是把num这个数从旧区间映射到新区间，就是高中数学知识那个映射，而且是最简单的线性映射。

我们的初始区间可以理解为舵机的初始模拟值范围，这个根据舵机摆放的位置不同，需要实地测量出它的转动模拟值范围。需要注意的是，根据供电功率的不同，模拟值范围会变化。

例如我们找到了六个舵机的12个范围值。

int floors  [6] = { 237, 115, 231, 111, 112, 109 };
int ceilings[6] = { 492, 358, 502, 362, 351, 356 };

接下来我们需要我们的新区间，新区间范围比较固定，根据你的舵机范围和安装角度来定，可以是0~180度，也可以表示为模拟值0~1024。同理，可以设定为更小角度10~170度。

最后map里的第一个参数map就是analogRead读取的模拟值。经过不断的读取模拟值——得出平台的倾斜角度——给舵机一个反向倾斜角度，就可以达到自平衡的效果。

我们发现读取的是瞬时模拟值，读取完后平台还在继续倾斜，如果直接把瞬时值带入，就会有误差，达不到平衡。怎么处理呢。

我们把得到的六个舵机模拟值叫做当前位置值pos[NUM_MOTORS]，把先前的移动位置差值叫pr