Posts Tagged ‘位置’

解魔方的机器人攻略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键退出

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