机器人避障.doc

源程序如下：/*TemplatetoaccessARIA(ActivMediaRoboticsInterfaceforApplications)usingMicrosoftVisualC++,consoleProjects.*/#include"Aria.h"#include<cstdlib>#include<iostream>#include<iomanip>#include<fstream>#include<ctime>#include<math.h>#include<cmath>usingnamespacestd;//therobotArRobot*robot;ArSonarDevicesonar;//sonar,mustbeaddedtotherobotArSicksick;//thelaserintstep=0;doubled0,d1,d3,d2,d4,d15;doubleth,r,PI=3.1415926;doubleminimum=0;//sonarsensoroffsetpositionswithrespecttorobotlocalcoordinateframedoublerobotSonarX[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];//last5setsofsonarreadingsdoubleposData[5];//odometryreadingslongframeCount=-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){//YourCodeHeregrabNewSonarData();grabNewPositionData();logPositionData();logRawSonarData();//grabNewLaserData();MapToGlobalFrame();