Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

Proiect
Modelarea si Identificarea
Robotilor Mobili

Profesor : s.l. Rzvan olea

Studeni:

Miron Marilena

Lazr Mariana

Andrei Drago-Ionu
Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

Triunghi :

#include "Aria.h"
int main(int argc, char **argv)
{ FILE * nume;
nume=fopen("mirm.txt","w");

Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArSimpleConnector simpleConnector(&parser);
ArRobot robot;
ArSonarDevice sonar;
ArAnalogGyro gyro(&robot);
robot.addRangeDevice(&sonar);

// Make a key handler, so that escape will shut down the program
// cleanly
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");

// Collision avoidance actions at higher priority

ArActionLimiterForwards limiterAction("speed limiter near", 300, 600,
250);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100,
400);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);

// Goto action at lower priority

ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

// Stop action at lower priority, so the robot stops if it has no goal

ArActionStop stopAction("stop");
robot.addAction(&stopAction, 40);

// Parse all command line arguments

if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(1);
}

// Connect to the robot

if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1);
}
robot.runAsync(true);
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);

const int duration = 30000; //msec

ArLog::log(ArLog::Normal, "Going to four goals in turn for %d seconds,
then cancelling goal and exiting.", duration/1000);

bool first = true;

int goalNum = 0;
ArTime start;
start.setToNow();
while (Aria::getRunning())
{
robot.lock();
fprintf(nume,"%f %f %f %f %f \n",robot.getX(),
robot.getY(),robot.getTh(),robot.getVel(),robot.getRotVel());

if (first || gotoPoseAction.haveAchievedGoal())
{
first = false;
goalNum++;
if (goalNum > 3)
goalNum = 1; // start again at goal #1

// set our positions for the different goals

if (goalNum == 1)
gotoPoseAction.setGoal(ArPose(3000,0));
else if (goalNum == 2)
gotoPoseAction.setGoal(ArPose(1500, 3000));
else if (goalNum == 3)
gotoPoseAction.setGoal(ArPose(0,0));

ArLog::log(ArLog::Normal, "Going to next goal at %.0f %.0f",

gotoPoseAction.getGoal().getX(),
gotoPoseAction.getGoal().getY());
}

if(start.mSecSince() >= duration) {

ArLog::log(ArLog::Normal, "%d seconds have elapsed. Cancelling
current goal, waiting 3 seconds, and exiting.", duration/1000);
gotoPoseAction.cancelGoal();
robot.unlock();
ArUtil::sleep(3000);
Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

break;
}

robot.unlock();
ArUtil::sleep(100);
}
Aria::shutdown();
fclose(nume);
return 0;

Cerc:

while (Aria::getRunning())
{
robot.lock();
fprintf(nume,"%f %f %f %f %f \n",robot.getX(),
robot.getY(),robot.getTh(),robot.getVel(),robot.getRotVel());

if (first || gotoPoseAction.haveAchievedGoal())
{
first = false;
goalNum++;

ArLog::log(ArLog::Normal, "Going to next goal at %.0f %.0f",

gotoPoseAction.getGoal().getX(),
gotoPoseAction.getGoal().getY());

robot.setVel2(600,500);
}
Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

Hexagon :

if (goalNum > 7)
goalNum = 1; // start again at goal #1

if (goalNum == 1)
gotoPoseAction.setGoal(ArPose(0, 0));
else if (goalNum == 2)
gotoPoseAction.setGoal(ArPose(2000, 0));
else if (goalNum == 3)
gotoPoseAction.setGoal(ArPose(3000,2000));
else if (goalNum == 4)
gotoPoseAction.setGoal(ArPose(2000, 4000));
else if (goalNum == 5)
gotoPoseAction.setGoal(ArPose(0,4000));
else if (goalNum == 6)
gotoPoseAction.setGoal(ArPose(-1000,2000));
else if (goalNum == 7)
gotoPoseAction.setGoal(ArPose(0, 0));
Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

OPT :

if (first || gotoPoseAction.haveAchievedGoal())
{
first = false;
goalNum++;
if (goalNum > 4)
goalNum = 1; // start again at goal #1

if(robot.getY()>0)
robot.setVel2(300,200);
else
robot.setVel2(200,300);

Parcurgerea unei traiectorii predefinite :

Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

robot.runAsync(true);

// turn on the motors, turn off amigobot sounds

robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);

const int duration = 60000; //msec

ArLog::log(ArLog::Normal, "Going to four goals in turn for %d seconds,
then cancelling goal and exiting.", duration/1000);

bool first = true;

int goalNum = 0;
ArTime start;
start.setToNow();
while (Aria::getRunning())
{
robot.lock();

xrobot=robot.getX(); yrobot=robot.getY(); theta=robot.getTh();

theta=(theta*6.14)/360;

xd=2000;
yd=0;

if (vcom<60)
{
k=1;
}

if (k==1)
Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

{xd=2000;
yd=2000;
}

if((yrobot>1800) && (vcom<60))

{
k=2;
}

if (k==2)
{xd=4000;
yd=2000;
}

dist=sqrt((xd-xrobot)*(xd-xrobot)+(yd-yrobot)*(yd-yrobot))/1000;
fi=atan2(yd-yrobot,xd-xrobot);

vcom=dist*400;

if(vcom>500)
vcom=500;

wcom=-5*(theta-fi);

printf("%f %f %f \n", vcom,wcom,dist);

vleft=vcom-(200*wcom)/4;
vright=vcom+(200*wcom)/4;

robot.setVel2(vright,vleft);

fprintf(nume,"%f %f %f %f %f %f %f %f %f
\n",xd,yd,xrobot,yrobot,theta,vcom,wcom,robot.getVel(),robot.getRotVel());

robot.unlock();

Sonarul cu si fara obstacole :

Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

float xr=0 , yr=0, tetar, d, fi, vc , wc , vL, vR,wr;

float xd=0, yd=0, k=2, v=600, l=200;
int poz=0;
int pozitie=0;
int min=3000;
float minteta=0;
void date(float xd,float yd){
Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

d=(sqrt((xd-xr)*(xd-xr)+(yd-yr)*(yd-yr)))/500;
fi=atan2(yd-yr,xd-xr);
vc=d*v/1000;

if (vc>600)
vc=600;
if(vc<-600)
vc=600;

wc=-k*(tetar-fi);
if(wc<-5) wc=-5;
if(wc>5) wc=5;
vR=vc+l/2*wc;
vL=vc-l/2*wc;
}

int main(int argc, char **argv)

{

Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArSimpleConnector simpleConnector(&parser);
ArRobot robot;
ArSonarDevice sonar;
ArAnalogGyro gyro(&robot);
robot.addRangeDevice(&sonar);
FILE * fisier;
fisier=fopen("mirm2.txt","w");
bool first=true;
int numSonar; //Number of sonar on the robot
int i; //Counter for looping

// Make a key handler, so that escape will shut down the program
// cleanly
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");

// Collision avoidance actions at higher priority

ArActionLimiterForwards limiterAction("speed limiter near", 100, 200,
50);
ArActionLimiterForwards limiterFarAction("speed limiter far", 900, 2000,
100);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);

ArSensorReading* sonarReading; //To hold each reading

// Goto action at lower priority

ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);

// Stop action at lower priority, so the robot stops if it has no goal

ArActionStop stopAction("stop");
robot.addAction(&stopAction, 40);
Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

// Parse all command line arguments

if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(1);
}

// Connect to the robot

if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1);
}
robot.runAsync(true);

// turn on the motors, turn off amigobot sounds

robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);

const int duration = 23000; //msec

ArLog::log(ArLog::Normal, "Going to four goals in turn for %d seconds,
then cancelling goal and exiting.", duration/1000);

// bool first = true;

int goalNum = 0;
ArTime start;
start.setToNow();
while (Aria::getRunning())

{ robot.lock();

min=3000;
minteta=0;

for (i = 0; i < 8; i++) //Loop through sonar

{
sonarReading = robot.getSonarReading(i); //Get each sonar reading
if( sonarReading->getRange()<min)
{min=sonarReading->getRange();
minteta=sonarReading->getSensorTh();

}
printf("%d %f \n", min, minteta );
}

xr=robot.getX();
yr=robot.getY();
tetar=robot.getTh()*(3.14/180);

xd=10000;yd=5000;
d=(sqrt((xd-xr)*(xd-xr)+(yd-yr)*(yd-yr)))/1000;
fi=atan2(yd-yr,xd-xr);
vc=d*v;
Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

if (vc>600)
vc=600;
if(vc<-600)
vc=600;
wc=-3*(tetar-fi);
if(wc<-5) wc=-5;
if(wc>5) wc=5;
vR=vc+l/2*wc;
vL=vc-l/2*wc;
robot.setVel2(vR,vL);

if(min<2500 && minteta<0) { robot.setVel2(300,200);}

if(min<2500 && minteta>0) { robot.setVel2(200,300);}
if(min<2000 && minteta<0) { robot.setVel2(300,150);}
if(min<2000 && minteta>0) { robot.setVel2(150,300);}

fprintf(fisier, " %f %f %f %f %f \n"

,robot.getX(),robot.getY(),robot.getTh(),robot.getVel(),robot.getRotVel()
);
robot.unlock();
ArUtil::sleep(100);
}

fclose(fisier);

// Robot disconnected or time elapsed, shut down

Aria::shutdown();
return 0;
}

Ocolire obstacole cu reducere a vitezei :

Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

#include "Aria.h"
#include "Math.h"

double vc=0,wc=0,vr=0,vl=0;
double xd=10000;
double yd=10000;
double L=200;
double k=8;
double k0=4;
double v=600;
double D=0;
double xr=0,yr=0;
double fi=0;
double tetha=0;
int poz=0;
bool first = true;

double d_min;
double Th_sonar;

void date(float xd,float yd){

Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

D=sqrt((xd-xr)*(xd-xr)+(yd-yr)*(yd-yr));
fi=atan2((yd-yr),(xd-xr));
vc=D*v/1000;

if (vc>600)
vc=600;
if(vc<-600)
vc=600;

//wc=-k*(tetha-fi);
wc=-5*(tetha-fi)+1.5*(-Th_sonar)*exp(abs(-Th_sonar));
if(wc<-5) wc=-5;
if(wc>5) wc=5;

if(d_min<1000 && Th_sonar>-3.14/4 && Th_sonar<3.14/4 )

{ vc = vc/4;
}

vr=vc+L/2*wc;
vl=vc-L/2*wc;
}

int main(int argc, char **argv)

{ FILE *casuta;
casuta=fopen("sapt11a.txt","w");
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArSimpleConnector simpleConnector(&parser);
ArRobot robot;
ArSonarDevice sonar;
ArAnalogGyro gyro(&robot);
robot.addRangeDevice(&sonar);

// Make a key handler, so that escape will shut down the program
// cleanly
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");

// Collision avoidance actions at higher priority

ArActionLimiterForwards limiterAction("speed limiter near", 300, 600,
250);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100,
400);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);

// Goto action at lower priority

ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);

// Stop action at lower priority, so the robot stops if it has no goal

ArActionStop stopAction("stop");
Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

robot.addAction(&stopAction, 40);

// Parse all command line arguments

if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(1);
}

// Connect to the robot

if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1);
}
robot.runAsync(true);

// turn on the motors, turn off amigobot sounds

robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
ArSensorReading* sonarReading;

const int duration = 10000; //msec

ArLog::log(ArLog::Normal, "Going to four goals in turn for %d seconds,
then cancelling goal and exiting.", duration/1000);

int goalNum = 0;
ArTime start;
start.setToNow();

while (Aria::getRunning())
{
robot.lock();
d_min=3000;
Th_sonar=0.0;

for ( int i = 0; i < 8; i++) //Loop through sonar

{
sonarReading = robot.getSonarReading(i);
//Get each sonar reading
if(sonarReading->getRange()<=d_min)
{d_min=double(sonarReading->getRange());
Th_sonar=double(sonarReading->getSensorTh())*3.14/180;
//printf("%d , %2.0f, ",sonarReading->getRange(),sonarReading-
>getSensorTh());

}
}
printf("\n");
xr=robot.getX();
yr=robot.getY();
tetha=double(robot.getTh())*(3.14/180);

date(xd,yd);
robot.setVel2(vr,vl);

printf("%f %f %f %f %f %f %f \n",xr,yr,tetha,Th_sonar,wc,d_min,vc);
Facultatea de Automatica,Calculatoare,Inginerie Electrica si Electronica

fprintf(casuta, " %f %f %f %f %f \n"

,robot.getX(),robot.getY(),robot.getTh(),robot.getVel(),robot.getRotVel()
);

robot.unlock();
ArUtil::sleep(100);
}

fclose(casuta);

// Robot disconnected or time elapsed, shut down

return 0;
}