解魔方的机器人攻略20 – 修正电机误差
由 动力老男孩 发表于 2010/01/18 22:01:03在上一篇攻略中，我们使用了一些角度的配置信息，例如：
//the motor angle for paw to hold the cube static int PawHoldPosition = 56; //the motor angle for paw to rotate the cube static int PawTurnOverPosition = 110;
这些用于Motor.rotate(n)的角度，都是相对于电机的原始位置而言的。在我的代码里，初始位置是这样定义的：
颜色传感器和魔方底座的初始位置
爪子的初始位置
在最初的版本里，我是在断电状态下，手动把电机拧到指定的初始位置。（程序一旦开始运行，角度信息就已经开始记录了，而且拧电机会有很大的阻力）
随后问题就来了，如果初始位置不准确的话，那么必然会导致旋转之后的位置不准确。其中最省心的是爪子的初始化位置，因为它是贴在后支架上，这个参照物非常稳定。
颜色传感器的杆很长，目测很难判断是否已经平行。魔方底座更是转十几次以后，误差越来越大。所以我们需要一段程序，把稍有偏差的初始位置纠正回来。
首先看一下如何修正魔方底座的误差。我们曾经介绍过，在魔方底座的下方安装了一个亮度传感器，当底座在某些位置的时候，会挡在亮度传感器的上面，再转过一定角度，就又把它露出来。亮度传感器有一个红色的小灯，可以通过light.setFloodlight(bool);来点亮或者关闭它。通过对比点亮和关闭前后的读数差，就可以判断出底座什么时候被挡住（在底座的下方需要贴一圈白纸，增强反光）。读数的曲线图是这样的：
读数的示意图
也就是说，随着传感器被慢慢的挡住，这个亮度差值会越来越大，理论上最大值就是被挡住的中心位置。考虑到传感器的读数是有误差的，所以不能只取一个最大值点来计算，需要设置一个阀值，把最大的N个点都找到，那么它的中心位置就比较准确了。
//Fix the position of cube base
public static void FixBasePosition() throws Exception
{
int step = 3;
int tolerance = 4;
light.setFloodlight(false);
bottom.rotate(-50);
int angle = 0, minLight = 10000;
int realtimeLight = ReadLightDifference();
while(realtimeLight < minLight + tolerance)
{
bottom.rotate(step);
realtimeLight = ReadLightDifference();
if(realtimeLight < minLight)
{
minLight = realtimeLight;
angle = 0;
}
else
{
angle += step;
}
}
bottom.rotate(- angle/2 - FixBasePositionOffset);
}
//Read the light difference between light on and light off
private static int ReadLightDifference() throws Exception
{
int l1 = 0, l2 = 0;
l1 = light.readValue();
light.setFloodlight(true);
Thread.sleep(20);
l2 = light.readValue();
light.setFloodlight(false);
return l1-l2;
}
可以测试一下，把魔方底座手动拧歪一个小角度（正负十几度^_^），运行这段代码之后，底座会还原到和爪子平行的位置。
颜色传感器的位置修正比较简单：让它慢慢的靠近魔方，在传感器下方遇到魔方之前，它的读数都是0。所以一旦发现有读数，我们让它返回32度，就回到了爪子平行的位置，这个度数通过几次实验就可以试出来。
//Fix color sensor position
public static void FixColorSensorPosition() throws Exception
{
int tolerance = 5;
ColorMotorBaseAngle = -25;
monitor.rotateTo(ColorMotorBaseAngle);
Thread.sleep(100);
monitor.setSpeed(50);
int r = color.getRawRed();
int g = color.getRawGreen();
int b = color.getRawBlue();
int baseColor = r + g + b;
int TargetExists = 0;
while(TargetExists < baseColor + tolerance && ColorMotorBaseAngle > -50)
{
monitor.rotateTo(ColorMotorBaseAngle--);
r = color.getRawRed();
g = color.getRawGreen();
b = color.getRawBlue();
TargetExists = r + g + b;
}
monitor.rotateTo(ColorMotorBaseAngle + 32);
}
下面也做一个实验，把颜色传感器的位置拧歪，它也能回复到指定的位置。点此下载这个例子的全部代码。实验方法为：按Left键修正魔方底座位置，按Right键修正颜色传感器位置，按Escape键退出。
时间仓促，每次贴的功能都不多，下一次介绍如何把魔方的颜色读取到数组中。
