解魔方的机器人攻略26 – “大眼睛”开关

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

最初的第一个版本是没有大眼睛的,那时候魔方必须一开始就放在转台上,萝卜头埋头一路转到完,然后就退出程序。这就带来了很多问题:
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"));

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



对 “解魔方的机器人攻略26 – “大眼睛”开关” 的 7 条 评论

  1. dead_lee 说:

    求error.wav…

  2. hml 说:

    看了你的博客 很有启发 我们也想做个解魔方的机器人 配套插件中没有颜色传感器 用摄像头识别颜色可以吗? 我们想用无线摄像头把图像传到视频采集卡上 再在pc机上处理

    • 当然可以,事实上现在大多数魔方机器人都改用摄像头了,识别速度更快
      还有用手机的,网上有个用N95解四阶魔方的视频

  3. iam3i 说:

    我正在制作你的魔方机器人的digital lego 模型。大力支持你!同时,能不能有空将你的做好的程序发给我,到870751632@163.com,我会尽快把文件发给你的~

  4. 休息的海 说:

    大哥,你真的好强,可以出本书了,肯定卖的很火。

  5. laowu 说:

    能干,今天才晓得。

发表评论

可以使用下列 XHTML 标签:<a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong>