解魔方的机器人攻略20 – 修正电机误差

在上一篇攻略中,我们使用了一些角度的配置信息,例如:

//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键退出

时间仓促,每次贴的功能都不多,下一次介绍如何把魔方的颜色读取到数组中。



对 “解魔方的机器人攻略20 – 修正电机误差” 的 11 条 评论

  1. 罗泽阳 说:

    唉,我还不如机器人

  2. dead_lee 说:

    不錯不錯, 慢慢寫

  3. bull 说:

    大哥:我真服了你了,可以拜你为师吗?请吃饭,送礼都行!

  4. 呵呵,拜师就不必了
    经常来捧个场,然后多到论坛参加讨论就好了 :)

  5. Duron 说:

    LZ有钱人 貌似那一个乐高3000多块

  6. belief 说:

    太牛了,哥们你狠高手

  7. 风雪冲 说:

    修正颜色传感器时
    //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();
    ^

  8. 风雪冲 说:

    老大帮帮忙啊。。。
    怎么改啊。。。
    为什么会出现“找不到符号”呢。。。
    先谢谢了。。。
    真心急。。。

    • 风雪冲同学,抱歉回复晚了,我看了你贴的错误提示,终于想起错误原因了。
      我当时做这个机器人的时候,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()有什么区别?

  9. 风雪冲 说:

    老大
    在NXT中运行程序时,魔方底座的角度很好地修正了,但是那个颜色传感器的修正时咋就不动了?只有碰一下颜色传感器下面的马达接口才动,并且LCD上出现下面:
    Java Excepition:
    Class:28
    Method: 182
    Pc: 10971
    这是哪儿出了问题?

发表评论

可以使用下列 XHTML 标签:<a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong>