避障程序.doc

源程序如下： /* Template to access ARIA (ActivMedia Robotics Interface for Applications) using Microsoft Visual C++, console Projects. */ # include Aria.h # include cstdlib # include iostream # include iomanip # include fstream # include ctime # include math.h # include cmath using namespace std; // the robot ArRobot *robot; ArSonarDevice sonar; // sonar, must be added to the robot ArSick sick; // the laser int step=0; double d0,d1,d3,d2,d4,d15; double th,r,PI=3.1415926; double minimum=0; //sonar sensor offset positions with respect to robot local coordinate frame double robotSonarX[8] = {69,114,148,166,166,148,114,69}; double robotSonarY[8] = {136,119,78,27,-27,-78,-119,-136}; double robotSonarTh[8] = {90.0, 50.0, 30.0, 10.0, -10.0, -30.0, -50.0, -90.0}; double laserData[2][180]; double sonarData[5][16]; // last 5 sets of sonar readings double posData[5]; // odometry readings long frameCount = -1; //how many iterations of the sensor grabbing functions have been run /********************************************************************************* Files for logging position and sensor data *********************************************************************************/ ofstream positionData(positionvals.txt); ofstream sonarVals(sonarvals.txt); ofstream globalFrame(globalFrame.txt); /********************************************************************************* Function Definitions *********************************************************************************/ void grabNewPositionData(void); void logPositionData(void); void grabNewSonarData(void); void logRawSonarData(void); void MapToGlobalFrame(void); bool flag = true; bool weWantToTurnTheRobot = true; bool decideWhichDirectionToTurn = true; bool turnRobotRight = false; bool turnRobotLeft = false; void update(void) {// Your Code Here grabNewSonarData(); grabNewPositionData(); logPositionData(); logRawSonarData(); //grabNewLaserData(); MapToGlobalFrame(); int sequence[]= { 0, 1, 2,