1、行走时利用超声波进行避障
这些都是在choregraphe中完成的,怎样修改指令盒达到自己的要求:创建一个指令盒:
class MyClass(GeneratedClass):
def __init__(self):
GeneratedClass.__init__(self)
def onLoad(self):
#~ puts code for box initialization here
pass
def onUnload(self):
#~ puts code for box cleanup here
pass
def onInput_onStart(self):
self.myfun()
self.onStopped() #~ activate output of the box
pass
def onInput_onStop(self):
self.onUnload() #~ it is recommanded to call onUnload of this box in a onStop method, as the code written in onUnload is used to stop the box as well
pass
def myfun()
self.logger.info("++++++++++++++++++++++++")
指令盒运行过程:
- 指令盒实例化
- 将指令盒注册为模块,这样它就可以通过链接,从其它指令盒里调用。
- 指令盒初始化
前两步是自动的,不受控制,默认生成的类的方法:
initialize(初始化)
onLoad
onUnload
onStart
onStop
initialize():执行行为时,自动执行初始话参数
onLaod():当加载指令盒流程图时,会调用这个方法。在当前行为的所有指令盒的__init__方法之后,有必要调用这个方法。加载流程图时,在任何输入/输出点被激活前,这一方法会被调用至当前一级的每个指令盒。
onInput_onStart():当onStart被激活时,会调用该方法。在onInput_onStart()中调用self.onStopped(),会激活onStoped()输出点。
onInput_onStop():当onStop被激活时,会调用该方法。
可以定义自己的函数并在合适的位置进行调用,例如上面的mufun(self)函数。
利用超声波进行避障:
原理简述:在内存中订阅关于声纳的探测的结果:包括 SonarLeftNothingDetected,SonarRightNothingDetected, SonarRightNothingDetected, SonarRightDetected,后面两个返回的是左侧和右侧探测到的障碍物的距离,通过添加一个指令盒来控制与障碍物的距离满足多少时才发出指令进行避障。
下面的指令盒相当一个过滤器,当距离小于设定值时,整个指令盒才会发送指令去避让(编辑指令盒选项中可以添加自己的参数),而参数的调用形式如下:
class MyClass(GeneratedClass): def __init__(self): GeneratedClass.__init__(self) def onLoad(self): #~ puts code for box initialization here pass def onUnload(self): #~ puts code for box cleanup here pass def onInput_onStart(self): #~ self.onStopped() #~ activate output of the box pass def onInput_onStop(self): self.onUnload() #~ it is recommanded to call onUnload of this box in a onStop method, as the code written in onUnload is used to stop the box as well pass def onInput_left(self, p): if(p<self.getParameter("leftDistance")): self.onStopped() pass def onInput_right(self, p): if(p<self.getParameter("rightDistance")): self.onStopped() pass
2、探究Walk Tracker 运行原理以实现红球、蓝球追踪。
所以如果我们要自己实现这一个功能的话,应该用C++编写一个module计算目标在NAO_SPACE的空间坐标,其余的工作可以放在Choregraphe的脚本语言里实现。
首先看下这个WalkToTarget里的一个函数返回值的意义:
自己创建并编写一个module进行测试
void ALMyMotion::walkMotion()
{
AL::ALValue x = 1.0f;
AL::ALValue y = 0.0f;
AL::ALValue frequency = 1.0f;
AL::ALValue theta = 0.0f;
motionProxy->setWalkTargetVelocity(x, y, theta, frequency);
}
测试结果:setWalkTargetVelcocity(x,y,thera,frequency),x:沿x 方向步伐值的大小;y:沿Y方向步伐值的大小;theta:转向幅度大小;frequency:行走频率。
这个API 的函数是设置行走的频率,步伐的大小,还有转角的大小;
通过getRobotPosition()和getRobotvelocity()两个函数可以返回出当前机器人的所处角度和位置,再利用红球与机器人的偏角就可以控制机器人走向红球。
对于getRobotPostion()返回的值(absoult position x, absoult position y, absoult angle z )
其中x表示的意义是其从开始运行程序开始,机器人沿x轴前进的距离,这里默认世界坐标系的原点在程序开始运行机器人所处的位置。(nao 双脚中间位置)如果这次停止,下次接着再运行,那么这个值会在原先X值的基础上继续累加;Y表示机器人沿Y轴行走的距离。Z表示机器人绕Z轴旋转的角度,假设nao绕Z轴一直转动(顺时针表示正,逆时针表示负?)那么这个值的变化规律为0.1,0.2,0.3….2.1….3.14….-0.1,-0.2…..-3.0….-3.14
getRobotvelocity()返回的是机器人沿X、Y轴移动的速率,和绕Z轴旋转的速率。
实验一、
向前行走30步后,倒退30步
getRobotPosition()返回x值的变化趋势为:
x : 0.006, 0.027, 0.048, 0.069……..0.55, 0.57---------0.55, 0.53, 0.51……….0.11, 0.09, 0.03
变化趋势: 小------------大------------小
结论:nao可以识别出前后左右的方向性,并且可以获取到nao行走的距离和方位。
实验二、
Nao原地转圈
getRobotPostion()返回的Z值的变化趋势:
Z :0.1, 0.2234, 0.31, 0.4…………3.025, 3.104 --------- -3.103, -3.034, -2.96……..-0.1, -0.03
Z值变化趋势: 正 小------------------------大 (负) 小-----------------------大 正
总的运行原理:
Tracker指令盒负责检测红球的位置并且将检测到的结果传递给WalkToTarget指令盒,并且Tracker指令盒实现了头部对红球的追踪,头部会随着红球位置的改变而改变。在WalkToTarget中,通过实时的检测自身的位置(通过getRobotPostion()函数获得)与红球到自身距离的进行比较,通过设置setWalkTargetVelocity()让机器人朝着目标红球前进。简而言之,机器人会一直行走(通过设定速度和方向)知道红球到机器人的距离满足一个条件停止,而距离呢,就是通过前面的Tracker指令盒一直在向WalkToTarget传递。