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

上周夸下海口，说准备做一个两轮的平衡小车。当时的想法实在是非常肤浅：把手机捆在NXT身上，不断的向NXT发送当前的倾斜角度，NXT根据角度进行调整。当小车向前倾的时候，轮子就向前滚，小车向后倾时，轮子就向后滚。同时根据角度倾斜的大小来设置轮子的滚动速度。

按照这个思路开始搭建，因为手头的乐高颗粒严重不够，只好从萝卜头身上割了点肉下来，即使这样也只能用小件拼大件，最后搭了个像怪物似的“两轮平衡小车”：

未成功的两轮平衡小车

细心的同学可能发现小车的背后有个亮度传感器，这个是后话，等会儿解释。最初我的想法是手机通过蓝牙和NXT连接并控制电机运动。HTC手机控制NXT的代码如下（其中Timer设置的时间间隔是50ms）：

private Nxt nxt;
private void ConnectButton_Click(object sender, EventArgs e)
{
    try
    {
        nxt = new Nxt();
        nxt.Connect("COM9");
    }
    catch (Exception)
    {
        nxt.Dispose();
        nxt = null;
    }
}

private void nxtTimer_Tick(object sender, EventArgs e)
{
    if (nxt != null)
    {
        try
        {
            GVector gvector = mySensor.GetGVector();
            //1.75 is the fix number for center line
            double gz = gvector.Z - 1.75;

            sbyte power = Convert.ToSByte(-8 * gz);

            nxt.SetOutputState(MotorPort.PortB, power, MotorModes.Regulated,
                MotorRegulationMode.Speed, 0, MotorRunState.Running, 0);
        }
        catch (Exception) { }
    }
}

冒着被丈母娘训斥的危险折腾到半夜，终于完工了。一般来说，工作的完成意味着悲剧的开始：这个小车反应非常迟钝，先躺到地上，然后轮子才开始动，就像个耍赖的小P孩。我试过把50ms的Timer调的更小，但是这时候手机已经处理不过来，反而更慢了。

本来想把这个失败的作品马上拆了（萝卜头的胳膊还处于脱臼状态），但是有点儿不甘心。于是第二天又想了个“好办法”：也许是蓝牙传输太慢，我能不能根据倾斜角度来改变屏幕的颜色，然后NXT用亮度传感器通过读屏幕颜色来控制电机呢？于是脱臼的萝卜头又被拆掉了一个传感器，凑成了这样一个小车：

背着手机的小车

用来改变屏幕颜色的代码如下：

private void UpdateSpeed()
{
    GVector gvector = mySensor.GetGVector();
    int gray = Convert.ToInt32(256 * (gvector.Z + 9.8) / 19.6);
    if (gray < 0) gray = 0;
    if (gray > 255) gray = 255;

    this.BackColor = Color.FromArgb(gray, gray, gray);
}

再次折腾到半夜。事实证明，变色的反应速度比蓝牙要快一点，但还是不足以让小车保持平衡。小车吱吱嘎嘎前后晃动几下，就倒在了地上。这个平衡小车的尝试到此宣告失败。失败的原因总结如下：
1，小车重心太高，以至于倒下的速度太快，这个因为零件太少实在没办法
2，数据反应速度太慢，经我测试，WM手机+C#的最快反应速度只能到50ms，对于平衡这样的工作来说实在是太漫长了

搭车再提供一项悲剧，我用了两年的笔记本坏掉了，虽然还能凑合用，但是显卡貌似没有绿色了。没错，这个就是315曝光的HP笔记本，从症状判断应该是显卡损坏，坏掉的时候正好过了保修期几天…..

昨晚拆性大发，把这个烂笔记本拆了个七零八落：

臭名昭著的HP Presario笔记本

拆完发现主板的显卡是集成的，没啥好玩的，只好又装回去。还好，重新装好后只剩下两颗螺丝，开机居然还可以使用，创下我破坏史上的最低纪录。

这就是一周总结了，奋战了三个晚上，没做出啥成果，只能供大家娱乐一下了。后来在网上搜了一下，还真有NXT的两轮平衡小车，用亮度传感器+两个电机，有兴趣的同学自己去研究一下吧：

http://www.nxtprograms.com/segway/index.html

NXT两轮平衡小车

上次提到了“甜美的声音”，今天介绍一下如何让萝卜头发出声音。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);
	}
}

经常有朋友向我要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#程序源代码。

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

//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

感谢网友bigapple的建议，目前正在征集志愿者翻译Lejos NXT的入门教程，参与翻译的朋友：bigapple , 弹力女超人, zxzxy1988,lifanxi, YODA。

翻译后的页面在这里。

另外还有一个需要感谢的朋友是程序猎人，他已经翻译了很多篇，可惜博客暂时无法访问了。

有兴趣的朋友也可以点这里报名一起参加翻译，谢谢

在远古时代，程序员们通常用写字板来编写Java程序，然后用Javac.exe和Java.exe来编译和执行。对于NXT来说，对应的命令是Nxjc和Nxj。写字板的好处是速度飞快，不用安装。据说直到现在还有一些固执的代码狂人会用写字板开发软件，顺便用CPU来爆玉米花。但是对于大多数开发人员来说，选一个好用的IDE（Integrated Development Environment）是非常重要的。

IDE就是传说中的开发环境，比如我们常用的VS2008，Eclipse等等。它可以帮助你记忆类名和函数名，减少代码的输入量，避免拼写错误，高亮显示不同的代码段，还可以中断和调试。
这里大力推荐的NXT开发环境是IBM的Eclipse：

Eclipse开发环境

安装Eclipse：
1，从www.eclipse.org下载最新版的Eclipse，我用的是3.4版本，可能有点老了
2，Eclipse是不需要安装的，直接把所有文件解压到一个目录。注意这个目录最好不要包含空格，而且安装之后最好不要随意移动
3，双击eclipse.exe就可以运行了，最好在桌面上创建一个快捷方式，比较方便
4，第一次运行Eclipse的时候，会有一些教程信息，有兴趣的可以看看

为Lejos配置Eclipse：
1，创建一个新的工程。选择File > New > Project打开下图所示的新工程选项窗口：

新建工程

选择Java Project并单击Next

2，输入你的工程名称，注意这里只能是英文。Eclipse会用这个名字创建一个新的目录：

输入工程名字

3，设置ClassPath
单击菜单中的 Project > Properties。在左侧选择“Java Build Path”，然后在右侧选择“Libraries”
这时候点击“Add External JARs…”，打开之前安装Lejos的目录，选中classes.jar文件。设置完成的结果如下图：

设置Class Path

4，接下来我们在Eclipse里面加上几个按钮，帮助我们编译和下载代码
选择菜单中的“Run > External Tools > External Tools Configuations”
先点一下“Program”，然后单击左上角的“New Launch Configuration”创建新的外部工具

添加外部工具

工具1：编译工具（NXJ compile tool)
location -> D:\lejos_nxj\bin\nxjc.bat (请换成自己的目录)
Working Directory -> ${project_loc}
Arguments-> ${java_type_name}.java

工具2：下载工具（Download To NXJ）
location -> D:\lejos_nxj\bin\nxj.bat
Working Directory -> ${project_loc}
Arguments-> ${java_type_name}

工具3：查看工具（NXT Explorer)
location -> D:\lejos_nxj\bin\nxjbrowse.bat
Working Directory -> D:\lejos_nxj\bin
Arguments-> 空的

5，现在把这三个工具添加到工具栏
点击工具栏中向下的箭头，选择“Organize Favorites”。在打开的窗口中，把刚才添加的三个工具全部加进来。

添加快捷操作按钮

6. 验证Eclipse环境搭建是否成功：
在新建的工程中添加一个MyFirstNxtProject.java文件，然后输入以下代码：

import lejos.nxt.*;
public class MyFirstNxtProject {
    public static void main (String[] arg)
        throws InterruptedException
    {
        do
        {
            String s = "test string";
            LCD.clear();
            LCD.drawInt( (int)(Runtime.getRuntime().freeMemory()),0,0);
            LCD.refresh();
            Thread.sleep(10);
        } while (true);
    }
}

完成后单击工具中的NXJ Compile进行编译，正常情况下不会出现任何错误，表示编译成功。
这时候打开NXT的电源，用USB线连接到电脑，听到“嘟”的一声，表示USB设备已经就绪。
这时候单击工具中的NXJ Download，把编译好的代码下载到NXT内部。
现在在NXT上操作，用按钮选中MyFirstNxtProject并执行，该程序会显示目前NXT可用的内存数。

第四步：安装Lejos

登陆 Lejos 主页点击 NXT 图片进入 Lejos 下载页面，点击 NXJ 的下载链接；

下载完成后将下载的压缩包解压到指定文件夹。这里注意你所指定的文件夹路径中不要包含空格；比如 C:\Program Files\legos 这个路径就不正确，因为文件夹“Program Files”中包含了一个空格。在使用 Java 开发的过程中空格总会引起这样或那样的问题，所以为了避免不必要的麻烦保存路径中一定不要包含任何空格。

接下来添加运行 Lejos 所需要的系统环境变量，变量添加的方法与添加 JDK 环境变量的方法相同，请参照第一步中的方法打开环境变量窗口进行设置。

添加环境变量： NXJ_HOME，变量值是 Lejos 的安装目录，即解压后 lejos_nxj 的全路径，完成后点击确定；

向环境变量 Path 的变量值后追加 ;%NXJ_HOME%\bin；

重新打开一个命令提示符窗口（原有的命令提示符窗口在设置环境变量以后必须重新打开才能生效）。输入nxj然后回车，用来验证Lejos是否已经安装成功。

验证Lejos安装是否成功

第五步：安装Libusb

Lejos 需要使用 Libusb 与 NXT 进行通讯。它的安装文件在 Lejos 的安装目录下 lejos_nxj\3rdparty\lib。

运行 Libusb 的安装程序 libusb-win32-filter-bin-0.1.12.1.exe ，点击 Install 开始安装。

这个程序安装以后会自动执行测试程序。

注意！！这个驱动程序在Vista或者Windows7下很可能导致电脑的USB端口全部失效，这种情况下只能卸载。

且慢，现在的鼠标和键盘可能都是USB的，怎么卸载呢？我上次遇到这个问题的时候，被折磨了一个上午，不断的萌生着重装系统或者是砸烂电脑的念头，最后解决方法很简单，借一个古老的PS2鼠标卸载搞定。

正确的装法是在xp兼容模式下安装：在exe文件上鼠标右键单击，选择属性，然后参考下图。我的电脑是英文版的系统，大家凑合对照一下：

在XP兼容模式下安装USB连接驱动

第六步：刷新NXT的Firmware注意：当你安装 Lejos 后 NXT 原来的标准系统将被覆盖，NXT 中所有的数据也将全部删除，所以开始安装前一定要备份好有用的数据。再注意：据也许可靠的小道消息说，NXT刷新Firmware的次数是有限的。有一个叫做LOCK BIT的数据位，每刷新一次Firmware这个值就会减一。最多刷新100次以后，这个位就会降到0，所以请不要没事刷着玩 ^_^如果想重新安装 NXT 默认的操作系统你可以使用 LEGO Mindstorms software 重新安装 LEGO 的标准系统（具体步骤请参照 Lego 玩具说明书）。 首先把NXT切换到固件上载模式（firmware upload mode），只有在这个模式下才能升级Firmware。切换方式是在开机状态下，用牙签或者曲别针持续按下 NXT 主机背面的重启按钮4秒钟以上。进入NXT的固件上载模式。

进入固件上载模式后，NXT的屏幕上什么也不显示，但是会连续发出微弱的滴答声；

这时候用 USB 线连接 NXT 和计算机，当计算机识别 NXT 的固件上载模式后（看USB图标提示），在“命令提示符”中输入 nxjflash 并回车；

跟刷手机完全不一样，NXT的升级固件非常神速，大概只需要几秒钟；

上载完成后 Lejos NXJ 会自动启动，出现欢迎界面与开始菜单；

在 Lejos 运行的过程中，包括正在运行程序时，同时按下桔色和灰色的按钮（Enter+Space）就可以立即将 NXT 关闭。如果要重新启动只要按下桔黄色的按钮。如果 NXT 突然死机的话，将电池全部取出后重新安上启动即可。

到此我们就完成了Lejos的安装！下面试着运行我们的第一个程序测试一下。

按下桔黄色按钮启动NXT电源，打开命令提示符窗口，切换目录到Lejos自带的样例目录并输入以下代码（请自行修改安装目录）：

CD D:\lejos_nxj\samples\Tune （设置程序路径）

d: （切换到指定盘符）

nxjc Tune.java（在电脑上编译程序）

nxj –r Tune（把编译好的程序写入NXT）

几秒钟后NXT会发出一声悦耳的声音，表示程序已经上载成功，你可以在NXT上选择执行这个程序。

如果操作成功的话，你会听到一组声音，并看到“Hello World”的字样。

经历了这么多繁琐的步骤，你的第一个程序终于顺利在 NXT 上运行成功啦