解魔方的机器人攻略25 – 解魔方

现在我们的工作已经接近尾声了,看看怎么把电脑变成一个NXT的蓝牙遥控器。这个部分大家其实可以自由发挥,我设计的数据通讯流程是这样的:

1,蓝牙连接成功
2,NXT扫描魔方,发送6个面,每个面9块共54组颜色数据到电脑
3,NXT发送一个字节(0xFF)到电脑,表示颜色读取完毕
4,电脑开始计算解法,得到解魔方的步骤,一共N步
5,电脑发送一个字节N到NXT
6,NXT进行从1到N的循环,每次发送一个字节n到电脑,请求第n步操作
7,电脑发送第n步操作给NXT
8,NXT执行完全部N个操作,发送一个字节(0xFE)到电脑,通知解魔方完成
9,电脑清空步骤和颜色数组,准备迎接下一次任务
10,按下Escape按钮,NXT发送三个(0XFF)给电脑,关闭蓝牙连接并退出

同学们松了一口气,核心算法都搞定了,这点任务算啥,准备十分钟交卷吧。。。。

且慢,我们得到的步骤是类似F1 U2 F2 D3 L2 D1 F1 U3 L2 D1这样的序列,但是萝卜头永远只能旋转最下面一层,怎么办?

这个也简单,把相应的面翻到底面就好了,毕竟萝卜头的胳膊也不是个摆设。

问题又来了,第一步F1时,把F变成了底面;这时候魔方已经经过了某些翻转操作,那么第二步U2该转哪一面呢?这下有点麻烦了…

如果每次都还原到原来的位置,会增加非常多的步骤。

最好的方法是每次都通过最近的路径把需要旋转的面翻到最底层,然后旋转它。

所以我们需要保存一个坐标系,在翻转魔方的时候,让这个坐标系永远跟魔方的真实位置同步,请看CenterColor类,用来记录六个面的中心位置:

public class CubeCenter
{
    public string[] CenterColor = new string[6] { "U", "R", "D", "L", "F", "B" };

    public void RotateBottom(bool colockwise)
    {
        if (colockwise)
        {
            string n = CenterColor[5];
            CenterColor[5] = CenterColor[1];
            CenterColor[1] = CenterColor[4];
            CenterColor[4] = CenterColor[3];
            CenterColor[3] = n;
        }
        else
        {
            string n = CenterColor[5];
            CenterColor[5] = CenterColor[3];
            CenterColor[3] = CenterColor[4];
            CenterColor[4] = CenterColor[1];
            CenterColor[1] = n;
        }
    }

    public void RotatePaw()
    {
        //Only can move forward
        string n = CenterColor[0];
        CenterColor[0] = CenterColor[3];
        CenterColor[3] = CenterColor[2];
        CenterColor[2] = CenterColor[1];
        CenterColor[1] = n;
    }

    public int FindCenter(string position)
    {
        int center = -1;
        for (int i = 0; i < 6; i++)
        {
            if (CenterColor[i] == position) center = i;
        }
        return center;
    }
}

有了这个参考坐标系,我们就可以把URDLFB表示法的解魔方步骤,转化成萝卜头能识别的PBS表示法。嗯,不用去Google搜索,这个PBS表示法是我发明的(也就是瞎编的^_^ ),它表示
P: Paw 爪子翻动一次
B:RotateBottom 从底面旋转魔方,后面需要接一个1~3的数字
S:RotateBottomSide 旋转魔方的底面,跟B的区别是这时候爪子抓住上两层,然后旋转底面

下面这段代码描述了从URDLFB操作到PBS操作的转换:

int findSidePosition = CenterStatus.FindCenter(targetSide);

//Rotate to corrent bottom
switch (findSidePosition)
{
    case 2:
        //Do Nothing
        break;
    case 1:
        CenterStatus.RotatePaw();
        Steps.Add(new MoveStep(MoveType.RotatePaw, 0));
        break;
    case 0:
        CenterStatus.RotatePaw();
        Steps.Add(new MoveStep(MoveType.RotatePaw, 0));
        CenterStatus.RotatePaw();
        Steps.Add(new MoveStep(MoveType.RotatePaw, 0));
        break;
    case 3:
        CenterStatus.RotateBottom(true);
        CenterStatus.RotateBottom(true);
        Steps.Add(new MoveStep(MoveType.RotateBottom, 2));
        CenterStatus.RotatePaw();
        Steps.Add(new MoveStep(MoveType.RotatePaw, 0));
        break;
    case 4:
        CenterStatus.RotateBottom(true);
        Steps.Add(new MoveStep(MoveType.RotateBottom, 1));
        CenterStatus.RotatePaw();
        Steps.Add(new MoveStep(MoveType.RotatePaw, 0));
        break;
    case 5:
        CenterStatus.RotateBottom(false);
        Steps.Add(new MoveStep(MoveType.RotateBottom, 3));
        CenterStatus.RotatePaw();
        Steps.Add(new MoveStep(MoveType.RotatePaw, 0));
        break;
}
Steps.Add(new MoveStep(MoveType.RotateBottomSide, Convert.ToInt32(rotateCount)));
Steps[Steps.Count - 1].OrginStep = currentStep;

下面是一个PBS表示法的步骤示例,基本上一个URDLFB旋转操作,会对应1~3个PBS操作:
P B3 P S2 B1 P S1

为了减少发送的数据量,我们用下面的规则来发送PBS表示法的步骤,每个步骤用一个字节来描述:

switch (MoveType)
{
    case MoveType.RotatePaw:
        return (byte)10;
    case MoveType.RotateBottom:
        return (byte)(20 + Count);
    case MoveType.RotateBottomSide:
        return (byte)(30 + Count);
    default:
        return (byte)0;
}

在NXT上对应的解析操作是:

//Get result
int step = BlueTooth.ReadBytes()[0];
if(step==10)
{
	//Rotate paw
	Robot.RotatePaw();
}
else if(step>=20 && step<30)
{
	//Rotate Bottom
	int count = step - 20;
	if(count == 3) count = -1;
	Robot.RotateBottom(count);
}
else if(step>=30 && step<40)
{
	//Rotate Bottom Side
	int count = step - 30;
	if(count == 3) count = -1;
	Robot.RotateBottomSide(count);
}

开始编译工程,佛祖&上帝&安拉&比尔盖子同时保佑,程序编译通过了。如果运气好的话,蓝牙连接成功以后,萝卜头就可以顺利解魔方了。

好了,所有的代码都介绍完了,之后还会介绍一些收尾和改进的工作,主要包括:
1,用超声波测距传感器(就是那对眼睛)制作“开关”;
2,读色错误,卡住等情况的异常处理
3,语音提示,让萝卜头开口说话
4,暂停功能,帮助我们进行调试



对 “解魔方的机器人攻略25 – 解魔方” 的 13 条 评论

  1. 动力小老头 说:

    博主辛苦了。

  2. dead_lee 说:

    辛苦.

  3. iam3i 说:

    博主:你好。
    请教一个问题,为什么rubiksolverv2.java,这个程序,在编译的时候,
    显示:找不到符号
    符号:类 ColorSensor
    位置:static ColorSensor color= new ColorSensor(SensorPort.S3);
    同时提示
    import java.io.IOException;
    import lejos.nxt.comm.*;
    import java.io.*;
    import javax.bluetooth.*;
    never used
    请指教。

    • 你可能没有设置classpath吧,看这个页面:
      http://www.diy-robots.com/?p=218
      搜索”Add External JARs”

      • iam3i 说:

        谢谢你能及时回复。
        我确定我设置了classpath的。
        并且,我还参考了你的朋友:程序猎人的如下网站:
        http://programus.blogbus.com/logs/49288956.html
        介绍的方法,直接在eclipse里编译,
        其他都没有问题,就是凡是与ColorSensor有关的地方,都显示错误。
        如下:
        Exception in thread “main” java.lang.Error: Unresolved compilation problems:
        ColorSensor cannot be resolved to a type
        ColorSensor cannot be resolved to a type
        ColorSensor cannot be resolved to a type
        ColorSensor cannot be resolved to a type
        ColorSensor cannot be resolved to a type
        ColorSensor cannot be resolved to a type

        at RubikSolverV2.main(RobikSolverV2.java:71)

        我用的lejos nxj是0.8.5版本的,不是你用的0.6版本的,不会是这个原因吧?
        因为我使用的lego 8547,nxt2.0版本,据了解,legos nxj 0.8.5才支持
        nxt 2.0版本的颜色传感器,请问是这样的吗?
        呵呵,我知道你工作很忙,不好意思,又要麻烦你。

        • 这个版本我也没用过,不能确定什么原因
          安装lejos以后,在\lejos_nxj\samples目录下有很多例子,你可以参考一下

        • 忧郁飞花 说:

          不知道你解决了没有。。根据现在的API库,LEGO的颜色传感器的类应该是ColorLightSensor…
          ColorSensor现在是属于lejos.nxt.addon这个包里面。。而且是为HiTechnic color sensor 编写的一个类。。。
          不过当前版本的LEJOS的ColorLightSensor这个类在读取颜色时有BUG,解决方法可以在LEJOS的论坛里找到。
          另外我不建议你直接使用博主的代码,因为每个人的设计应该总是会有差别的,博主的代码不是在任何情况下都适用的,理解博主的思路更重要。

        • 程序猎人 说:

          不知道是不是来晚了。
          ColorSensor类不好用,是因为LeJOS 0.85版的bug,需要自己修改一下。
          或者你也可以参看如下文章:
          http://www.cmnxt.com/thread-909-1-1.html

          • 不晚不晚,呵呵
            我上周入手NXT2.0的时候,在Lejos的论坛上搜到了那个帖子
            居然看到你的名字,所以迅速把那个jar文件下载了,一试果然好用

            世界太小,真是无处不相逢 :)

  4. yard128 说:

    牛的,博主真是历害

  5. theflash 说:

    支持~ 您一直更新啊 加油~

  6. rjh 说:

    问下 我想用单片机做个机器人 问下你那个C#程序能直接接收单片机输出的关于颜色的信号么 算好算法后再自动发给单片机么

发表评论

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