nao机器人学习笔记3
一、闭环控制
从内存中取得 ”Device/SubDeviceList/LFoot/FSR/CenterofPressure/X/Sensor/Value” 的值，这个值也就是左脚底中心压力的值的X方向，中心压力值，X方向是向前的
Y方向是左右的的。在自己的模块中申明如下的变量AL::ALPtr
void ClimbStair::keepBalance()
{
enableStiffness();
AL::ALValue LAnkleRoll = "LAnkleRoll";
AL::ALValue LAnkleRollAngle = 0.0f;
float fractionMaxSpeed = 0.1f;
std::string pressureX = std::string("Device/SubDeviceList/LFoot/FSR/CenterOfPressure/X/Sensor/Value");
std::string pressureY = std::string("Device/SubDeviceList/LFoot/FSR/CenterOfPressure/Y/Sensor/Value");
// enter the loop
while (true)
{
AL::ALValue x = mMemory->getData(pressureX);
AL::ALValue y = mMemory->getData(pressureY);
LAnkleRollAngle = computeAngleFromPressure(x, y);
mMotion->changeAngles(LAnkleRoll, LAnkleRollAngle, fractionMaxSpeed);
}
}
AL::ALValue ClimbStair::computeAngleFromPressure(AL::ALValue x, AL::ALValue y)
{
return y;
}
这其中CenterOfPressure是一只脚上受力的位置，“CenterOfPressure”的值计算每只脚上FRS的重心（使用各自的重量和位置）X和Y的返回值以m为单位，来自足部参数。请参见“硬件”说明书。使用“centerOfPressure”和来自两只脚的重量，可以计算机器人上所有重量的重心。
操作步骤：
利用目录：
D:\naoqi\Software\Windows\vs2008\naoqi-sdk-1.10.10-windows-vs2008\naoqi-sdk-1.10.10-windows-vs2008\modules\src
下的module_generator.py产生自己的module,输入相关信息后，产生一个模块。然后利用cmake交叉编译（选择为远程执行模块），设置启动项，调试IP设置。然后添加自己的成员变量，成员函数，将要调用的成员函数绑定到模块上。 在Choregraphe中添加指令盒—编辑指令盒：
添加以下指令：
class MyClass(GeneratedClass):
def __init__(self):
GeneratedClass.__init__(self)
def onLoad(self):
#~ puts code for box initialization here
self.proxy = ALProxy("ClimbStair ")#这里添加的是自己的module名
pass
def onUnload(self):
#~ puts code for box cleanup here
pass
def onInput_onStart(self):
#~ self.onStopped() #~ activate output of the box
self.proxy.start()#调用module函数
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
二、利用API单目测距
订阅ALRedBallDetection事件后可以在Memory里变量名 ”redBallDetected” 里得到关于红球目标的信息包括红球的夹角，红球在图像中的距离中心位置的偏角。这些信息里包括时间、球的偏角夹角、摄像机在NAO_SPACE中的位置。（NAO_SPACE的原点在双脚的中心位置）如何订阅事件：
mMemory->subscribeToEvent ("redBallDetected", getName(), "redBallDetectedCallback");
参数1：事件名；参数二：模块名；参数三回调函数，每当检测到红球就去调用redBallDetectedCallback（）这个函数。
Tag time_info = [timestamp_seconds, timestamp_microseconds]
The time Stamp when image was take.
Tag ball_info = [u, v, centerX, centerY, radius, score]
u and v are the average u and v components in the YUV color
spacecenterX and centerY are the angular coordinates in camera angles.
radius is the ball radius in pixel and score is 1.0 if red ball detected and 0.0 else.
Tag camera_info = [x, y, z, wx, wy, wz] in NAO_SPACE (see motion documentation)
Subscribe to this module with a minimum of 60ms period.
利用如下代码就可以获得球距离nao的距离：
AL::ALValue redBallData = mMemory->getData("redBallDetected");
AL::ALValue ballInfo = (*(redBallData.getArrayPtr()))[1];
const float BALL_RADIUS = 0.03f;
float solidAngle = (float)((*(ballInfo.getArrayPtr()))[2]);
solidAngle /= 2.0f;
float distance = BALL_RADIUS / sinf(solidAngle);
已知半径已知夹角可以求出摄像头到球的距离，在通过三角形定理可以求出红球到nao的距离。下图是上面计算的一个简单表示
三、全身控制
开启了全身控制后，在做动作的过程中，nao会试着自己保持平衡。将上面所求的球的距离作为一个目标点，控制右手臂到达目标点。在这里wbSetEffectorControl（）函数就隐含的开启了全身控制。
float angleX = (float)((*(ballInfo.getArrayPtr()))[0]);
float angleY = (float)((*(ballInfo.getArrayPtr()))[1]);
float x = BALL_RADIUS / tanf(angleX);
float y = BALL_RADIUS / tanf(angleY);
std::cout << x << " " << y << std::endl;
std::string effectorName = "RArm";
AL::ALValue targetCoordinate = AL::ALValue::array(distance, x, 0.5f);
mMotion->wbSetEffectorControl(effectorName, targetCoordinate);