解魔方的机器人攻略25 – 解魔方
由 动力老男孩 发表于 2010/04/18 17:42:03现在我们的工作已经接近尾声了,看看怎么把电脑变成一个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,暂停功能,帮助我们进行调试