前言
很久以前就知道LEGO-NXT可以解魔方，但网上最经典的解决方法需要用到RGB模块，这个模块在机器人套装中没有，单独购买还挺贵。除此之外，也能见到有些人用手机来识别魔方的颜色，但由于自己当时编程能力有限，也就没有去做。
在上一篇博文中，我学会了如何使用Python来控制NXT。有了Python之后，就感觉此事，有戏。因为有了Python作为桥梁，大多数问题都可以同过import来解决😂。
进入正文之前，首先看一下最终效果
这篇文章旨在向同样想使用NXT解魔方的人提供一些思路和方法，不会特别全面。
如果大家遇到了其他的问题或想要知道更多的细节，欢迎留言告知。
准备工作
NXT机器人
机器人最大的作用是拧魔方，还有一个附带的作用是展示魔方的每一面，以方便识别颜色。
我直接使用了MindCuber for LEGO MINDSTORMS NXT 2.0的设计，这个设计十分巧妙且经典，用两个电机就能完成魔方的所有操作。不过，由于我们并不打算使用RGB感应器来识别颜色，因此右侧那一坨（最右侧的电机及以上）不需要安装。如果像我一样比较懒，超声波部分也不需要安装。
我拥有的是LEGO 9797+9695，这里面有的部件数量不满足MindCuber for LEGO MINDSTORMS NXT 2.0的要求，所有拼的时候灵活一些，遇见不够了就拿别的代替就好了。
提示：尽量多代替底部支架部分，少代替顶部的机械臂。
手机
手机需要安装一个叫IP Webcam的APP，免费的版本就可以。
打开软件后滑到最底部点击start server，则软件开始运行，屏幕下方会显示服务器的地址即端口号，例如我的是192.168.0.101:8080，此时，使用电脑（同一个子网，或者说路由器内）浏览器访问网址
地址:端口号/shot.jpg
（例如192.168.0.101:8080/shot.jpg）
如果显示出了手机及时拍摄的照片，则说明手机部分没有问题。
Python
推荐使用Python2.7。如果使用其他版本，主要的阻碍在nxt-python，如果其能够正常工作，则其他的问题应该不难解决。下面是一些包
nxt-python
通过这个包控制NXT机器人。安装方法见使用python控制LEGO-NXT（Win10）。
urllib
通过这个包来访问手机上的IP WebCame，Python2.7已内置。
opencv && numpy
处理图像难免会用到的两个包。
opencv在这个网站找到想要的版本，下载whl文件，之后执行pip install xxx.whl即可。
numpy已经安装太久记不清，估计直接pip install numpy就行。
kociemba
这个包用来计算魔方的解法，可以不用或换别的。安装的时候一定要按照Github主页的教程来，特别提醒需要先安装Installation后半部分的build tools，再执行pip安装。
编程
如果前面的准备工作都已经完成，则你将完全可以使用不超过300行的代码，使NXT机器人成功还原魔方！
下面将分为3个部分给出编程的思路及提示。
NXT机器人
机器人部分的编程就是控制电机，拧魔方。
具体到MindCuber for LEGO MINDSTORMS NXT 2.0而言，就是控制两个电机。其中底部的电机比较简单，电机上和底座上的齿轮分别由12齿和36齿，也就是3：1的转速，因此如果想要魔方旋转90度，则需要电机（反方向）旋转270度。
我们可以使用如下的方式旋转
brick = nxt.locator.find_one_brick(name='NXT')
base = Motor(brick, PORT_A)
base.turn(50, 270)
这个写法会使PORT_A连接的电机以50的正向速度旋转270度。如果想要反向旋转，则应该把速度改为-50，而非角度改为-270。
不过，使用这个方法进行旋转并不是十分精确，可能会有几度的偏差，如果多旋转几次，偏差可能会越来越大。不过，NXT的电机还有读取角度的功能，这样，我们就可以不使用相对角度，转而使用绝对角度。即记录一个变量now_degree，表示电机应该处在的角度，然后通过它与电机实际读数的差值进行旋转。例如，第一次旋转270度，实际上转了275度，第二次又要转270度，经过计算，now_degree应该是540度，差值为265度，则这次只需要转265度就行了。这样子偏差始终都能在一个可控范围。
除了底部，还有一个控制机械臂的电机，这个电机主要在三个角度之间来回切换，仍然推荐使用绝对角度，角度值可以自己试出来。如果不知道机械臂的三种状态长什么样子，可以看MindCuber for LEGO MINDSTORMS NXT 2.0网站上的视频。
在基础的旋转搞定之后，推荐将机器人封装成五种（或以上）操作，这样后面的编程就不用再思考这里的细节了。
| 名称 | 具体操作 |
|---|---|
| 翻转 | 对应机械臂的两步操作 |
| 整体旋转（顺时针） | 机械臂抬起，底座旋转 |
| 整体旋转（逆时针） | 机械臂抬起，底座旋转 |
| 单面旋转（顺时针） | 机械臂放下，底座旋转 |
| 单面旋转（逆时针） | 机械臂放下，底座旋转 |
颜色获取
一些废话（可跳过）
书到用时方恨少，事非经过不知难
大家或许都看到过这样的图片，A和B区域的颜色其实是一样的。
第一次看到这样的图片还觉得，人类果然还是没有机器客观准确啊。
但是，这次问题是反过来的，如果你要识别这个棋盘的颜色，拍照的时候因为有个圆柱挡了点光，然后电脑硬说A和B颜色一样，你就会发现电脑愚钝死板了！
在识别魔方颜色上我就遇到了这样的问题，最容易出问题的就是橙色和红色，在太阳光下还好，如果在台灯下，就会经常出现“远处深红近处橙”的现象。
由于图像方面我不甚了解，最终也没有太好的解决方法，只能是写了一个自动重设颜色的函数：按顺序把魔方的六种颜色放在魔方的同一个面里，然后拍一张照片计算颜色，并保存。
正文
颜色获取的第一步是定位，魔方每一面都有九个方块，我们需要先找到这九个方块在哪里。
我的思路非常简单粗暴，虽然每次拍摄魔方的位置都有些许变化，但大致位置相同，因此直接把位置写成固定的，然后等间隔地选取九个小方块即可。如图
当然，此时我们不能完全相信小方块中的所有颜色，或者它们的均值，因为有些像素是不属于魔方的。
如果我们事先知道魔方的六种颜色的rgb值，则可以设置一个阈值$\epsilon$，然后对每一种颜色计算距离小于$\epsilon$的像素的个数，哪种颜色多就认为是哪种颜色，写成优化问题就是
说起来复杂，其实计算起来一句话就搞定，$\epsilon$大概设50左右，可以多试试调整，下面这句话，可以计算sub_window所有像素中与第m种颜色相近的个数。
np.sum(np.sum(np.power(sub_window - colors[m], 2), axis=2) < epsilon*epsilon)
至此，唯一的问题就变成了如何事先知道六种颜色的rgb值。由于我们这种方法非常受光线的影响，可能下午三点的rgb值到了下午五点就不好用了，因此我们需要一个快捷的获取六种rgb值的方法。
我的做法是把魔方的六种颜色按照固定顺序展示在同一面上，然后尽量摆正，让小方块里全是同一种颜色，然后写一个代码自动获取六种颜色的均值，并存储即可，如图
获取到的颜色为
可以看出，这次获取到的颜色已经与之前有不同了。接下来使用np.save将六种颜色保存起来即可。
魔方朝向
除了NXT机器人和颜色获取之外，再有值得一提的就是魔方朝向了。kociemba的输出是一个如下的字符串
F R' L' B' L' D' F U' D2 B2 R' U L2 U2 F2 B2 R2 B2 U2 B2 U'
六个子母分别代表六个面，正常是顺时针90度，带'是逆时针90度，带2是180度。
我们的机器人只能拧底下的面，因此如果需要拧别的面，就需要先把那一面挪到底下，这就要求机器人能够做到两件事：
- 时刻清楚每一面的位置
- 能够把任意一面挪到底下
对于第一件事，如果熟悉置换的概念，则不难实现，如果不熟悉，直接看下面的代码，应该也能理解
position = ['U', 'R', 'F', 'D', 'L', 'B'] # 初始朝向
position = [position[i] for i in [1, 3, 2, 4, 0, 5]] # 机械臂翻转魔方时调整朝向
至于URFDLB这个顺序，以及他们的具体意义，可以参考下图
|************|
|*U1**U2**U3*|
|************|
|*U4**U5**U6*|
|************|
|*U7**U8**U9*|
|************|
************|************|************|************
*L1**L2**L3*|*F1**F2**F3*|*R1**R2**R3*|*B1**B2**B3*
************|************|************|************
*L4**L5**L6*|*F4**F5**F6*|*R4**R5**R6*|*B4**B5**B6*
************|************|************|************
*L7**L8**L9*|*F7**F8**F9*|*R7**R8**R9*|*B7**B8**B9*
************|************|************|************
|************|
|*D1**D2**D3*|
|************|
|*D4**D5**D6*|
|************|
|*D7**D8**D9*|
|************|
详情在kociemba的主页上。
对于第二件事，想要把某个面挪到底下，只要机器人能够知道这个面现在在哪，则挪过去的动作可以提前写好，一共才六种情况。
示例代码
motors.py是里面实现了控制机器人所需的方法，目前托管在GitHub Gist上。
我没有将所有代码直接放在一个github仓库内，是因为我的这份代码别人直接拿来是一定用不了的，尤其是图像的部分。当然，如果有人想要参考的话，也欢迎留言告诉我。