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

由 动力老男孩 发表于 2010/01/18 22:01:03

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

view plaincopy to clipboardprint?
  1. //the motor angle for paw to hold the cube  
  2. static int PawHoldPosition = 56;  
  3. //the motor angle for paw to rotate the cube  
  4. static int PawTurnOverPosition = 110;  

这些用于Motor.rotate(n)的角度，都是相对于电机的原始位置而言的。在我的代码里，初始位置是这样定义的：

颜色传感器和魔方底座的初始位置

颜色传感器和魔方底座的初始位置

爪子的初始位置

爪子的初始位置

在最初的版本里，我是在断电状态下，手动把电机拧到指定的初始位置。（程序一旦开始运行，角度信息就已经开始记录了，而且拧电机会有很大的阻力）
随后问题就来了，如果初始位置不准确的话，那么必然会导致旋转之后的位置不准确。其中最省心的是爪子的初始化位置，因为它是贴在后支架上，这个参照物非常稳定。

颜色传感器的杆很长，目测很难判断是否已经平行。魔方底座更是转十几次以后，误差越来越大。所以我们需要一段程序，把稍有偏差的初始位置纠正回来。

首先看一下如何修正魔方底座的误差。我们曾经介绍过，在魔方底座的下方安装了一个亮度传感器，当底座在某些位置的时候，会挡在亮度传感器的上面，再转过一定角度，就又把它露出来。亮度传感器有一个红色的小灯，可以通过light.setFloodlight(bool);来点亮或者关闭它。通过对比点亮和关闭前后的读数差，就可以判断出底座什么时候被挡住（在底座的下方需要贴一圈白纸，增强反光）。读数的曲线图是这样的：

读数的示意图

读数的示意图

也就是说，随着传感器被慢慢的挡住，这个亮度差值会越来越大，理论上最大值就是被挡住的中心位置。考虑到传感器的读数是有误差的，所以不能只取一个最大值点来计算，需要设置一个阀值，把最大的N个点都找到，那么它的中心位置就比较准确了。

view plaincopy to clipboardprint?
  1. //Fix the position of cube base  
  2. public static void FixBasePosition() throws Exception  
  3. {  
  4. int step = 3;  
  5. int tolerance = 4;  
  6. light.setFloodlight(false);  
  7. bottom.rotate(-50);  
  8. int angle = 0, minLight = 10000;  
  9. int realtimeLight = ReadLightDifference();  
  10. while(realtimeLight < minLight + tolerance)  
  11. {  
  12. bottom.rotate(step);  
  13. realtimeLight = ReadLightDifference();  
  14. if(realtimeLight < minLight)  
  15. {  
  16. minLight = realtimeLight;  
  17. angle = 0;  
  18. }  
  19. else  
  20. {  
  21. angle += step;  
  22. }  
  23. }  
  24. bottom.rotate(- angle/2 - FixBasePositionOffset);  
  25. }  
  26.   
  27. //Read the light difference between light on and light off  
  28. private static int ReadLightDifference() throws Exception  
  29. {  
  30. int l1 = 0, l2 = 0;  
  31. l1 = light.readValue();  
  32. light.setFloodlight(true);  
  33. Thread.sleep(20);  
  34. l2 = light.readValue();  
  35. light.setFloodlight(false);  
  36. return l1-l2;  
  37. }  

可以测试一下，把魔方底座手动拧歪一个小角度（正负十几度^_^），运行这段代码之后，底座会还原到和爪子平行的位置。

颜色传感器的位置修正比较简单：让它慢慢的靠近魔方，在传感器下方遇到魔方之前，它的读数都是0。所以一旦发现有读数，我们让它返回32度，就回到了爪子平行的位置，这个度数通过几次实验就可以试出来。

view plaincopy to clipboardprint?
  1. //Fix color sensor position  
  2.   public static void FixColorSensorPosition() throws Exception  
  3.   {  
  4.    int tolerance = 5;  
  5.    ColorMotorBaseAngle = -25;  
  6.    monitor.rotateTo(ColorMotorBaseAngle);  
  7.    Thread.sleep(100);  
  8.    monitor.setSpeed(50);  
  9.    int r = color.getRawRed();  
  10.    int g = color.getRawGreen();  
  11.    int b = color.getRawBlue();  
  12.    int baseColor = r + g + b;  
  13.    int TargetExists = 0;  
  14.    while(TargetExists < baseColor + tolerance && ColorMotorBaseAngle > -50)  
  15.    {  
  16.     monitor.rotateTo(ColorMotorBaseAngle--);  
  17.     r = color.getRawRed();  
  18.     g = color.getRawGreen();  
  19.     b = color.getRawBlue();  
  20.     TargetExists = r + g + b;  
  21.    }  
  22.    monitor.rotateTo(ColorMotorBaseAngle + 32);  
  23.   }  

下面也做一个实验，把颜色传感器的位置拧歪，它也能回复到指定的位置。点此下载这个例子的全部代码。实验方法为：按Left键修正魔方底座位置，按Right键修正颜色传感器位置，按Escape键退出。

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



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

  1. 罗泽阳 说：
    2010/01/18 于 22:34

    唉，我还不如机器人

    回复
  2. dead_lee 说：
    2010/01/18 于 23:23

    不錯不錯, 慢慢寫

    回复
  3. bull 说：
    2010/01/19 于 19:28

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

    回复
  4. 动力老男孩 说：
    2010/01/20 于 08:57

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

    回复
  5. Duron 说：
    2010/03/28 于 03:20

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

    回复
  6. belief 说：
    2011/05/25 于 07:42

    太牛了，哥们你狠高手

    回复
  7. 风雪冲 说：
    2012/02/29 于 15:36

    修正颜色传感器时
    //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. 风雪冲 说：
    2012/03/02 于 21:08

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

    回复
    • 动力老男孩 说：
      2012/03/04 于 20:38

      风雪冲同学，抱歉回复晚了，我看了你贴的错误提示，终于想起错误原因了。
      我当时做这个机器人的时候，leJOS版本是0.6，而现在的版本和当时的API稍有差别，其中最明显的就是颜色传感器的部分变化了。
      下面有两个当时写的日记，里面内容比较乱，你挑颜色传感器的部分看。
      http://www.diy-robots.com/?p=632
      http://www.diy-robots.com/?p=641

      大概意思是以前8547的colorSensor，在新的lejos里叫colorLightSensor，而新的hitech的颜色传感器，才叫colorSensor。
      这个地方需要根据你买的传感器单独修改

      回复
      • 风雪冲 说：
        2012/03/09 于 20:40

        先谢谢老大的帮忙！！！你的日记对我很有帮助！！！
        果断把我的9797刷成lejos0.85，但不是用牙签刷的，9797貌似没有重启键。
        我把代码的ColorSensor改成了ColorLightSensor
        把color.getRed();和color.getRawRed()都改成了color.getRedComponent()，编译成功！！！激动坏了！！！
        但是这么改对吗？还有color.getRed()和color.getRawRed()有什么区别？

        回复
  9. 风雪冲 说：
    2012/06/09 于 08:41

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

    回复
  10. 阿勇 说：
    2020/07/19 于 20:29

    十年了

    回复

发表评论

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