做做AI，造造人 - 动力老男孩的博客

经过这么长的时间，终于把萝卜头的制作攻略全部发完了。这次发攻略的过程，我把萝卜头重新搭建了一遍，把所有的代码也整理重写了。新版的小萝卜头看上去更好看一点（个人感觉，呵呵），代码容错能力也强了很多。现在在各种不同的灯光条件下，基本都可以正确识别颜色，并且转动过程中，魔方掉下来的情况也很少见了。终于松了一口气，对博客和对萝卜头都算有一个交代了。

我家圈圈很快就要降生，接下来的一段时间博客可能会很少更新。小爱的进度看来已经远远落后，目前还只是一个可以用手机遥控的小车。不过来日方长，我会继续制作并且和大家分享的。

NXT端的程序下载(源代码)

PC端的程序下载(exe)

下面转几个国外达人制作的魔方机器人，其中最快的已经可以在十秒左右解出魔方。据说人类的世界记录是7秒多，有志于突破人类极限的朋友，可以参考参考，我非常愿意为此贡献一点力量。

世界最快的乐高魔方机器人

目前最快的魔方机器人

作者把视频发在了youtube上，无法转载，有兴趣的同学请自行翻墙过去看看：
http://www.youtube.com/user/RoboticSolutions
介绍文章：
http://singularityhub.com/2010/02/17/lego-robot-solves-any-rubiks-cube-in-less-than-12-seconds-video/

五阶魔方机器人
用的也是Lego studio摄像头+NXT，5阶魔方还原算法自然是这个机器人的核心，但并非lego范畴，这里可以看到David Gilday愈加成熟的PC端处理程序，看起来相当酷。此外玩过多阶魔方的朋友都知道，这种魔方的翻转定位是最大的问题，所以这款机器人采用了一个相对丑陋的框式造型来帮助解决这个问题，气势上差了一点，但毕竟是第一款，6分43秒185步的测试水平也算世界纪录了。

视频地址

URL:
http://www.tudou.com/programs/view/HXeCwaDdXW0/

大型魔方机器人
大型机械臂，从取魔方，识别，到翻转，放回，仿佛可以看见不远将来家里的小机器人接过你随手拧乱的魔方，咻咻复原的可爱场景。
视频地址：

URL：
http://www.tudou.com/programs/view/TLuTp8mWLPM/

四阶魔方机器人
这是一款用NXT+N95手机的魔方机器人，旋转的结构很酷：
http://blog.izxg.com/?tag=%E4%B9%90%E9%AB%98

现在我们的工作已经接近尾声了，看看怎么把电脑变成一个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，暂停功能，帮助我们进行调试

经常有朋友向我要QQ号，很遗憾我属于拖了时代后腿的那种人，暂时还没有QQ号。如果有事的话请直接留言或者给我发邮件，邮箱地址在右侧的下方。还有另外一些朋友是要源代码的，事实上我曾经分享过源代码，但是反馈基本上都是“你这个怎么不能用啊？”

晕倒，电机阻力不同，连杆的倾斜角误差不同，魔方大小不同，魔方润滑程度不同，颜色传感器的读数不同。这么多不同，我在代码里面留了很多参数，就是用来调节和配置的。如果我耐心的在QQ上解释的话，我的老板一定会很耐心地给我写一封热情洋溢的开除通知

所以我会在写攻略的同时逐步公开源代码，一方面可以更好的了解原理，另一方面也可以在攻略中找到想要的答案。有问题的朋友请先仔细看攻略，然后再发邮件提问。下面有一些问题请恕我不回邮件：
1，攻略里已经讲过的。例如：请问解魔方的算法是什么？我很久以前就发过代码了（不过估计这样的同学也看不到这个声明，惆怅啊）。
2，一些特别基础的问题。例如：颜色传感器如何使用？这种问题网上一搜一大堆，请自己google一下，比等邮件更省时间
3，对于参加竞赛的，做毕设的，或者保研需要加分的。非常抱歉，时间紧是您自己的事。我不会帮助投机取巧的行为，况且我其实比你们更忙。

好了，今天会介绍颜色识别剩下的部分。到这一篇结束，所有重要的技术细节就都介绍完了，我相信这些攻略对一个真正的DIY爱好者已经足够了。

下面继续介绍颜色识别的代码实现。

4，设置app.config

上一篇介绍了分辨颜色的六个规则，考虑到不同的颜色传感器可能规则不尽相同，所以把它们放到config文件里，可以随时修改：

<?xml version="1.0" encoding="utf-8" ?>
<configuration>
  <appSettings>
    <add key="Rank0" value="W:Min" />
    <add key="Rank1" value="Y:G" />
    <add key="Rank2" value="B:B" />
    <add key="Rank3" value="G:-R" />
    <add key="Rank4" value="O:R+2*RawG-2*RawB" />
    <add key="Rank5" value="R:1" />
  </appSettings>
</configuration>

5，定义ColorItem类和排序类

接下来是根据排序规则对颜色数组排序，事实上这个跟机器人无关，完全是C#语言的知识。不熟悉的同学请复习一下C#中对List的排序功能。首先我们定义一个ColorItem类，每个实例对应一块魔方的色块:

public class ColorItem
{
    public int R, G, B, RawR, RawG, RawB;
    public int Max, Min, RawMax, RawMin;
    public int I, J, K;
	//省略一些赋值操作
}

然后定义一个对ColorItem进行排序的类：

public class ColorItemCompare : IComparer
{
    private string CompareExpression;

    public ColorItemCompare() { }
    public ColorItemCompare(string exp)
    {
        CompareExpression = exp;
    }

    public int Compare(ColorItem c1, ColorItem c2)
    {
        if (c1 == null || c2 == null) return 0;
        else
        {
            return GetEvalOfColor(c1, CompareExpression) - GetEvalOfColor(c2, CompareExpression);
        }
    }

    private int GetEvalOfColor(ColorItem c, string exp)
    {
        string realExp = exp.ToLower();
        realExp = realExp.Replace("rawmin", c.RawMin.ToString());
        realExp = realExp.Replace("rawmax", c.RawMax.ToString());
        realExp = realExp.Replace("min", c.Min.ToString());
        realExp = realExp.Replace("max", c.Max.ToString());
        realExp = realExp.Replace("rawr", c.RawR.ToString());
        realExp = realExp.Replace("rawg", c.RawG.ToString());
        realExp = realExp.Replace("rawb", c.RawB.ToString());
        realExp = realExp.Replace("r", c.R.ToString());
        realExp = realExp.Replace("g", c.G.ToString());
        realExp = realExp.Replace("b", c.B.ToString());
        return Convert.ToInt32(Evaluator.Eval(realExp));
    }
}

其中Evaluator是一个自定义的函数，它的功能是对一个字符串格式的表达式求值，例如：Evaluator.Eval(“1+2″)的值是3。

然后通过下面这一段代码，对读到的54个色块进行分辨：

for (int n = 0; n < 6; n++)
{
    string[] rankStr = ConfigurationSettings.AppSettings["Rank" + n].Split(':');
    string resultColor = rankStr[0];
    string compareExp = rankStr[1];

    ColorItems.Sort(new ColorItemCompare(compareExp));
    for (int i = 0; i < 9; i++)
    {
        ColorItem item = ColorItems[ColorItems.Count - 1];
        int ijk = item.I * 100 + item.J * 10 + item.K;
        ColorSortResult.Add(ijk, resultColor);
        ColorItems.RemoveAt(ColorItems.Count - 1);
    }
}

通过上面的运算，位置坐标为ijk的色块，颜色值就保存在ColorSortResult字典对象中。

6，生成魔方数组

排序之后我们已经知道ijk对应的色块的颜色，接下来再按照i,j,k的顺序读取一遍，就可以生成颜色数组。
ReadColors函数会返回两个字符串，第一个字符串是 “R,G,B,Y….” 格式的返回值，这个是显示那个三维立体魔方用的。第二个字符串是“3,5,2,6….” 这样的格式，在下一步转换为速魔方算法的表示法。

private string[] ReadColors()
{
    string ColorStr = "";
    string RealStr = "";
    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            for (int k = 0; k < 3; k++)
            {
                if (!string.IsNullOrEmpty(ColorStr))
                {
                    ColorStr += ",";
                    RealStr += ",";
                }
                int c = i * 100 + j * 10 + k;
                string r = ColorSortResult[c];
                ColorStr += ColorValue(r);
                RealStr += r;
            }
        }
    }
    return new string[] { ColorStr, RealStr };
}

private int ColorValue(string c)
{
    if (c.Contains("Y") || c.Contains("y")) return 1;
    if (c.Contains("B") || c.Contains("b")) return 2;
    if (c.Contains("R") || c.Contains("r")) return 3;
    if (c.Contains("W") || c.Contains("w")) return 4;
    if (c.Contains("O") || c.Contains("o")) return 5;
    if (c.Contains("G") || c.Contains("g")) return 6;
    return 0;
}

7，魔方表示法的转换

上面我们得到了6*3*3的魔方数组表示法，为了调用魔方快速算法，必须转换到URDLFB的表示法。这个转换没啥捷径可走，优雅的程序员偶尔也要使用暴力：

//其中s是把6*3*3的数组，用逗号按顺序连接成的字符串
private void SolveReadColors(string s)
{
    string[] ArrColors = s.Split(','); ;
    string sInput = "";
    string ReadQ = "URDLFB";
    string[] PosQ = new string[6];
    for (int i = 0; i < 6; i++) PosQ[Convert.ToInt32(ArrColors[4 + i * 9]) - 1] = ReadQ[i].ToString();

    sInput += PosQ[Convert.ToInt32(ArrColors[7]) - 1] + PosQ[Convert.ToInt32(ArrColors[37]) - 1] + " ";  //UF
    sInput += PosQ[Convert.ToInt32(ArrColors[5]) - 1] + PosQ[Convert.ToInt32(ArrColors[12]) - 1] + " ";  //UR
    sInput += PosQ[Convert.ToInt32(ArrColors[1]) - 1] + PosQ[Convert.ToInt32(ArrColors[52]) - 1] + " ";  //UB
    sInput += PosQ[Convert.ToInt32(ArrColors[3]) - 1] + PosQ[Convert.ToInt32(ArrColors[32]) - 1] + " ";  //UL
    sInput += PosQ[Convert.ToInt32(ArrColors[25]) - 1] + PosQ[Convert.ToInt32(ArrColors[43]) - 1] + " ";  //DF
    sInput += PosQ[Convert.ToInt32(ArrColors[21]) - 1] + PosQ[Convert.ToInt32(ArrColors[14]) - 1] + " ";  //DR
    sInput += PosQ[Convert.ToInt32(ArrColors[19]) - 1] + PosQ[Convert.ToInt32(ArrColors[46]) - 1] + " ";  //DB
    sInput += PosQ[Convert.ToInt32(ArrColors[23]) - 1] + PosQ[Convert.ToInt32(ArrColors[30]) - 1] + " ";  //DL
    sInput += PosQ[Convert.ToInt32(ArrColors[41]) - 1] + PosQ[Convert.ToInt32(ArrColors[16]) - 1] + " ";  //FR
    sInput += PosQ[Convert.ToInt32(ArrColors[39]) - 1] + PosQ[Convert.ToInt32(ArrColors[34]) - 1] + " ";  //FL
    sInput += PosQ[Convert.ToInt32(ArrColors[50]) - 1] + PosQ[Convert.ToInt32(ArrColors[10]) - 1] + " ";  //BR
    sInput += PosQ[Convert.ToInt32(ArrColors[48]) - 1] + PosQ[Convert.ToInt32(ArrColors[28]) - 1] + " ";  //BL

    sInput += PosQ[Convert.ToInt32(ArrColors[8]) - 1] + PosQ[Convert.ToInt32(ArrColors[38]) - 1] + PosQ[Convert.ToInt32(ArrColors[15]) - 1] + " ";  //UFR
    sInput += PosQ[Convert.ToInt32(ArrColors[2]) - 1] + PosQ[Convert.ToInt32(ArrColors[9]) - 1] + PosQ[Convert.ToInt32(ArrColors[53]) - 1] + " ";  //URB
    sInput += PosQ[Convert.ToInt32(ArrColors[0]) - 1] + PosQ[Convert.ToInt32(ArrColors[51]) - 1] + PosQ[Convert.ToInt32(ArrColors[29]) - 1] + " ";  //UBL
    sInput += PosQ[Convert.ToInt32(ArrColors[6]) - 1] + PosQ[Convert.ToInt32(ArrColors[35]) - 1] + PosQ[Convert.ToInt32(ArrColors[36]) - 1] + " ";  //ULF

    sInput += PosQ[Convert.ToInt32(ArrColors[24]) - 1] + PosQ[Convert.ToInt32(ArrColors[17]) - 1] + PosQ[Convert.ToInt32(ArrColors[44]) - 1] + " ";  //DRF
    sInput += PosQ[Convert.ToInt32(ArrColors[26]) - 1] + PosQ[Convert.ToInt32(ArrColors[42]) - 1] + PosQ[Convert.ToInt32(ArrColors[33]) - 1] + " ";  //DFL
    sInput += PosQ[Convert.ToInt32(ArrColors[20]) - 1] + PosQ[Convert.ToInt32(ArrColors[27]) - 1] + PosQ[Convert.ToInt32(ArrColors[45]) - 1] + " ";  //DLB
    sInput += PosQ[Convert.ToInt32(ArrColors[18]) - 1] + PosQ[Convert.ToInt32(ArrColors[47]) - 1] + PosQ[Convert.ToInt32(ArrColors[11]) - 1];  //DBR

    ResultSteps = RubikSolve.GetResult(sInput);
}

这个神奇的SolveReadColors函数，吃进去的是颜色数组，挤出来的是解魔方的步骤。结果保存在ResultSteps变量中，格式为：
F1 U2 F2 D3 L2 D1 F1 U3 L2 D1
其中每两个字符表示一个旋转步骤，第一个字母表示操作的面，第二个字母表示旋转的方向。1是顺时针，3是逆时针，2是旋转180度。

至此萝卜头已经知道了解魔方的方法，在前面的攻略中，我们已经介绍了旋转魔方的分解动作。

接下来的工作就简单了，下一篇会介绍如何通过蓝牙遥控萝卜头动手干活。

今天看到架子上的萝卜头，已经落了很多灰尘。想起萝卜头的攻略还剩几篇迟迟没有写完。前一段时间一直在试验小爱的手机遥控器功能，从今天开始准备陆续把萝卜头的攻略补完，给博客也打扫打扫灰尘。

说起来真是很惭愧，颜色识别在萝卜头制作过程中是花费时间最多的部分。其中还有一段小插曲：
我在淘宝上买的颜色传感器，在NXT上测试时，发现只有用强光照射在魔方表面的时候，传感器才有读数。那时候在网上很难找到相关的资料，不知道是我买了次品，还是设置不当。后来我猜想传感器中心的那个透明小灯泡是光源，就擅自去电子市场买了一个LED小灯，然后把这个500块钱的传感器敲开换上。一通电，嘿，灯居然亮了，然后我就把拆下来的小灯扔到垃圾桶继续测试。结果…..这次传感器彻底废了。接下来是从垃圾桶里翻那个透明的小灯泡，非常悲剧的是那天正好吃了虾和鱼，我把整垃圾桶的虾皮鱼骨摸了两遍，才找到那个透明的小灯泡，把它洗洗干净又换上了。后来才知道，这个灯泡其实是用来读取颜色的，而不是照明的。而我买的那个颜色传感器确实是个次品，必须用灯光照射才能勉强读数。所以你们看到第一版的萝卜头，在悬臂上是带有一个照明灯的。

在此提醒一下朋友们：颜色传感器在普通的光照环境下，应该是有读数的，而且很敏感，读数会不断小幅跳动。如果你买的传感器读数一直是0或者跳动非常大，那么请尽快找奸商退换。另外，在这里感谢一下北京西觅亚公司，他们给我提供了几个测试用的颜色传感器，并且给我换了一个新的。我也因此了解了一些颜色传感器的特性。

好了，进入正题。上一篇介绍了如何在电脑和NXT之间使用蓝牙进行通讯。有了蓝牙，我们就可以把颜色传感器的读数发送给电脑，然后用电脑识别颜色后调用解魔方的算法。

1，从NXT发送颜色数据到电脑
在之前的一篇博客里，我介绍了三个函数：ReadAllSide，ReadOneSide和SendColorToPC。现在蓝牙已经调通，可以改写SendColorToPC函数用来发送数据。其中getRed()和getRawRed()等函数的说明，请参考颜色传感器的API文档

//Send colors to PC
public static void SendColorToPC(int center, int n) throws Exception
{
	//get the x,y of n
	int y = n % 3;
	int x = (n - y) / 3;

	//send to PC by bluetooth
	byte[] data = new byte[9];
	data[0] = (byte)center;    //center表示是魔方的某一面
	data[1] = (byte)x;         //x 表示魔方这一面3*3的色块中，第x行的色块
	data[2] = (byte)y;         //y 表示魔方这一面3*3的色块中，第y列的色块
	data[3] = (byte)color.getRed();
	data[4] = (byte)color.getGreen();
	data[5] = (byte)color.getBlue();
	data[6] = (byte)(color.getRawRed() / 3);
	data[7] = (byte)(color.getRawGreen() / 3);
	data[8] = (byte)(color.getRawBlue() / 3);
	BlueTooth.WriteBytes(data);
}

2，在PC端接受颜色数据
PC程序中的BlueToothDataReceived函数，用来响应接受到蓝牙数据的事件。我们加上下面这段函数：

else if (length == 9)
{
    int i = data[0];
    int j = data[1];
    int k = data[2];
    int r = data[3];
    int g = data[4];
    int b = data[5];
    int rawR = data[6];
    int rawG = data[7];
    int rawB = data[8];
    ColorItem newItem = new ColorItem(i, j, k, r, g, b, rawR, rawG, rawB);
    colorDistinguish.ColorItems.Add(newItem);
    DisplayMessage += newItem.ToString() + "\r\n";
    Status = "成功获取数据：" + i + "," + j + "," + k;
}

其中用到了两个类 ColorItem 和 ColorItemDistinguish。这两个类的作用后面再说，总之这里把所有的颜色数据都先保存到一个阵列（Array）里，最后统一识别颜色。

3，解析颜色的方案
细心的朋友可能在API中看到了getColor()函数，我们何必要全部保存颜色后再统一分辨呢，直接读一个分辨一个不是更好？事实证明这个函数基本没什么用，红色和橙色都会解析成红色，而且环境光线变化时影响很大。还有一些朋友建议用HSV颜色模型，这种方案我也试过了，基本上也很难分辨。为什么呢？请看下面一组读数：

Red
[0,1,2]=>RGB=(23,0,0),RawRGB={45,1,8}
[0,2,2]=>RGB=(30,0,0),RawRGB={60,1,5}
[0,2,1]=>RGB=(25,0,0),RawRGB={49,3,12}
[0,2,0]=>RGB=(32,0,0),RawRGB={63,2,6}
[0,1,0]=>RGB=(22,0,0),RawRGB={43,2,11}
[0,0,0]=>RGB=(25,0,0),RawRGB={59,3,3}
[0,0,1]=>RGB=(30,0,0),RawRGB={58,5,17}
[0,0,2]=>RGB=(31,0,0),RawRGB={61,8,17}
[0,1,1]=>RGB=(31,0,0),RawRGB={62,15,22}

Orange
[2,1,2]=>RGB=(28,0,0),RawRGB={55,12,8}
[2,2,1]=>RGB=(30,0,0),RawRGB={57,14,14}
[2,0,1]=>RGB=(32,0,0),RawRGB={62,15,13}
[2,1,0]=>RGB=(32,0,0),RawRGB={63,16,12}
[2,2,2]=>RGB=(42,0,0),RawRGB={83,24,10}
[2,2,0]=>RGB=(41,0,0),RawRGB={82,24,13}
[2,0,0]=>RGB=(41,0,0),RawRGB={80,23,10}
[2,0,2]=>RGB=(39,0,0),RawRGB={76,22,13}
[2,1,1]=>RGB=(41,5,0),RawRGB={81,30,21}

这是在自然光条件下，对红色和橙色的9个色块分别读数的结果。可以看到，它们的Green和Blue分量全部是0，只有红色分量有差别。但是红色的red分量从23~32，橙色的red分量从28~42，它们中间是有重叠的。对于这些读数，HSV完全没用。
有一段时期我几乎已经绝望了，不过终于在最后让我找到了一点区别：红色的RawBlue分量基本上比RawGreen分量稍大，而橙色恰好相反。另外请对比一下[0,0,0]和[2,2,1]，它们的RawBlue分量和RawGreen分量是相同的，但是仍然可以找到区别：按公式R+2*RawG-2*RawB计算，橙色的永远比红色大！

也就是说，我们单独取到一组颜色数值时，很难直接知道它是什么颜色，只有对一组数进行排序后，才能区分出不同的颜色。就像刚才这18个数，我们按照R+2*RawG-2*RawB从大到小排序，最终结果的前9个是橙色，后9个就是红色。类似的，我们还可以定义出分辨颜色的判断规则：
1，假设RGB三个值的最小值为Min，按Min从大到小排序，前9个是白色
2，剩下的颜色，按照G分量从大到小排序，前9个是黄色（有意思吧，绿色分量最大的是黄色）
3，剩下的颜色，按照B分量从大到小排序，前9个是蓝色（这个还算靠谱）
4，剩下的颜色，按照R分量从小到大排序，前9个是绿色
5，剩下的颜色，按照R+2*RawG-2*RawB从大到小排序，前9个是橙色
6，剩下的颜色全是红色

当光线从弱到强变化时，这些值基本会成比例的变大，所以这些规则依然有效。
有兴趣的朋友可以查看一组完整的颜色读数，来验证以上这些规则：http://www.diy-robots.com/rubiksolver/readcolors.txt
下一篇继续介绍这种分辨方式的具体代码实现。

之前已经介绍了萝卜头转魔方的各个分解动作，今天介绍如何用颜色传感器读取魔方的颜色。这一部分可以分成三部曲：
1，依次扫描魔方的6*9=54个色块
2，用蓝牙连接把数据发送到电脑
3，通过颜色分组函数，从读数分辨出不同的颜色

这里先说明一下：虽然乐高的颜色传感器以RGB的形式返回颜色值，但是并不是想象中的那样，红色返回(255,0,0)，蓝色（0,0,255）这么轻松。事实上这个数值受环境光线强度影响非常大，即使相同的环境下，读数仍然会有跳动。例如下面几个读数：

[0,1,2]=>RGB=(23,0,0),RawRGB={45,1,8}  //红色
[0,2,2]=>RGB=(30,0,0),RawRGB={60,1,5}  //红色
[2,1,2]=>RGB=(28,0,0),RawRGB={55,12,8}  //橙色
[2,2,1]=>RGB=(29,0,0),RawRGB={57,14,14}  //橙色

如果你仅想从RGB来分辨颜色的话，将会“很受伤”。不过今天我只介绍第一步（扫描），在PC端程序部分再介绍如何识别颜色。

因为颜色传感器一次只能读一个点，所以要扫描魔方的话，必须把54个点都扫到。我们以扫描其中一个面为例，看看两个电机怎么配合扫描到所有的9个点。首先，颜色传感器的电机中心位置，应该在2，4两个点的中心延长线上。这样在魔方位置不动的情况下，就可以扫描到中心和四个角了。（相关文章：颜色传感器的安装）

扫描魔方中心和魔方的角

然后，让底座旋转一定的角度，同时传感器电机也稍微调整，这样就可以扫到4个棱的颜色。

扫描魔方的棱

以这种方式扫描一圈，就可以把魔方的一个面读完了。最后使用魔方操作的分解动作，把魔方翻过来倒过去，直到把六个面依次扫描出来。有一件非常麻烦的事情是，在魔方翻来翻去的过程中，数组并不是每次都以0为左上角，它是不停的变换的（相关文章：魔方坐标系）。我用了下面这个map表，用来标记读数的顺序，这可是牺牲了数百个脑细胞换来的，其中idx数组是每个面内的依次读取顺序，idex2数组是不同的面的读取顺序：

int[][] idx={
	{4,6,7,8,5,2,1,0,3},
	{4,0,3,6,7,8,5,2,1},
	{4,2,1,0,3,6,7,8,5},
	{4,8,5,2,1,0,3,6,7},
	{4,2,1,0,3,6,7,8,5},
	{4,2,1,0,3,6,7,8,5}};
int[] idx2={5,1,4,3,2,0};

我们以上一次的程序为基础，添加以下变量和函数：

	//add offset positions for color sensor motor
	static int ColorMotorOffset1 = 33;
	static int ColorMotorOffset2 = 9;
	static int ColorMotorOffset3 = 18;
	static int ColorReadPostion1 = 162;
	static int ColorReadPostion2 = 154;

	//Read each side colors of the cube
	public static void ReadAllSide()
	{ }

	//Read one side by the index
	public static void ReadOneSide(int nSideIndex)
	{ }

最后加一个测试入口，当按下Enter键时，开始扫描魔方。相信看过前面文章的朋友，这里不需要说明了。点此查看具体的代码吧。

补充个小小的说明：我在代码里面统一用英文加了注释，不是在装酷，主要是因为上班的时候是这样强制要求的，以至于自己做东西也养成这种习惯了。

一星期没更新，原因就不多说了，总之请见谅。从今天开始继续发攻略

我原来的代码又多又乱还没有注释，自己看着都眼晕，找点代码晕的跟坐过山车似的。现在正在把它们重新整理优化，再加上注释。我打算整理一部分就发一部分攻略，攻略发完了也就整理完了。另外，这几天有很多网友正帮忙翻译lejos的中文教程，我在整理的过程中也学到了不少东西，有兴趣的同学还可以加入。

下面开始正题。假设现场的观众们按照前面的攻略，已经把萝卜头搭建好了。第一段程序先让魔方能动起来，实现的功能是：

• 按Left键，魔方底座旋转90度
• 按Right键，爪子抓住魔方，然后底座带动最下面的层旋转90度
• 按Enter键，爪子把魔方翻转90度
• 按Escape键，程序退出

下面介绍需要用到的一些知识点

1，创建传感器和电机的实例：

//Define Sensors
 static UltrasonicSensor distance=new UltrasonicSensor(SensorPort.S1);
 static LightSensor light = new LightSensor(SensorPort.S2);
 static ColorSensor color = new ColorSensor(SensorPort.S3);
 //Define Motors
 static Motor paw=Motor.A;
static Motor monitor=Motor.B;
 static Motor bottom=Motor.C;

这部分对应的是我们的接线方式：
传感器1口接超声波传感器，也就是眼睛
传感器2口接亮度传感器
传感器3口接颜色传感器
电机A口接爪子的电机
电机B口接颜色传感器的电机
电机C口接魔方底座的电机

2，创建一个Robot类，这个类用于控制机器人结构上的各种动作，下面三个方法分别对应上面说的三个功能：

public static class Robot
{
 public static void RotateBottomSide(int nQuarter)
 {   }

 public static void RotateBottom(int nQuarter)
 {   }

 public static void RotatePaw()throws Exception
 {   }
}

这里使用了关键字static，因为萝卜头只有一个实例，所以把它设置成静态类。静态类可以直接使用静态方法，不需要创建实例，还是看一段代码对比下：

//创建实例的用法
Robot instance = new Robot();
instance.rotate();
//静态类的用法
Robot.rotate();

3，设置了一些参数

//如果爪子部分改装了那个3:1的减速齿轮，设置成true，不明白的请看 http://www.diy-robots.com/?p=147 最后两张图
 static boolean HasReducer = true;
 //爪子抓住魔方时的电机角度
 static int PawHoldPosition = 56;
 //爪子翻动魔方时的电机角度
 static int PawTurnOverPosition = 110;
 //底座旋转90时，电机的旋转角度（因为齿轮组的原因）
 static int BaseOneQuarter = 315;
 //当底座旋转魔方底面时，因为魔方的阻力，需要先多转一个小角度，然后再转回来，这是用来修正误差的角度
 static int BaseRotateFix = 40;

4，控制电机（motor）的几个函数

paw.setSpeed(400); //设置转速
paw.rotateTo(nPawHoldPosition); //旋转到一个指定角度（绝对定位）
bottom.rotate(-nFixAngle);    //旋转一定角度（相对定位）

更多的电机相关函数，请点这里看刚刚翻译好的教程。

5，亮度传感器的一个函数，用于把它的灯打开或者关闭。这里是关闭它，省的晃眼睛，需要的时候再开

light.setFloodlight(false);

好了，最终解魔方的动作，都是通过调用这几个函数来完成的。事实上如果你足够无聊的话，现在就可以通过NXT上的几个按键来控制萝卜头玩魔方了。
该吃早饭了，大家自己看看源代码吧。
http://www.diy-robots.com/RubikSolver/SourceCode/NXT/RubikSolverV2.java_20100115.txt

我们的快速魔方算法要隆重登场了，在此缺席感谢一下来自Netherlands的Jaap Scherphuis同学。看前面这个页面的第三名。

魔方表示法
咱们先看一串天书般的字母：UF UR UB UL DF DR DB DL FR FL BR BL UFR URB UBL ULF DRF DFL DLB DBR
这种表示法是由一个叫Mike Reid的兄弟首先使用的，它表示一个已经被解好的魔方。
先不要被这串字母吓倒，看算法就像追mm一样，要迎难而上。仔细观察，你会发现其中只有六种字母：
U: Up
F: Front
R: Right
L: Left
D: Down
B: Back
其实这就是代表了空间坐标系的六个方向，就是传说中的“眼观六路”的那六路。
表示法中包含了12组双字母的组合，分别代表了魔方的12个棱，第一组UF就表示Up和Front之间夹角的棱。
另外还包含了8组三字母的组合，分别代表了魔方的8个角，每个角由三块颜色组成。看下面的示意图：

魔方坐标系

等等，细心的朋友至少会想到两个问题：
1，为什么没有中心的数据？
因为魔方的六个心在任何旋转过程中，相对位置都是不会变的，这点拆过魔方的人应该比较容易理解。
2，如果是一个打乱的魔方，棱和边的颜色已经和中心不一样了，这时候怎么表示？
读取方法是：按照刚才那个天书字符串的顺序，先找到UF位置所对应的棱，假设现在U是红色，F是黄色；
那么对照图里的中心，红色的中心是R，黄色的中心是U，所以这时候的第一组棱字母是 RU
嗯，希望你看到这里还没有晕车的感觉。

输出表示法
这个程序的输出是这个样子：F- U+ F- D- L- D- F- U- L2 D-
FRL之类的字母依然表示六个面，F-表示前层逆时针转90度，U+表示上层顺时针转90度，L2表示左边层转180度。
如果你是魔友的话，会经常看到这样的字符串：F’UF’D'L’F'U’L2D’
这是魔方论坛上比较常见的“黑话”，其实就是默认顺时针不加符号，逆时针的加一个单引号，180度的加2。
请注意这里的顺时针和逆时针使用的是“观察者迎着某个面看”的参照系，例如B’是从下往上看的逆时针，如果你没有把脑袋钻到桌子下，你事实上看到的是顺时针旋转。

改写到C#
这段程序是用C写的，说实话它的原理还比较复杂，有兴趣的同学可以搜索“Thistlethwaite’s algorithm”
我直接依葫芦画瓢用C#把它重写了一遍，请点击这里下载源代码。请主要要安装VS2008或更高版本。

http://www.diy-robots.com/RubikSolver/RubikSolverSample.zip

补充：如何使用这个程序

鉴于很多朋友询问如何运行这个程序，下载这段代码，用C语言的编译器编译成Jaap.exe。然后在命令行输入：
Jaap.exe UF UR UB UL DF DR DB DL FR FL BR BL UFR URB UBL ULF DRF DFL DLB DBR

输出结果就是类似 F- U+ F- D- L- D- F- U- L2 D- 这样的步骤。
注意这段程序没有验证功能，如果你输入的颜色表达式错误，会导致程序死循环或者错误。以我的经验看，普通电脑都能在1秒以内算完，如果你一秒钟还没有看到结果，就检查检查输入吧。

本来我想把这个攻略做成一个NXT开发的教程，把传感器，电机，发声等部分都介绍一遍。不过现在看来有些同学很心急，希望早点看到“核心代码”，所以我提前把解魔方的算法写出来。其实魔方的算法网上有很多，只要你耐心并且有效的使用搜索引擎，会发现上个世纪就已经有人公布算法或源代码了，例如
算法：http://www.zunny.com/RUBIK.HTM
代码：http://tomas.rokicki.com/cubecontest/

不过我做第一版的时候，还是决定自己动手写算法。原因很简单：我玩魔方很多年了，把玩法转成算法也是我的目标之一。在写程序之前，我画了以下的几张草图：

魔方算法的草稿

 话说曾经有位同事本打算和我一起做萝卜头的，看了这些草图以后，决定还是继续打游戏更靠谱。这不禁让我想起一首歌“1979年，那是一个春天，有一位老人在中国的南海边画了一个圈…” 这位老同志一定是资深软件架构师，改革开放这么宏伟的事情，画个圈就搞定了。
这样说来我这个草图是太复杂了，难怪把人吓跑了。今天特地又重画了些好看的图，以便大家理解。

魔方表示法
算法的第一个问题就是，怎么用数学方式描述一个魔方状态。我的做法是把魔方想象成一个纸盒子，沿边缝剪开铺平，就形成了六个面，我按照图里的顺序给它们编了号。
每个面又包含了9个颜色小方块，我也按照图中的顺序给它们编了号。

魔方的数组表示法

这样一来，立体的魔方就变成了一个 6*9 的数组。例如下面是一个普通的被打乱的魔方：

static String SideColors[] = {
  "orgorwwoo",
  "oyggbobrg",
  "yyrgowwbw",
  "yrybgybbo",
  "gwwyybror",
  "bgrwwrbgy"

魔方坐标系：
啥？怎么又有坐标系，刚才的表示法不就完全描述了一个魔方吗？没错，但是咱们的萝卜头每次只能旋转魔方的最下面一层，假设我们需要旋转最上面一层，就必须先把它翻到下面。
请注意在翻跟头的过程中，魔方本身并没有变化，只是坐标系变了。所以还需要一个坐标系来对应萝卜头的空间：

魔方坐标系

状态变化：
正如刚才所说，魔方在萝卜头的数字世界里有两种变化形式：1，翻跟头；2，旋转某一面。
每次状态变化都会造成SideColors数组发生变化，这种转换用最简单的查表法就可以搞定：

坐标变化的大概示意图，坐标变化没啥难度，主要看耐心

例如，这是一段旋转底面后状态转换的代码：

public static void RotateBottomSide(boolean ClockWise) throws Exception
{
 int temp=0;
 int i;

 CopyMatrics(2,6,ClockWise?2:1); //Bottom ClockWise = Top Anti-ClockWise
 CopyMatrics(6,2,0);
 if(ClockWise)
 {
  for(i=0;i<3;i++)
  {
   temp=Sides[5][0][i];
   Sides[5][0][i]=Sides[3][2-i][0];
   Sides[3][2-i][0]=Sides[4][2][2-i];
   Sides[4][2][2-i]=Sides[1][i][2];
   Sides[1][i][2]=temp;
  }
 }
 else
 {
  for(i=0;i<3;i++)
  {
   temp=Sides[5][0][i];
   Sides[5][0][i]=Sides[1][i][2];
   Sides[1][i][2]=Sides[4][2][2-i];
   Sides[4][2][2-i]=Sides[3][2-i][0];
   Sides[3][2-i][0]=temp;
  }
 }
}

CFOP解法：
这是由一位叫Jessica Fridrich女士发明的一种速解法，是目前世界上最流行的方块解法。
CROSS：字面上的意思为“十字”，是Fridrich Method中的第一步骤。
F2L：是“First 2 Layer”的缩写，意思为“一、二层”，是Fridrich Method中的第二步骤。
OLL：是“Orientation of Last Layer”的缩写，意思为“最后一层的角块排序”，这是Fridrich Method中的第三个步骤。
PLL：是“Permutation of Last Layer”的缩写，意思为“最后一层的排序”，这是Fridrich Method中的第四步骤。
CFOP：是Fridrich Method的的别称，就是四个步骤“Cross、F2L、OLL、PLL”原文的第一个字母合起来而成的。

上面这些文字比较费解，看下面的图就比较清楚了：

魔方的CFOP入门解法

或者你可以去魔方小站或者魔方吧看更详细的教程。

CFOP解法的实现：
这一部分比较繁琐，输入玩法公式的输入，按照上面的步骤实现以下函数：

TopCross();
TopCorner();
SecondLayer();
BottomCross();
BottomCorner();
ThirdLayerCorner();
ThirdLayerCornerSnap();
ThirdLayerBorderSnap();

CFOP算法的源代码可以点这里下载
通过这个CFOP算法，萝卜头完成了第一版：http://v.youku.com/v_show/id_XNDcwMDQ3NDQ=.html

这个算法的最大问题就是步骤太多，一般来说要120步左右，平均时间12分钟，大多数观众等不到转完就睡着了……
因为这个原因，我改用了一个更快的算法。写博客真是挺累啊，这个算法下次再介绍，心急的同学请看下面这个链接：
http://tomas.rokicki.com/cubecontest/ 点最上面的Winners，我用的是第二名的算法。

这一节要介绍搭建结构的最后一个部分–安装各种传感器。之后我会继续发布软件部分的攻略。
正好今天有位小朋友留言问道，能不能分享源代码。我在这里统一回答一下：在这系列的攻略里，会讲到制作萝卜头所有需要的知识点和注意事项，也会逐步贴出所有源代码。但是在攻略写完之前暂时不会提供下载，因为我希望大家最终自己动手实现，这也正是DIY机器人的乐趣所在！在我制作的过程中，遇到了许多困难，你们已经不需要走太多弯路了（谁敢说我小气，我跟他急^_^）。

引用一段和菜头的话：上网以后，我们把信息当做了知识，把收藏当做了学习，把阅读当做了思考，把储存当做了掌握。像个花栗鼠在秋天收藏坚果一样，把自己的阅读器和硬盘塞满，却依旧觉得饥渴难耐。

言归正传了：

颜色传感器需要的颗粒

把颜色传感器安装到电机上

非常重要的一步！因为传感器的杆很长，需要一根橡皮筋来避免晃动。知足吧，这一根皮筋是我被折磨了一星期之后才想到的...

超声波测距传感器

安装在最高点，它的作用是判断转台上有没有魔方

亮度传感器

组装成一个奇怪的样子

安装的目标是这里

安装之后的效果，注意看上面的底盘，贴了一圈白纸，原因在软件部分会介绍

距离上需要留一个2到3毫米的间隙

固定NXT主体

从另一个角度看看怎么固定

最后的一些工作：接线，贴提示的彩条，把剩余的散件用来加固，还有就是化化妆

接线方式说明：

电机A：爪子
电机B：魔方的旋转底盘
电机C：颜色传感器电机

传感器1：超声波测距传感器
传感器2：亮度传感器
传感器3：颜色传感器
传感器4：按钮（这个是我调试的时候用来中断的）

这次不说废话了，直接上图，节约时间：

一大堆散件

组装起来

横杆加固，另外点缀些颜色

一组连杆，注意蓝色的那根需要磨得薄一点

组装好的连杆

与悬臂的连接

大体框架已经搭成了！

加个小人点缀一下