机器人避障教学内容.docx
- 文档编号:8266695
- 上传时间:2023-01-30
- 格式:DOCX
- 页数:12
- 大小:18.58KB
机器人避障教学内容.docx
《机器人避障教学内容.docx》由会员分享,可在线阅读,更多相关《机器人避障教学内容.docx(12页珍藏版)》请在冰豆网上搜索。
机器人避障教学内容
源程序如下:
/*
TemplatetoaccessARIA(ActivMediaRoboticsInterfaceforApplications)
usingMicrosoftVisualC++,consoleProjects.
*/
#include"Aria.h"
#include
#include
#include
#include
#include
#include
#include
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
longframeCount=-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;
//intintriger[]={0,0,0,0,0,0};
intminSonar=0;
for(inti=0;i<8;i++)
{
if(sonarData[0][sequence[i]] { minVal=sonarData[0][sequence[i]]; minSonar=i; } } //-------------------------1--------------------------- //normalbehaviournoobstaclesveryclosetotherobot if(((sonarData[0][3]>300)&&(sonarData[0][4]>300)&&(sonarData[0][2]>250)&&(sonarData[0][5]>250)&&(sonarData[0][1]>200)&&(sonarData[0][6]>200)&&(sonarData[0][0]>150)&&(sonarData[0][7]>150))&&(flag)) { robot->setVel2(leftValue[minSonar],rightValue[minSonar]); //resetwhichdirectiontoturn(turneitherleftorright) decideWhichDirectionToTurn=true; //wedonotknowwhichdirectiontoturnyet turnRobotRight=false; turnRobotLeft=false; } else//someobstaclesareveryclosetotherobot { //----------------------------2------------------------- //decidewhichdirectiontoturntherobot if(decideWhichDirectionToTurn) { if((sonarData[0][2]<250)||(sonarData[0][1]<200)||(sonarData[0][0]<150))//isthenearestobstacleontheleftoftherobot { turnRobotRight=true;//yesthenearestobstableisontheleftoftherobot(turnright) } elseif((sonarData[0][4]<300)||(sonarData[0][3]<300)) { turnRobotRight=true;//nosothenearestobstacleisontherightandleft,butjustonthefront(turnright) //turnRobotLeft=true; } else { turnRobotLeft=true;//nosothenearestobstacleisontheright(turnleft) } decideWhichDirectionToTurn=false; } //-------------------------------------------------------- //-----------------------------3-------------------------- //weturntherobotby30degreesinaparticulardirection if(weWantToTurnTheRobot) { robot->stop();//firststoptherobot if(turnRobotRight)//weturnright { robot->setDeltaHeading(-20); } if(turnRobotLeft)//orweturnleft { robot->setDeltaHeading(20); } flag=false; weWantToTurnTheRobot=false; //----------------------------------------------------- }//--------------------------4---------------------------- elseif(robot->isHeadingDone())//checktoseeiftherobothasfinishedturning { flag=true; weWantToTurnTheRobot=true; } //--------------------------------------------------------- } }//endupdate voidMapToGlobalFrame() { doubler,u,v,th; doubleXr,Yr,Thr; doubleXo,Yo,Xg,Yg; for(inti=0;i<1;i++) { r=sonarData[0][i]; u=robotSonarX[i]; v=robotSonarY[i]; th=(robotSonarTh[i]*PI)/180; Xr=posData[0]; Yr=posData[1]; Thr=(posData[2]*PI)/180; doubleu2=u*cos(Thr)-v*sin(Thr); doublev2=u*sin(Thr)+v*cos(Thr); doublea=r*cos(th+Thr); doubleb=r*sin(th+Thr); Xg=a+u2+Xr; Yg=b+v2+Yr; /* Xo=u+r*cos(th); Yo=v+r*sin(th); Xg=(Xo*cos(Thr)-Yo*sin(Thr))+Xr; Yg=(Xo*sin(Thr)+Yo*cos(Thr))+Yr; */ cout< \t"< \t"< globalFrame< } } /* voidMapToGlobalFrame2() { doubler,u,v,th; doubleXr,Yr,Thr; doubleXo,Yo,Xg,Yg; for(inti=1;i<2;i++) { r=sonarData[0][i]; u=robotSonarX[i]; v=robotSonarY[i]; th=robotSonarTh[i]; Xr=posData[0]; Yr=posData[1]; Thr=posData[2]; doubleu2=u*cos(Thr)-v*sin(Thr); doublev2=u*sin(Thr)+v*cos(Thr); doublea=r*cos(th+Thr); doubleb=r*sin(th+Thr); Xg=a+u2+Xr; Yg=b+v2+Yr; //Xo=u+r*cos(th); //Yo=v+r*sin(th); //Xg=(Xo*cos(Thr)-Yo*sin(Thr))+Xr; //Yg=(Xo*sin(Thr)+Yo*cos(Thr))+Yr; cout<<"x: \t"< \t"< globalFrame< } }*/ /*********************************************************************************** Savesthecurrentglobalframeodometryreadingsforusebyotherpartsoftheprogram ***********************************************************************************/ voidgrabNewPositionData(){ frameCount++; posData[0]=robot->getX(); posData[1]=robot->getY(); posData[2]=robot->getTh(); posData[3]=robot->getLeftVel(); posData[4]=robot->getRightVel(); } /************************************** LogthecurrentOdometryvalues **************************************/ voidlogPositionData(){ if(frameCount==0){ positionData<<"X\tY\tTh\tlVel\trVel"< } positionData< posData[1]<<"\t"<< posData[2]<<"\t"<< posData[3]<<"\t"<< posData[4]< } /************************************************************************************ Savesasnapshotofthesonarreadingsatthestartofeachcycleto ensurebothefficiencyandconsistancy.Foruseintherestofaprocessingcycle. ************************************************************************************/ voidgrabNewSonarData() { inti; //getthecurrentrangereadingsforeachofthe8sonarsensors for(i=0;i<16;i++) sonarData[0][i]=robot->getSonarRange(i); } /************************************************************************************ Savesasnapshotofthelaserreadingsatthestartofeachcycleto ensurebothefficiencyandconsistancy.Foruseintherestofaprocessingcycle. ************************************************************************************/ /*voidgrabNewLaserData() { doubleangle; inti,start,end; //startangleofthefirstlaser'bucket' start=-90; //endangleofthefirstlaserbucket end=start+LASER_ANGLE_RANGE_STEP-1; //foreachofthelaserbuckets for(i=0;i { //returntherange(inmm)andangle(inradians) //forthebucketstartingat'start'degreesthroughto'end'degrees laserData[0][i]=sick.currentReadingPolar(start,end,&angle); laserData[1][i]=angle; //updatethebucketbyincremenentingthestartandendlaserbucketpossitionsby //thedesiredstep start+=LASER_ANGLE_RANGE_STEP; end+=LASER_ANGLE_RANGE_STEP; } }*/ /************************************** LogthecurrentRawSonarRangedata **************************************/ voidlogRawSonarData(){ if(frameCount==0){ sonarVals<<"No.\t"; } sonarVals< for(inti=0;i<16;i++){ sonarVals< //cout< } sonarVals< //cout< } intmain(intargc,char**argv) { intret; std: : stringstr; //theconnectionforRemoteHostandSimulator //ArTcpConnectioncon; //theconnectionthroughSerialLinktotherobot ArSerialConnectioncon; ArGlobalFunctorupdateCB(&update); robot=newArRobot; //mandatoryinit Aria: : init(); //opentheconnection,ifthisfailsexit if((ret=con.open())! =0) { str=con.getOpenMessage(ret); printf("Openfailed: %s\n",str.c_str()); Aria: : shutdown(); return1; } //setthedeviceconnectionontherobot robot->setDeviceConnection(&con); //addthesonartotherobot robot->addRangeDevice(&sonar); //trytoconnect,ifwefailexit if(! robot->blockingConnect()) { printf("Couldnotconnecttorobot...exiting\n"); Aria: : shutdown(); return1; (1)专业知识限制} 500元以上1224%//usertask robot->addUserTask("update",50,&updateCB); //turnonthemotors,turnoffamigobotsounds 自制饰品一反传统的饰品消费模式,引导的是一种全新的饰品文化,所以非常容易被我们年轻的女生接受。 robot->comInt(ArCommands: : SONAR,1); (二)对“碧芝”自制饰品店的分析robot->comInt(ArCommands: : ENABLE,1); 开了连锁店,最大的好处是让别人记住你。 “漂亮女生”一律采用湖蓝底色的装修风格,简洁、时尚、醒目。 “品牌效应”是商家梦寐以求的制胜法宝。 robot->comInt(ArComma
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 机器人 教学内容
![提示](https://static.bdocx.com/images/bang_tan.gif)