解魔方的机器人攻略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键退出。
时间仓促，每次贴的功能都不多，下一次介绍如何把魔方的颜色读取到数组中。
唉，我还不如机器人
不錯不錯, 慢慢寫
大哥:我真服了你了,可以拜你为师吗?请吃饭,送礼都行!
呵呵，拜师就不必了
经常来捧个场，然后多到论坛参加讨论就好了
LZ有钱人 貌似那一个乐高3000多块
太牛了，哥们你狠高手
修正颜色传感器时
//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 -50)
{
monitor.rotateTo(ColorMotorBaseAngle–);
r = color.getRawRed();
g = color.getRawGreen();
b = color.getRawBlue();
TargetExists = r + g + b;
}
monitor.rotateTo(ColorMotorBaseAngle + 32);
}
命令行编译时 C:\f>nxjc RubikSolverV2.java
怎么会：
RubikSolverV2.java:318: 找不到符号
符号： 方法 getRawRed()
位置： 类 lejos.nxt.ColorSensor
int r = color.getRawRed();
^
RubikSolverV2.java:319: 找不到符号
符号： 方法 getRawGreen()
位置： 类 lejos.nxt.ColorSensor
int g = color.getRawGreen();
^
RubikSolverV2.java:320: 找不到符号
符号： 方法 getRawBlue()
位置： 类 lejos.nxt.ColorSensor
int b = color.getRawBlue();
^
老大帮帮忙啊。。。
怎么改啊。。。
为什么会出现“找不到符号”呢。。。
先谢谢了。。。
真心急。。。
风雪冲同学，抱歉回复晚了，我看了你贴的错误提示，终于想起错误原因了。
我当时做这个机器人的时候，leJOS版本是0.6，而现在的版本和当时的API稍有差别，其中最明显的就是颜色传感器的部分变化了。
下面有两个当时写的日记，里面内容比较乱，你挑颜色传感器的部分看。
http://www.diy-robots.com/?p=632
http://www.diy-robots.com/?p=641
大概意思是以前8547的colorSensor，在新的lejos里叫colorLightSensor，而新的hitech的颜色传感器，才叫colorSensor。
这个地方需要根据你买的传感器单独修改
先谢谢老大的帮忙！！！你的日记对我很有帮助！！！
果断把我的9797刷成lejos0.85，但不是用牙签刷的，9797貌似没有重启键。
我把代码的ColorSensor改成了ColorLightSensor
把color.getRed();和color.getRawRed()都改成了color.getRedComponent()，编译成功！！！激动坏了！！！
但是这么改对吗？还有color.getRed()和color.getRawRed()有什么区别？
老大
在NXT中运行程序时，魔方底座的角度很好地修正了，但是那个颜色传感器的修正时咋就不动了？只有碰一下颜色传感器下面的马达接口才动，并且LCD上出现下面：
Java Excepition：
Class：28
Method： 182
Pc： 10971
这是哪儿出了问题？
十年了