机器人避障.doc

源程序如下：
/*
TemplatetoaccessARIA(ActivMediaRoboticsInterfaceforApplications)
usingMicrosoftVisualC++,consoleProjects.
*/

#include"Aria.h"
#include<cstdlib>
#include<iostream>
#include<iomanip>
#include<fstream>
#include<ctime>
#include<math.h>
#include<cmath>

usingnamespacestd;

//therobot
ArRobot*robot;
ArSonarDevicesonar;//sonar,mustbeaddedtotherobot
ArSicksick; //thelaser

intstep=0;
doubled0,d1,d3,d2,d4,d15;
doubleth,r,PI=3.1415926;
doubleminimum=0;

//sonarsensoroffsetpositionswithrespecttorobotlocalcoordinateframe
doublerobotSonarX[8]={69,114,148,166,166,148,114,69};
doublerobotSonarY[8]={136,119,78,27,-27,-78,-119,-136};
doublerobotSonarTh[8]={90.0,50.0,30.0,10.0,-10.0,-30.0,-50.0,-90.0};

doublelaserData[2][180];
doublesonarData[5][16]; //last5setsofsonarreadings
doubleposData[5]; //odometryreadings
long frameCount=-1; //howmanyiterationsofthesensorgrabbingfunctionshavebeenrun

/*********************************************************************************
Filesforloggingpositionandsensordata
*********************************************************************************/
ofstreampositionData("positionvals.txt");
ofstreamsonarVals("sonarvals.txt");
ofstreamglobalFrame("globalFrame.txt");
/*********************************************************************************
FunctionDefinitions
*********************************************************************************/
voidgrabNewPositionData(void);
voidlogPositionData(void);
voidgrabNewSonarData(void);
voidlogRawSonarData(void);
voidMapToGlobalFrame(void);

boolflag=true;
boolweWantToTurnTheRobot=true;
booldecideWhichDirectionToTurn=true;
boolturnRobotRight=false;
boolturnRobotLeft=false;

voidupdate(void)

{//YourCodeHere
grabNewSonarData();
grabNewPositionData();
logPositionData();
logRawSonarData();
//grabNewLaserData();
MapToGlobalFrame();

intsequence[]={0,1,2,3, 4, 5,6, 7};
intleftValue[]={200,200,200,200,-50,0,40,150};
intrightValue[]={150,40,0,-50,200,200,200,200};
intminVal=9999;
//int