做做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的发声装置就是盒子上那个小喇叭，因为喇叭质量不是很高，所以别太指望萝卜头能演奏世界名曲。从开发角度来说，NXT内置了很多种发声的方式，详细内容可以参考leJOS中文教程 – 播放声音。例如，我们用下面的代码就可以让NXT蜂鸣两声：

Sound.twoBeeps();

NXT还可以直接播放8位的wav文件，播放命令是：

Sound.playSample(new File("Start.wav"));

下面介绍一下如何制作这样的wav文件。

首先要录制声音，用手头的任意录音工具都可以，例如手机，mp3，其实电脑本身也可以录音。萝卜头一共需要三句台词：“开始”、“结束”和“出错啦”。可以一次录完，把三段声音录在一起，每句台词之间留一点停顿，后期容易裁剪。

录好音之后，一般会生成*.wav或者*.mp3的文件。这种直接录好的文件一般都是双声道，而且采样频率比较高。这种高级音频萝卜头是没法识别的，要适当压缩成萝卜头可以处理的格式。

强烈推荐一款叫“CoolEdit”的音频处理工具，打开录好的音频文件，可以看到有声音的部分是波形图，中间停顿的部分几乎是平的直线。可以用鼠标拖动选择相应的音频，然后在菜单中选择“选中部分另存为..”，保存的格式选择“Wave PCM unsigned 8 bit,8000HZ,64kbps,单声道”，就可以生成相应的wav文件了。最后可以生成以下三个文件：

Start.wav
End.wav
Error.wav

下面把这几个文件下载到NXT中，在命令行中运行：E:\lejos_nxj\bin\nxjbrowse.bat。如果萝卜头开机的话，这个工具的“download”功能，依次把三个文件都保存到NXT中。

最后在程序的相应地方加入播放代码即可，上一篇中已经看到了出错信息的播放，“开始”和“结束”的播放程序为：

if(hasCube && isChaotic)
{
	//The cube is read, init the error status
	hasError = false;

	//Play some sound to notice the "Start"
	Thread.sleep(1000);
	Sound.twoBeeps();
	Thread.sleep(1000);
	Sound.playSample(new File("Start.wav"));

	//Ignore solve cube codes......

	if(!hasError)
	{
		//The cube has been solved
		isChaotic = false;

		Sound.playSample(new File("End.wav"));
		Thread.sleep(1000);

		//Rotate the cube two circles for annoucement
		Robot.RotateBottom(8);
	}
}

萝卜头主要功能的实现都已经介绍过了，接下来是一些优化和美化的工作。曾经有人留言说萝卜头的眼睛完全没有用，只是用来做装饰的。其实乐高积木的乐趣在于功能的拼装，每一处设计都可以有它的作用。今天就介绍一下这个大眼睛的作用。

最初的第一个版本是没有大眼睛的，那时候魔方必须一开始就放在转台上，萝卜头埋头一路转到完，然后就退出程序。这就带来了很多问题：
1. 必须一开始就放好魔方。如果没有魔方，萝卜头依然会傻乎乎的执行读颜色的操作（读出来全是黑色），然后出错退出程序
2. 有时候魔方卡住了，会掉下转台，萝卜头依然孜孜不倦的把所有步骤执行完。
3. 操作不方便，给观众表演时，需要：打乱魔方->放上转台->启动程序->解魔方->停止程序->取下魔方，实在是太不智能了。

其中第2点最让人无法接受，想起一个笑话：两个人在路边干活，一个人挖坑，另一个人填土。有人问，你们这是在干什么啊？他们说，我们平时是三个人种树，今天负责放树苗的兄弟生病没来。要不怎么说，眼睛是心灵的窗户。我特地加上了这个超声波的眼睛，主要功能包括：
1. 蓝牙连接成功后，判断有没有魔方，如果有则开始读颜色并解魔方，如果没有则进入等待状态
2. 中间过程，如果魔方掉下转台，则报告错误，并停止当前程序
3. 解好魔方以后，进入另外一个等待循环，如果有人把魔方拿开再放回来，则认为这时候的魔方又被打乱了，重新启动解魔方程序。

其实判断部分非常简单，魔方在正常位置时，大眼睛的读数应该在14cm左右。

眼睛到魔方的距离

为了避免误差，当距离读数在12~16之间时，我就认为转台上有魔方。另外，为了避免偶尔的数据跳动，我认为连续十次测量结果都相同的情况下，才是距离“稳定”的状态。是下面看看代码：

while(!Button.ESCAPE.isPressed())
{
	//Wait for the distance being in the correct range: 12~16
	int CheckStatusTimes=0;
	LCD.clear();
	boolean previousStatus = true;
	boolean currentStatus = true;
	while(CheckStatusTimes++ < 10)
	{
		int n = distance.getDistance();
		LCD.drawString("distance=" + n + "   ", 0, 0);
		currentStatus = (n>=12 && n<=16);
		if(currentStatus != previousStatus)
		{
			CheckStatusTimes = 0;
			previousStatus = currentStatus;
		}
		Thread.sleep(100);
	}
	hasCube = currentStatus;
	if(!hasCube)
	{
		//if the cube is take away, we consume it is been upset
		isChaotic = true;
	}
	if(hasCube && isChaotic)
	{
		//这里是解魔方的部分
		isChaotic = false;
	}
}

上面的代码中，hasCube表示“魔方是否在转台上”，isChaotic表示“魔方是否处于打乱状态”。
如果魔方被拿走，就认为再放回来时已经被打乱了。

在旋转的过程中，每一步操作之前，都需要这样判断一下魔方位置，只要检测到距离异常，就立刻中止程序。为了便于管理，可以定义一个hasError的全局静态变量，并把判断部分封装成一个函数。

static boolean hasError = false;

//check if the cube is still on the base
public static boolean CheckCubeReady() throws Exception
{
	//if already error, return directly to avoid play *.wav again
	if(hasError) return false;

	int d = distance.getDistance();
	int errorCount = 0;
	while((d<12 || d>16) && errorCount < 10)
	{
		errorCount++;
		Thread.sleep(20);
	}
	if(errorCount >= 10)
	{
		//The cube is break out;
		hasError = true;
		Sound.playSample(new File("Error.wav"));
	}
	return !hasError;
}

这里的逻辑和上面那段代码的逻辑稍有差别，主要是如果hasError已经是true，表示魔方已经不再转台上，那么直接返回错误，不再进行后续的判断。另外把sleep的时间从100毫秒变成20毫秒。这样改的原因是用手把魔方放上转台时，可能会使用比较多的时间（1秒），而萝卜头如果把魔方推到转台外面，这个在200毫秒内应该是足够稳定下来了。
最后把所有具体的操作，全部添加这个判断函数：

//原来：
RotatePaw();
//现在：
if(CheckCubeReady()) RotatePaw();

大家可能会注意到，在出错的判断中，有一句：

Sound.playSample(new File("Error.wav"));

这行代码执行时会有一个甜美的声音说:“出错啦~~”（其实那是我家娘子的录音）预告一下，在下一篇里给大家介绍如何让萝卜头开口说话。

现在我们的工作已经接近尾声了，看看怎么把电脑变成一个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
下一篇继续介绍这种分辨方式的具体代码实现。

前面提到了分辨颜色的三部曲，今天给大家介绍一下NXT和电脑之间的蓝牙通讯。其中在NXT端使用的是Lejos自带的Bluetooth类，在PC端使用的开发工具是VS2008，使用的语言是c#。

有些人鄙视这种连接PC的做法，在他们的眼里，连接了PC以后，乐高就变成了一个遥控玩具。其实对编程开发来说，用Java还是用c#并没有本质的区别。魔方的算法也可以写成Java的版本，无奈的是NXT的内存不足，只能把这种体力活交给电脑了。

1. 蓝牙配对

正所谓千里姻缘一线牵，首先我们要给NXT和PC安排一个相亲大会。NXT已经内置了蓝牙模块，要把它设置成打开并且可见的状态。设置方法请看Lejos的中文教程“蓝牙菜单”。现在很多笔记本也自带了蓝牙模块，如果没有的话，必须买一个蓝牙适配器。注意WinXP开始就都已经自带蓝牙驱动了，如果你的电脑安装了第三方的蓝牙驱动，最好先删除。

蓝牙适配器

准备好定情信物以后，就该安排PC和NXT见面了。PC比较主动，由他开负责寻找：

控制面板中旋转“添加新的蓝牙设备”，可以找到当前可见的NXT

找到NXT后，两人会羞答答的先来个握手协议，接下来是交换电话号码。Lejos设置的蓝牙连接密码是1234。

输入蓝牙连接密码

你看他们一个是能力超强，名车豪宅，另一个能歌善舞，秀色可餐。简直就是一拍即合啊。到此牵线完毕，以后他们就可以直接通讯了。我们查看一下电脑上的NXT属性，可以看到有个带“DevB”的端口，这个相当于是他们之间的私人电话，记下来后面会用到。

注意看端口号

2. C#中使用蓝牙通讯

其实配对以后，蓝牙就被模拟成了一个端口，我们可以用最简单的端口通讯来收发信息。首先，在每次启动时，需要连接端口：

BluetoothConnection = new SerialPort();
ConnectButton.Enabled = false;
BluetoothConnection.PortName = PortList.SelectedItem.ToString();
BluetoothConnection.Open();
BluetoothConnection.ReadTimeout = 10000;
BluetoothConnection.DataReceived += new SerialDataReceivedEventHandler(BlueToothDataReceived);

然后可以通过这个端口来发送信息。需要注意的是，在发送的原始数据之前，需要添加两个表示长度的字节，Byte[0]+Byte[1]*255=length。所以发送数据的函数如下：

private void BlueToothDataSend(byte[] data)
{
    int length = data.Length;
    byte[] readData = new byte[length + 2];
    readData[0] = (byte)(length % 255);
    readData[1] = (byte)(length / 255);
    for (int i = 0; i < length; i++)
    {
        readData[i + 2] = data[i];
    }
    BluetoothConnection.Write(readData, 0, length + 2);
    Status = "发送数据字节数：" + length;
}

收到数据的时候，也是类似的情况，头两个字节表示了数据的长度，然后才是真正的数据内容：

private void BlueToothDataReceived(object o, SerialDataReceivedEventArgs e)
{
    int length = BluetoothConnection.ReadByte();
    length += BluetoothConnection.ReadByte() * 256;

    byte[] data = new byte[length];
    BluetoothConnection.Read(data, 0, length);
    for (int i = 0; i < length; i++)
    {
        BlueToothReceivedData += string.Format("data[{0}] = {1}\r\n", i, data[i]);
    }
}

断开蓝牙连接的命令如下：

BluetoothConnection.Close();
BluetoothConnection.Dispose();
BluetoothConnection = null;

3. Lejos中使用蓝牙通讯

在Lejos中使用蓝牙有几点区别：首先，Lejos中不支持收到消息的事件触发（我怀疑用多线程可以实现，不过对Java不太熟悉，没有调试成功）所以在需要接受PC信息时，只能挂起等候消息传来；其次，虽然PC发来的信息头两个字节表示长度，但是Lejos接收时，是从第三个字节开始显示的；另外，Lejos发送蓝牙信息时，不需要添加那两个字节的长度信息。

下面是建立蓝牙连接的方式：

public static void Connect() throws Exception
{
	LCD.clear();
	LCD.drawString("Waiting BTC...",0,0);
	btc = Bluetooth.waitForConnection();
	LCD.drawString("Connected",0,2);
	LCD.refresh();
	dis = btc.openDataInputStream();
	dos = btc.openDataOutputStream();
}

接受蓝牙信息：

public static byte[] ReadBytes() throws Exception
{
  byte[] buffer = new byte[255];
  int length = btc.read(buffer, buffer.length);
  if(length==-2)
  {
   //lost data, re-sync
   btc.read(null, 255);
   return new byte[0];
  }
  else
  {
   byte[] data = new byte[length];
   for(int i=0;i<length;i++)
   {
    data[i] = buffer[i];
   }
   return data;
  }
}

发送蓝牙信息

public static void WriteBytes(byte[] data) throws Exception
{
 for(int i=0;i<data.length;i++)
 {
  dos.writeByte(data[i]);
 }
 dos.flush();
}

关闭蓝牙连接

public static void Disconnect() throws Exception
{
   if(btc!=null)
   {
    WriteBytes(new byte[]{(byte)255,(byte)255,(byte)255});
    Thread.sleep(100);
    dos.close();
    dis.close();
    btc.close();
   }
}

4. 蓝牙通讯小实验

下面进行一个小实验，在PC上运行一个程序。
当发送1时，NXT初始化魔方底盘位置；
当发送2时，NXT初始化颜色传感器位置；
当发送3时，NXT读取颜色信息，并回传给电脑；
当发送其他数字时，NXT断开蓝牙连接，并退出程序

蓝牙连接通讯实验

大部分函数在前面都介绍过了，只需要在main函数中指定操作即可：

BlueTooth.Connect();
byte[] colorData = new byte[6];

while(true)
{
 byte[] readData = BlueTooth.ReadBytes();
 if(readData.length > 0)
 {
  int action = readData[0];
   switch(action)
  {
  case 1:
   Robot.FixBasePosition();
   break;
  case 2:
   Robot.FixColorSensorPosition();
   break;
  case 3:
      colorData[0] = (byte) color.getRed();
      colorData[1] = (byte) color.getGreen();
      colorData[2] = (byte) color.getBlue();
      colorData[3] = (byte) (color.getRawRed() / 3);
      colorData[4] = (byte) (color.getRawGreen() / 3);
      colorData[5] = (byte) (color.getRawBlue() / 3);
      BlueTooth.WriteBytes(colorData);
      break;
  default:
   BlueTooth.Disconnect();
   return;
  }
 }
 Thread.sleep(1000);
}

好了，其余部分自己看代码吧，搭车赠送一个生成三维魔方图形的小程序。点此查看运行在NXT中Java源代码代码；点此下载运行在电脑上的C#程序源代码。

之前已经介绍了萝卜头转魔方的各个分解动作，今天介绍如何用颜色传感器读取魔方的颜色。这一部分可以分成三部曲：
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键时，开始扫描魔方。相信看过前面文章的朋友，这里不需要说明了。点此查看具体的代码吧。

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

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

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

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

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

我原来的代码又多又乱还没有注释，自己看着都眼晕，找点代码晕的跟坐过山车似的。现在正在把它们重新整理优化，再加上注释。我打算整理一部分就发一部分攻略，攻略发完了也就整理完了。另外，这几天有很多网友正帮忙翻译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