programming robots using tivipe · 5 robot programming 26 5.1 programming a state space routine ......

TiViPEVisual Programming

Programming Robots using TiViPE–A step by step approach using Aldebaran’s NAO robots

Technical Report: Draft version 0.2.0

Copyright c©TiViPE 2010-2011. All rights reserved.

Tino LourensTiViPE

Kanaaldijk ZW 115706 LD HelmondThe [email protected]

September 5, 2011

CONTENTS 2

Contents

1 Introduction 4

2 Setup of NAO robots 4

3 Running ready robot components 53.1 Running the video module . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53.2 Hello world in Robotics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63.3 A bit of Tai Chi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63.4 Textual language and available commands . . . . . . . . . . . . . . . . . . . . . . . . 11

3.4.1 LEDS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133.4.2 Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

3.5 Enable the sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163.6 Available sensor commands . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

3.6.1 Using states . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

4 Programming new modules 234.1 Setting up a C++ library . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 234.2 Adding content to the library . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 234.3 Embedding a routine call into TiViPE . . . . . . . . . . . . . . . . . . . . . . . . . . 24

5 Robot Programming 265.1 Programming a state space routine . . . . . . . . . . . . . . . . . . . . . . . . . . . . 265.2 HeadMenu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 275.3 Object detection and tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 285.4 Standing up and sitting down . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 295.5 Nao demonstration 2011 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

6 Exercises 326.1 Exercise 1: Replace walka by walkto command . . . . . . . . . . . . . . . . . . . . . 326.2 Exercise 2: Calibration of walk, walks, and walka . . . . . . . . . . . . . . . . . . . . 326.3 Exercise 3: Create a more advanced face tracking module . . . . . . . . . . . . . . . . 326.4 Exercise 4: Flexible state activation . . . . . . . . . . . . . . . . . . . . . . . . . . . 326.5 Exercise 5: Getting grip over active states . . . . . . . . . . . . . . . . . . . . . . . . 336.6 Advanced Exercise 6: Auditory processing . . . . . . . . . . . . . . . . . . . . . . . . 336.7 Exercise 7: User scenario construction . . . . . . . . . . . . . . . . . . . . . . . . . . 33

7 Advanced Robot Programming 337.1 Adding commands to the NAO robot . . . . . . . . . . . . . . . . . . . . . . . . . . . 337.2 Adding sensing elements to the NAO robot . . . . . . . . . . . . . . . . . . . . . . . 337.3 Enabling other features on the NAO robot . . . . . . . . . . . . . . . . . . . . . . . . 337.4 Adding other robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

A TVProbotics source code 34A.1 Project file src.pro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34A.2 Source code of TVProboticsdefs.h . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36A.3 Source code of TVPnaocommand.h . . . . . . . . . . . . . . . . . . . . . . . . . . . 37A.4 Source code of TVPnaocommand.cpp . . . . . . . . . . . . . . . . . . . . . . . . . . 44

CONTENTS 3

Abstract

The aim of TiViPE is to integrate different technologies in a seamless way using graphicalicons [1]. Due to these icons the user does not need to have in depth knowledge of the underlyinghardware architecture.

This report elaborates on controlling robots, NAO robots in particular, with the TiViPE envi-ronment. The report can be used as a step by step manual, starting to setup and use the software,get acquainted, with the textual robot command language [2], use of ready TiViPE modules tocontrol the robot, and will be provided substantial information on how to adapt, extend, and tocreate modules within TiViPE.

1 Introduction 4

1 Introduction

The aim of TiViPE is to integrate different technologies in a seamless way using graphical icons [1].Due to these icons the user does not need to have in depth knowledge of the underlying hardwarearchitecture.

This report is outlined as follows: Section 2 provides an introduction of setting up software forNAO robots using TiViPE. Section 3 provides some examples how to create a robot program, and setparameters to gain full control over the robot using ready TiViPE modules. Section 4 elaborates onhow to construct new modules by explaining how to set up a new project for a library, and how towrite new routines, useful for robot interaction. Section 5 provides insight in how to construct roboticsapplications by means of examples. Useful and necessary exercises are provided in Section 6. The lastsection provides some ideas on how to further extend the TVProbotics library.

2 Setup of NAO robots

The NAO software can either be installed from the provided CDROM or by logging in to Aldebaran’swebsite. The version used by TiViPE is 1.10.52, and is assumed to be available on your local computer.

The setup of TiViPE starts by downloading the windows installation program from www.tivipe.com.The windows installation program asks for the path where the NAO software has been installed. Inits subdirectories bin, include, lib, and other directories are found. In case of a unix operatingsystem the NAO path is provided in the $TVPHOME/vars file. Likewise on the windows OS its pathcan be found in %TVPHOME%\vars.bat and in case the path needs to be adjusted, it can be eitherby editing the file or using the installer a second time.

TiViPE provides a small package called naohome.tar, it contains files for grabbing imagesfrom the camera, and a socket server, to send data to your local machine. First connect the robot tothe internet and switch it on, after the robot has started, press its chest button briefly to obtain thenao-ip-address. Install naohome.tar as follows:

%TVPHOME%\vars.batpscp naohome.tar nao@[nao-ip-address]:plink nao@[nao-ip-address]tar xvf naohome.tarexit

The first time of logging on to the nao robot a key is generated, type y to accept this key. Try

plink -l nao -pw nao [nao-ip-address]

Your local machine should be granted access to the nao robot and be logged in. Logout by typing

exit

Start up the TiViPE environment and select File - Edit remote machines, and fill out arow for every NAO robot, as demonstrated by example in Figure 1 for a windows configuration. For adetailed explanation on setting up remote processes press the Help button in this window.

From the command line one should test, if one can login on to the remote machine by typing firstthe shell command followed by the host name. Once access is granted type exit to logout.

3 Running ready robot components 5

Figure 1: Editing hostnames.

Figure 2: Editing hostnames.

3 Running ready robot components

In this section we introduce step by step the available robot modules, and make the user familiar withboth TiViPE and available robot modules.

3.1 Running the video module

The installed files from naohome.tar contains a module called FrameGrabber. As a first exer-cise, please construct the network given in Figure 2.

By right clicking the modules toggle from gray, yellow, to red. Red denotes that it is a permanentand streaming process. Between these red colored modules a connection makes use of shared memorywhen the modules run on the local machine. The FrameGrabber module has to be adjusted since itshould run on the robot, hence change the left lower button to the desired IP address of the robot, andchange the output connection to a socket, which is indicated by a green color at the upper part of theoutput connection. By right clicking on the connection the color changes, red denotes file or sharedmemory, blue denotes socket and file output, and green denotes a socket, the installed socket serverswill take care of the communication between two modules.

3.2 Hello world in Robotics 6

Try to start the USBgrabber on the local machine, this will start grabbing images from yourlocal machine, and function only if there is a camera installed. There is multiple camera support, andwhen more than one cameras are available on the system, select the upper right square in the module,and change the device from 0 to 1 (or 2, 3, ... depending on the number of camera’s installed) in thewindow that has popped up.

Next try to start FrameGrabber, and try to change the settings, by clicking on the right upper(parameter) button in the FrameGrabber module. In case you have an NVIDIA CUDA capablevideo card, and have enabled this during the TiViPE installation, you can connect the GPU display tothe Framegrabber module, to make optimal use out of all the available hardware.

The FrameGrabber example provides full access to the video system of the robot and can becombined with one or more of the available image processing modules available within TiViPE.

3.2 Hello world in Robotics

About two decades ago programming hello world in C was as follows:

#include<stdio.h>

main (){

printf (‘‘Hello world!\n’’);}

Saving the text above to hello.c and depending on the used compiler typing gcc hello.c orcc hello.c on the command prompt resulted in a program that in turn could be executed by abinary called a.out. Later different more sophisticated hello world programs have become popular,for instance the one provided by Qt, that demonstrates a window with a waving hello flag.

Within TiViPE we would like to do a hello world program as well. It is performed in two steps:

1. lie the robot on its back, and let it get up

2. wave with hand and say hello

This program is illustrated in Figure 3, and should be self explanatory. CommandNaoStandUpFromBackproduces a string that performs the first step. CommandString that generates a string “say(Hello,50)” which means that the robot will produce the word ’Hello’ at a volume of 50 percent using a textto speech engine. CommandNaoHello1 perform hand waving. CommandParellel constructs astring where latter two commands are executed at the same time, while CommandSerial ensuresthat the robot will first get up before it starts talking and waving.

3.3 A bit of Tai Chi

Like the handwaving module, Aldebaran robotics has been providing a Tai-Chi module, within TiViPEthis module has been reproduced to yield a very similar behavior. When the robot is standing, theCommandNaoTaiChi is connected to the NaoRobot module, as illustrated in Figure 4. The com-mands can be exchanged easily since the commanding robot language is using pure text. In the C orC++ programming language performed by a so called string.

Tai chi is a single module (CommandNaoTaiChi), it gives a performance of 50 seconds. This isdone by producing a string that is as follows:

3.3 A bit of Tai Chi 7

Figure 3: Hello world example.

Figure 4: Tai chi module (in green) to replace hello world example.


[speed (183) &[movem (HeadYaw,

0.0, 1500, 0.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000,18.0, 1000, -17.0, 1000, -68.0, 1000, -16.0, 1000, 12.0, 1000,90.0, 1300, 12.0, 1300, 8.0, 1100, 0.0, 1000, -8.0, 1000,16.0, 900, -12.0, 1300, -90.0, 1300, -12.0, 1300, -8.0, 1100,0.0, 900, 0.1, 2000, 0.0, 2000,

HeadPitch,0.0, 1500, 0.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000,19.0, 1000, 18.0, 1000, 0.0, 1000, -19.0, 1000, 8.0, 1000,-5.0, 1300, 8.0, 1300, 22.0, 1100, 32.0, 1000, 22.0, 1000,

-19.0, 900, 8.0, 1300, -5.0, 1300, 8.0, 1300, 22.0, 1100,0.0, 900, 0.1, 2000, 1.0, 2000,

LShoulderPitch,90.0, 1500, 110.0, 1000, 120.0, 1000, 90.0, 1000, 0.0, 1000,21.0, 1000, 20.0, 1000, 11.0, 1000, -46.0, 1000, -10.0, 1000,

-17.0, 1300, -10.0, 1300, 30.0, 1100, 27.0, 1000, 19.0, 1000,-27.0, 900, 4.0, 1300, -4.0, 1300, 4.0, 1300, 19.0, 1100,97.0, 900, 90.0, 2000, 81.1, 2000,

LShoulderRoll,10.0, 1500, 20.0, 1000, 10.0, 1000, 10.0, 1000, 10.0, 1000,40.0, 1000, 0.0, 1000, 5.0, 1000, 10.0, 1000, 23.0, 1000,66.0, 1300, 23.0, 1300, 23.0, 1100, 10.0, 1000, 0.0, 1000,23.0, 900, 0.0, 1300, 0.0, 1300, 0.0, 1300, 12.0, 1100,54.0, 900, 10.0, 2000, 19.0, 2000,

LElbowYaw,-90.0, 1500, -90.0, 1000, -90.0, 1000, -90.0, 1000, -90.0, 1000,-22.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000,

0.0, 1300, 0.0, 1300, 0.0, 1100, 12.0, 1000, 11.0, 1000,-24.0, 900, -24.0, 1300, -5.0, 1300, -24.0, 1300, 11.0, 1100,-14.0, 900, -89.8, 2000, -80.3, 2000,LElbowRoll,

0.0, 1500, -40.0, 1000, -60.0, 1000, 0.0, 1000, 0.0, 1000,-95.0, 1000, -55.0, 1000, -85.0, 1000, -58.0, 1000, -58.0, 1000,

0.0, 1300, -58.0, 1300, -58.0, 1100, -51.0, 1000, -49.0, 1000,-64.0, 900, -49.0, 1300, -72.0, 1300, -49.0, 1300, -49.0, 1100,-92.0, 900, -0.1, 2000, -58.8, 2000,LWristYaw,-88.0, 1500, -85.6,25500,LHand,

0.0, 1500, 0.6,25500,RShoulderPitch,90.0, 1500, 110.0, 1000, 120.0, 1000, 90.0, 1000, 0.0, 1000,10.0, 1000, 35.0, 1000, 60.0, 1000, -27.0, 1000, 4.0, 1000,-4.0, 1300, 4.0, 1300, 19.0, 1100, 27.0, 1000, 30.0, 1000,

-46.0, 900, -10.0, 1300, -17.0, 1300, -10.0, 1300, 30.0, 1100,97.0, 900, 90.0, 2000, 81.5, 2000,

RShoulderRoll,-10.0, 1500, -10.0, 1000, -20.0, 1000, -10.0, 1000, -10.0, 1000,-4.0, 1000, -48.0, 1000, -87.0, 1000, -23.0, 1000, 0.0, 1000,0.0, 1300, 0.0, 1300, 0.0, 1100, -10.0, 1000, -23.0, 1000,

-10.0, 900, -23.0, 1300, -66.0, 1300, -23.0, 1300, -32.0, 1100,-54.0, 900, -10.0, 2000, -18.3, 2000,RElbowYaw,


90.0, 1500, 90.0, 1000, 90.0, 1000, 90.0, 1000, 90.0, 1000,11.0, 1000, 20.0, 1000, 90.0, 1000, 24.0, 1000, 24.0, 1000,5.0, 1300, 24.0, 1300, -11.0, 1100, -12.0, 1000, 0.0, 1000,0.0, 900, 0.0, 1300, 0.0, 1300, 0.0, 1300, 0.0, 1100,14.0, 900, 90.0, 2000, 79.8, 2000,

RElbowRoll,0.0, 1500, 40.0, 1000, 60.0, 1000, 0.0, 1000, 0.0, 1000,71.0, 1000, 94.0, 1000, 4.0, 1000, 64.0, 1000, 49.0, 1000,72.0, 1300, 49.0, 1300, 49.0, 1100, 51.0, 1000, 58.0, 1000,58.0, 900, 58.0, 1300, 0.0, 1300, 58.0, 1300, 58.0, 1100,92.0, 900, 0.1, 2000, 58.8, 2000,

RWristYaw,88.0, 1500, 87.0,25500,

RHand,0.0, 1500, 5.5,25500,

LHipYawPitch,0.0, 1500, 0.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000,-5.1, 1000, -5.7, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000,0.0, 1300, 0.0, 1300, 0.0, 1100, 0.0, 1000, 0.0, 1000,0.0, 900, 0.0, 1300, 0.0, 1300, 0.0, 1300, -3.9, 1100,0.0, 900, -0.1, 2000, 0.0, 2000,

LHipRoll,-3.0, 1500, -7.0, 1000, -10.0, 1000, 6.0, 1000, 6.0, 1000,

-10.0, 1000, 10.0, 1000, 24.1, 1000, 30.3, 1000, 35.0, 1000,35.0, 1300, 35.0, 1300, 20.0, 1100, 0.0, 1000, -15.0, 1000,11.2, 900, 8.0, 1300, 40.0, 1300, 8.0, 1300, -15.0, 1100,0.0, 900, -2.8, 2000, 0.0, 2000,

LHipPitch,0.0, 1500, 0.0, 1000, -20.0, 1000, 0.0, 1000, 0.0, 1000,

-40.0, 1000, -35.0, 1000, -60.0, 1000, -60.0, 1000, -60.0, 1000,-60.0, 1300, -60.0, 1300, -60.0, 1100, -50.0, 1000, -42.5, 1000,-7.0, 900, -50.0, 1300, 0.0, 1300, -50.0, 1300, -37.5, 1100,

-60.0, 900, -0.2, 2000, -25.0, 2000,LKneePitch,

0.0, 1500, 0.0, 1000, 40.0, 1000, 0.0, 1000, 0.0, 1000,80.0, 1000, 70.0, 1000, 120.0, 1000, 120.0, 1000, 120.0, 1000,

120.0, 1300, 120.0, 1300, 120.9, 1100, 100.0, 1000, 85.0, 1000,7.0, 900, 100.0, 1300, 0.0, 1300, 100.0, 1300, 75.0, 1100,

120.0, 900, 0.2, 2000, 40.1, 2000,LAnklePitch,

0.0, 1500, 0.0, 1000, -20.0, 1000, 0.0, 1000, 0.0, 1000,-40.0, 1000, -35.0, 1000, -60.0, 1000, -60.0, 1000, -60.0, 1000,-60.0, 1300, -60.0, 1300, -60.0, 1100, -50.0, 1000, -42.5, 1000,

0.0, 900, 0.0, 1300, 30.0, 1300, 0.0, 1300, -37.5, 1100,-60.0, 900, -0.2, 2000, -20.1, 2000,LAnkleRoll,

3.0, 1500, 7.0, 1000, 10.0, 1000, -6.0, 1000, -6.0, 1000,10.0, 1000, -10.0, 1000, 10.0, 1000, 10.0, 1000, 5.0, 1000,5.0, 1300, 5.0, 1300, 10.0, 1100, 0.0, 1000, -15.0, 1000,

-44.2, 900, -20.0, 1300, 0.0, 1300, -20.0, 1300, -15.0, 1100,0.0, 900, 2.8, 2000, 0.1, 2000,

RHipRoll,3.0, 1500, -7.0, 1000, -25.1, 1000, -6.0, 1000, -6.0, 1000,

-20.0, 1000, -45.0, 1000, -31.0, 1000, -8.0, 1000, -8.0, 1000,


-40.0, 1300, -8.0, 1300, 15.0, 1100, 0.0, 1000, -20.0, 1000,-30.9, 900, -35.0, 1300, -35.0, 1300, -35.0, 1300, -28.4, 1100,

0.0, 900, 2.8, 2000, 0.1, 2000,RHipPitch,

0.0, 1500, 0.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000,-40.0, 1000, -10.0, 1000, -6.0, 1000, -7.0, 1000, -50.0, 1000,

0.0, 1300, -50.0, 1300, -42.5, 1100, -50.0, 1000, -60.0, 1000,-60.0, 900, -60.0, 1300, -60.0, 1300, -60.0, 1300, -60.0, 1100,-60.0, 900, -0.2, 2000, -25.1, 2000,RKneePitch,

0.0, 1500, 0.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000,80.0, 1000, 20.0, 1000, 7.0, 1000, 7.0, 1000, 100.0, 1000,0.0, 1300, 100.0, 1300, 85.0, 1100, 100.0, 1000, 120.0, 1000,

120.0, 900, 120.0, 1300, 120.0, 1300, 120.0, 1300, 120.0, 1100,120.0, 900, 0.2, 2000, 39.9, 2000,RAnklePitch,

0.0, 1500, 0.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000,-40.0, 1000, -10.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000,30.0, 1300, 0.0, 1300, -42.5, 1100, -50.0, 1000, -60.0, 1000,

-60.0, 900, -60.0, 1300, -60.0, 1300, -60.0, 1300, -60.0, 1100,-60.0, 900, -0.2, 2000, -20.0, 2000,RAnkleRoll,-3.0, 1500, 7.0, 1000, 25.0, 1000, 6.0, 1000, 6.0, 1000,24.5, 1000, 40.0, 1000, 45.0, 1000, 42.6, 1000, 25.0, 1000,0.0, 1300, 20.0, 1300, 15.0, 1100, 0.0, 1000, -10.0, 1000,

-10.0, 900, -5.0, 1300, -5.0, 1300, -5.0, 1300, -10.0, 1100,0.0, 900, -2.8, 2000, -0.1, 2000) |

play (/opt/naoqi/share/naoqi/mp3/swiftswords_ext.mp3, 75) |[ledoff (AllLeds) & wait (1500) &[[ledfade (LeftFaceLedRed0, 0, 1000) |

ledfade (RightFaceLedRed0, 0, 1000) |ledfade (LeftFaceLedRed7, 0, 1000) |ledfade (RightFaceLedRed7, 0, 1000) |ledfade (LeftFaceLedGreen0, 0, 1000) |ledfade (RightFaceLedGreen0, 0, 1000) |ledfade (LeftFaceLedGreen7, 0, 1000) |ledfade (RightFaceLedGreen7, 0, 1000) |ledfade (LeftFaceLedBlue0, 1, 1000) |ledfade (RightFaceLedBlue0, 1, 1000) |ledfade (LeftFaceLedBlue7, 1, 1000) |ledfade (RightFaceLedBlue7, 1, 1000) ] |

[ledfade (LeftFaceLedRed1, 0, 1000) |ledfade (RightFaceLedRed5, 0, 1000) |ledfade (LeftFaceLedRed2, 0, 1000) |ledfade (RightFaceLedRed6, 0, 1000) |ledfade (LeftFaceLedGreen1, 1, 1000) |ledfade (RightFaceLedGreen5, 1, 1000) |ledfade (LeftFaceLedGreen2, 1, 1000) |ledfade (RightFaceLedGreen6, 1, 1000) |ledfade (LeftFaceLedBlue1, 0, 1000) |ledfade (RightFaceLedBlue5, 0, 1000) |ledfade (LeftFaceLedBlue2, 0, 1000) |ledfade (RightFaceLedBlue6, 0, 1000) ] |

[ledfade (LeftFaceLedRed3, 0, 1000) |

3.4 Textual language and available commands 11

ledfade (RightFaceLedRed3, 0, 1000) |ledfade (LeftFaceLedRed4, 0, 1000) |ledfade (RightFaceLedRed4, 0, 1000) |ledfade (LeftFaceLedGreen3, 0, 1000) |ledfade (RightFaceLedGreen3, 0, 1000) |ledfade (LeftFaceLedGreen4, 0, 1000) |ledfade (RightFaceLedGreen4, 0, 1000) |ledfade (LeftFaceLedBlue3, 1, 1000) |ledfade (RightFaceLedBlue3, 1, 1000) |ledfade (LeftFaceLedBlue4, 1, 1000) |ledfade (RightFaceLedBlue4, 1, 1000) ] |

[ledfade (LeftFaceLedRed5, 0, 1000) |ledfade (RightFaceLedRed1, 0, 1000) |ledfade (LeftFaceLedRed6, 0, 1000) |ledfade (RightFaceLedRed2, 0, 1000) |ledfade (LeftFaceLedGreen5, 0, 1000) |ledfade (RightFaceLedGreen1, 0, 1000) |ledfade (LeftFaceLedGreen6, 0, 1000) |ledfade (RightFaceLedGreen2, 0, 1000) |ledfade (LeftFaceLedBlue5, 1, 1000) |ledfade (RightFaceLedBlue1, 1, 1000) |ledfade (LeftFaceLedBlue6, 1, 1000) |

.... repeat blue and green blinking leds 19 times

ledfade (RightFaceLedBlue2, 1, 1000) ] ] &ledfade (RightFaceLedBlue6, 0, 1000) ] &ledfade (FaceLeds, 0, 5000) ]]&speed (100)]

The tai chi command consists of a set of move, play, led, and speed commands. In Section 3.4we will elaborate on the available commands and their syntax.

3.4 Textual language and available commands

The aim of constructing a textual robot language is that no in depth knowledge of a robot is required,nor that a specific robot is required. The effectiveness of a generic set of textual commands willhowever depend on the capabilities of such a robot. The goal is that a compact language would berequired where command instructions can be executed in a serial or parallel manner. Our aim is to geta language like adding and multiplying, a concept that is understood from this principle

(a+ b) ∗ c+ d ∗ e (1)

knowing that brackets bind stronger than multiplication, and multiplication has higher priority overaddition, we can construct a powerful language that is in essence identical

[a | b] & c | d & e (2)

where a | b denotes that a and b are executed in parallel, d & e denotes that d and e are executedsubsequently first d then e, the square brackets denote the binding order of execution. Commands a, b,c, d, and e are individual elementary commands that can be executed on a robot.

The commands available and functioning on the NAO robot are


wait (time)flush ()

ledon (ledname)ledoff (ledname)ledset (ledname, val)ledfade (ledname, val, time)

say (text[, volume[, language[, voice]]])play (filename[, volume])

move (name, angle, duration, [stiffness])movem ([name, [angle, duration]+]+)movemsi ([name, [angle, ty1, a1, t1, ty2, a2, t2, duration]+]+)stiff ([value, duration, idle]+)speed (value)walk (distance, time)walks (distance, time)walkto (x, y, theta)walkd (x, y, theta, freq, duration)

These 17 commands are divided into 4 (generic, leds, audio, and motion) distinct groups. Com-mand wait waits for a given number of milliseconds, and flush flushes commands that have beennot being processed.

Leds can be controlled addressing a led by a name that is coupled. The list of these names for theNAO robot is given in Section 3.4.1. These leds have a value between 0 and 1, it implies that ledoff(ledname) is equal to ledset (lename, 0), likewise for ledon with value 1. The ledfadecommands transforms from the current value to the given value, over a time given in milliseconds.

The say command is converting text to speech on the robot, in practise the first 2 arguments areused. Text is coupled using underscores, for example:

say(My_name_is_Nao)

The other audio command play plays mp3 files that are stored on the robot.The attractive part of robots is their mobility, and different commands can be used to provide this

mobility. For the NAO robot this starts by enabling the motors by setting its stiffness, 0 for disabledor relaxed, and 1 for enabled or stressed. It is executed by the command stiff, for example stiff(1.0, 1500, 0) denotes that within 1.5 seconds the motors are fully enabled, and stay like thatuntil a new stiff command is given. It is allowed to give multiple triples in a single command.

The move and movem are commands for individual control of every motor available, and is givenby a list of names, in Section 3.4.2 these names are specified for the NAO robot. The movem commandis a move multiple command where multiple motors are controlled over time by providing desired an-gle, and time to move from current angle to desired angle, see also the Tai Chi example given inSection 3.3. Tai chi is played in slow motion using the global speed command. In this example thespeed is set with value 183, and denotes a slow down of 1.83. It implies that every time argument inmovem is multiplied with 1.83. The movemsi command is very similar to movem, but uses splineinterpolation to make movements a bit smoother compared to the movem command. The spline inter-polation contains two triples, every triple starts with an integer value denoting the type of interpolation,and two floating point values a (delta in) angle, and a (delta in) time.


Finally there are 4 commands to let the robot walk, and of course this command could have beenperformed by the movem command, but the walk commands are substantially more compact to use.The walkto, command makes a robot move accurately over a distance (x, y) in meters, and rotate therobot torso (in the vertical axis) around -180 to 180 degrees. The walkd command makes a robot walka distance over a given duration in milliseconds, x, y, theta, and frequency are relative values, the first3 are given between -1 and 1 while the frequency is between 0 and 1. In addition to these commandswalk straight walk and walk sideways walks are available, the aim of these two commands is tomove the robot the specified distance over the specified time.

3.4.1 LEDS

The LED names available, given in the second column, on the NAO robot are

Index Name Internal description0 AllLeds1 AllLedsBlue2 AllLedsGreen3 AllLedsRed4 ChestLedBlue ChestBoard/Led/Blue/Actuator/Value5 ChestLedGreen ChestBoard/Led/Green/Actuator/Value6 ChestLedRed ChestBoard/Led/Red/Actuator/Value7 ChestLeds8 EarLeds9 FaceLed0

10 FaceLed111 FaceLed212 FaceLed313 FaceLed414 FaceLed515 FaceLed616 FaceLed717 FaceLedLeft018 FaceLedLeft119 FaceLedLeft220 FaceLedLeft321 FaceLedLeft422 FaceLedLeft523 FaceLedLeft624 FaceLedLeft725 FaceLedRight026 FaceLedRight127 FaceLedRight228 FaceLedRight329 FaceLedRight430 FaceLedRight531 FaceLedRight632 FaceLedRight733 FaceLeds


34 FaceLedsBottom35 FaceLedsExternal36 FaceLedsInternal37 FaceLedsLeftBottom38 FaceLedsLeftExternal39 FaceLedsLeftInternal40 FaceLedsLeftTop41 FaceLedsRightBottom42 FaceLedsRightExternal43 FaceLedsRightInternal44 FaceLedsRightTop45 FaceLedsTop46 FeetLeds47 LeftEarLed0 Ears/Led/Left/0Deg/Actuator/Value48 LeftEarLed1 Ears/Led/Left/36Deg/Actuator/Value49 LeftEarLed2 Ears/Led/Left/72Deg/Actuator/Value50 LeftEarLed3 Ears/Led/Left/108Deg/Actuator/Value51 LeftEarLed4 Ears/Led/Left/144Deg/Actuator/Value52 LeftEarLed5 Ears/Led/Left/180Deg/Actuator/Value53 LeftEarLed6 Ears/Led/Left/216Deg/Actuator/Value54 LeftEarLed7 Ears/Led/Left/252Deg/Actuator/Value55 LeftEarLed8 Ears/Led/Left/288Deg/Actuator/Value56 LeftEarLed9 Ears/Led/Left/324Deg/Actuator/Value57 LeftEarLeds58 LeftEarLedsBack59 LeftEarLedsEven60 LeftEarLedsFront61 LeftEarLedsOdd62 LeftFaceLeds63 LeftFaceLedBlue0 Face/Led/Blue/Left/0Deg/Actuator/Value64 LeftFaceLedBlue1 Face/Led/Blue/Left/45Deg/Actuator/Value65 LeftFaceLedBlue2 Face/Led/Blue/Left/90Deg/Actuator/Value66 LeftFaceLedBlue3 Face/Led/Blue/Left/135Deg/Actuator/Value67 LeftFaceLedBlue4 Face/Led/Blue/Left/180Deg/Actuator/Value68 LeftFaceLedBlue5 Face/Led/Blue/Left/225Deg/Actuator/Value69 LeftFaceLedBlue6 Face/Led/Blue/Left/270Deg/Actuator/Value70 LeftFaceLedBlue7 Face/Led/Blue/Left/315Deg/Actuator/Value71 LeftFaceLedGreen0 Face/Led/Green/Left/0Deg/Actuator/Value72 LeftFaceLedGreen1 Face/Led/Green/Left/45Deg/Actuator/Value73 LeftFaceLedGreen2 Face/Led/Green/Left/90Deg/Actuator/Value74 LeftFaceLedGreen3 Face/Led/Green/Left/135Deg/Actuator/Value75 LeftFaceLedGreen4 Face/Led/Green/Left/180Deg/Actuator/Value76 LeftFaceLedGreen5 Face/Led/Green/Left/225Deg/Actuator/Value77 LeftFaceLedGreen6 Face/Led/Green/Left/270Deg/Actuator/Value78 LeftFaceLedGreen7 Face/Led/Green/Left/315Deg/Actuator/Value79 LeftFaceLedRed0 Face/Led/Red/Left/0Deg/Actuator/Value80 LeftFaceLedRed1 Face/Led/Red/Left/45Deg/Actuator/Value81 LeftFaceLedRed2 Face/Led/Red/Left/90Deg/Actuator/Value


82 LeftFaceLedRed3 Face/Led/Red/Left/135Deg/Actuator/Value83 LeftFaceLedRed4 Face/Led/Red/Left/180Deg/Actuator/Value84 LeftFaceLedRed5 Face/Led/Red/Left/225Deg/Actuator/Value85 LeftFaceLedRed6 Face/Led/Red/Left/270Deg/Actuator/Value86 LeftFaceLedRed7 Face/Led/Red/Left/315Deg/Actuator/Value87 LeftFaceLedsBlue88 LeftFaceLedsGreen89 LeftFaceLedsRed90 LeftFootLedBlue LFoot/Led/Blue/Actuator/Value91 LeftFootLedGreen LFoot/Led/Green/Actuator/Value92 LeftFootLedRed LFoot/Led/Red/Actuator/Value93 LeftFootLeds94 RightEarLeds95 RightEarLed0 Ears/Led/Right/0Deg/Actuator/Value96 RightEarLed1 Ears/Led/Right/36Deg/Actuator/Value97 RightEarLed2 Ears/Led/Right/72Deg/Actuator/Value98 RightEarLed3 Ears/Led/Right/108Deg/Actuator/Value99 RightEarLed4 Ears/Led/Right/144Deg/Actuator/Value

100 RightEarLed5 Ears/Led/Right/180Deg/Actuator/Value101 RightEarLed6 Ears/Led/Right/216Deg/Actuator/Value102 RightEarLed7 Ears/Led/Right/252Deg/Actuator/Value103 RightEarLed8 Ears/Led/Right/288Deg/Actuator/Value104 RightEarLed9 Ears/Led/Right/324Deg/Actuator/Value105 RightEarLedsBack106 RightEarLedsEven107 RightEarLedsFront108 RightEarLedsOdd109 RightFaceLedBlue0 Face/Led/Blue/Right/0Deg/Actuator/Value110 RightFaceLedBlue1 Face/Led/Blue/Right/45Deg/Actuator/Value111 RightFaceLedBlue2 Face/Led/Blue/Right/90Deg/Actuator/Value112 RightFaceLedBlue3 Face/Led/Blue/Right/135Deg/Actuator/Value113 RightFaceLedBlue4 Face/Led/Blue/Right/180Deg/Actuator/Value114 RightFaceLedBlue5 Face/Led/Blue/Right/225Deg/Actuator/Value115 RightFaceLedBlue6 Face/Led/Blue/Right/270Deg/Actuator/Value116 RightFaceLedBlue7 Face/Led/Blue/Right/315Deg/Actuator/Value117 RightFaceLedGreen0 Face/Led/Green/Right/0Deg/Actuator/Value118 RightFaceLedGreen1 Face/Led/Green/Right/45Deg/Actuator/Value119 RightFaceLedGreen2 Face/Led/Green/Right/90Deg/Actuator/Value120 RightFaceLedGreen3 Face/Led/Green/Right/135Deg/Actuator/Value121 RightFaceLedGreen4 Face/Led/Green/Right/180Deg/Actuator/Value122 RightFaceLedGreen5 Face/Led/Green/Right/225Deg/Actuator/Value123 RightFaceLedGreen6 Face/Led/Green/Right/270Deg/Actuator/Value124 RightFaceLedGreen7 Face/Led/Green/Right/315Deg/Actuator/Value125 RightFaceLedRed0 Face/Led/Red/Right/0Deg/Actuator/Value126 RightFaceLedRed1 Face/Led/Red/Right/45Deg/Actuator/Value127 RightFaceLedRed2 Face/Led/Red/Right/90Deg/Actuator/Value128 RightFaceLedRed3 Face/Led/Red/Right/135Deg/Actuator/Value129 RightFaceLedRed4 Face/Led/Red/Right/180Deg/Actuator/Value

3.5 Enable the sensors 16

130 RightFaceLedRed5 Face/Led/Red/Right/225Deg/Actuator/Value131 RightFaceLedRed6 Face/Led/Red/Right/270Deg/Actuator/Value132 RightFaceLedRed7 Face/Led/Red/Right/315Deg/Actuator/Value133 RightFaceLeds134 RightFaceLedsBlue135 RightFaceLedsGreen136 RightFaceLedsRed137 RightFootLedBlue RFoot/Led/Blue/Actuator/Value138 RightFootLedGreen RFoot/Led/Green/Actuator/Value139 RightFootLedRed RFoot/Led/Red/Actuator/Value140 RightFootLeds

3.4.2 Motors

The motor names available on the NAO robot are

Motor name Minimum Angle Maximum Angle Maximum Change Per Cycle (deg)HeadYaw -120 120 9.47HeadPitch -45 45 8.24LShoulderPitch -120 120 9.47LShoulderRoll 0 95 8.24LElbowYaw -120 120 9.47LElbowRoll -95 0 8.24LWristYaw -105 150 8.24LHand 0 57.2958 8.24LHipYawPitch -55 40 4.76LHipRoll -25 45 4.76LHipPitch -90 30 7.33LKneePitch 0 130 7.33LAnklePitch -70 45 7.33LAnkleRoll -45 45 4.76RHipYawPitch -55 40 4.76RHipRoll -45 25 4.76RHipPitch -90 30 7.33RKneePitch 0 130 7.33RAnklePitch -70 45 7.33RAnkleRoll -45 45 4.76RShoulderPitch -120 120 9.47RShoulderRoll -95 0 8.24RElbowYaw -120 120 9.47RElbowRoll 0 95 8.24RWristYaw -105 150 8.24RHand 0 57.2958 8.24

3.5 Enable the sensors

Besides the camera available on the robot a number of sensing values can be obtained from the robot.This is obtained by the NaoSense module (Figure 5), it reads out all single valued sensory data.

3.5 Enable the sensors 17

Figure 5: Sensing internal data.

The added CommandNaoBySensing module servers as an example, how to create a command thatdepends on sensory data.

The code for enabling sensory data is given below, for the Left foot bumper sensor, it the sameway all other sensors can be added, for a full list of the available sensors, see Section 3.6.

In a c++ class define

/*** List of sensory elements

*/TVPnaoSensors *ns;/*** index for left foot bummper at left side

*/int LFootBumperLeft;

In the constructor use

/********************************************************* Setup list of sensors

********************************************************/ns = new TVPnaoSensors ();/********************************************************* Get indices

********************************************************/LFootBumperLeft = ns->getNameId ("LFootBumperLeft");

In the destructor use

/********************************************************* Free list of sensors

********************************************************/delete ns;

In the routine that reads out the sensory data d of type Data5D use

/********************************************************* evaluate input data

********************************************************/if ((d.datatype == DATA5D_FLOAT) &&

3.6 Available sensor commands 18

(d.w == ns->size ()) && (d.h == 1) &&(d.d == 1) && (d.t == 1) &&(d.e == 1))

{/***************************************************** data is sensory data

****************************************************/float *data = (float *) d.data;/***************************************************** 0 = LFootBumperLeft value is 0 or 1 (pushed)

****************************************************/if (data[LFootBumperLeft] > 0.5f)

{// You pushed the left foot bumper

}}

3.6 Available sensor commands

The sensor names and values available on the NAO robot are

Group Number Name DescriptionBumpers 0 LFootBumperLeft Value

1 LFootBumperRight Value2 RFootBumperLeft Value3 RFootBumperRight Value

HeadPitch 4 HeadPitch Sensor value5 HeadPitchA Actuator value6 HeadPitchEC Electric current sensor value7 HeadPitchH Hardness actuator value8 HeadPitchT Temperature value

HeadYaw 9 HeadYaw10 HeadYawEC11 HeadYawH12 HeadYawT

LAnklePitch 13 LAnklePitch14 LAnklePitchA15 LAnklePitchEC16 LAnklePitchH17 LAnklePitchT

LAnkleRoll 18 LAnkleRoll19 LAnkleRollA20 LAnkleRollEC21 LAnkleRollH22 LAnkleRollT

LElbowRoll 23 LElbowRoll24 LElbowRollA25 LElbowRollEC26 LElbowRollH


27 LElbowRollTLElbowYaw 28 LElbowYaw

29 LElbowYawA30 LElbowYawEC31 LElbowYawH32 LElbowYawT

LHand 33 LHand34 LHandA35 LHandEC36 LHandH37 LHandT

LHipPitch 38 LHipPitch39 LHipPitchA40 LHipPitchE41 LHipPitchH42 LHipPitchT

LHipRoll 43 LHipRoll44 LHipRollA45 LHipRollEC46 LHipRollH47 LHipRollT

LHipYawPitch 48 LHipYawPitch49 LHipYawPitchA50 LHipYawPitchEC51 LHipYawPitchH52 LHipYawPitchT

LKneePitch 53 LKneePitch54 LKneePitchA55 LKneePitchEC56 LKneePitchH57 LKneePitchT

LShoulderPitch 58 LShoulderPitch59 LShoulderPitchA60 LShoulderPitchEC61 LShoulderPitchH62 LShoulderPitchT

LWristYaw 63 LWristYaw64 LWristYawA65 LWristYawEC66 LWristYawH67 LWristYawT

RAnklePitch 68 RAnklePitch69 RAnklePitchA70 RAnklePitchEC71 RAnklePitchH72 RAnklePitchT

RAnkleRoll 73 RAnkleRoll74 RAnkleRollA


75 RAnkleRollEC76 RAnkleRollH77 RAnkleRollT

RElbowRoll 78 RElbowRoll79 RElbowRollA80 RElbowRollEC81 RElbowRollH82 RElbowRollT

RElbowYaw 83 RElbowYaw84 RElbowYawA85 RElbowYawEC86 RElbowYawH87 RElbowYawT

RHand 88 RHand89 RHandA90 RHandEC91 RHandH92 RHandT

RHipPitch 93 RHipPitch94 RHipPitchA95 RHipPitchEC96 RHipPitchH97 RHipPitchT

RHipRoll 98 RHipRoll99 RHipRollA

100 RHipRollEC101 RHipRollH102 RHipRollT

RHipYawPitch 103 RHipYawPitch104 RHipYawPitchEC105 RHipYawPitchH

RKneePitch 106 RKneePitch107 RKneePitchA108 RKneePitchEC109 RKneePitchH110 RKneePitchT

RShoulderPitch 111 RShoulderPitch112 RShoulderPitchA113 RShoulderPitchEC114 RShoulderPitchH115 RShoulderPitchT

RShoulderRoll 116 RShoulderRoll117 RShoulderRollA118 RShoulderRollEC119 RShoulderRollH120 RShoulderRollT

RWristYaw 121 RWristYaw122 RWristYawA


123 RWristYawEC124 RWristYawH125 RWristYawT

AccX 126 AccX X is forward in m/s2

AccY 127 AccY Y is leftAccZ 128 AccZ Z is upGyrAngleX 129 GyrAngleX in radiansGyrAngleY 130 GyrAngleYGyrX 131 GyrXGyrY 132 GyrYGyrRef 133 GyrRefFSRLcenterX 134 FSRLcenterX force sensors in (kilo)gramsFSRLcenterY 135 FSRLcenterYFSRLfrontLeft 136 FSRLfrontLeftFSRLfrontRight 137 FSRLfrontRightFSRLrearLeft 138 FSRLrearLeftFSRLrearRight 139 FSRLrearRightFSRLweight 140 FSRLweightFSRRcenterX 141 FSRRcenterXFSRRcenterY 142 FSRRcenterYFSRRfrontLeft 143 FSRRfrontLeftFSRRfrontRight 144 FSRRfrontRightFSRRrearLeft 145 FSRRrearLeftFSRRrearRight 146 FSRRrearRightFSRRweight 147 FSRRweightsonarLeft 148 sonarLeft 500ms speed updatessonarRight 149 sonarRight useful range 0.25-1.25recognizedWords 150 recognizedWords give confidence levelspeechDetected 151 speechDetected 1 is yes; 0 is noHeadTouchFront 152 HeadTouchFront 0 or 1 being touchedHeadTouchMiddle 153 HeadTouchMiddleHeadTouchRear 154 HeadTouchRear

3.6.1 Using states

Figure 5 illustrates a natural cycle of sensing, planning, and generation. In real time processes it iscommon to use state spaces, its a simple way subdivide a large problem into partial components. Theexample constructed moves the arms forward, and after that starts moving the arms in opposite direc-tion. Five states are constructed, the first (state 1) is to position both arms properly, when completedstates 2 and 3 are activated, one moves one arm upward, the other state the other arm downward, whencompleted state 2 activates 4, which in turn moves the arm in th other direction, and when completedit activates state 2 again. Like states 2 and 4 are alternating, The same is done with states 3 and 5 forthe other arm.

An example of using these 5 states is given in Figure 6.


StateCommand

StateCommand

Command 1

StateCommand

StateCommand

StateCommandId: 1Ready:2, 3Move left and rightshoulder pitch to 60 deg

Id: 2Ready:4Move left elbowroll to -50 deg

Id: 3Ready:5Move right elbowroll to 0 deg

Id: 4Ready:2Move left elbowroll to 0 deg

Id: 5Ready:3Move right elbow roll to 50 deg

Command 2

Command 3

Command 4

Command 5

Figure 6: An example using 5 different states. Top left design. Top right implementation. The threewindows provide the parameter settings of the three implemented modules.

4 Programming new modules 23

4 Programming new modules

The concept of TiViPE is to embed a routine call from a programmed library into the environmentwithout additional programming. Unfortunately, sometimes the desired routine is not available andneeds to be programmed from scratch, by construction of a software library. The aim of this section isto provide a step by step introduction to project construction, C++ programming of some routine calls,and embedding these routines into TiViPE.

4.1 Setting up a C++ library

Construction of a project is done by setting up a text file that contains all necessary files to construct alibrary. This text file in turn is converted to a Makefile. A makefile is used by another program calledmake or nmake, both programs in turn call the compiler and linker to compile the source code and linkit together to a library.

A project file for the TVProbotics library is a text file called src.pro, and can be found inAppendix A.1.

For the construction of a project file the TEMPLATE variable should be set to lib. Next a flag(DEFINES += ’flag’ is required to ensure exportation of routine calls into a library, when usinga Microsoft compiler. Note that ’+=’ stands for concatenation. All header files used (HEADERS+= ’filename’ together with the paths to these files (INCLUDEPATH += ’path’ need to beprovided, likewise for all other libraries where this library depends on are specified using LIBS +=-L’path’ -l’library’. Most important are the files with the source code, these are providedby SOURCES += ’file’. Finally the library needs a name which is specified by TARGET =’name’.

The syntax used in the project file is provided by Qt, hence Nokia’s qmake is required to converta project file into a Makefile by running

qmake src.pro

where src.pro is the project file. Note that TiViPE depends on Qt, and when running the abovecommand fails, try running %TVPHOME%\vars.bat first.

The library will be compiled using

nmake

and on unix make.By default the gnu compiler exports all routine calls, for the microsoft compiler this is not the case,

hence a ’defs’ file is created to properly export routines. For the robotics library this is done by theTVProboticsdefs.h file that can be found at Appendix A.2.

4.2 Adding content to the library

Two files TVPnaocommand.h and TVPnaocommand.cpp, both files are provided in Appendix A.3and A.4, have been included in the project. In these two files a some routines have been implementedand are discussed below.

The first routine created is

String TVPrespond (Data5D d,String rs)

4.3 Embedding a routine call into TiViPE 24

This test routine reads all sensory data obtained from the robot, stored in variable d, and retrieves arecognized word or set of words rs. The aim is to respond to pushing the feet bumper sensors and tosome of the words recognized.

The second test command is

String TVPwalkArc (Data5D d,float angle,float radius,float duration)

this should let the robot walk in a circle, but in case it has fallen it will first get up before walking. Theroutine has not been completed, see excercise at Section 6.1.

The third command is

QString TVPheadmenu (Data5D d)

touching the first or last of the captive sensors on the head of the NAO robot allows a user to stepthrough a menu, and when the desired item in the menu is found an action can be provided touchingthe central sensor. The TVPheadmenu routine provides a single step only, and must be used in a loopconstruction. Latter is performed by the commandStateStringHeadmenu routine. The TiViPEthe headmenu example is explained in more detail in Section 5.2.

Two other commands implemented in TVPnaocommand.cpp are standing TVPstandup andsitting down TVPsitdown. Both commands make use of the routine TVPgetBodyPose latterreads out the sensory data and provides the desired body posture by means of an integer number. Bothstandup and sitdown generate a string command that is dependent on the body posture, and are partialsolutions towards a goal position, either standing or sitting. Hence both routines are useful if they areused in repetition until the solution is found.

Repetition is performed by using routines in a state-space concept. In TVPnaocommand.cppthese routines all start with commandStateString. In the current implementation Headmenu,StandUp, Sitdown,LookForObject, MoveToObject, and Pose are available.

In Section 5 the use of states will be explained in more detail and applications are given how statesare used within TiViPE.

4.3 Embedding a routine call into TiViPE

In Section 4.2 two routines have been (partly) implemented TVPwalkArc and TVPrespond.Sartup TiViPE and select File->Module->New and use an appropriate name for the module as

described in Help->Modifying Modules.For the TVPrespond module the following is filled out in the 10 tabloids:

# ---Key-Brief-Description-----------------------------------------------------CommandNaoBySensing yields a string that can be used as command based uponsensed data.# ---Key-Full-Description------------------------------------------------------CommandNaoBySensing yields a string that can be used as command based uponsensed data.# ---Key-Input-Description-----------------------------------------------------Input sensory data.Input recognized sentence sensory data.# ---Key-Output-Description----------------------------------------------------Output output command string.# ---Key-Parameter-Description-------------------------------------------------# ---Key-Examples--------------------------------------------------------------

4.3 Embedding a routine call into TiViPE 25

None.# ---Key-Libraries--------------------------------------------------------------L$(TVPHOME)/lib -lTVProbotics# ---Key-Include---------------------------------------------------------------$(TVPHOME)/Libraries/TVProbotics/src:TVPnaocommand.h# ---Key-Gui-------------------------------------------------------------------Input("Input sensory data", "-i", DATA5D, DATA5D_FLOAT) [1];Input("Input recognized sentence", "-irs", STRING) [2];

Output("Output", "-o", STRING) [F];

Integer("Poll", "-poll", 100, 0, INT_MAX);

# ---Key-Code------------------------------------------------------------------TVPnaocommand::TVPrespond

For the TVPwalkArc module the following is filled out in the 10 tabloids:

# ---Key-Brief-Description-----------------------------------------------------CommandMoveArc moves an arc given a radius, and anangle with the aim to complete it in an a-priori given time.# ---Key-Full-Description------------------------------------------------------CommandMoveArc moves an arc given a radius, and anangle with the aim to complete it in an a-priori given time.# ---Key-Input-Description-----------------------------------------------------Input sensor data provides sensor data.# ---Key-Output-Description----------------------------------------------------Output command output command yieldseither output for arc movement or preparation for arc movement.# ---Key-Parameter-Description-------------------------------------------------Angle in degrees.Radius in meters.Duration in milliseconds.Robot type of robot that is being used.# ---Key-Examples--------------------------------------------------------------None.# ---Key-Libraries--------------------------------------------------------------L$(TVPHOME)/lib -lTVProbotics# ---Key-Include---------------------------------------------------------------$(TVPHOME)/Libraries/TVProbotics/src:TVPmotion.h# ---Key-Gui-------------------------------------------------------------------Input("Sensor data", "-i", DATA5D, DATA5D_FLOAT) [1];

Output("Output", "-o", STRING) [F];

BeginGroup(2);Integer("Poll", "-poll", 33, 0, INT_MAX);Float("Angle in degrees", "-angle", 90, -360, 360, 10) [2];Float("Radius in meters", "-radius", 0.25, 0, DBL_MAX, 10) [3];Float("Duration in milliseconds", "-time", 5000, 0, DBL_MAX, 10) [4];

EndGroup();Toggle("Robot", EXCLUSIVE) [5];

ToggleElement("Admoveo", "-admoveo");ToggleElement("Nao", "-nao", CHECKED);

EndToggle();

# ---Key-Code------------------------------------------------------------------TVPmotion::TVPmoveArc

Pressing the ok button will generate the module and is ready to be used within the TiViPE envi-

5 Robot Programming 26

ronment.

5 Robot Programming

This section is created to provide the user insight in how to program a robot using the concept ofTiViPE by showing an application.

5.1 Programming a state space routine

The definition of a state element within TiViPE is that a state

1. contains a unique and fixed id; implemented by an integer number

2. generates a command; implemented as a string

3. activates other states when command is completed; implemented as a string

or more formal a state is a triple (id, command, states).States enter a loop that is timer and event driven. For instance when one state is being used (10,

“say(Hello)”, “10”), it implies that Hello is said over and over again.TiViPE makes no restrictions to command and states A command can be an empty string, and can

be different over time. For instance (10, C[“say(Hello)”][“Say(Bye)”], C[“10”][“20”]), (20, ””, “20”),where C is a condition that can be implemented by the user, if the condition holds the first command,and first set of states is taken, i.e., “Say(Hello)”, and “10”, or otherwise second command and set ofstates is taken.

A state module is connected to another state module by passing states using istates and ostates andby passing commands using icommands and ocommands. A minimalistic state space module Name ofclass ClassName is implemented as follows:

void ClassName::commandStateStringName (Data5D istates,String icommands,Data5D &ostates,String &ocommands){/***************************************************************************** Three variables need to be set:

* -int id has a fixed and unique value and is a-priori known

* -QString states are the states that are activated once

* the command is executed

* -QString comm is the command to be executed on the robot

* here the TiViPE robot text language is used

****************************************************************************/if (initStream)

{id = 10;comm = "";states = QString ("%1").arg (id); // activate itselfinitStream = false;

}/***************************************************************************** Generate command to be executed and set of states to be activated

****************************************************************************/states = "20";comm = "say (Hello)";/****************************************************************************

5.2 HeadMenu 27

* Create state string

****************************************************************************/tcc->commandStateString (istates, icommands, id, comm, states, ostates,ocommands);

}

The magic happens in the commandStateString routine call. This routine passes on thestates that will be activated and the commands that will be executed. It is done by appending statesand comm if and only if id is currently active. The order of processing the state modules in TiViPEtherefore does not matter, the only requirement is that the modules need to be connected in a sequentialorder.

The last module passes the final set of commands to the SendCommand2 module that in turngives data to the robot module. This module is also connected to writeSharedData that in turntriggers the UpdateStates module to begin a new cycle.

As mentioned before, TiViPE allows the user to change the states states and command commvariables, most likely on the basis of sensory information, to make a single state powerful in itselfby changing commands, but also extremely powerful by deciding which states to activate when thiscommand is completed.

The best way to design robot behaviors is to graphically design a state first, and couple a list ofcommands that can be generated within this state. Next select all states necessary to obtain the desiredbehavior, anf finally draw the connections between the different states, and use differenct color codingsto denote different activation types upon completion.

Some examples, where the command generated depends on the sensory information obtained, aregiven in Appendixapp:naocommand.cpp for standing up at lines 988 to 1042 and sitting down at lines1062-1116.

The state command commandStateStringLookForObject implemented at lines 1119 to1411 of Appendixapp:naocommand.cpp uses a timer to decide what states to active. If within a givenmaximum time an object is found, different states are activated compared to when no object would befound. An a-priori known set of options can be passed on as states to be activated in this module. Forinstance, the variable astates can be set to [20,30][40,50] to denote that the module activatesstates 20 and 30 when for instance a criterion x is met, while it activates the second set when it fails tomeet x. In the code it implies that a substring is copied, from the variable astates, e.g., illustratedat lines 1570-1571, 1617-1618, and 1650-1651.

5.2 HeadMenu

The module CommandStateNaoHeadMenu is a specific module where individual commands canbe executed stepping through a menu. The internal construction is made so that every menu ele-ment contains 4 states, one for no action, a front button, a middle button, and a rear button touchedaction. Detailed comments can be found in the TVPnaocommand.cpp source code as well, seeAppendixapp:naocommand.cpp lines 760 to 788.

The module uses an offset, and makes the starting state is equal to the offset + 1. The menu of thismodule has a cyclic property, so it cannot be used with other state modules without modification.

The whole headmenu.net program in TiViPE, found in directory $TVPHOME/misc/Networks,and illustrated in Figure 7 contains solely 2 modules NaoSense and CommandStateNaoHeadMenuMerged.Latter is a merged module illustrated by 5 green modules at the right side of this module. Merging allmodules UpdateState, all state modules, writeSharedData5D, SendCommand2, and NaoRobot isrequired. Creating a single module involves a single process (with multiple threads) that guarranteesthe order of processing, hence compounding these modules is required. These five modules contain a

5.3 Object detection and tracking 28

Figure 7: TiViPE implementation of headmenu control.

key (in some cases a second key is used it has an “R” appended) for the robot, and one for the states.For every robot these keys need to be unique.

Note that the CommandString and NaoRobot are used in the beginning and the end to enableand disable the stiffness of the motors mannually to avoid overheating of the motors.

5.3 Object detection and tracking

Sensing the environment and acting upon what is happening is one of the key elements in robotics. Asmalle example has been created to show how face tracking is performed using two states, see also thetop part of Figure 8. Both states are active in an alternating manner.

The first state tracks a face, here there are 3 different commands possible, depending on the sensoryinformation:

1. no face has been found: do nothing

2. a face has been found and it is in the middle of the visual field: show that a face is in the middleby enabling the leds of the eyes

3. a face has been found but it is not in the middle of the visual field: move face in the middle ofthe visual field.

In all cases the following state is activated. The code of tracking faces is found in TVPnaocommand1.cpp.The second state says how many faces have been found.

5.4 Standing up and sitting down 29

Figure 8: TiViPE implementation (twostates.net of face tracking at the top and alternationbetween standing up and sitting at the bottom part. On the right side the merged components areillustrated.

States are updates either by time or by event, therefore all modules are evaluated as if they arein a loop. The activation of states determines what commands are activated. A single state moduleshould activate itself to get updated continously, if it activates another (non-existing) module, it willbe executed one time before activating an (illegal) unknown state. By creating 2 states a ping-pongeffect between 2 states can be created as is done in this example.

5.4 Standing up and sitting down

A similar example is given when sensing the embodiment rather than the environment by letting arobot stand up and sit down in an alternating manner. The implemented network is given at the bottomof Figure 8.

Both modules generate a command that is depending on the current body posture, but that does notnecessarily bring the robot in one step to the desired position, see A.4 lines 522-584 and lines 586-646what commands are generated for standing up and sitting down respectively.

The implementation of the state modules for standing up and sitting down are given in A.4 lines970-1042 and 1044-1116. Both modules stay in the current state until the desired pose is obtained, seealso lines 1033-1036 and 1107-1110.

Two states have been created and connected to get a ping-pong effect. In this example we showthat multiple pings (or pongs) can occur, by deciding when the robot has obtained a desired position.Hence one can consider that there is an if-the-else (or case) construction created within a module. Inthis case to activate itself or to activate the other module. Therefore a state, when activating itself, canbe subdivided into small steps and can be called multiple times before reaching its goal.

5.5 Nao demonstration 2011 30

a)

10 Enable stiffness

20 Stand up

stiff (1, 500, 0)

Step by step approach to sit down;Stays in this state until robot sits

30 Find objectFind object (Face areas as input)60 second criterion on success

40 Pose: wave hand 50 Anybody there?

51 Sit down

Step by step approach to stand;Stays in this state until robot stands

52 Lie down

Start

100 Say-exercise

110 Pose: tai-chi

229 Say-move to object

230-350 Goto object250, 260, 350

b)

Step by step approach to sit down;Stays in this state until robot sits

51 Sit down

229 Say-move to object

230-350 Goto object

230 Find ObjectFind object (Disc areas as input)60 second criterion on success

350 Say: No Object

240 Move to object

250 Found move back

260 Give up

Figure 9: Scheme of Nao demonstration 2011. (a) Overall scheme. (b) 230-350 Goto Object ex-planation. Black arrow denotes goto this state when ready, green arrow denotes goto this state whencondition is met, and a red arrow denotes goto this state upon failure.

5.5 Nao demonstration 2011

The aim of this demonstration was to give an indication that within short period of time an interactivescenario can be designed and implemented. The robot shows different actions on an area of approxi-mately one square meter.

In the demonstration face and object recognition are performed, the robot interacts on the basis ofthis sensory information. In addition, some activities are done to entertain people, in this case by atai-chi exercise, and relax, i.e., sit and lie down to avoid overheating of the motors.

A design was made that is illustrated in Figure 9.In total 15 different states have been used. The starting states are provided by parameter Active

states in module UpdateeStates. In the demonstration the parameter is set to “10”. State 10is implemented as (10, “stiff (1, 500, 0)”, “20”) for the NAO robot it is required to enable and disablestiffness from the motors. In the current generation of robots motors need to be regularly set to disabledstiffness “stiff (0, 500, 0)” to avoid overheated motors. Its implementation is perfromed by moduleCommandState, where the three parameters Id, Command, and States to activate are filled out asgiven as explained before. The module calls routine commandStateString directly.

State 20 reads out the body position from the sensed values and determines what move should beperformed to get to a standing position, once the robot is standing state 3 is activated. The implemen-tation of this module is called commandStateStringStandup and is found in Appendix A.4 atlines 970-1043. Note that all commandStateString commands generate a triple (id, command,states to activate) before calling commandStateString.

State 30 obtains all areas of recognized faces. In case a face area is found the robot moves its headtowared the position of the face, i.e. it tries to position the face in the middle of the object. Once this

5.5 Nao demonstration 2011 31

Figure 10: TiViPE implementation of nao robot demonstration. Five modules (in green) form thedemonstration. The CommandStateRobotDemo2011Mereged module is a compound module,the content of this module is given at the left. The content of the CVfacediscDetectMergedmodule is given at the bottom-right.

is done state 40 will be activated. If no faces are found over a certain period of time, the module timesout and activates state 50. Module CommandStateNaoFindObject is used to execute routinecommandStateStringLookForObject and can be found in Appendix A.4 at lines 1118-1411.

State 50 is implemented by the triple (50, “say(Hello there is anybody home?, 100) & wait(10000)”,“51”). State 51 lets the robot sit down. The implementation of this module is very similar to standingup, as has been explained before. When ready state 52 is activated. State 52 let the robot wait in asitting position and then lie down its triple is as follows (52, “stiff (0, 500, 0) & wait(60000) & stiff (1,500, 0) & [[[movem (HeadYaw, -39.0, ... LAnkleRoll, 0.0, 1000, 0.0, 1000, 0.0, 1000, 0.0, 1000, 0.0,1000, 0.0, 2000) —stiff (1.0, 1000, 3000, 0.1, 1000, 0)]] & wait(120000)] & say(That was relaxing,100) & stiff (1, 500, 0)”, “10”). State 52 activates state 10 and thus creates repetitive behavior sincethe first state is being activated again.

The CommandStatePose performs a preprogrammed pose. Index 9 denotes a waving posture,parallel to this pose the robot says “Hello”, as performed by state 40. After execution state 100 is acti-vated. State 100 is a standard triple (100, “say(Lets start a bit of excercising together)”, “110”) State110 performs a preprogrammed pose. Index 10 denotes tai-chi. After completion 229 is activated. Thisstate is again a standard triple (229, “wait(2000) & say(That was fun I feel relaxed) & wait(2000) &say(Time to go to work) & wait(1000) & say(I first have to find an object and then to go towards this object),“230”).

The next module is a merged object called CommandStatesToObjectMerged and contains5 modules. The first one searches disc type of objects that have a known diameter. Hence walkingtowards the object is possible by calibrating the number of camera pixels with the known object.The calibrated camera parameters are used in module CommandStateNaoMoveToObject. Itsimplementation is given in Appendix A.4 at lines 1413-1699.

The implementation of the design is given in Figure 10. The green modules contain the at the right

6 Exercises 32

upper part represent the demonstration. Two robot sensory blocks have been used called NaoSenseand FrameGrabber the first one provides all relevant sensory data from the robot, while the secondprovides video images from the robot. The ReadData5D module reads a set of disc objects storedon the local machine from file, and uses this database of objects for detecting objects that is performedby the RecognizCircularObjects module that is part of the CVfacediscDetectMergedmodule. The module shows recognized face, eye, and disc areas in different colors on the display togive the an impression how visual data is observed from the perspective of the robot.

6 Exercises

The aim of this section is to create attractive exercises that contribute to the robotics library. Thissection is therefore likely to change version by version, and solved exercises might shift to a newsection called ’Solved Exercises’.

6.1 Exercise 1: Replace walka by walkto command

Moving an arc was in 1.3.17 a standard call but it has been superseded by the move to a positioncommand. Recalculate the the angle and radius into the an x, y position and substitute the code in theTVPwalkArc routine of TVPnaocommand.cpp.

6.2 Exercise 2: Calibration of walk, walks, and walka

The walk, walks, and walka should move to a position in a specified time, unlike the walkto commandwhich moves to the exact position. Consider movement over 1, 3, 5, 10, 15, 20, and ≥30 seconds, andcalibrate the three routines.

6.3 Exercise 3: Create a more advanced face tracking module

Reimplement the face tracking module provided in TVPnaocommand1.cpp, and create a searchingmechanism when no object has been found. In case no object is found, keep the current state active.Use a timer to let the module time out once the robot has searched around but did not find anything.

6.4 Exercise 4: Flexible state activation

Implement the throwing dice example:

(10, "say(Throwing_dice)", "[20][30][40][50][60][70]")(20, "say(One)", "10")(30, "say(Two)", "10")(40, "say(Three)", "10")(50, "say(Four)", "10")(60, "say(Five)", "10")(70, "say(Six)", "10")

Note that the first module commandStateDice needs to be implemented, while for the other sixmodules the ready module commandStateString can be used.

6.5 Exercise 5: Getting grip over active states 33

6.5 Exercise 5: Getting grip over active states

Create of a state that activates two other states. When two states have been activated, reactive thesestates such that a cascade of states is activated. Think of a solution that when two states have beenactivated that after completion, one state is active.

One could think of the fibonacci numbers (Leonardo di Pisa, 1202) 1, 1, 2, 3, 5, 8, 13, 21, 34, 55,89, 144, 233, 377, 610, 987, 1597, 2584, ... . In formula f0 = 0, f1 = 1, and fn = fn−1 + fn−2, forn > 1, where a positive n is given and the robot is supposed to say fn times “One”. Note that it iswise to keep n small.

6.6 Advanced Exercise 6: Auditory processing

Create a module that processes auditory information on your local machine using for instance a usb-camera or microphone. Take relevant features from the processed auditory data, for instance, pitches,or recognized words.

Create a state module that uses processed auditory data.

6.7 Exercise 7: User scenario construction

A free exercise, where it is recommended to combine blocks, using multiple states.

7 Advanced Robot Programming

7.1 Adding commands to the NAO robot

For instance a command like movemsi has been added. An in depth explanation might be given forthis textual robot command.

One can imagine that textual commands like listen, or understand could be added as textualrobot commands.

7.2 Adding sensing elements to the NAO robot

For instance infra red, a NAO robot is equipped with IR, and hence with a remote control the robot canbe instructed! It might be an option to give additional control, to a robot. Note that the robot can bothsend and receive IR.

The reason that IR has not been implemented yet is that a remote computer is a virtual remotecontrol and hence there has been no true need to implement IR.

7.3 Enabling other features on the NAO robot

For instance sonar, in case it is not too much energy consuming. If this is the case commands toenable and disable sonar by adding text commands might be desired, e.g., enable(’name’) anddisable (name).

7.4 Adding other robots

Simpler robots like an Admoveo or Khepera can be added to this setting as well by adding modifiedversions of TVPnao files in the TVProbotics library.

A TVProbotics source code 34

A TVProbotics source code

This section contains the source code some of the files in the TVProbotics library.

A.1 Project file src.pro

1 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−2 # C o p y r i g h t (C) 2010−2011 Tino Lourens . A l l r i g h t s r e s e r v e d .3 #4 # P e r m i s s i o n t o use , copy , modify , and d i s t r i b u t e t h i s5 # s o f t w a r e and i t s d o c u m e n t a t i o n unde r t h e t e r m s of t h e GNU6 # G e n e r a l P u b l i c L i c e n s e i s he r e b y g r a n t e d . No r e p r e s e n t a t i o n s7 # a r e made a b o u t t h e s u i t a b i l i t y o f t h i s s o f t w a r e f o r any8 # p u r p o s e . I t i s p r o v i d e d ” as i s ” w i t h o u t e x p r e s s e d o r i m p l i e d9 # w a r r a n t y . See t h e GNU G e n e r a l P u b l i c L i c e n s e f o r more

10 # d e t a i l s .11 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−12 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−13 # \ f i l e s r c . p ro14 # \ b r i e f Qt M a k e f i l e f o r TVP r o b o t i c s l i b r a r y .15 # \ a u t h o r Tino Lourens16 # \ d a t e 10 Apr 201017 # \ v e r s i o n 16 Apr 201118 #19 # Qt M a k e f i l e f o r TVP r o b o t i c s l i b r a r y .20 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−21 # Se tup d e t a i l e d i n f o r m a t i o n a b o u t p r o d u c t and owner sh ip22 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−23 QMAKE TARGET COMPANY = TiViPE24 QMAKE TARGET PRODUCT = TVProbo t i c s25 QMAKE TARGET DESCRIPTION = TVP r o b o t i c s l i b r a r y26 QMAKE TARGET COPYRIGHT = C o p y r i g h t (C) 2010−2011 TiViPE27 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−28 # C o n s t r u c t a l i b r a r y29 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−30 TEMPLATE = l i b31 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−32 VERSION = 2 . 0 . 933 QT += g u i ne twork op en g l34 CONFIG += q t t h r e a d35 CONFIG += r e l e a s e36 CONFIG += c o n s o l e37 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−38 # Under M i c r o s o f t windows when u s i n g msvc / i n t e l we need t o e x p o r t t h e r o u t i n e s39 # of a c l a s s by p r o v i d i n g our own l i b r a r y f l a g TVPROBOTICS LIB .40 #41 # Under GNU by d e f a u l t e v e r y t h i n g i s e x p o r t e d ; f o r e f f i c i e n c y r e a s o n s i t i s42 # f a s t e r t o e x p o r t what i s n e c e s s a r y on ly . In a d d i t i o n we t r y t o keep t h e43 # l i b r a r y c l e a n / compact by s t r i p p i n g a l l symbol i n f o r m a t i o n .44 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−45 win32−msvc ∗ |win32−i c c {46 DEFINES += TVPROBOTICS LIB47 }48 un ix {49 DEFINES += TVPROBOTICS LIB50 QMAKE CFLAGS += − f v i s i b i l i t y = h i dde n51 QMAKE CXXFLAGS += − f v i s i b i l i t y = h i dde n52 QMAKE CXXFLAGS += − f v i s i b i l i t y −i n l i n e s −h i dd en

A.1 Project file src.pro 35

53 QMAKE LFLAGS += −s54 }55 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−56 # P a t h s57 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−58 # Loca l59 INCLUDEPATH = .60 # TVP61 INCLUDEPATH += $ (TVPHOME) / L i b r a r i e s / TVP / s r c62 INCLUDEPATH += $ (TVPHOME) / S t r u c t u r e s / s r c63 INCLUDEPATH += $ (TVPHOME) / L i b r a r i e s / TVP / TVPextens ion / s r c64 INCLUDEPATH += $ (TVPHOME) / L i b r a r i e s / TVPip / s r c65 INCLUDEPATH += $ (TVPHOME) / L i b r a r i e s / Data5D / s r c66 INCLUDEPATH += $ (TVPHOME) / L i b r a r i e s / TVPcuda / s r c67 INCLUDEPATH += $ (TVPHOME) / misc / Images /Xpm68 # Aldeba ran NAO i n c l u d e s69 # V 1 . 1 0 . 2 570 INCLUDEPATH += $ ( AL DIR ) / i n c l u d e71 INCLUDEPATH += $ ( AL DIR ) / i n c l u d e / alcommon72 INCLUDEPATH += $ ( AL DIR ) / i n c l u d e / r t t o o l s73 INCLUDEPATH += $ ( AL DIR ) / i n c l u d e / a l a u d i o74 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−75 # Libs76 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−77 # TVP78 LIBS = −L$ (TVPHOME) / l i b −lTVP −lData5D79 # Aldeba ran l i b s cmakes a r e found i n l i b \cmake\modules80 # V 1 . 1 0 . 2 581 LIBS += −L$ ( AL DIR ) / l i b82 LIBS += −L$ ( AL DIR ) / l i b / n a o q i83 LIBS += −lalcommon84 LIBS += − l a l v a l u e85 LIBS += − l a l c o r e86 LIBS += − l p t h r e a d87 LIBS += − l a l t o o l s88 LIBS += − l a l s o a p89 LIBS += − l t i n y x m l90 LIBS += − l r t t o o l s91 LIBS += − l a l t h r e a d92 LIBS += − l a l f a c t o r y93 LIBS += − l a l f i l e94 LIBS += − l a l l o g95 # o p t i o n a l Aldeba ran l i b s96 #LIBS += − l l i b b o o s t s e r i a l i z a t i o n −vc90−mt−1 3897 #LIBS += − l l i b b o o s t d a t e t i m e −vc90−mt−1 3898 #LIBS += − l l i b b o o s t t h r e a d −vc90−mt−1 3899 #LIBS += − l l i b b o o s t s i g n a l s −vc90−mt−1 38

100 #LIBS += − l l i b b o o s t f i l e s y s t e m −vc90−mt−1 38101 #LIBS += − l l i b b o o s t p r o g r a m o p t i o n s−vc90−mt−1 38102 #LIBS += − l i p p c103 #LIBS += − l f i n d i p p c104 #LIBS += −l p y t h o n105 #LIBS += − l a l i p p c106 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−107 # F i l e s108 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−109 HEADERS = T V P r o b o t i c s d e f s . h110 HEADERS += TVPcommandconst ruct ion . h111 SOURCES = TVPcommandconst ruct ion . cpp

A.2 Source code of TVProboticsdefs.h 36

112 HEADERS += TVPmotion . h113 SOURCES += TVPmotion . cpp114 HEADERS += TVPs t r inggen . h115 SOURCES += TVPs t r i nggen . cpp116 HEADERS += TVProbotcommand . h117 SOURCES += TVProbotcommand . cpp118 #119 # NAO120 #121 HEADERS += TVPnao . h122 SOURCES += TVPnao . cpp123 HEADERS += TVPnaoaldebaran . h124 HEADERS += TVPnaocommand . h125 SOURCES += TVPnaocommand . cpp126 HEADERS += TVPnaocommand1 . h127 SOURCES += TVPnaocommand1 . cpp128 HEADERS += TVPnaoleds . h129 SOURCES += TVPnaoleds . cpp130 HEADERS += TVPnaomotion . h131 SOURCES += TVPnaomotion . cpp132 HEADERS += TVPnaomotion1 . h133 SOURCES += TVPnaomotion1 . cpp134 HEADERS += TVPnaorobot . h135 HEADERS += TVPnaopython . h136 SOURCES += TVPnaopython . cpp137 HEADERS += TVPnaosensors . h138 SOURCES += TVPnaosensors . cpp139

140 #HEADERS += TVPnaosound . h141 #SOURCES += TVPnaosound . cpp142 #HEADERS += mysoundprocess ingmodu le . h143 #SOURCES += mysoundprocess ingmodu le . cpp144 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−145 # O b j e c t d i r e c t o r y146 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−147 OBJECTS DIR = . . / o b j148 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−149 # T a r g e t150 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−151 TARGET = TVProbo t i c s152 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−153 # D e s t i n a t i o n154 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−155 #DESTDIR = $ (TVPHOME) / l i b156 DESTDIR = . . / . . / . . / l i b157 #−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

A.2 Source code of TVProboticsdefs.h

1 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗2 ∗ C o p y r i g h t (C) 2010 Tino Lourens . A l l r i g h t s r e s e r v e d .3 ∗4 ∗ P e r m i s s i o n t o use , copy , modify , and d i s t r i b u t e t h i s s o f t w a r e and i t s5 ∗ d o c u m e n t a t i o n under t h e t e r m s of t h e GNU G e n e r a l P u b l i c L i c e n s e i s h e r e by6 ∗ g r a n t e d . No r e p r e s e n t a t i o n s a r e made a b o u t t h e s u i t a b i l i t y o f t h i s s o f t w a r e7 ∗ f o r any p u r p o s e . I t i s p r o v i d e d ” as i s ” w i t h o u t e x p r e s s e d o r i m p l i e d8 ∗ w a r r a n t y . See t h e GNU G e n e r a l P u b l i c L i c e n s e f o r more d e t a i l s .

A.3 Source code of TVPnaocommand.h 37

9 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /10 /∗ ∗11 ∗ \ f i l e T V P r o b o t i c s d e f s . h12 ∗ \ b r i e f Th i s f i l e c o n t a i n s d e f i n i t i o n s13 ∗ \ a u t h o r Tino Lourens14 ∗ \ d a t e 10 Apr 201015 ∗ \ v e r s i o n 10 Apr 201016 ∗17 ∗ \ h t m l o n l y \ e n d h t m l o n l y18 ∗ Thi s f i l e c o n t a i n s s t r u c t u r e s and p r o t o t y p e s f o r l i b r a r y e x p o r t .19 ∗ \ h t m l o n l y \ e n d h t m l o n l y20 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /21 # i f n d e f TVPROBOTICSDEFS H22 # d e f i n e TVPROBOTICSDEFS H23

24 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗25 ∗ Windows : M i c r o s o f t v i s u a l s t u d i o and i n t e l c o m p i l e r26 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /27 # i f d e f i n e d ( MSC VER ) | | d e f i n e d ( INTEL COMPILER )28 /∗ ∗29 ∗ msvc r e q u i r e s l i b r a r y e x p o r t s o t h e r w i s e t h e l i b r e m a i n s empty30 ∗ use o f ” d e f i n e TVPROBOTICS LIB” i n t h e h e a d e r t o g e t h e r w i th31 ∗ TVPROBOTICS EXPORT a t t h e c l a s s .32 ∗ /33 # i f d e f TVPROBOTICS LIB34 /∗ ∗35 ∗ L i b r a r y e x p o r t f l a g36 ∗ /37 # d e f i n e TVPROBOTICS EXPORT d e c l s p e c ( d l l e x p o r t )38 # e l s e39 /∗ ∗40 ∗ L i b r a r y e x p o r t f l a g41 ∗ /42 # d e f i n e TVPROBOTICS EXPORT d e c l s p e c ( d l l i m p o r t )43 # e n d i f44 # e l i f GNUC >= 4 / / gnu c o m p i l e r v e r s i o n 4 and up45 /∗ ∗46 ∗ gnu c o m p i l e r by d e f a u l t e x p o r t s a l l r o u t i n e so add47 ∗ QMAKE CXXFLAGS += − f v i s i b i l i t y = h i dde n48 ∗ QMAKE CXXFLAGS += − f v i s i b i l i t y −i n l i n e s −h i dd en49 ∗ t o your p r o j e c t f i l e50 ∗ TVPROBOTICS EXPORT a t t h e c l a s s .51 ∗ /52 # d e f i n e TVPROBOTICS EXPORT a t t r i b u t e ( ( v i s i b i l i t y ( ” d e f a u l t ” ) ) )53 # e l s e / / o t h e r c o m p i l e r54 /∗ ∗55 ∗ L i b r a r y e x p o r t f l a g o f any o t h e r c o m p i l e r i s empty56 ∗ /57 # d e f i n e TVPROBOTICS EXPORT58 # e n d i f59

60 # e n d i f /∗ TVPROBOTICSDEFS H ∗ /

A.3 Source code of TVPnaocommand.h

1 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗2 ∗ C o p y r i g h t (C) 2010−2011 Tino Lourens . A l l r i g h t s r e s e r v e d .

3 ∗4 ∗ P e r m i s s i o n t o use , copy , modify , and d i s t r i b u t e t h i s s o f t w a r e and i t s5 ∗ d o c u m e n t a t i o n under t h e t e r m s of t h e GNU G e n e r a l P u b l i c L i c e n s e i s h e r e by6 ∗ g r a n t e d . No r e p r e s e n t a t i o n s a r e made a b o u t t h e s u i t a b i l i t y o f t h i s s o f t w a r e7 ∗ f o r any p u r p o s e . I t i s p r o v i d e d ” as i s ” w i t h o u t e x p r e s s e d o r i m p l i e d8 ∗ w a r r a n t y . See t h e GNU G e n e r a l P u b l i c L i c e n s e f o r more d e t a i l s .9 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /

10 /∗ ∗11 ∗ \ f i l e TVPnaocommand . h12 ∗ \ b r i e f Th i s f i l e c o n t a i n s s t r u c t u r e s and p r o t o t y p e s .13 ∗ \ a u t h o r Tino Lourens14 ∗ \ d a t e 24 Apr 201015 ∗ \ v e r s i o n 11 Jun 201116 ∗17 ∗ \ h t m l o n l y \ e n d h t m l o n l y18 ∗ Thi s f i l e c o n t a i n s s t r u c t u r e s and p r o t o t y p e s f o r s t r i n g g e n e r a t i o n19 ∗ \ h t m l o n l y \ e n d h t m l o n l y20 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /21 # i f n d e f TVPNAOCOMMAND H22 # d e f i n e TVPNAOCOMMAND H23

24 / / TVP25 # i n c l u d e <TVPgenera l . h>26 # i n c l u d e <TVPSStr ing . h>27 / / TVProbo t i c s l i b28 # i n c l u d e <T V P r o b o t i c s d e f s . h>29 # i n c l u d e <TVPnaosensors . h>30 # i n c l u d e <TVPnaomotion . h>31 # i n c l u d e <TVPnaomotion1 . h>32 # i n c l u d e <TVPs t r i nggen . h>33 # i n c l u d e <TVPnaocommand1 . h>34 # i n c l u d e <TVPcommandconst ruct ion . h>35

36 /∗ ∗37 ∗ \ b r i e f C l a s s T V P s t a t e s p a r s e s s t a t e s e t t i n g s38 ∗ \ a u t h o r Tino Lourens39 ∗ \ d a t e 11 Jun 201140 ∗ \ v e r s i o n 11 Jun 201141 ∗42 ∗ \ h t m l o n l y \ e n d h t m l o n l y43 ∗ C l a s s TVPnaocommand1 c l a s s t o g e n e r a t e a commands f o r nao r o b o t .44 ∗ \ h t m l o n l y \ e n d h t m l o n l y45 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /46 c l a s s TVPROBOTICS EXPORT T V P s t a t e s47 {48 p u b l i c :49 Q S t r i n g g e t S t a t e s ( Q S t r i n g s ,50 i n t i n d e x ) ;51 } ;52

53 /∗ ∗54 ∗ \ b r i e f C l a s s TVPnaocommand c l a s s t o g e n e r a t e a commands f o r nao r o b o t .55 ∗ \ a u t h o r Tino Lourens56 ∗ \ d a t e 10 Apr 201057 ∗ \ v e r s i o n 23 Apr 201158 ∗59 ∗ \ h t m l o n l y \ e n d h t m l o n l y60 ∗ C l a s s TVPnaocommand c l a s s t o g e n e r a t e a commands f o r nao r o b o t .61 ∗ \ h t m l o n l y \ e n d h t m l o n l y


62 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /63 c l a s s TVPROBOTICS EXPORT TVPnaocommand : p u b l i c TVPs t r i nggen64 {65 p u b l i c :66 TVPnaocommand ( ) ;67 ˜ TVPnaocommand ( ) ;68 S t r i n g TVPrespond ( Data5D d ,69 S t r i n g r s ) ;70 Q S t r i n g TVPwalkArc ( Data5D d ,71 f l o a t ang le ,72 f l o a t r a d i u s ,73 f l o a t d u r a t i o n ) ;74 i n t TVPgetBodyPose ( Data5D d ) ;75 Q S t r i n g TVPstandup ( Data5D d ) ;76 Q S t r i n g TVPsitdown ( Data5D d ) ;77 Q S t r i n g TVPheadmenu ( Data5D d ) ;78 vo id commandSta teSt r ingHeadmenu ( Data5D i s t a t e s ,79 S t r i n g icommands ,80 i n t i d o f f s e t ,81 Data5D &o s t a t e s ,82 S t r i n g &ocommands ,83 Data5D d ) ;84 vo id c o m m a n d S t a t e S t r i n g S t a n d u p ( Data5D i s t a t e s ,85 S t r i n g icommands ,86 i n t i d s t a r t ,87 Q S t r i n g i d r e a d y ,88 Data5D &o s t a t e s ,89 S t r i n g &ocommands ,90 Data5D d ) ;91 vo id c o m m a n d S t a t e S t r i n g S i t d o w n ( Data5D i s t a t e s ,92 S t r i n g icommands ,93 i n t i d s t a r t ,94 Q S t r i n g i d r e a d y ,95 Data5D &o s t a t e s ,96 S t r i n g &ocommands ,97 Data5D d ) ;98 vo id c o m m a n d S t a t e S t r i n g L o o k F o r O b j e c t ( Data5D i s t a t e s ,99 S t r i n g icommands ,

100 i n t i d s t a r t ,101 Q S t r i n g a s t a t e s ,102 Data5D &o s t a t e s ,103 S t r i n g &ocommands ,104 Data5D d ,105 Data5D s ,106 i n t maxTime ) ;107 vo id commandSta teSt r ingMoveToObjec t ( Data5D i s t a t e s ,108 S t r i n g icommands ,109 i n t i d s t a r t ,110 Q S t r i n g a s t a t e s ,111 Data5D &o s t a t e s ,112 S t r i n g &ocommands ,113 Data5D d ,114 Data5D s ,115 i n t maxTime ,116 do ub l e d i s t a n c e ,117 do ub l e d i a m e t e r ) ;118 vo id c o m m a n d S t a t e S t r i n g P o s e ( Data5D i s t a t e s ,119 S t r i n g icommands ,120 i n t i d s t a r t ,


121 Q S t r i n g i d r e a d y ,122 Data5D &o s t a t e s ,123 S t r i n g &ocommands ,124 i n t index ,125 Q S t r i n g command = ” ” ) ;126 /∗ ∗127 ∗ walk an a r c128 ∗ /129 S t r i n g TVPwalkArcS ( Data5D d ,130 f l o a t ang le ,131 f l o a t r a d i u s ,132 f l o a t d u r a t i o n )133 { r e t u r n TVPs t r i ng ( TVPwalkArc ( d , ang le , r a d i u s , d u r a t i o n ) ) ; } ;134 /∗ ∗135 ∗ pe r fo rm s t a n d up136 ∗ /137 S t r i n g TVPstandupS ( Data5D d )138 { r e t u r n TVPs t r i ng ( TVPstandup ( d ) ) ; } ;139 /∗ ∗140 ∗ pe r fo rm s i t down141 ∗ /142 S t r i n g TVPsitdownS ( Data5D d )143 { r e t u r n TVPs t r i ng ( TVPsitdown ( d ) ) ; } ;144 /∗ ∗145 ∗ pe r fo rm head menu t e s t146 ∗ /147 S t r i n g TVPheadmenuS ( Data5D d )148 { r e t u r n TVPs t r i ng ( TVPheadmenu ( d ) ) ; } ;149 p r i v a t e :150 /∗ ∗151 ∗ f r o n t b u t t o n pushed152 ∗ /153 boo l fbp ;154 /∗ ∗155 ∗ midd le b u t t o n pushed156 ∗ /157 boo l mbp ;158 /∗ ∗159 ∗ r e a r b u t t o n pushed160 ∗ /161 boo l rbp ;162 /∗ ∗163 ∗ command used i n s t a t e164 ∗ /165 Q S t r i n g comm ;166 /∗ ∗167 ∗ command s t a t e s t o be a c t i v a t e d168 ∗ /169 Q S t r i n g s t a t e s ;170 /∗ ∗171 ∗ s t a t e i d172 ∗ /173 i n t i d ;174 /∗ ∗175 ∗ command c o n s t r u c t i o n u s i n g s t a t e s176 ∗ /177 TVPcommandConstruct ion ∗ t c c ;178 /∗ ∗179 ∗ f l a g used t o s t a r t


180 ∗ /181 boo l i n i t S t r e a m ;182 /∗ ∗183 ∗ Index used f o r u p d a t e s184 ∗ /185 i n t i n d e x ;186 /∗ ∗187 ∗ indexMenu used menu f o r u p d a t e s188 ∗ /189 i n t indexMenu ;190 /∗ ∗191 ∗ L i s t o f s e n s o r y e l e m e n t s192 ∗ /193 TVPnaoSensors ∗ns ;194 /∗ ∗195 ∗ L i s t o f s e n s o r y e l e m e n t s196 ∗ /197 TVPnaoMotion ∗nm ;198 /∗ ∗199 ∗ l a s t r e c o g n i z e d s e n t e n c e o r word ( s )200 ∗ /201 Q S t r i n g l a s t S e n t e n c e ;202 /∗ ∗203 ∗ l a s t c o n f i d e n c e l e v e l on r e c o g n i z e d s e n t e n c e o r word ( s )204 ∗ /205 f l o a t l a s t C o n f i d e n c e L e v e l ;206 /∗ ∗207 ∗ i n d e x f o r l e f t f o o t bummper a t l e f t s i d e208 ∗ /209 i n t LFootBumperLef t ;210 /∗ ∗211 ∗ i n d e x f o r l e f t f o o t bummper a t r i g h t s i d e212 ∗ /213 i n t RFootBumperLeft ;214 /∗ ∗215 ∗ i n d e x f o r r i g h t f o o t bummper a t l e f t s i d e216 ∗ /217 i n t LFootBumperRight ;218 /∗ ∗219 ∗ i n d e x f o r r i g h t f o o t bummper a t r i g h t s i d e220 ∗ /221 i n t RFootBumperRight ;222 /∗ ∗223 ∗ i n d e x f o r r e c o g n i z e d words224 ∗ /225 i n t r ecogn izedWords ;226 /∗ ∗227 ∗ i n d e x f o r s pe ec h d e t e c t e d228 ∗ /229 i n t s p e e c h D e t e c t e d ;230 /∗ ∗231 ∗ i n d e x f o r a n g l e from gyro i n X−d i r e c t i o n232 ∗ /233 i n t GyrAngleX ;234 /∗ ∗235 ∗ i n d e x f o r a n g l e from gyro i n Y−d i r e c t i o n236 ∗ /237 i n t GyrAngleY ;238 /∗ ∗


239 ∗ i n d e x f o r head t o u c h f r o n t b u t t o n240 ∗ /241 i n t HeadTouchFront ;242 /∗ ∗243 ∗ i n d e x f o r head t o u c h r e a r b u t t o n244 ∗ /245 i n t HeadTouchRear ;246 /∗ ∗247 ∗ i n d e x f o r head t o u c h midd le b u t t o n248 ∗ /249 i n t HeadTouchMiddle ;250 /∗ ∗251 ∗ i n d e x f o r head yaw252 ∗ /253 i n t HeadYaw ;254 /∗ ∗255 ∗ i n d e x f o r head p i t c h256 ∗ /257 i n t HeadP i t ch ;258 /∗ ∗259 ∗ i n d e x f o r l e f t s h o u l d e r p i t c h260 ∗ /261 i n t L S h o u l d e r P i t c h ;262 /∗ ∗263 ∗ i n d e x f o r l e f t s h o u l d e r r o l l264 ∗ /265 i n t L S h o u l d e r R o l l ;266 /∗ ∗267 ∗ i n d e x f o r l e f t elbow yaw268 ∗ /269 i n t LElbowYaw ;270 /∗ ∗271 ∗ i n d e x f o r l e f t elbow r o l l272 ∗ /273 i n t LElbowRoll ;274 /∗ ∗275 ∗ i n d e x f o r l e f t w r i s t yaw276 ∗ /277 i n t LWristYaw ;278 /∗ ∗279 ∗ i n d e x f o r r i g h t s h o u l d e r p i t c h280 ∗ /281 i n t R S h o u l d e r P i t c h ;282 /∗ ∗283 ∗ i n d e x f o r r i g h t s h o u l d e r r o l l284 ∗ /285 i n t R S h o u l d e r R o l l ;286 /∗ ∗287 ∗ i n d e x f o r r i g h t elbow yaw288 ∗ /289 i n t RElbowYaw ;290 /∗ ∗291 ∗ i n d e x f o r r i g h t elbow r o l l292 ∗ /293 i n t RElbowRoll ;294 /∗ ∗295 ∗ i n d e x f o r r i g h t w r i s t yaw296 ∗ /297 i n t RWristYaw ;


298 /∗ ∗299 ∗ i n d e x f o r l e f t h i p yaw p i t c h300 ∗ /301 i n t LHipYawPitch ;302 /∗ ∗303 ∗ i n d e x f o r l e f t h i p r o l l304 ∗ /305 i n t LHipRol l ;306 /∗ ∗307 ∗ i n d e x f o r l e f t h i p p i t c h308 ∗ /309 i n t LHipP i t ch ;310 /∗ ∗311 ∗ i n d e x f o r l e f t knee p i t c h312 ∗ /313 i n t LKneePi tch ;314 /∗ ∗315 ∗ i n d e x f o r l e f t a n k l e p i t c h316 ∗ /317 i n t LAnk leP i t ch ;318 /∗ ∗319 ∗ i n d e x f o r l e f t a n k l e r o l l320 ∗ /321 i n t LAnkleRol l ;322 /∗ ∗323 ∗ i n d e x f o r r i g h t yaw p i t c h324 ∗ /325 i n t RHipYawPitch ;326 /∗ ∗327 ∗ i n d e x f o r r i g h t h i p r o l l328 ∗ /329 i n t RHipRoll ;330 /∗ ∗331 ∗ i n d e x f o r r i g h t h i p p i t c h332 ∗ /333 i n t RHipPi t ch ;334 /∗ ∗335 ∗ i n d e x f o r r i g h t knee p i t c h336 ∗ /337 i n t RKneePi tch ;338 /∗ ∗339 ∗ i n d e x f o r r i g h t a n k l e p i t c h340 ∗ /341 i n t RAnkleP i t ch ;342 /∗ ∗343 ∗ i n d e x f o r r i g h t a n k l e r o l l344 ∗ /345 i n t RAnkleRol l ;346 /∗ ∗347 ∗ s t a r t t ime used i n f i n d i n g a f a c e348 ∗ /349 Time s t a r t T i m e ;350 /∗ ∗351 ∗ f l a g t o s e t when o b j e c t i s found352 ∗ /353 boo l found ;354 /∗ ∗355 ∗ f l a g t o s e t when r e a d y356 ∗ /

A.4 Source code of TVPnaocommand.cpp 44

357 boo l f i n i s h e d ;358 } ;359 # e n d i f /∗ TVPNAOCOMMAND H ∗ /

A.4 Source code of TVPnaocommand.cpp

1 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗2 ∗ C o p y r i g h t (C) 2010−2011 Tino Lourens . A l l r i g h t s r e s e r v e d .3 ∗4 ∗ P e r m i s s i o n t o use , copy , modify , and d i s t r i b u t e t h i s s o f t w a r e and i t s5 ∗ d o c u m e n t a t i o n under t h e t e r m s of t h e GNU G e n e r a l P u b l i c L i c e n s e i s h e r e by6 ∗ g r a n t e d . No r e p r e s e n t a t i o n s a r e made a b o u t t h e s u i t a b i l i t y o f t h i s s o f t w a r e7 ∗ f o r any p u r p o s e . I t i s p r o v i d e d ” as i s ” w i t h o u t e x p r e s s e d o r i m p l i e d8 ∗ w a r r a n t y . See t h e GNU G e n e r a l P u b l i c L i c e n s e f o r more d e t a i l s .9 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /

10 /∗ ∗11 ∗ \ f i l e TVPnaocommand . cpp12 ∗ \ b r i e f Th i s f i l e c o n t a i n s r o u t i n e s f o r s t r i n g g e n e r a t i o n .13 ∗ \ a u t h o r Tino Lourens14 ∗ \ d a t e 24 Apr 201015 ∗ \ v e r s i o n 23 Apr 201116 ∗17 ∗ \ h t m l o n l y \ e n d h t m l o n l y18 ∗ Thi s f i l e c o n t a i n s r o u t i n e s f o r s t r i n g g e n e r a t i o n .19 ∗ \ h t m l o n l y \ e n d h t m l o n l y20 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /21 # i n c l u d e <TVPnaocommand . h>22 # i n c l u d e <t ime . h>23

24 / / # d e f i n e TVPNAOCOMMAND DEBUG25 / / # d e f i n e TVPNAOCOMMAND DEBUG126

27 /∗ ∗28 ∗ \ b r i e f TVPnaocommand c o n s t r u c t o r i n i t i a l i z e s .29 ∗ \ a u t h o r Tino Lourens30 ∗ \ d a t e 10 Apr 201031 ∗ \ v e r s i o n 23 Apr 201132 ∗ \ r e t u r n None33 ∗34 ∗ \ h t m l o n l y \ e n d h t m l o n l y35 ∗ TVPnaocommand c o n s t r u c t o r i n i t i a l i z e s .36 ∗ \ h t m l o n l y \ e n d h t m l o n l y37 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /38 TVPnaocommand : : TVPnaocommand ( )39 {40 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)41 qDebug ( ) << ”TVPnaocommand : : TVPnaocommand” << f l u s h ;42 # e n d i f43 i n d e x = 0 ;44 indexMenu = −1;45 i n i t S t r e a m = t r u e ;46 t c c = NULL;47 comm = ” ” ;48 s t a t e s = ” ” ;49 i d = −1;50 fbp = f a l s e ;51 mbp = f a l s e ;


52 rbp = f a l s e ;53 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗54 ∗ Se tup l i s t o f s e n s o r s55 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /56 ns = new TVPnaoSensors ( ) ;57 nm = new TVPnaoMotion ( ) ;58 l a s t C o n f i d e n c e L e v e l = −1.0 f ;59 l a s t S e n t e n c e = ” n o t h i n g ” ;60 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗61 ∗ Get i n d i c e s62 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /63 LFootBumperLef t = ns−>getNameId ( ” LFootBumperLef t ” ) ;64 RFootBumperLeft = ns−>getNameId ( ” RFootBumperLeft ” ) ;65 LFootBumperRight = ns−>getNameId ( ” LFootBumperRight ” ) ;66 RFootBumperRight = ns−>getNameId ( ” RFootBumperRight ” ) ;67 HeadTouchFront = ns−>getNameId ( ” HeadTouchFront ” ) ;68 HeadTouchRear = ns−>getNameId ( ” HeadTouchRear ” ) ;69 HeadTouchMiddle = ns−>getNameId ( ” HeadTouchMiddle ” ) ;70 s p e e c h D e t e c t e d = ns−>getNameId ( ” s p e e c h D e t e c t e d ” ) ;71 r ecogn izedWords = ns−>getNameId ( ” recogn izedWords ” ) ;72 GyrAngleX = ns−>getNameId ( ” GyrAngleX ” ) ;73 GyrAngleY = ns−>getNameId ( ” GyrAngleY ” ) ;74 HeadYaw = ns−>getNameId ( ”HeadYaw” ) ;75 HeadP i t ch = ns−>getNameId ( ” HeadP i t ch ” ) ;76

77 L S h o u l d e r P i t c h = ns−>getNameId ( ” L S h o u l d e r P i t c h ” ) ;78 L S h o u l d e r R o l l = ns−>getNameId ( ” L S h o u l d e r R o l l ” ) ;79 LElbowYaw = ns−>getNameId ( ”LElbowYaw” ) ;80 LElbowRoll = ns−>getNameId ( ” LElbowRoll ” ) ;81 LWristYaw = ns−>getNameId ( ” LWristYaw ” ) ;82

83 R S h o u l d e r P i t c h = ns−>getNameId ( ” R S h o u l d e r P i t c h ” ) ;84 R S h o u l d e r R o l l = ns−>getNameId ( ” R S h o u l d e r R o l l ” ) ;85 RElbowYaw = ns−>getNameId ( ”RElbowYaw” ) ;86 RElbowRoll = ns−>getNameId ( ” RElbowRoll ” ) ;87 RWristYaw = ns−>getNameId ( ” RWristYaw ” ) ;88

89 LHipYawPitch = ns−>getNameId ( ” LHipYawPitch ” ) ;90 LHipRol l = ns−>getNameId ( ” LHipRol l ” ) ;91 LHipP i t ch = ns−>getNameId ( ” LHipP i t ch ” ) ;92 LKneePi tch = ns−>getNameId ( ” LKneePi tch ” ) ;93 LAnk leP i t ch = ns−>getNameId ( ” LAnk leP i t ch ” ) ;94 LAnkleRol l = ns−>getNameId ( ” LAnkleRol l ” ) ;95

96 RHipYawPitch = ns−>getNameId ( ” RHipYawPitch ” ) ;97 RHipRoll = ns−>getNameId ( ” RHipRoll ” ) ;98 RHipPi t ch = ns−>getNameId ( ” RHipPi t ch ” ) ;99 RKneePi tch = ns−>getNameId ( ” RKneePi tch ” ) ;

100 RAnkleP i t ch = ns−>getNameId ( ” RAnkleP i t ch ” ) ;101 RAnkleRol l = ns−>getNameId ( ” RAnkleRol l ” ) ;102 }103

104 /∗ ∗105 ∗ \ b r i e f ˜ TVPnaocommand d e s t r u c t o r f r e e d a t a .106 ∗ \ a u t h o r Tino Lourens107 ∗ \ d a t e 10 Apr 2010108 ∗ \ v e r s i o n 08 May 2010109 ∗ \ r e t u r n None110 ∗

111 ∗ \ h t m l o n l y \ e n d h t m l o n l y112 ∗ ˜ TVPnaocommand d e s t r u c t o r f r e e d a t a .113 ∗ \ h t m l o n l y \ e n d h t m l o n l y114 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /115 TVPnaocommand : : ˜ TVPnaocommand ( )116 {117 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)118 qDebug ( ) << ”TVPnaocommand : : ˜ TVPnaocommand” << f l u s h ;119 # e n d i f120 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗121 ∗ Free l i s t o f s e n s o r s122 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /123 d e l e t e ns ;124 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗125 ∗ Free mot ion126 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /127 d e l e t e nm ;128 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗129 ∗ Free command c o n s t r u c t i o n130 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /131 d e l e t e t c c ;132 }133

134 /∗ ∗135 ∗ \ b r i e f TVPrespond r e s p o n d s t o s e n s o r y d a t a .136 ∗ \ a u t h o r Tino Lourens137 ∗ \ d a t e 10 Apr 2010138 ∗ \ v e r s i o n 08 May 2010139 ∗ \param d i n p u t nao s e n s o r y d a t a .140 ∗ \param r s i n p u t nao r e c o g n i z e d s e n t e n c e o r word ( s ) .141 ∗ \ r e t u r n command s t r i n g142 ∗143 ∗ \ h t m l o n l y \ e n d h t m l o n l y144 ∗ TVPrespond r e s p o n d s t o s e n s o r y d a t a .145 ∗ \ h t m l o n l y \ e n d h t m l o n l y146 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /147 S t r i n g TVPnaocommand : : TVPrespond ( Data5D d ,148 S t r i n g r s )149 {150 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)151 qDebug ( ) << ”TVPnaocommand : : TVPrespond ” << f l u s h ;152 # e n d i f153 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗154 ∗ Update : i n c r e m e n t i n d e x155 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /156 i n d e x ++;157 i n d e x = i n d e x % 3 ;158 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗159 ∗ I n i t r e s p o n s e s t r i n g160 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /161 Q S t r i n g s = ” l e d o f f ( A l l l e d s ) &” ;162 i f ( i n d e x == 0)163 s += ” [ l e d o n ( Lef tFaceLedsRed ) | l e d o n ( Righ tFaceLedsRed ) ] ” ;164 e l s e165 i f ( i n d e x == 1)166 s += ” [ l e d o n ( L e f tF ac eL ed sG re en ) | l e d o n ( Righ tFaceLedsGreen ) ] ” ;167 e l s e168 i f ( i n d e x == 2)169 s += ” [ l e d o n ( L e f t F a c e L e d s B l u e ) | l e d o n ( R ig h tF ace Led sB l ue ) ] ” ;

170 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗171 ∗ e v a l u a t e i n p u t d a t a172 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /173 i f ( ( d . d a t a t y p e == DATA5D FLOAT) &&174 ( ( d .w == ns−>s i z e ( ) ) | | ( d .w == ns−>s i z e ( ) − 2) ) && / / 2 s o n a r v a l u e s175 ( d . h == 1) && ( d . d == 1) && ( d . t == 1) && ( d . e == 1) )176 {177 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗178 ∗ d a t a i s s e n s o r y d a t a179 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /180 f l o a t ∗ d a t a = ( f l o a t ∗ ) d . d a t a ;181 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗182 ∗ e v a l u a t e r e c o g n i z e d s e n t e n c e183 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /184 Q S t r i n g r s s = ” ” ;185 f o r ( i n t i = 0 ; i < r s . l e n ; i ++)186 r s s += r s . s [ i ] ;187 i f ( d a t a [ s p e e c h D e t e c t e d ] > 0 . 5 ) / / no s pe e ch = 0 ; s pe e ch = 1188 i f ( d a t a [ r ecogn izedWords ] > 0 . 2 ) / / c o n f i d e n c e l e v e l 0−1189 {190 /∗191 c o u t << ” Recognized [ ” << r s s . t o S t d S t r i n g ( ) << ” ] w i th c e r t a i n t y ”192 << d a t a [ r ecogn izedWords ] ∗ 100 << ”%\n ” << f l u s h ;193 ∗ /194 r s s . r e p l a c e ( ” ” , ” ” ) ;195 i f ( r s s . l e n g t h ( ) > 1)196 i f ( ( f a b s ( l a s t C o n f i d e n c e L e v e l −197 d a t a [ r ecogn izedWords ] ) > 0 . 0 0 0 1 ) | |198 ( ( f a b s ( l a s t C o n f i d e n c e L e v e l −199 d a t a [ r ecogn izedWords ] ) <= 0 . 0 0 0 1 ) &&200 ( l a s t S e n t e n c e != r s s ) ) )201 {202 i f ( s . l e n g t h ( ) > 0)203 s += ”&” ;204 s += ” [ say ( Y o u s a i d ” + r s s + ” ) ] ” ;205 l a s t C o n f i d e n c e L e v e l = d a t a [ r ecogn izedWords ] ;206 l a s t S e n t e n c e = r s s ;207 }208 }209 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗210 ∗ 0 = LFootBumperLef t v a l u e i s 0 o r 1 ( pushed )211 ∗ 1 = LFootBumperRight v a l u e i s 0 o r 1 ( pushed )212 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /213 i f ( ( d a t a [ LFootBumperLef t ] > 0 . 5 f ) &&214 ( d a t a [ LFootBumperRight ] > 0 . 5 f ) )215 {216 i f ( s . l e n g t h ( ) > 0)217 s += ”&” ;218 s += ” [ say ( L e f t f o o t ) ” ;219 s += ” | ” ;220 s += ”move ( HeadYaw , 35 , 500) ] ” ;221 }222 e l s e223 i f ( d a t a [ 0 ] > 0 . 5 f )224 {225 i f ( s . l e n g t h ( ) > 0)226 s += ”&” ;227 s += ” say ( L e f t f o o t l e f t ) ” ;228 }

229 e l s e230 i f ( d a t a [ 1 ] > 0 . 5 f )231 {232 i f ( s . l e n g t h ( ) > 0)233 s += ”&” ;234 s += ” say ( L e f t f o o t r i g h t ) ” ;235 }236 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗237 ∗ 2 = RFootBumperLeft v a l u e i s 0 o r 1 ( pushed )238 ∗ 3 = RFootBumperRight v a l u e i s 0 o r 1 ( pushed )239 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /240 i f ( ( d a t a [ RFootBumperLeft ] > 0 . 5 f ) &&241 ( d a t a [ RFootBumperRight ] > 0 . 5 f ) )242 {243 i f ( s . l e n g t h ( ) > 0)244 s += ”&” ;245 s += ” [ say ( R i g h t f o o t ) ” ;246 s += ” | ” ;247 s += ”move ( HeadYaw , −35, 500) ] ” ;248 }249 e l s e250 i f ( d a t a [ 2 ] > 0 . 5 f )251 {252 i f ( s . l e n g t h ( ) > 0)253 s += ”&” ;254 s += ” say ( R i g h t f o o t l e f t ) ” ;255 }256 e l s e257 i f ( d a t a [ 3 ] > 0 . 5 f )258 {259 i f ( s . l e n g t h ( ) > 0)260 s += ”&” ;261 s += ” say ( R i g h t f o o t r i g h t ) ” ;262 }263 }264 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗265 ∗ o u t p u t r e s u l t i n g s t r i n g266 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /267 S t r i n g s t r ;268 s t r . l e n = s . l e n g t h ( ) ;269 i f ( s t r . l e n > 0)270 {271 s t r . s = new c h a r [ s t r . l e n ] ;272 s t r i n g s s = s . t o S t d S t r i n g ( ) ;273 f o r ( i n t i = 0 ; i < s t r . l e n ; i ++)274 s t r . s [ i ] = s s [ i ] ;275 }276 e l s e277 s t r . s = NULL;278 r e t u r n s t r ;279 }280

281 /∗ ∗282 ∗ \ b r i e f TVPwalkArc walks an a r c o r s e t s up b e h a v i o r t o walk .283 ∗ \ a u t h o r Tino Lourens284 ∗ \ d a t e 05 Jun 2010285 ∗ \ v e r s i o n 05 Jun 2010286 ∗ \param d i n p u t s e n s o r y d a t a287 ∗ \param a n g l e a n g l e i n d e g r e e s

288 ∗ \param r a d i u s r a d i u s i n m e t e r s289 ∗ \param d u r a t i o n t ime t o pe r fo rm a r c g i v e n i t s a n g l e and r a d i u s290 ∗ \ r e t u r n S t r i n g t h a t p e r f o r m s an a c t i o n291 ∗292 ∗ \ h t m l o n l y \ e n d h t m l o n l y293 ∗ TVPwalkArc walks an a r c o r s e t s up b e h a v i o r t o walk .294 ∗ \ h t m l o n l y \ e n d h t m l o n l y295 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /296 Q S t r i n g TVPnaocommand : : TVPwalkArc ( Data5D d ,297 f l o a t ang le ,298 f l o a t r a d i u s ,299 f l o a t d u r a t i o n )300 {301 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)302 qDebug ( ) << ”TVPnaocommand : : TVPwalkArc ” << f l u s h ;303 # e n d i f304 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗305 ∗ G e n e r a t e command306 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /307 Q S t r i n g c = ” ” ;308 i f ( TVPgetBodyPose ( d ) == 1) / / S t a n d i n g309 {310 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗311 ∗ Robot i s s t a n d i n g312 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /313 Q S t r i n g a = Q S t r i n g ( ”%1” ) . a r g ( a n g l e ) ;314 Q S t r i n g r = Q S t r i n g ( ”%1” ) . a r g ( r a d i u s ) ;315 Q S t r i n g d = Q S t r i n g ( ”%1” ) . a r g ( d u r a t i o n ) ;316 / / needs t o be t u n e d work t o be pe r fo rmed by Jan G i l l e s e n ?317 c = ” walka ( ” + a + ” , ” + r + ” , ” + d + ” ) ” ;318 / / c o u t << c . t o S t d S t r i n g ( ) << e n d l << f l u s h ;319 }320 e l s e321 c = TVPstandup ( d ) ;322 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗323 ∗ Re tu rn s t r i n g324 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /325 r e t u r n c ;326 }327

328 /∗ ∗329 ∗ \ b r i e f TVPgetBodyPose g e t s t h e p o s i t i o n o f t h e body .330 ∗ \ a u t h o r Tino Lourens331 ∗ \ d a t e 05 Jun 2010332 ∗ \ v e r s i o n 23 Apr 2011333 ∗ \param d i n p u t s e n s o r y d a t a334 ∗ \ r e t u r n v a l u e f o r body pose −1. .8 = unknown , back , s t a n d crouch , knee , s i t335 ∗ b e l l y l e f t , r i g h t , f r o g336 ∗337 ∗ \ h t m l o n l y \ e n d h t m l o n l y338 ∗ TVPgetBodyPose g e t s t h e p o s i t i o n o f t h e body .339 ∗340 ∗ Note t h a t v e r s i o n 1 . 6 . 1 3 does n o t know t h e f r o g p o s i t i o n341 ∗ \ h t m l o n l y \ e n d h t m l o n l y342 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /343 i n t TVPnaocommand : : TVPgetBodyPose ( Data5D d )344 {345 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)346 qDebug ( ) << ”TVPnaocommand : : TVPgetBodyPose ” << f l u s h ;

347 # e n d i f348 / / v1 . 6 . 1 3 body pose349 /∗350 # g e t v a l u e Torso Angle Y & X from Memory t o check i f t h e r o b o t i s on t h e

b e l l y o r on t h e back351

352 rAngleX , rAngleY = ALMemory . g e t L i s t D a t a ( [ ” Device / S u b D e v i c e L i s t /I n e r t i a l S e n s o r / AngleX / Sen so r / Value ” , ” Device / S u b D e v i c e L i s t /I n e r t i a l S e n s o r / AngleY / Sen so r / Value ” ] ) ;

353

354 s e l f . d e b u g P r i n t ( ” D e t e c t P o s i t i o n : rAngleX : %5.3 f ; rAngleY : %5.3 f ” % (rAngleX , rAngleY ) ) ;

355

356 r J o i n t V a l H i p P i t c h = ( ALMotion . g e t A n g l e s ( ” LHipP i t ch ” , True ) [ 0 ] +ALMotion . g e t A n g l e s ( ” RHipPi t ch ” , True ) [ 0 ] ) / 2 . ;

357 r J o i n t V a l K n e e P i t c h = ( ALMotion . g e t A n g l e s ( ” LKneePi tch ” , True ) [ 0 ] +ALMotion . g e t A n g l e s ( ” RKneePi tch ” , True ) [ 0 ] ) / 2 . ;

358 r J o i n t V a l A n k l e P i t c h = ( ALMotion . g e t A n g l e s ( ” LAnk leP i t ch ” , True ) [ 0 ] +ALMotion . g e t A n g l e s ( ” RAnkleP i t ch ” , True ) [ 0 ] ) / 2 . ;

359

360 i f ( r J o i n t V a l H i p P i t c h > −0.9 and r J o i n t V a l K n e e P i t c h > 1 . 2 andr J o i n t V a l A n k l e P i t c h > 0 . 3 and rAngleY < 1 . 2 ) :

361 s e l f . d e b u g P r i n t ( ” o u t p u t k n e e l i n g ” ) ;362 s e l f . o u t p u t k n e e l i n g ( ) ;363 r e t u r n ;364

365 i f rAngleY > −0.82 and rAngleY < 0 . 9 and abs ( rAngleX ) < 0 . 5 :366 s e l f . d e b u g P r i n t ( ” t h e t o r s o i s v e r t i c a l ” ) ;367

368 s t r S p e c i f J o i n t N a m e = ” LHipRol l ” ;369 r J o i n t V a l H i p R o l l = ALMotion . g e t A n g l e s ( s t r S p e c i f J o i n t N a m e , True ) [ 0 ] ;370 s t r S p e c i f J o i n t N a m e = ” LHipYawPitch ” ;371 r J o i n t V a l L H i p Y a w P i t c h = ALMotion . g e t A n g l e s ( s t r S p e c i f J o i n t N a m e , True )

[ 0 ] ;372

373 s e l f . d e b u g P r i n t ( ” D e t e c t P o s i t i o n : H i p P i t c h : %5.3 f ; KneeP i t ch : %5.3 f ;A n k l e P i t c h : %5.3 f ; LHipRol l : %5.3 f ; LHipYawPitch : %5.3 f ” % (r J o i n t V a l H i p P i t c h , r J o i n t V a l K n e e P i t c h , r J o i n t V a l A n k l e P i t c h ,r J o i n t V a l H i p R o l l , r J o i n t V a l L H i p Y a w P i t c h ) ) ;

374 i f ( r J o i n t V a l H i p P i t c h < −0.45 and r J o i n t V a l K n e e P i t c h > 1 . 8 andr J o i n t V a l A n k l e P i t c h < −0.7 ) :

375 s e l f . d e b u g P r i n t ( ” o u t p u t c r o u c h ” ) ;376 s e l f . o u t p u t c r o u c h ( ) ;377 e l i f ( r J o i n t V a l H i p P i t c h < −1.2 o r ( r J o i n t V a l H i p P i t c h < −0.8 and

r J o i n t V a l K n e e P i t c h < 0 . 5 ) ) :378 s e l f . d e b u g P r i n t ( ” o u t p u t s i t ” ) ;379 s e l f . o u t p u t s i t ( ) ;380 e l i f ( r J o i n t V a l H i p P i t c h < 0 . 5 and r J o i n t V a l K n e e P i t c h < 1 . 1 and

r J o i n t V a l A n k l e P i t c h < 0 . 1 and r J o i n t V a l H i p R o l l < 0 . 5 ) : # Alma :a v a n t l e h i p p i t c h e t a t a 0 . 3 mais p o u r q u o i pas 0 . 5 ? idem pourr J o i n t V a l H i p R o l l ( pour p o s i t i o n avec une jambe s u r l e c o t )

381 s e l f . d e b u g P r i n t ( ” o u t p u t s t a n d i n g ” ) ;382 s e l f . o u t p u t s t a n d i n g ( ) ;383 e l i f ( r J o i n t V a l H i p P i t c h < −1.2 and r J o i n t V a l K n e e P i t c h > 1 . 1 ) :384 # p r e d e f i n e d p o s i t i o n : ” i n t e r m e d i a r y ” => s t a n d , s i t o r l y i n g (

a l r e a d y done i n t h e h o r i z o n t a l c a s e )385 i f ( rAngleY > −0.7 and rAngleY < 0 . 1 ) :386 s e l f . d e b u g P r i n t ( ” o u t p u t s i t 2 ” ) ;387 s e l f . o u t p u t s i t ( ) ;

388 e l i f ( rAngleY > 0 . 4 ) :389 s e l f . d e b u g P r i n t ( ” o u t p u t s t a n d i n g 2 ” ) ;390 s e l f . o u t p u t s t a n d i n g ( ) ;391 e l s e :392 s e l f . d e b u g P r i n t ( ” ou tpu t unknown1 ” ) ;393 s e l f . ou tpu t unknown ( ) ;394 e l s e :395 s e l f . d e b u g P r i n t ( ” ou tpu t unknown2 ” ) ;396 s e l f . ou tpu t unknown ( ) ;397 e l s e :398 s e l f . d e b u g P r i n t ( ” t h e t o r s o i s h o r i z o n t a l ” ) ;399 s e l f . d e b u g P r i n t ( ” D e t e c t P o s i t i o n : H i p P i t c h : %5.3 f ; KneeP i t ch : %5.3 f ” %

( r J o i n t V a l H i p P i t c h , r J o i n t V a l K n e e P i t c h ) ) ;400 rAng leX Side = 1 . 3 ;401 rEr rorMax = 0 . 3 ;402 i f ( rAngleY > 1 . 0 ) :403 s e l f . d e b u g P r i n t ( ” o u t p u t b e l l y ” ) ;404 s e l f . o u t p u t b e l l y ( ) ;405 e l i f ( rAngleY < −0.85 ) :406 s e l f . d e b u g P r i n t ( ” o u t p u t b a c k ” ) ;407 s e l f . o u t p u t b a c k ( ) ;408 e l i f ( rAngleY > −0.6 and rAngleY < 0 . 6 and409 (410 ( rAngleX > rAngleX Side−rEr rorMax and rAngleX <

rAng leX Side + rErrorMax )411 or ( rAngleX < −( rAngleX Side−rEr rorMax ) and rAngleX > −(

rAng leX Side + rErrorMax ) )412 )413 ) :414 # on t h e r i g h t s i d e = ( 0 . 0 , 1 . 3 5 ) , on t h e l e f t s i d e : ( 0 . 0 , −1.30)415 i f ( rAngleX > 0 ) :416 s e l f . d e b u g P r i n t ( ” o u t p u t s i d e r i g h t ” ) ;417 s e l f . o u t p u t s i d e r i g h t ( ) ;418 e l s e :419 s e l f . d e b u g P r i n t ( ” o u t p u t s i d e l e f t ” ) ;420 s e l f . o u t p u t s i d e l e f t ( ) ;421 e l s e :422 s e l f . d e b u g P r i n t ( ” ou tpu t unknown3 ” ) ;423 s e l f . ou tpu t unknown ( ) ;424 ∗ /425 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗426 ∗ e v a l u a t e i n p u t d a t a427 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /428 i f ( ( d . d a t a t y p e == DATA5D FLOAT) &&429 ( ( d .w == ns−>s i z e ( ) ) | | ( d .w == ns−>s i z e ( ) − 2) ) && / / 2 s o n a r v a l u e s430 ( d . h == 1) && ( d . d == 1) && ( d . t == 1) && ( d . e == 1) )431 {432 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗433 ∗ d a t a i s s e n s o r y d a t a434 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /435 f l o a t ∗ d a t a = ( f l o a t ∗ ) d . d a t a ;436 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗437 ∗ Get r o b o t i n f o438 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /439 f l o a t angleX = d a t a [ GyrAngleX ] ;440 f l o a t angleY = d a t a [ GyrAngleY ] ;441 f l o a t l h i p p i t c h = d a t a [ LHipP i t ch ] ;442 f l o a t l k n e e p i t c h = d a t a [ LKneePi tch ] ;443 f l o a t l a n k l e p i t c h = d a t a [ LAnk leP i t ch ] ;

444 f l o a t r h i p p i t c h = d a t a [ RHipPi tch ] ;445 f l o a t r k n e e p i t c h = d a t a [ RKneePi tch ] ;446 f l o a t r a n k l e p i t c h = d a t a [ RAnkleP i t ch ] ;447 f l o a t h i p p i t c h = ( r h i p p i t c h + l h i p p i t c h ) / 2 . 0 f ;448 f l o a t k n e e p i t c h = ( r k n e e p i t c h + l k n e e p i t c h ) / 2 . 0 f ;449 f l o a t a n k l e p i t c h = ( r a n k l e p i t c h + l a n k l e p i t c h ) / 2 . 0 f ;450 f l o a t h i p r o l l = d a t a [ LHipRol l ] ;451 # i f d e f i n e d (TVPNAOCOMMAND DEBUG1) | | d e f i n e d (ALL DEBUG)452 c o u t << ” angleX ” << angleX453 << ” angleY ” << angleY454 << ” h i p p i t c h ” << h i p p i t c h455 << ” k n e e p i t c h ” << k n e e p i t c h456 << ” a n k l e p i t c h ” << a n k l e p i t c h457 << ” h i p r o l l ” << h i p r o l l458 << e n d l << f l u s h ;459 # e n d i f460 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗461 ∗ K n e e l i n g462 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /463 i f ( ( h i p p i t c h > −0.9) && ( k n e e p i t c h > 1 . 2 ) && ( a n k l e p i t c h > 0 . 3 ) &&464 ( angleY < 1 . 2 ) )465 r e t u r n 3 ; / / K n e e l i n g466 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗467 ∗ V e r t i c a l p o s i t i o n468 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /469 i f ( ( angleY > −0.82) && ( angleY < 0 . 9 ) && ( f a b s ( angleX ) < 0 . 5 ) )470 {471 / / i f ( ( h i p p i t c h < 0 . 4 5 ) && ( k n e e p i t c h > 1 . 8 ) && ( a n k l e p i t c h < −0.7) )472 i f ( ( h i p p i t c h < 0 . 4 5 ) && ( k n e e p i t c h > 1 . 5 ) && ( a n k l e p i t c h < −0.7) )473 r e t u r n 2 ; / / Crouch474 i f ( ( h i p p i t c h < −1.2) && ( h i p p i t c h < −0.8) && ( k n e e p i t c h < 0 . 5 ) )475 r e t u r n 4 ; / / S i t476 i f ( ( h i p p i t c h < 0 . 5 ) && ( k n e e p i t c h < 1 . 1 ) && ( a n k l e p i t c h < 0 . 1 ) &&477 ( h i p r o l l < 0 . 5 ) ) / / h i p p i t c h / h i p r o l l bo th < 0 . 3 ?478 r e t u r n 1 ; / / S t a n d i n g479 i f ( ( h i p p i t c h < −1.2) && ( k n e e p i t c h > 1 . 1 ) )480 {481 / / p r e d e f i n e d p o s i t i o n : ” i n t e r m e d i a r y ”482 i f ( ( angleY > −0.7) && ( angleY < 0 . 1 ) )483 r e t u r n 4 ; / / S i t ( second one )484 i f ( angleY > 0 . 4 )485 r e t u r n 1 ; / / S t a n d i n g ( second one )486 # i f d e f i n e d (TVPNAOCOMMAND DEBUG1) | | d e f i n e d (ALL DEBUG)487 c o u t << ”Unknown 1 ” << e n d l << f l u s h ;488 # e n d i f489 r e t u r n −1; / / Unknown490 }491 # i f d e f i n e d (TVPNAOCOMMAND DEBUG1) | | d e f i n e d (ALL DEBUG)492 c o u t << ”Unknown 2 ” << e n d l << f l u s h ;493 # e n d i f494 r e t u r n −1; / / Unknown ( second one )495 }496 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗497 ∗ H o r i z o n t a l p o s i t i o n498 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /499 i f ( angleY > 1 . 0 )500 r e t u r n 5 ; / / B e l l y501 i f ( angleY < −0.85)502 r e t u r n 0 ; / / Back

503 i f ( ( f a b s ( angleY ) < 0 . 6 ) &&504 ( ( ( angleX > 1 . 0 ) && ( angleX > 1 . 6 ) ) | |505 ( ( angleX < −1.0) && ( angleX > −1.6) ) ) )506 {507 i f ( angleX > 0)508 r e t u r n 7 ; / / R i g h t s i d e509 r e t u r n 6 ; / / L e f t s i d e510 }511 # i f d e f i n e d (TVPNAOCOMMAND DEBUG1) | | d e f i n e d (ALL DEBUG)512 c o u t << ”Unknown 3 ” << e n d l << f l u s h ;513 # e n d i f514 r e t u r n −1; / / Unknown ( t h i r d one )515 }516 # i f d e f i n e d (TVPNAOCOMMAND DEBUG1) | | d e f i n e d (ALL DEBUG)517 c o u t << ”Unknown 4 ” << e n d l << f l u s h ;518 # e n d i f519 r e t u r n −1; / / Unknown f a i l u r e o f d a t a520 }521

522 /∗ ∗523 ∗ \ b r i e f TVPstandup l e t s t h e r o b o t s t a n d u p524 ∗ \ a u t h o r Tino Lourens525 ∗ \ d a t e 05 Jun 2010526 ∗ \ v e r s i o n 23 Apr 2011527 ∗ \param d i n p u t s e n s o r y d a t a528 ∗ \ r e t u r n S t r i n g t h a t p e r f o r m s a c t i o n t o s t a n d up529 ∗530 ∗ \ h t m l o n l y \ e n d h t m l o n l y531 ∗ TVPstandup l e t s t h e r o b o t s t a n d u p532 ∗ \ h t m l o n l y \ e n d h t m l o n l y533 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /534 Q S t r i n g TVPnaocommand : : TVPstandup ( Data5D d )535 {536 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)537 qDebug ( ) << ”TVPnaocommand : : TVPstandup ” << f l u s h ;538 # e n d i f539 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗540 ∗ e v a l u a t e i n p u t d a t a541 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /542 i n t pos = TVPgetBodyPose ( d ) ;543 # i f d e f i n e d (TVPNAOCOMMAND DEBUG1) | | d e f i n e d (ALL DEBUG)544 c o u t << ” C u r r e n t p o s i t i o n i n s t a n d u p r o u t i n e ” << pos << e n d l << f l u s h ;545 # e n d i f546 Q S t r i n g c = ” ” ;547 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗548 ∗ G e n e r a t e command depend ing on p o s i t i o n549 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /550 s w i t c h ( pos )551 {552 c a s e 0 : / / Back553 c = nm−>backToStand ( ) ;554 b r e a k ;555 c a s e 1 : / / S t and556 r e t u r n ” ” ;557 b r e a k ;558 c a s e 2 : / / Crouch559 c = nm−>c rouchToStand ( ) ;560 b r e a k ;561 c a s e 3 : / / Kneel

562 c a s e 5 : / / B e l l y563 c a s e 8 : / / Frog564 c = nm−>b e l l y T o S t a n d ( ) ;565 b r e a k ;566 c a s e 4 : / / S i t567 c = nm−>s i t T o C r o u c h ( ) ;568 b r e a k ;569 c a s e 6 : / / L e f t s i d e570 c a s e 7 : / / R i g h t s i d e571 c = nm−>s i d e T o B e l l y ( ) ;572 b r e a k ;573 d e f a u l t : / / −1 Unknown574 r e t u r n ” ” ;575 }576 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗577 ∗ Wait a second a f t e r t h e a c t i o n t o e n s u r e a p r o p e r r o b o t pose578 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /579 c += ”& w a i t ( 1 0 0 0 ) ” ;580 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗581 ∗ Re tu rn command582 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /583 r e t u r n c ;584 }585

586 /∗ ∗587 ∗ \ b r i e f TVPsitdown l e t s t h e r o b o t s i t down588 ∗ \ a u t h o r Tino Lourens589 ∗ \ d a t e 05 Jun 2010590 ∗ \ v e r s i o n 23 Apr 2011591 ∗ \param d i n p u t s e n s o r y d a t a592 ∗ \ r e t u r n S t r i n g t h a t p e r f o r m s a c t i o n t o s t a n d up593 ∗594 ∗ \ h t m l o n l y \ e n d h t m l o n l y595 ∗ TVPsitdown l e t s t h e r o b o t s i t down596 ∗ \ h t m l o n l y \ e n d h t m l o n l y597 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /598 Q S t r i n g TVPnaocommand : : TVPsitdown ( Data5D d )599 {600 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)601 qDebug ( ) << ”TVPnaocommand : : TVPsitdown ” << f l u s h ;602 # e n d i f603 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗604 ∗ e v a l u a t e i n p u t d a t a605 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /606 i n t pos = TVPgetBodyPose ( d ) ;607 # i f d e f i n e d (TVPNAOCOMMAND DEBUG1) | | d e f i n e d (ALL DEBUG)608 c o u t << ” C u r r e n t p o s i t i o n i n s i t down r o u t i n e ” << pos << e n d l << f l u s h ;609 # e n d i f610 Q S t r i n g c = ” ” ;611 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗612 ∗ G e n e r a t e command depend ing on p o s i t i o n613 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /614 s w i t c h ( pos )615 {616 c a s e 0 : / / Back617 c = nm−>b a c k T o S i t ( ) ;618 b r e a k ;619 c a s e 1 : / / S t and620 c a s e 2 : / / Crouch

621 c = nm−>c r o u c h T o S i t ( ) ;622 b r e a k ;623 c a s e 4 : / / S i t624 r e t u r n ” ” ;625 b r e a k ;626 c a s e 3 : / / Kneel627 c a s e 5 : / / B e l l y628 c a s e 8 : / / Frog629 c = nm−>be l l yToCrouch ( ) ;630 b r e a k ;631 c a s e 6 : / / L e f t s i d e632 c a s e 7 : / / R i g h t s i d e633 c = nm−>s i d e T o B e l l y ( ) ;634 b r e a k ;635 d e f a u l t : / / −1 Unknown636 r e t u r n ” ” ;637 }638 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗639 ∗ Wait a second a f t e r t h e a c t i o n t o e n s u r e a p r o p e r r o b o t pose640 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /641 c += ”& w a i t ( 1 0 0 0 ) ” ;642 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗643 ∗ Re tu rn command644 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /645 r e t u r n c ;646 }647

648 /∗ ∗649 ∗ \ b r i e f TVPheadmenu p e r f o r m s head menu demo .650 ∗ \ a u t h o r S t i j n Boere and Tino Lourens651 ∗ \ d a t e 10 Apr 2010652 ∗ \ v e r s i o n 09 Apr 2011653 ∗ \param d i n p u t nao s e n s o r y d a t a .654 ∗ \ r e t u r n command s t r i n g655 ∗656 ∗ \ h t m l o n l y \ e n d h t m l o n l y657 ∗ TVPheadmenu p e r f o r m s head menu demo .658 ∗ \ h t m l o n l y \ e n d h t m l o n l y659 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /660 Q S t r i n g TVPnaocommand : : TVPheadmenu ( Data5D d )661 {662 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)663 qDebug ( ) << ”TVPnaocommand : : TVPheadmenu ” << f l u s h ;664 # e n d i f665 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗666 ∗ I n i t r e s p o n s e s t r i n g667 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /668 Q S t r i n g s = ” ” ;669 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗670 ∗ I n i t phase i n d e x i s 0671 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /672 i f ( indexMenu == −1)673 s += ” p l a y ( / home / nao / cus tomsound / d r u k t o e t s e m o t i e . mp3 , 100) ” ;674 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗675 ∗ e v a l u a t e i n p u t d a t a676 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /677 i f ( ( d . d a t a t y p e == DATA5D FLOAT) &&678 ( ( d .w == ns−>s i z e ( ) ) | | ( d .w == ns−>s i z e ( ) − 2) ) && / / 2 s o n a r v a l u e s679 ( d . h == 1) && ( d . d == 1) && ( d . t == 1) && ( d . e == 1) )

680 {681 f l o a t ∗ d a t a = ( f l o a t ∗ ) d . d a t a ;682 f l o a t P re s sHeadTouchFron t = d a t a [ HeadTouchFront ] ;683 f l o a t PressHeadTouchRear = d a t a [ HeadTouchRear ] ;684 f l o a t PressHeadTouchMiddle = d a t a [ HeadTouchMiddle ] ;685 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗686 ∗ F r o n t b u t t o n pushed687 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /688 i f ( ( P re s sHeadTouchFron t > 0 . 5 f ) &&689 ( PressHeadTouchRear < 0 . 5 f ) &&690 ( PressHeadTouchMiddle < 0 . 5 f ) )691 {692 indexMenu ++;693 i f ( indexMenu == nm−>TVPposeElements ( ) )694 indexMenu = 0 ;695 s = ” say ( ” + nm−>TVPposeText ( indexMenu ) + ” ) ” ;696 }697 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗698 ∗ Rear b u t t o n pushed699 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /700 i f ( ( PressHeadTouchRear > 0 . 5 f ) &&701 ( P re s sHeadTouchFron t < 0 . 5 f ) &&702 ( PressHeadTouchMiddle < 0 . 5 f ) )703 {704 i f ( indexMenu < 0)705 indexMenu = 0 ;706 e l s e707 indexMenu−−;708 i f ( indexMenu < 0)709 indexMenu = nm−>TVPposeElements ( ) − 1 ;710 s = ” say ( ” + nm−>TVPposeText ( indexMenu ) + ” ) ” ;711 }712 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗713 ∗ Middle b u t t o n pushed714 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /715 i f ( ( PressHeadTouchMiddle > 0 . 5 f ) &&716 ( PressHeadTouchRear < 0 . 5 f ) &&717 ( P re s sHeadTouchFron t < 0 . 5 f ) )718 {719 i f ( indexMenu < 0)720 indexMenu = 0 ;721 TVPs t r i nggen t s g ;722 Q S t r i n g s t i f f = nm−>T V P s t i f f n e s s ( 1 . 0 , 500) ;723 s = ” say (OK) &” ;724 s += t s g . T V P s e r i a l ( s t i f f , nm−>TVPpose ( indexMenu ) ) ;725 }726 }727 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗728 ∗ o u t p u t r e s u l t i n g s t r i n g729 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /730 r e t u r n s ;731 }732

733 /∗ ∗734 ∗ \ b r i e f commandSta teStr ingHeadmenu headmenu demo .735 ∗ \ a u t h o r S t i j n Boere and Tino Lourens736 ∗ \ d a t e 23 Apr 2011737 ∗ \ v e r s i o n 23 Apr 2011738 ∗ \param i s t a t e s i n p u t s t a t e s

739 ∗ \param icommands i n p u t commands740 ∗ \param i d o f f s e t i d o f f s e t o f s t a t e741 ∗ \param o s t a t e s o u t p u t s t a t e s742 ∗ \param ocommands o u t p u t commands743 ∗ \param d i n p u t nao s e n s o r y d a t a744 ∗ \ r e t u r n none745 ∗746 ∗ \ h t m l o n l y \ e n d h t m l o n l y747 ∗ commandSta teSt r ingHeadmenu headmenu demo .748 ∗ \ h t m l o n l y \ e n d h t m l o n l y749 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /750 vo id TVPnaocommand : : commandSta teStr ingHeadmenu ( Data5D i s t a t e s ,751 S t r i n g icommands ,752 i n t i d o f f s e t ,753 Data5D &o s t a t e s ,754 S t r i n g &ocommands ,755 Data5D d )756 {757 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)758 qDebug ( ) << ”TVPnaocommand : : commandStringHeadmenu ” << f l u s h ;759 # e n d i f760 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗761 ∗ Headmenu has N e l e m e n t s i n d e x e d 0 . . . 4N−1762 ∗ M u l t i p l e s t a t e s i n one r o u t i n e :763 ∗ − S t a r t i n g i n d e x i d = o f f s e t + 1764 ∗ − Other i n d i c e s i d = o f f s e t + 2 t o o f f s e t + 1 + 4N765 ∗766 ∗ Every i n d e x u s e s 4 s t a t e s d e p e n d e n t on i n p u t767 ∗ s t a t e M=4 i + o f f s e t + 2768 ∗ −None s t a t e s =M; commm=””769 ∗ −FB s t a t e s =M+1; comm=” say . . . ”770 ∗ −MB s t a t e s =M+2; comm= c t i o n . . .771 ∗ −RB s t a t e =M+3; comm=” say . . . ”772 ∗ s t a t e M+1773 ∗ − a l l s t a t e s =M+4; comm=””774 ∗ s t a t e M+2775 ∗ − a l l s t a t e s =M; comm=””776 ∗ s t a t e M+3777 ∗ − a l l s t a t e s =M−4; comm=””778 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /779 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗780 ∗ The c o n c e p t o f s t a t e s i s t h a t t h e l a t e s t i n f o r m a t i o n i s p r o v i d e d .781 ∗ B u t t o n s need t o be e v a l u a t e d as an e v e n t .782 ∗ −Once p r e s s e d one s h o u l d keep t h a t s t a t u s u n t i l i t has been p r o c e s s e d .783 ∗ We use a c l a s s v a r i a b l e t o keep t h e b u t t o n s e t t i n g s fbp rbp mbp ;784 ∗ −For v i s u a l i n f o r m a t i o n t h e l a t e s t i n f o r m a t i o n i s d e s i r e d u s u a l l y .785 ∗ The l a t e s t read−o u t o f s e n s o r y i n f o r m a t i o n i s p r o v i d e d and can be used786 ∗ −In c a s e h s t o r y i s d e s i r e d a c l a s s v a r i a b l e i s r e q u i r e d t o keep t r a c k787 ∗ of t h i s h i s t o r y .788 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /789 i f ( ( d . d a t a t y p e == DATA5D FLOAT) &&790 ( ( d .w == ns−>s i z e ( ) ) | |791 ( d .w == ns−>s i z e ( ) − 2) ) && / / 2 s o n a r v a l .792 ( d . h == 1) && ( d . d == 1) && ( d . t == 1) && ( d . e == 1) )793 {794 f l o a t ∗ d a t a = ( f l o a t ∗ ) d . d a t a ;795 f l o a t P re s sHeadTouchFron t = d a t a [ HeadTouchFront ] ;796 f l o a t PressHeadTouchRear = d a t a [ HeadTouchRear ] ;797 f l o a t PressHeadTouchMiddle = d a t a [ HeadTouchMiddle ] ;

798 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗799 ∗ F r o n t b u t t o n pushed800 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /801 i f ( ( P re s sHeadTouchFron t > 0 . 5 f ) &&802 ( PressHeadTouchRear < 0 . 5 f ) &&803 ( PressHeadTouchMiddle < 0 . 5 f ) )804 fbp = t r u e ;805 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗806 ∗ Rear b u t t o n pushed807 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /808 i f ( ( PressHeadTouchRear > 0 . 5 f ) &&809 ( P re s sHeadTouchFron t < 0 . 5 f ) &&810 ( PressHeadTouchMiddle < 0 . 5 f ) )811 rbp = t r u e ;812 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗813 ∗ Middle b u t t o n pushed814 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /815 i f ( ( PressHeadTouchMiddle > 0 . 5 f ) &&816 ( PressHeadTouchRear < 0 . 5 f ) &&817 ( P re s sHeadTouchFron t < 0 . 5 f ) )818 mbp = t r u e ;819 }820 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗821 ∗ C r e a t e an i n i t phase822 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /823 i f ( i n i t S t r e a m )824 {825 t c c = new TVPcommandConstruct ion ( ) ;826 i d = i d o f f s e t + 1 ; / / s e t u p s t a r t i n g s t a t e f o r head menu827 comm = ” p l a y ( / home / nao / cus tomsound / d r u k t o e t s e m o t i e . mp3 , 100) ” ;828 i n i t S t r e a m = f a l s e ;829 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d + 1) ; / / a c t i v a t e n e x t s t a t e830 }831 e l s e832 {833 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗834 ∗ we have a dynamic i d ; so g e t f i r s t a c t i v e s t a t e i n t h e r a n g e835 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /836 i n t l b = i d o f f s e t + 1 ;837 i n t ub = i d o f f s e t + 2 + 4 ∗ nm−>TVPposeElements ( ) ;838 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗839 ∗ g e t l i s t o f s t a t e s i n e x e c u t i o n ( = 0 )840 ∗ −t a k e t h e s t a t e s used i n t h i s r o u t i n e r a n g i n g from l b t o ub−1841 ∗ −i n c a s e t h e r e a r e a c t i v e s t a t e s ( by i d ) t a k e t h e f i r s t one .842 ∗ − i f no a c t i v e s t a t e use t h e f i r s t i d = l b843 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /844 QList ∗ a s t = t c c−>g e t S t a t e s ( i s t a t e s , lb , ub , 0 ) ;845 i f ( a s t−>s i z e ( ) > 0)846 i d = a s t−>a t ( 0 ) ;847 e l s e848 i d = l b ;849 d e l e t e a s t ;850 }851 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗852 ∗ l e t s e v a l u a t e what t o do853 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /854 i n t i n d e x = i d − i d o f f s e t − 2 ;855 i f ( i n d e x >= 0) / / a l l e x c e p t s t a r t856 {


857 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗858 ∗ i n d e x%4 == 1 , 2 , o r 3 a r e c au se d by p r e s s i n g t h e head b u t t o n s .859 ∗ Once i n such a s t a t e one s h o u l d b r i n g i t back t o a s t a t e where860 ∗ a b u t t o n a c t i o n can be pe r fo rmed a g a i n .861 ∗ −b u t t o n s t o d e f a u l t s t a t e ( f a l s e )862 ∗ −command = ””863 ∗ −b r i n g s t a t e depend ing on b u t t o n t o864 ∗ ’ l i d ’ midd le865 ∗ ’ l i d −1’ r e a r866 ∗ ’ l i d +1 ’ f r o n t b u t t o n .867 ∗ Note i d −> i n d e x = id−i d o f f s e t −2 −> l i d = i n d e x / 4 ( and i n d e x %4==0)868 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /869 i f ( i n d e x % 4 == 1)870 {871 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗872 ∗ F r o n t B u t t on p r e s s e d a c t i o n has been e x e c u t e d873 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /874 fbp = f a l s e ;875 rbp = f a l s e ;876 mbp = f a l s e ;877 comm = ” ” ;878 i f ( ( i n d e x / 4 ) == (nm−>TVPposeElements ( ) − 1) )879 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d o f f s e t +2) ; / / menu e l e m e n t 0880 e l s e881 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d +3) ; / / go to menu e l e m e n t i +1882 }883 e l s e884 i f ( i n d e x % 4 == 2)885 {886 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗887 ∗ Middle Bu t ton p r e s s e d a c t i o n has been e x e c u t e d888 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /889 fbp = f a l s e ;890 rbp = f a l s e ;891 mbp = f a l s e ;892 comm = ” ” ;893 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( id −2) ; / / s t a y a t menu e l e m e n t i894 }895 e l s e896 i f ( i n d e x % 4 == 3)897 {898 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗899 ∗ Rear Bu t ton p r e s s e d a c t i o n has been e x e c u t e d900 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /901 fbp = f a l s e ;902 rbp = f a l s e ;903 mbp = f a l s e ;904 comm = ” ” ;905 i f ( i n d e x / 4 == 0)906 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d o f f s e t +2+ / / menu e l e m e n t n−1907 4 ∗ (nm−>TVPposeElements ( ) − 1) ) ;908 e l s e909 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( id −7) ; / / go to menu e l e m e n t i−1910 }911 e l s e / / ( i n d e x % 4 == 0)912 {913 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗914 ∗ Ready f o r a c t i o n s t a t e ; f o r a g i v e n menu i n d e x c a l l e d ’ l i d ’915 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /


916 i n t l i d = i n d e x / 4 ;917 i f ( mbp )918 {919 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗920 ∗ a c t i o n r e q u i r e d midd le b u t t o n was p r e s s e d921 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /922 TVPs t r i nggen t s g ;923 Q S t r i n g say = ” say (OK) ” ;924 comm = t s g . T V P p a r a l l e l ( say , nm−>TVPpose ( l i d ) ) ;925 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d +2) ; / / MB +=2926 }927 e l s e928 i f ( fbp )929 {930 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗931 ∗ a c t i o n r e q u i r e d f r o n t b u t t o n was p r e s s e d932 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /933 i f ( l i d == nm−>TVPposeElements ( ) − 1)934 l i d = 0 ;935 e l s e936 l i d ++;937 comm = ” say ( ” + nm−>TVPposeText ( l i d ) + ” ) ” ;938 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d +1) ; / / FB +=1939 }940 e l s e941 i f ( rbp )942 {943 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗944 ∗ a c t i o n r e q u i r e d r e a r b u t t o n was p r e s s e d945 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /946 i f ( l i d == 0)947 l i d = nm−>TVPposeElements ( ) − 1 ;948 e l s e949 l i d −−;950 comm = ” say ( ” + nm−>TVPposeText ( l i d ) + ” ) ” ;951 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d +3) ; / / RB +=3952 }953 e l s e954 {955 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗956 ∗ n o t h i n g go ing on keep t h i s module a c t i v e by r e a c t i v a t i n g i d957 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /958 comm = ” ” ;959 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d ) ; / / keep same s t a t e a c t i v e960 }961 }962 }963 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗964 ∗ C r e a t e s t a t e s t r i n g965 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /966 t c c−>c o m m a n d S t a t e S t r i n g ( i s t a t e s , icommands , id , comm , s t a t e s , o s t a t e s ,967 ocommands ) ;968 }969

970 /∗ ∗971 ∗ \ b r i e f c o m m a n d S t a t e S t r i n g S t a n d u p module t h a t makes t h e r o b o t s t a n d u p972 ∗ \ a u t h o r Tino Lourens973 ∗ \ d a t e 23 Apr 2011974 ∗ \ v e r s i o n 23 Apr 2011

975 ∗ \param i s t a t e s i n p u t s t a t e s976 ∗ \param icommands i n p u t commands977 ∗ \param i d s t a r t i d o f s t a r t i n g s t a t e978 ∗ \param i d r e a d y s t a t e ( s ) t o a c t i v a t e when r e a d y979 ∗ \param o s t a t e s o u t p u t s t a t e s980 ∗ \param ocommands o u t p u t commands981 ∗ \param d i n p u t nao s e n s o r y d a t a982 ∗ \ r e t u r n none983 ∗984 ∗ \ h t m l o n l y \ e n d h t m l o n l y985 ∗ c o m m a n d S t a t e S t r i n g S t a n d u p module t h a t makes t h e r o b o t s t a n d u p986 ∗ \ h t m l o n l y \ e n d h t m l o n l y987 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /988 vo id TVPnaocommand : : c o m m a n d S t a t e S t r i n g S t a n d u p ( Data5D i s t a t e s ,989 S t r i n g icommands ,990 i n t i d s t a r t ,991 Q S t r i n g i d r e a d y ,992 Data5D &o s t a t e s ,993 S t r i n g &ocommands ,994 Data5D d )995 {996 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)997 qDebug ( ) << ”TVPnaocommand : : commandStr ingStandup ” << f l u s h ;998 # e n d i f999 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

1000 ∗ S t a t e i d s used :1001 ∗ − i d s t a r t = a c t i o n s t a n d up1002 ∗ − i d r e a d y = s t a t e s t o a c t i v a t e when r e a d y1003 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1004 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1005 ∗ C r e a t e an i n i t phase t o p r o p e r l y s e t u p id , comm , and s t a t e s1006 ∗ check i f r e a d y s t a t e s i s an a p p r o p r i a t e s e t o f s t a t e s .1007 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1008 i f ( i n i t S t r e a m )1009 {1010 TVPcommandConstruct ion t c ;1011 i f ( ! t c . t o S t a t e L i s t ( i d r e a d y ) )1012 {1013 G e n e r a l Gen ;1014 Gen . e r r o r ( ”TVPnaocommand” , ” commandStr ingStandup ” ,1015 QObject : : t r ( ” S t a t e s when r e a d y s h o u l d be i n t e g e r v a l u e s ”1016 ” s e p a r a t e d by comma ’ s . The c u r r e n t l i s t i s ”1017 ” %1. ” ) . a r g ( i d r e a d y ) ) ;1018 }1019 t c c = new TVPcommandConstruct ion ( ) ;1020 i d = i d s t a r t ;1021 comm = ” ” ;1022 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d ) ; / / a c t i v a t e i t s e l f1023 i n i t S t r e a m = f a l s e ;1024 }1025 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1026 ∗ l e t s e v a l u a t e what t o do1027 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1028 comm = TVPstandup ( d ) ;1029 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1030 ∗ The outcome i s empty when an unknown s e t t i n g i s e n c o u n t e r e d o r1031 ∗ we have r e a c h e d our g o a l1032 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1033 i f (comm . l e n g t h ( ) == 0)

1034 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d r e a d y ) ; / / we a r e done1035 e l s e1036 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d s t a r t ) ; / / a c t i v a t e t h e same1037 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1038 ∗ C r e a t e s t a t e s t r i n g1039 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1040 t c c−>c o m m a n d S t a t e S t r i n g ( i s t a t e s , icommands , id , comm , s t a t e s , o s t a t e s ,1041 ocommands ) ;1042 }1043

1044 /∗ ∗1045 ∗ \ b r i e f c o m m a n d S t a t e S t r i n g S i t d o w n module t h a t makes t h e r o b o t s i t d o w n1046 ∗ \ a u t h o r Tino Lourens1047 ∗ \ d a t e 23 Apr 20111048 ∗ \ v e r s i o n 23 Apr 20111049 ∗ \param i s t a t e s i n p u t s t a t e s1050 ∗ \param icommands i n p u t commands1051 ∗ \param i d s t a r t i d o f s t a r t i n g s t a t e1052 ∗ \param i d r e a d y s t a t e ( s ) t o a c t i v a t e when r e a d y1053 ∗ \param o s t a t e s o u t p u t s t a t e s1054 ∗ \param ocommands o u t p u t commands1055 ∗ \param d i n p u t nao s e n s o r y d a t a1056 ∗ \ r e t u r n none1057 ∗1058 ∗ \ h t m l o n l y \ e n d h t m l o n l y1059 ∗ c o m m a n d S t a t e S t r i n g S i t d o w n module t h a t makes t h e r o b o t s i t d o w n1060 ∗ \ h t m l o n l y \ e n d h t m l o n l y1061 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1062 vo id TVPnaocommand : : c o m m a n d S t a t e S t r i n g S i t d o w n ( Data5D i s t a t e s ,1063 S t r i n g icommands ,1064 i n t i d s t a r t ,1065 Q S t r i n g i d r e a d y ,1066 Data5D &o s t a t e s ,1067 S t r i n g &ocommands ,1068 Data5D d )1069 {1070 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)1071 qDebug ( ) << ”TVPnaocommand : : commandStr ingSi tdown ” << f l u s h ;1072 # e n d i f1073 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1074 ∗ S t a t e i d s used :1075 ∗ − i d o f f s e t = a c t i o n s i t down1076 ∗ − i d r e a d y = s t a t e s t o a c t i v a t e when r e a d y1077 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1078 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1079 ∗ C r e a t e an i n i t phase t o p r o p e r l y s e t u p id , comm , and s t a t e s1080 ∗ check i f r e a d y s t a t e s i s an a p p r o p r i a t e s e t o f s t a t e s .1081 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1082 i f ( i n i t S t r e a m )1083 {1084 TVPcommandConstruct ion t c ;1085 i f ( ! t c . t o S t a t e L i s t ( i d r e a d y ) )1086 {1087 G e n e r a l Gen ;1088 Gen . e r r o r ( ”TVPnaocommand” , ” commandStr ingSi tdown ” ,1089 QObject : : t r ( ” S t a t e s when r e a d y s h o u l d be i n t e g e r v a l u e s ”1090 ” s e p a r a t e d by comma ’ s . The c u r r e n t l i s t i s ”1091 ” %1. ” ) . a r g ( i d r e a d y ) ) ;1092 }

1093 t c c = new TVPcommandConstruct ion ( ) ;1094 i d = i d s t a r t ;1095 comm = ” ” ;1096 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d ) ; / / a c t i v a t e i t s e l f1097 i n i t S t r e a m = f a l s e ;1098 }1099 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1100 ∗ L e t s e v a l u a t e what t o do1101 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1102 comm = TVPsitdown ( d ) ;1103 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1104 ∗ The outcome i s empty when an unknown s e t t i n g i s e n c o u n t e r e d o r1105 ∗ we have r e a c h e d our g o a l1106 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1107 i f (comm . l e n g t h ( ) == 0)1108 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d r e a d y ) ; / / we a r e done1109 e l s e1110 s t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d s t a r t ) ; / / a c t i v a t e t h e same1111 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1112 ∗ C r e a t e s t a t e s t r i n g1113 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1114 t c c−>c o m m a n d S t a t e S t r i n g ( i s t a t e s , icommands , id , comm , s t a t e s , o s t a t e s ,1115 ocommands ) ;1116 }1117

1118 /∗ ∗1119 ∗ \ b r i e f c o m m a n d S t a t e S t r i n g L o o k F o r O b j e c t g e n e r a t e s a command and u p d a t e s1120 ∗ i t s s t a t e u s i n g o b j e c t s .1121 ∗ \ a u t h o r Tino Lourens1122 ∗ \ d a t e 11 Jun 20111123 ∗ \ v e r s i o n 11 Jun 20111124 ∗ \param i s t a t e s i n p u t s t a t e s1125 ∗ \param icommands i n p u t commands1126 ∗ \param i d s t a r t s t a r t i n g i d1127 ∗ \param a s t a t e s s t a t e s t o be a c t i v a t e d when command i s e x e c u t e d1128 ∗ \param o s t a t e s o u t p u t s t a t e s1129 ∗ \param ocommands o u t p u t commands1130 ∗ \param d i n p u t o b j e c t a r e a s x1 , x2 , y1 , y2 , w, h1131 ∗ \param s i n p u t nao s e n s o r y d a t a1132 ∗ \param maxTime maximum t ime t o f i n d a o b j e c t i n m i l l i s e c o n d s1133 ∗ \ r e t u r n none1134 ∗1135 ∗ \ h t m l o n l y \ e n d h t m l o n l y1136 ∗ c o m m a n d S t a t e S t r i n g L o o k F o r O b j e c t g e n e r a t e s a command and u p d a t e s1137 ∗ i t s s t a t e u s i n g o b j e c t s .1138 ∗1139 ∗ Module moves head i n 9 d i f f e r e n t p o s i t i o n s t o s e a r c h f o r an o b j e c t .1140 ∗ I t e i t h e r f i n d s t h e o b j e c t o r when t h e s e a r c h t ime has e x p i r e d i t f a i l s .1141 ∗ \ h t m l o n l y \ e n d h t m l o n l y1142 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1143 vo id TVPnaocommand : : c o m m a n d S t a t e S t r i n g L o o k F o r O b j e c t ( Data5D i s t a t e s ,1144 S t r i n g icommands ,1145 i n t i d s t a r t ,1146 Q S t r i n g a s t a t e s ,1147 Data5D &o s t a t e s ,1148 S t r i n g &ocommands ,1149 Data5D d ,1150 Data5D s ,1151 i n t maxTime )

1152 {1153 # i f d e f i n e d (TVPNAOCOMMAND1 DEBUG) | | d e f i n e d (ALL DEBUG)1154 qDebug ( ) << ”TVPnaocommand : : c o m m a n d S t r i n g S t a t e L o o k F o r O b j e c t ” << f l u s h ;1155 # e n d i f1156 G e n e r a l Gen ;1157 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1158 ∗ I n i t phase1159 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1160 i f ( i n i t S t r e a m )1161 {1162 t c c = new TVPcommandConstruct ion ( ) ;1163 i n i t S t r e a m = f a l s e ;1164 s t a r t T i m e = Gen . ge tTime ( ) ;1165 found = t r u e ;1166 }1167 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1168 ∗ Module i s n o t a c t i v e u p d a t e s t a r t i n g t ime1169 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1170 i f ( ( ! t c c−>i s A c t i v e ( i s t a t e s , i d s t a r t ) ) &&1171 ( ! t c c−>i s S c h e d u l e d ( i s t a t e s , i d s t a r t ) ) )1172 {1173 s t a r t T i m e = Gen . ge tTime ( ) ;1174 }1175 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1176 ∗ Put d e f a u l t v a l u e s1177 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1178 i n t x = −1;1179 i n t y = −1;1180 i n t w = 320 ;1181 i n t h = 320 ;1182 comm = ” ” ;1183 Q S t r i n g a c t i v e S t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d s t a r t ) ;1184 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1185 ∗ Try t o g e t f i r s t o b j e c t1186 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1187 i f ( ( d . d a t a t y p e == DATA5D INT ) &&1188 ( ( d .w == 6) | | ( d .w == 7) ) && ( d . h >= 1) && ( d . d == 1) && ( d . t == 1) &&1189 ( d . e == 1) )1190 {1191 i n t ∗ d a t a = ( i n t ∗ ) d . d a t a ;1192 x = ( d a t a [ 0 ] + d a t a [ 1 ] ) / 2 ;1193 y = ( d a t a [ 2 ] + d a t a [ 3 ] ) / 2 ;1194 w = d a t a [ 4 ] ;1195 h = d a t a [ 5 ] ;1196 }1197 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1198 ∗ E v a l u a t e i f t h e r e i s a f i r s t o b j e c t a v a i l a b l e1199 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1200 i f ( ( x >= 0) && ( x < w) && ( y >= 0) && ( y < h ) )1201 {1202 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1203 ∗ An o b j e c t was found1204 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1205 s t a r t T i m e = Gen . ge tTime ( ) ;1206 found = t r u e ;1207 Q S t r i n g lcomm = ” ” ;1208 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1209 ∗ e v a l u a t e i n p u t d a t a1210 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /

1211 i f ( ( s . d a t a t y p e == DATA5D FLOAT) &&1212 ( ( s .w == ns−>s i z e ( ) ) | | ( s .w == ns−>s i z e ( ) − 2) ) && / / 2 s o n a r v a l .1213 ( s . h == 1) && ( s . d == 1) && ( s . t == 1) && ( s . e == 1) )1214 {1215 f l o a t ∗ d a t a = ( f l o a t ∗ ) s . d a t a ;1216 f l o a t cur ren tYaw = d a t a [ HeadYaw ] ∗ ( 1 8 0 / PI ) ;1217 f l o a t c u r r e n t P i t c h = d a t a [ HeadP i t ch ] ∗ ( 1 8 0 / PI ) ;1218 f l o a t t a rge tYaw = cur ren tYaw + (w/ 2 − x ) / 8 ; / / h a l f t i m e s w1219 f l o a t t a r g e t P i t c h = c u r r e n t P i t c h − ( h / 2 − y ) / 8 ; / / h a l f t i m e s h1220 Q S t r i n g a = Q S t r i n g ( ”%1” ) . a r g ( t a rge tYaw ) ;1221 Q S t r i n g b = Q S t r i n g ( ”%1” ) . a r g ( t a r g e t P i t c h ) ;1222 f l o a t eps = 2 ;1223 i f ( ( cu r ren tYaw < t a rge tYaw + eps ) &&1224 ( cu r ren tYaw > t a rge tYaw − eps ) &&1225 ( c u r r e n t P i t c h < t a r g e t P i t c h + eps ) &&1226 ( c u r r e n t P i t c h > t a r g e t P i t c h − eps ) )1227 {1228 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1229 ∗ We have g o t a o b j e c t i n t h e c e n t e r ; A c t i v a t e f i r s t group i n1230 ∗ a s t a t e s : [ x , y ] [ a , b ] −> x , y1231 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1232 T V P s t a t e s s t ;1233 a c t i v e S t a t e s = s t . g e t S t a t e s ( a s t a t e s , 0 ) ; / / f i r s t group1234 }1235 e l s e1236 {1237 lcomm =1238 ” [ move ( HeadYaw , ” + a + ” , 3 0 0 , 1 ) | ”1239 ” move ( HeadPi tch , ” + b + ” , 2 0 0 , 1 ) ]& w a i t ( 1 0 0 0 ) ” ;1240 }1241 }1242 comm = lcomm ;1243 }1244 e l s e1245 {1246 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1247 ∗ No o b j e c t found s t a r t l o o k i n g around1248 ∗ −Dete rmine t ime t h a t we have n o t seen an o b j e c t ; i f l a r g e r t h a n N s e c1249 ∗ we a r e done1250 ∗ −S t a r t l o o k i n g around1251 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1252 Time t = Gen . getTime ( ) ;1253 i n t t i m e E l a p s e d = Gen . g e t T i m e E l a p s e d ( t , s t a r t T i m e ) ;1254 i f ( t i m e E l a p s e d > maxTime )1255 {1256 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1257 ∗ No o b j e c t found i n t h e maximum t ime a v a i l a b l e1258 ∗ A c t i v a t e second group of s t a t e s1259 ∗ a s t a t e s : [ x , y ] [ a , b ] −> a , b1260 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1261 T V P s t a t e s s t ;1262 a c t i v e S t a t e s = s t . g e t S t a t e s ( a s t a t e s , 1 ) ; / / s econd group1263 }1264 e l s e1265 i f ( ( t i m e E l a p s e d < 10000) && ( found ) ) / / o b j e c t found < 10 s e c ago .1266 {1267 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1268 ∗ Do n o t h i n g l i k e l y t h e o b j e c t i s s een soon1269 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /

1270 }1271 e l s e1272 {1273 found = f a l s e ; / / n o t h i n g found l a t e l y1274 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1275 ∗ Look around1276 ∗ use 9 d i f f e r e n t p o s i t i o n s ( i n an e l l i p s e s t a r t i n g i n t h e c e n t e r )1277 ∗ From naomot ion . cpp :1278 ∗ HeadYaw −120 120 9 . 4 71279 ∗ HeadP i t ch − 39 30 8 . 2 41280 ∗ H, V1281 ∗ pos 0 : ( 0 , 0 )1282 ∗ pos 1 : ( 0 , 30)1283 ∗ pos 2 : ( 85 , 21)1284 ∗ pos 3 : ( 120 , 0 )1285 ∗ pos 4 : ( 85 , −21)1286 ∗ pos 5 : ( 0 , −30)1287 ∗ pos 6 : ( −85, −21)1288 ∗ pos 7 : (−120 , 0 )1289 ∗ pos 8 : ( −85, 21)1290 ∗ pos −1: a l l o t h e r s1291 ∗1292 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1293 f l o a t cur ren tYaw = −10.0 f ;1294 f l o a t c u r r e n t P i t c h = 1 0 . 0 f ;1295 i f ( ( s . d a t a t y p e == DATA5D FLOAT) &&1296 ( ( s .w == ns−>s i z e ( ) ) | | ( s .w == ns−>s i z e ( ) − 2) ) && / / 2 s o n a r1297 ( s . h == 1) && ( s . d == 1) && ( s . t == 1) && ( s . e == 1) )1298 {1299 f l o a t ∗ d a t a = ( f l o a t ∗ ) s . d a t a ;1300 cur ren tYaw = d a t a [ HeadYaw ] ∗ ( 1 8 0 . 0 / PI ) ;1301 c u r r e n t P i t c h = d a t a [ HeadP i t ch ] ∗ ( 1 8 0 . 0 / PI ) ;1302 }1303 i n t pos = −1;1304 c o n s t f l o a t D = 2 . 0 f ;1305 c o n s t f l o a t H0 = −70.0 f ;1306 c o n s t f l o a t H1 = −35.0 f ;1307 c o n s t f l o a t H2 = 0 . 0 f ;1308 c o n s t f l o a t H3 = 3 7 . 0 f ;1309 c o n s t f l o a t H4 = 7 0 . 0 f ;1310 c o n s t f l o a t V0 = −10.0 f ;1311 c o n s t f l o a t V1 = −5.0 f ;1312 c o n s t f l o a t V2 = 0 . 0 f ;1313 c o n s t f l o a t V3 = 5 . 0 f ;1314 c o n s t f l o a t V4 = 1 0 . 0 f ;1315 i f ( ( cu r ren tYaw >= H2 − D) && ( cur ren tYaw <= H2 + D) )1316 {1317 i f ( ( c u r r e n t P i t c h >= V2 − D) && ( c u r r e n t P i t c h <= V2 + D) )1318 pos = 0 ;1319 e l s e1320 i f ( ( c u r r e n t P i t c h >= V4 − D) && ( c u r r e n t P i t c h <= V4 + D) )1321 pos = 1 ;1322 e l s e1323 i f ( ( c u r r e n t P i t c h >= V0 − D) && ( c u r r e n t P i t c h <= V0 + D) )1324 pos = 5 ;1325 }1326 e l s e1327 i f ( ( cu r ren tYaw >= H3 − D) && ( cur ren tYaw <= H3 + D) )1328 {

1329 i f ( ( c u r r e n t P i t c h >= V3 − D) && ( c u r r e n t P i t c h <= V3 + D) )1330 pos = 2 ;1331 e l s e1332 i f ( ( c u r r e n t P i t c h >= V1 − D) && ( c u r r e n t P i t c h <= V1 + D) )1333 pos = 4 ;1334 }1335 e l s e1336 i f ( ( cu r ren tYaw >= H4 − D) && ( cur ren tYaw <= H4 + D) )1337 {1338 i f ( ( c u r r e n t P i t c h >= V2 − D) && ( c u r r e n t P i t c h <= V2 + D) )1339 pos = 3 ;1340 }1341 e l s e1342 i f ( ( cu r ren tYaw >= H1 − D) && ( cur ren tYaw <= H1 + D) )1343 {1344 i f ( ( c u r r e n t P i t c h >= V3 − D) && ( c u r r e n t P i t c h <= V3 + D) )1345 pos = 8 ;1346 e l s e1347 i f ( ( c u r r e n t P i t c h >= V1 − D) && ( c u r r e n t P i t c h <= V1 + D) )1348 pos = 6 ;1349 }1350 e l s e1351 i f ( ( cu r ren tYaw >= H0 − D) && ( cur ren tYaw <= H0 + D) )1352 {1353 i f ( ( c u r r e n t P i t c h >= V2 − D) && ( c u r r e n t P i t c h <= V2 + D) )1354 pos = 7 ;1355 }1356 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1357 ∗ Look around1358 ∗ 9 d i f f e r e n t p o s i t i o n s ( i n an e l l i p s e s t a r t i n g i n t h e c e n t e r )1359 ∗ From naomot ion . cpp :1360 ∗ HeadYaw −120 120 9 . 4 71361 ∗ HeadP i t ch − 30 39 8 . 2 41362 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1363 pos ++;1364 f l o a t t a rge tYaw = H2 ;1365 f l o a t t a r g e t P i t c h = V2 ;1366 s w i t c h ( pos )1367 {1368 d e f a u l t : / / 01369 b r e a k ;1370 c a s e 1 :1371 t a r g e t P i t c h = V4 ;1372 b r e a k ;1373 c a s e 2 :1374 t a r g e t P i t c h = V3 ;1375 t a rge tYaw = H3 ;1376 b r e a k ;1377 c a s e 3 :1378 t a rge tYaw = H4 ;1379 b r e a k ;1380 c a s e 4 :1381 t a r g e t P i t c h = V1 ;1382 t a rge tYaw = H3 ;1383 b r e a k ;1384 c a s e 5 :1385 t a r g e t P i t c h = V0 ;1386 b r e a k ;1387 c a s e 6 :

1388 t a r g e t P i t c h = V1 ;1389 t a rge tYaw = H1 ;1390 b r e a k ;1391 c a s e 7 :1392 t a rge tYaw = H0 ;1393 b r e a k ;1394 c a s e 8 :1395 t a r g e t P i t c h = V3 ;1396 t a rge tYaw = H1 ;1397 b r e a k ;1398 }1399 Q S t r i n g a = Q S t r i n g ( ”%1” ) . a r g ( t a rge tYaw ) ;1400 Q S t r i n g b = Q S t r i n g ( ”%1” ) . a r g ( t a r g e t P i t c h ) ;1401 comm =1402 ” [ move ( HeadYaw , ” + a + ” , 1000 , 1 ) | ”1403 ” move ( HeadPi tch , ” + b + ” , 800 , 1 ) ] & w a i t ( 3 0 0 0 ) ” ;1404 }1405 }1406 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1407 ∗ C r e a t e s t a t e s t r i n g1408 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1409 t c c−>c o m m a n d S t a t e S t r i n g ( i s t a t e s , icommands , i d s t a r t , comm , a c t i v e S t a t e s ,1410 o s t a t e s , ocommands ) ;1411 }1412

1413 /∗ ∗1414 ∗ \ b r i e f commandSta teSt r ingMoveToObjec t g e n e r a t e s a command and u p d a t e s1415 ∗ i t s s t a t e u s i n g o b j e c t s .1416 ∗ \ a u t h o r Tino Lourens1417 ∗ \ d a t e 11 Jun 20111418 ∗ \ v e r s i o n 11 Jun 20111419 ∗ \param i s t a t e s i n p u t s t a t e s1420 ∗ \param icommands i n p u t commands1421 ∗ \param i d s t a r t s t a r t i n g i d1422 ∗ \param a s t a t e s s t a t e s t o be a c t i v a t e d when command i s e x e c u t e d1423 ∗ \param o s t a t e s o u t p u t s t a t e s1424 ∗ \param ocommands o u t p u t commands1425 ∗ \param d i n p u t o b j e c t a r e a s x1 , x2 , y1 , y2 , w, h1426 ∗ \param s i n p u t nao s e n s o r y d a t a1427 ∗ \param maxTime maximum t ime t o f i n d a o b j e c t i n m i l l i s e c o n d s1428 ∗ \param d i s t a n c e c a l i b r a t e d d i s t a n c e f o r an o b j e c t o f 0 . 1 5m @ 50 p i x e l s1429 ∗ \param d i a m e t e r o b j e c t d i a m e t e r i n m e t e r s1430 ∗ \ r e t u r n none1431 ∗1432 ∗ \ h t m l o n l y \ e n d h t m l o n l y1433 ∗ commandSta teSt r ingMoveToObjec t g e n e r a t e s a command and u p d a t e s1434 ∗ i t s s t a t e u s i n g o b j e c t s .1435 ∗1436 ∗ An o b j e c t has been found1437 ∗ 1 . Mova a r c t o w a r d s o b j e c t ( such t h a t head i s s t r a i g h t )1438 ∗ 2 . Move t o w a r d s o b j e c t ( use c a l i b r a t i o n )1439 ∗ \ h t m l o n l y \ e n d h t m l o n l y1440 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1441 vo id TVPnaocommand : : commandSta teSt r ingMoveToObjec t ( Data5D i s t a t e s ,1442 S t r i n g icommands ,1443 i n t i d s t a r t ,1444 Q S t r i n g a s t a t e s ,1445 Data5D &o s t a t e s ,1446 S t r i n g &ocommands ,

1447 Data5D d ,1448 Data5D s ,1449 i n t maxTime ,1450 do ub l e d i s t a n c e ,1451 do ub l e d i a m e t e r )1452 {1453 # i f d e f i n e d (TVPNAOCOMMAND1 DEBUG) | | d e f i n e d (ALL DEBUG)1454 qDebug ( ) << ”TVPnaocommand : : c o m m a n d S t r i n g S t a t e L o o k F o r O b j e c t ” << f l u s h ;1455 # e n d i f1456 G e n e r a l Gen ;1457 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1458 ∗ I n i t phase1459 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1460 i f ( i n i t S t r e a m )1461 {1462 t c c = new TVPcommandConstruct ion ( ) ;1463 i n i t S t r e a m = f a l s e ;1464 s t a r t T i m e = Gen . ge tTime ( ) ;1465 found = t r u e ;1466 f i n i s h e d = f a l s e ;1467 }1468 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1469 ∗ Put d e f a u l t v a l u e s1470 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1471 i n t x = −1;1472 i n t y = −1;1473 i n t x l = −1;1474 i n t yu = −1;1475 i n t x r = −1;1476 i n t yd = −1;1477 i n t w = 320 ;1478 i n t h = 240 ;1479 comm = ” ” ;1480 Q S t r i n g a c t i v e S t a t e s = Q S t r i n g ( ”%1” ) . a r g ( i d s t a r t ) ;1481 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1482 ∗ Module i s n o t a c t i v e u p d a t e s t a r t i n g t ime1483 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1484 i f ( ( ! t c c−>i s A c t i v e ( i s t a t e s , i d s t a r t ) ) &&1485 ( ! t c c−>i s S c h e d u l e d ( i s t a t e s , i d s t a r t ) ) )1486 {1487 s t a r t T i m e = Gen . ge tTime ( ) ;1488 f i n i s h e d = f a l s e ;1489 }1490 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1491 ∗ Try t o g e t f i r s t o b j e c t1492 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1493 i f ( ( d . d a t a t y p e == DATA5D INT ) &&1494 ( ( d .w == 6) | | ( d .w == 7) ) && ( d . h >= 1) && ( d . d == 1) && ( d . t == 1) &&1495 ( d . e == 1) )1496 {1497 i n t ∗ d a t a = ( i n t ∗ ) d . d a t a ;1498 x l = d a t a [ 0 ] ;1499 xr = d a t a [ 1 ] ;1500 yu = d a t a [ 2 ] ;1501 yd = d a t a [ 3 ] ;1502 x = ( x l + x r ) / 2 ;1503 y = ( yu + yd ) / 2 ;1504 w = d a t a [ 4 ] ;1505 h = d a t a [ 5 ] ;

1506 }1507 i f ( TVPgetBodyPose ( s ) != 1 ) / / S t a n d i n g1508 comm = TVPstandup ( s ) ;1509 e l s e1510 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1511 ∗ E v a l u a t e i f t h e r e i s a f i r s t o b j e c t a v a i l a b l e1512 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1513 i f ( ( x >= 0) && ( x < w) && ( y >= 0) && ( y < h ) )1514 {1515 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1516 ∗ An o b j e c t was found1517 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1518 s t a r t T i m e = Gen . ge tTime ( ) ;1519 found = t r u e ;1520 Q S t r i n g lcomm = ” ” ;1521 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1522 ∗ e v a l u a t e i n p u t d a t a1523 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1524 i f ( ( s . d a t a t y p e == DATA5D FLOAT) &&1525 ( ( s .w == ns−>s i z e ( ) ) | | ( s .w == ns−>s i z e ( ) − 2) ) && / / 2 s o n a r v a l .1526 ( s . h == 1) && ( s . d == 1) && ( s . t == 1) && ( s . e == 1) )1527 {1528 f l o a t ∗ d a t a = ( f l o a t ∗ ) s . d a t a ;1529 f l o a t cur ren tYaw = d a t a [ HeadYaw ] ∗ ( 1 8 0 / PI ) ;1530 f l o a t c u r r e n t P i t c h = d a t a [ HeadP i t ch ] ∗ ( 1 8 0 / PI ) ;1531 f l o a t t a rge tYaw = cur ren tYaw + (w/ 2 − x ) / 8 ; / / h a l f t i m e s w1532 f l o a t t a r g e t P i t c h = c u r r e n t P i t c h − ( h / 2 − y ) / 8 ; / / h a l f t i m e s h1533 Q S t r i n g a = Q S t r i n g ( ”%1” ) . a r g ( t a rge tYaw ) ;1534 Q S t r i n g b = Q S t r i n g ( ”%1” ) . a r g ( t a r g e t P i t c h ) ;1535 f l o a t eps = 2 ;1536 i f ( ( cu r ren tYaw < t a rge tYaw + eps ) &&1537 ( cu r ren tYaw > t a rge tYaw − eps ) &&1538 ( c u r r e n t P i t c h < t a r g e t P i t c h + eps ) &&1539 ( c u r r e n t P i t c h > t a r g e t P i t c h − eps ) )1540 {1541 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1542 ∗ We have g o t a o b j e c t i n t h e c e n t e r1543 ∗ L e t s e v a l u a t e how f a r we a r e away from t h e o b j e c t1544 ∗ between 0.25−2.0 m e t e r s .1545 ∗ ppcm i s p i x e l s p e r c e n t i m e t e r1546 ∗ d i s t m i s d i s t a n c e i n c e n t i m e t e r s1547 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1548 i n t ox = abs ( x r − x l ) ;1549 do ub l e ppcm = ox / ( 1 0 0 . 0 ∗ d i a m e t e r ) ;1550 do ub l e i s i z e = 320 .0 / w;1551 ppcm ∗= i s i z e ; / / map as i f image was 320 x2401552 / / c o u t << ppcm << ” ” << i s i z e << e n d l << f l u s h ;1553 do ub l e d i s t m = 3 5 4 . 8 4 1 1 ;1554 do ub l e xp = ppcm ;1555 d i s t m −= 110 .2569 ∗ xp ;1556 xp ∗= ppcm ;1557 d i s t m += 14 .4703 ∗ xp ;1558 xp ∗= ppcm ;1559 d i s t m −= 0 .8245 ∗ xp ;1560 xp ∗= ppcm ;1561 d i s t m += 0 .0169 ∗ xp ;1562 i f ( ppcm >= 1 6 . 0 )1563 {1564 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

1565 ∗ We a r e j u s t i n f r o n t o f t h e o b j e c t ˜30 cm ppcm=151566 ∗ ˜25cm ppcm=161567 ∗ Thus we a r e done .1568 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1569 / / c o u t << ”DONE! ! ! \ n ” << f l u s h ;1570 T V P s t a t e s s t ;1571 a c t i v e S t a t e s = s t . g e t S t a t e s ( a s t a t e s , 0 ) ; / / f i r s t group1572 f i n i s h e d = t r u e ;1573 }1574 e l s e1575 {1576 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1577 ∗ L e t s move toward t h e o b j e c t and r o t a t e i f needed1578 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1579 / / c o u t << ox << ” X−Y ” << ox << ” Yaw ” << cur ren tYaw1580 / / << e n d l << f l u s h ;1581 / / c o u t << ” D i s t a n c e ” << d i s t m << e n d l << f l u s h ;1582 i f ( d i s t m > 60)1583 {1584 i f ( f a b s ( cur ren tYaw ) < eps )1585 {1586 lcomm =1587 ” [ w a l k t o ( 0 . 2 , 0 . 0 , 0 . 0 ) ”1588 ” | move ( HeadYaw , 0 , 1000) ] & w a i t ( 3 0 0 0 ) ” ;1589 }1590 e l s e1591 {1592 Q S t r i n g cy = Q S t r i n g ( ”%1” ) . a r g ( cur ren tYaw / 1 8 0 . 0 ) ;1593 lcomm = ” [ [ walkd ( 0 . 0 , 0 . 0 , ” + cy + ” , 0 . 2 , 5000) &”1594 ” w a l k t o ( 0 . 2 , 0 . 0 , 0 . 0 ) ] ”1595 ” | move ( HeadYaw , 0 , 1000) ] & w a i t ( 3 0 0 0 ) ” ;1596 }1597 }1598 e l s e1599 i f ( d i s t m > 45)1600 {1601 i f ( f a b s ( cur ren tYaw ) < eps )1602 {1603 lcomm =1604 ” [ w a l k t o ( 0 . 2 , 0 . 0 , 0 . 0 ) ”1605 ” | move ( HeadYaw , 0 , 1000) ] & w a i t ( 3 0 0 0 ) ” ;1606 }1607 e l s e1608 {1609 Q S t r i n g cy = Q S t r i n g ( ”%1” ) . a r g ( cur ren tYaw / 1 8 0 . 0 ) ;1610 lcomm = ” [ [ walkd ( 0 . 0 , 0 . 0 , ” + cy + ” , 0 . 2 , 5000) &”1611 ” w a l k t o ( 0 . 1 , 0 . 0 , 0 . 0 ) ] ”1612 ” | move ( HeadYaw , 0 , 1000) ] & w a i t ( 3 0 0 0 ) ” ;1613 }1614 }1615 e l s e / / <= 401616 {1617 T V P s t a t e s s t ;1618 a c t i v e S t a t e s = s t . g e t S t a t e s ( a s t a t e s , 0 ) ; / / f i r s t group1619 / / c o u t << ”DONE! ! ! \ n ” << f l u s h ;1620 f i n i s h e d = t r u e ;1621 }1622 }1623 }

1624 e l s e1625 {1626 lcomm =1627 ” [ move ( HeadYaw , ” + a + ” , 3 0 0 , 1 ) | ”1628 ” move ( HeadPi tch , ” + b + ” , 2 0 0 , 1 ) ]& w a i t ( 3 0 0 0 ) ” ;1629 }1630 }1631 comm = lcomm ;1632 }1633 e l s e1634 {1635 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1636 ∗ No o b j e c t found anymore1637 ∗ −Dete rmine t ime t h a t we have n o t seen a o b j e c t ; i f l a r g e r t h a n N s e c1638 ∗ we a r e done1639 ∗ −S t a r t l o o k i n g around a b i t !1640 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1641 Time t = Gen . getTime ( ) ;1642 i n t t i m e E l a p s e d = Gen . g e t T i m e E l a p s e d ( t , s t a r t T i m e ) ;1643 i f ( t i m e E l a p s e d > maxTime )1644 {1645 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1646 ∗ No o b j e c t found i n t h e maximum t ime a v a i l a b l e1647 ∗ A c t i v a t e second group of s t a t e s1648 ∗ a s t a t e s : [ x , y ] [ a , b ] −> a , b1649 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1650 T V P s t a t e s s t ;1651 a c t i v e S t a t e s = s t . g e t S t a t e s ( a s t a t e s , 1 ) ; / / s econd group1652 / / c o u t << ”Bye ” << a c t i v e S t a t e s . t o S t d S t r i n g ( ) << e n d l << f l u s h ;1653 }1654 e l s e1655 i f ( ( t i m e E l a p s e d >= 10000) && ( found ) && ( f i n i s h e d ) )1656 {1657 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1658 ∗ We a r e r e a d y we made t e l a s t move and now we a r e t o c l o s e t o s e e1659 ∗ t h e o b j e c t1660 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1661 T V P s t a t e s s t ;1662 a c t i v e S t a t e s = s t . g e t S t a t e s ( a s t a t e s , 0 ) ; / / f i r s t group1663 }1664 e l s e1665 i f ( ( t i m e E l a p s e d < 10000) && ( found ) ) / / o b j e c t found < 10 s e c ago .1666 {1667 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1668 ∗ Do n o t h i n g l i k e l y t h e o b j e c t i s s een soon1669 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1670 / / c o u t << ” Los t o b j e c t l e s s t h a n 10 s e c \n ” << f l u s h ;1671 / / comm = ” w a i t ( 1 0 0 0 ) ” ;1672 }1673 e l s e1674 {1675 found = f a l s e ; / / n o t h i n g found l a t e l y1676 / / c o u t << ” Los t o b j e c t . . . \ n ” << f l u s h ;1677 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1678 ∗ Make t i n y h o r i z o n t a l (YAW) moves a t random1679 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1680 i f ( ( s . d a t a t y p e == DATA5D FLOAT) &&1681 ( ( s .w == ns−>s i z e ( ) ) | | ( s .w == ns−>s i z e ( ) − 2) ) &&1682 ( s . h == 1) && ( s . d == 1) && ( s . t == 1) && ( s . e == 1) )

1683 {1684 f l o a t ∗ d a t a = ( f l o a t ∗ ) s . d a t a ;1685 f l o a t cur ren tYaw = d a t a [ HeadYaw ] ∗ ( 1 8 0 / PI ) ;1686 s r a n d ( t ime (NULL) ) ;1687 f l o a t v a l = ( ( r and ( ) % 101) −50) / 1 0 . 0 ; / / −50 . . . 50 / 101688 Q S t r i n g a = Q S t r i n g ( ”%1” ) . a r g ( cur ren tYaw + v a l ) ;1689 comm =1690 ” [ move ( HeadYaw , ” + a + ” , 1000) & w a i t ( 3 0 0 0 ) ] ” ;1691 }1692 }1693 }1694 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1695 ∗ C r e a t e s t a t e s t r i n g1696 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1697 t c c−>c o m m a n d S t a t e S t r i n g ( i s t a t e s , icommands , i d s t a r t , comm , a c t i v e S t a t e s ,1698 o s t a t e s , ocommands ) ;1699 }1700

1701 /∗ ∗1702 ∗ \ b r i e f c o m m a n d S t a t e S t r i n g P o s e p r o v i d e a pose by i n d e x1703 ∗ \ a u t h o r Tino Lourens1704 ∗ \ d a t e 11 Jun 20111705 ∗ \ v e r s i o n 11 Jun 20111706 ∗ \param i s t a t e s i n p u t s t a t e s1707 ∗ \param icommands i n p u t commands1708 ∗ \param i d s t a r t i d o f s t a r t i n g s t a t e1709 ∗ \param i d r e a d y s t a t e ( s ) t o a c t i v a t e when r e a d y1710 ∗ \param o s t a t e s o u t p u t s t a t e s1711 ∗ \param ocommands o u t p u t commands1712 ∗ \param i n d e x i n d e x s t a r t i n g a t 0 = Stand up from f r o n t ; 10 = t a i c h i1713 ∗ \param command d e f a u l t ” ” ; add t o pose a command1714 ∗ \ r e t u r n none1715 ∗1716 ∗ \ h t m l o n l y \ e n d h t m l o n l y1717 ∗ c o m m a n d S t a t e S t r i n g P o s e p r o v i d e a pose by i n d e x1718 ∗ \ h t m l o n l y \ e n d h t m l o n l y1719 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1720 vo id TVPnaocommand : : c o m m a n d S t a t e S t r i n g P o s e ( Data5D i s t a t e s ,1721 S t r i n g icommands ,1722 i n t i d s t a r t ,1723 Q S t r i n g i d r e a d y ,1724 Data5D &o s t a t e s ,1725 S t r i n g &ocommands ,1726 i n t index ,1727 Q S t r i n g command )1728 {1729 # i f d e f i n e d (TVPNAOCOMMAND DEBUG) | | d e f i n e d (ALL DEBUG)1730 qDebug ( ) << ”TVPnaocommand : : commandStr ingPose ” << f l u s h ;1731 # e n d i f1732 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1733 ∗ S t a t e i d s used :1734 ∗ − i d s t a r t = a c t i o n pose by i n d e x1735 ∗ − i d r e a d y = s t a t e s t o a c t i v a t e when r e a d y1736 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1737 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1738 ∗ C r e a t e an i n i t phase t o p r o p e r l y s e t u p id , comm , and s t a t e s1739 ∗ check i f r e a d y s t a t e s i s an a p p r o p r i a t e s e t o f s t a t e s .1740 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1741 i f ( i n i t S t r e a m )

REFERENCES 74

1742 {1743 TVPcommandConstruction t c ;1744 i f ( ! t c . t o S t a t e L i s t ( i d r e a d y ) )1745 {1746 G e n e r a l Gen ;1747 Gen . e r r o r ( ”TVPnaocommand” , ” commandStr ingPose ” ,1748 QObject : : t r ( ” S t a t e s when r e a d y s h o u l d be i n t e g e r v a l u e s ”1749 ” s e p a r a t e d by comma ’ s . The c u r r e n t l i s t i s ”1750 ” %1. ” ) . a r g ( i d r e a d y ) ) ;1751 }1752 t c c = new TVPcommandConstruct ion ( ) ;1753 i n i t S t r e a m = f a l s e ;1754 }1755 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1756 ∗ L e t s e v a l u a t e what t o do1757 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1758 comm = nm−>TVPpose ( i n d e x ) ;1759 i f ( command . l e n g t h ( ) > 0)1760 comm = ” [ ” + comm + ” ] | [ ” + command + ” ] ” ;1761 /∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗1762 ∗ C r e a t e s t a t e s t r i n g1763 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ /1764 t c c−>c o m m a n d S t a t e S t r i n g ( i s t a t e s , icommands , i d s t a r t , comm , i d r e a d y , o s t a t e s ,1765 ocommands ) ;1766 }

References

[1] T. Lourens. Tivipe –tino’s visual programming environment. In The 28th Annual InternationalComputer Software & Applications Conference, IEEE COMPSAC 2004, pages 10–15, 2004.

[2] T. Lourens and E. I. Barakova. User-friendly robot environment for creation of social scenarios.In J. M. Ferrandez, J. R. Alvarez, F. de la Paz, and F. J. Toledo, editors, IWINAC 2011, number6686 in Lecture Notes in Computer Science, pages 212–221, La Palma, Canary Islands, Spain,May-June 2011. Springer-verlag.

programming robots using tivipe · 5 robot programming 26 5.1 programming a state space routine ......

Documents