2011

Assignment 1

Enrique Gallardo - 1000754

University Of Essex - CE315
22/02/2011
Table of Contents
Introduction................................................................................................................................... 2
Software Algorithm........................................................................................................................ 2
Global Variables ......................................................................................................................... 3
Local Variables ........................................................................................................................... 3
Algorithm Details ....................................................................................................................... 4
Simulation Testing ......................................................................................................................... 5
Speed Plotting............................................................................................................................ 5
Laser Readings with Odometry................................................................................................... 6
Sonar Readings with Odometry .................................................................................................. 7
Source Code................................................................................................................................... 8
Introduction

A robot is an automated virtual or mechanical entity, which is designed generally to ease

he human work in any aspect or to bring entertainment. Usually this robots are programmed
directly for an specific purpose, nevertheless there are new entities in research which could learn
everything on-field, saving the programmer the time to program the robot for every possible
scenario.

For this assignment, a robot, which was previously designed with a frontal laser and 16
sonar devices, is to be used to simulate a virtual scenario shown in figure 1 which includes
obstacles and a ball which should be pushed (or grabbed) by the robot to place it inside a goal area
at the top of the diagram showed on the same figure.

(Fig. 1: Virtual Map)

Software Algorithm
The automat entity that was created for this assignment for the purpose of traveling from
a point to another on a similar trajectory than the one shown on figure 1 previously, and
afterwards move a ball from its normal position to a goal area, was designed specifically for that
purpose, therefore a simple one way state machine for the automat is enough to complete the
algorithm of the activity mentioned.
 (Fig. 2: State Diagram)

The State Diagram shown on Figure 2, shows the general picture of the algorithm, even
though on that diagram the sonar reading and the speed correction for obstacle avoidance is not
included, it will be explained on the next paragraphs.

Global Variables
Several global variables were defined and although we don’t have very limited memory on
the robot, the variables were wisely selected and will be explained next:

• int stateDeplacement=0: the state identifier for the state machine.

• int Wspeed = 100: Gives a base speed from which all speed decisions are made.
• bool fg=0: Flag used to print for unique time the final line on the console (state=99)
• int cont = 0,cont_stop=0: counters in time to enumerate the lines on the output file and to
delay the exit of the program after stopping the robot.

All the 5 global variables are defined global because the method update is initially defined
as a task on the Robot argument, which is an instance of ArRobot that has its own included
Operating system to allow to execute several tasks (or functions) on different timing, but for
purpose of congruence of timing on the readings, the laser and the sonar functions were imposed
on the same update function with a tick time of 50 ticks and with a tick timer of 20 counts. This
means that depending on the resolution time of the processor of the robot, this would determine
the time on which the function update is executed, giving the robot a faster reaction time by
reducing or the amount of ticks that the robot requires, or the tick counter to reduce the tick time,
and vice versa to reduce the reaction time.

Local Variables
To reduce the ROM memory usage, local variables can be defined, and even though more
RAM is therefore used (mostly depending on the assembly part of the compiler and therefore
depending on each compiler), local variables are preferred when it is possible to use them.

For that reason local variables are more abundant than the global variables mentioned
above, those local variables include some constants used for reference of some readings on the
range devices as will be explained next:
 Starting with the constants we have numberOfLaserRanges which will be having a single
number which will show the number of divisions that will be taken into account in-between the
180° total range of the laser, which leads us to the next two constants which are startAngle and
endAngle, which as their names imply, include the start and end angles of the ranges and must
have the same number of elements as numberOfLaserRanges. The last constant is for the sonar,
which has the angles of the fixed sonars, which are fixed directly on the robot, but for
simplification of the code these were placed on the angle_of_sonar constant of double floats.

Other local variables were defined for multiple purposes, including the ‘for’ cycles
arguments as is the int i=0.

Since the odometry is already calculated on the robot with the wheel movement sensors,
we only need to retrieve that information, for which the variables xgotten, ygotten and thgotten
are defined. As well speed_right and speed_left accomplish the same objective as the previous but
to retrieve the speed and the sonar_read [16] array is created to save the values of the sonars into
the function locally for simplification of code and to avoid making several requests of the same
data in the same cycle.

Lastly and very important variables are the ones with the calculations of the interpretation
of each of the sonars and each of the laser ranges, to get the calculated position of the obstacle on
the global plane, to be able to draw later the hypothetical map of the environment which could be
used later by the robot to find the shortest path to a point for which several algorithms like A* (A
star) could be used but not implemented for this design since there is no such requirement for the
purpose that the robot must do. The variables mentioned on this paragraph are XS [16], YS [16],
XL[numberOfLaserRanges] and YL[numberOfLaserRanges] which are the X and Y positions of the
obstacles of each of the sonars in the current cycle and the X and Y obstacle position calculation of
each of the laser ranges (by default 20) respectively, which will allow the robot to have a second
map and also on a single cycle because of which the data must be printed into a file for later
analysis, and data could be manipulated back by the robot to create the map for obstacle
avoidance and as mentioned above.

The other local variables are temporal variables which could be used several times with no
effect on the code as they would be used after being assigned a value. These variables are
angle_temp, angle_laser_temp and distance_laser_temp, which are respectively the angle on the
global plane from the robot position to the obstacle found, the exact laser angle from the local
(robot) plane inside the range where the obstacle was found and the distance of the obstacle from
the robot read by the laser.

Algorithm Details
The algorithm includes not only the state machine by itself but also some other
calculations required to obtain the output file with the coordinates of the obstacles and also to
reduce the speed or on its case the full stop of the robot if it gets too near to an obstacle.
 Also the speed is reduced to 50 in case the range device detects an object in the front of
the robot in a shorter distance than 1050, and in case the object gets closer to 600, then stops the
robot to avoid damage of the entity. Otherwise the speed is set to 150.

For the sonar and laser readings the formula showed on Formula 1 and Formula 2 for X
and Y respectively, which are used to calculate the position of the obstacle in accordance to the
angle in the global plane, the position of the robot (XOY) and the distance of the obstacle. This is
the data printed used for the mapping. The data of the speed is also printed on this same basis,
where the fprintf command was preferred to be used.

= 
 
 + 
 ∗ cos  

(Formula 1: X obstacle position calculation)

 =   
 + 
 ∗ sin  

(Formula 2: Y obstacle position calculation)

Simulation Testing
The simulation results are as shown below on figures 3 to 5.

Speed Plotting
700
600
500
400
300 Right
200 Left

100
0
0 500 1000 1500
-100
-200

Fig 3: Speed Plot of both wheels

The speed of the robot can be easily obtained by using the command robot.getRightVel(); and
robot.getLeftVel(); for the right and left wheel respectively.

There is one possible way of interpreting the speed grap, and it is to be considered that
when both lines separate from each other means that the robot is turning, I f they have the
opposite value, then it is turning on the same position, and if only one line can be seen then both
are advancing on the same speed.

Laser Readings with Odometry

5000
L0

4000 L1
L2
3000 L3
L4
2000
L5

1000 L6
L7
0 L8
-1000 0 1000 2000 3000 4000 5000
L9
-1000

Fig 4: Laser obstacle lecture with Trajectory

It can be seen on figure 4 that a good precision of map is obtained thanks to the laser
reading, and it you manage to compare the sonar readings on figure 5 and the laser readings, you
will be able to see the big difference of the precision of both sensors.

This defines to us that if the laser is possible to be used for mapping, it is better to use only
the laser, since it will have more accuracy than other range devices.

The laser have another great advantage over the sonar, which is that it has no fixed angle,
but that it gets the lecture from the closest obstacle in a range. Meaning, more accuracy by itself ,
but this also means that the angle changes on every reading, because the object or the robot
moves, therefore an additional line of code is required than on the sonar, but as it can be seen on
figure 4, the results are worth the cost.
Sonar Readings with Odometry

5000
S0

4000 S1
S2
3000 S3
S4
2000 S5
S6
1000 S7
S8
0 S9
-1000 0 1000 2000 3000 4000 5000
S10
-1000

Fig 5: Sonar Obstacle lecture with Trajectory

The sonar lectures are a very good approximation of the obstacles as can be seen on figure
5. These points can be considering for a good obstacle avoidance and to make a better
approximation of the obstacle position, some average of the points could be made so that a more
linear map could be drawn.

The advantage of the sonar calculation is the fixed angles, but the biggest problem, is the
self-induced noise. Meaning that one sonar could give noise to the other one, and that could give
a mistaken lecture of the distance of the obstacle, causing either a bad lecture or in the worst case,
an unexpected reaction of the robot due to that wrong data.
Source Code
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "Aria.h"
FILE *pFile = fopen("OutputFile.csv","wt");//output file for all the data.
ArRobot robot;
#define PI 3.14159265 //PI: used later on to convert degree's to radian's. (Needed for Creating the Sonar MAp)
void update(void);
ArGlobalFunctor updateCB(&update);
void laser(void);
ArGlobalFunctor laserCB(&laser);
//global variables
int stateDeplacement=0;//state machine
int Wspeed = 100;//speed variable reference
bool fg=0;//flag for end
int cont = 0,cont_stop=0;//counters (tick)
void update(void)
{
ArLaser *myLaser = robot.findLaser(1);//laser instance init
//constants
const int numberOfLaserRanges = 20;//laser divisions
const int startAngle [numberOfLaserRanges] = {-90,-81,-72,-63,-54,-45,-36,-27,-18,-9,0,9,18,27,36,45,54,63,72,81};//start angles
const int endAngle [numberOfLaserRanges] = {-81,-72,-63,-54,-45,-36,-27,-18,-9,0,9,18,27,36,45,54,63,72,81,90};//end angles
const double angle_of_sonar [16] = {90,50,30,10,-10,-30,-50,-90,-90,-130,-150,-170,170,150,130,90};//fixed sonar angles
//local variables
int i=0;//temp
int xgotten, ygotten, thgotten;//odometry from robot
int speed_right,speed_left;//speed of each wheel
int sonar_read [16];//sonar distance
double XS [16] , YS [16] , XL[numberOfLaserRanges] , YL[numberOfLaserRanges];//calculation of obstacles on global plane
double angle_temp = 0;//angle in global plane
double angle_laser_temp,distance_laser_temp;//disntance and angle from robot
//read and save locally all sonars
for(int i=0;i<16;i++){
sonar_read [i] = (int)robot.getSonarRange(i);//assign locally sonar values
}
//inspect current status on programmed movement
xgotten = (int)robot.getX();
ygotten = (int)robot.getY();
thgotten = (int)robot.getTh();
//adapt angle to full circle range (360 degrees)
if(thgotten<0)
{
thgotten = thgotten +360;//from (-180,180) to (0,360]
}
//state machine
switch (stateDeplacement)
{
case 99://stop and end machine and disconnect and save file
robot.setVel2(0,0);
//robot.stop();
if(!fg)//do once
{
printf("\nRobot Stopped with no more commands configured (please restart)\n");
fg=1;
}
if(cont-cont_stop>=30)//delay
{
fclose (pFile);
robot.lock();
robot.disconnect();
robot.unlock();
Aria::shutdown(); // Now exit.
}
break;
case 0: //straight
robot.setVel2(Wspeed,Wspeed);
if(ygotten<=1650)
{
stateDeplacement=1;
printf("\nturning\n");
}
break;
case 1://turn right
robot.setVel2(Wspeed,(int)Wspeed*1.5);
if(thgotten<=178 )
{
stateDeplacement=2;
printf("\n stop turning\n");
}
break;
case 2://straight
robot.setVel2(Wspeed,Wspeed);
if(xgotten<=1800)
{
stateDeplacement=3;
printf("\nturn right\n");
}
break;
case 3://turn right
robot.setVel2(Wspeed,(int)Wspeed*1.85);
if(thgotten<=75)
{
stateDeplacement=4;
printf("\nstraight\n");
}
break;
case 4://straight
robot.setVel2(Wspeed,Wspeed);
if(ygotten>=2100)
{
stateDeplacement=5;
printf("\nright\n");
}
break;
case 5://turn right
robot.setVel2(Wspeed,(int)Wspeed*2);
if(thgotten<=5 || thgotten >340)
{
stateDeplacement=6;
printf("\nstraight to stop\n");
}
break;
case 6://straight
robot.setVel2(Wspeed,Wspeed);

if(xgotten>=2090)
{
robot.setVel2(0,0);
}
if(robot.getVel() == 0)//after full stop
{
stateDeplacement=7;
printf("\nstopped\n");
printf("turning left\n");
}
break;
case 7://turn left to goal on position
if(thgotten>=70 && thgotten<180)
{
robot.setVel2((double)Wspeed*0.2,(double)Wspeed*-0.2);//reduce speed for accurate angle
}
else
{
robot.setVel2(Wspeed,(double)Wspeed*-1);//turn on position
}
if(thgotten>=88 && thgotten<180)//go 90 degrees
{
robot.setVel2(0,0);
stateDeplacement=8;
printf("\nstopped\n");
printf("straight to goal\n");
}
break;
case 8:
robot.setVel2(Wspeed*4,Wspeed*4);//increase speed to hit ball
if(ygotten >=4000)//goal reached
{
stateDeplacement = 99;
 cont_stop=cont;
printf("\nSTOP!\n----------------End Of File------------\n");
}
break;
}//end switch (state machine)

if(sonar_read [3] <= 1050 || sonar_read[4] <= 1050)//close range obstacle (reduce speed)
{
Wspeed=50;//low speed
}
else if(stateDeplacement!=99&&(sonar_read [3] <= 600 || sonar_read[4] <= 600))//too close. Stop!
{
stateDeplacement = 99;
cont_stop=cont;
}
else
{
Wspeed=150;//normal speed
}
//get speed of wheel from robot
speed_right = (int)robot.getRightVel();
speed_left = (int)robot.getLeftVel();
//sonar: calculations to get distance of obstacle in global plane (from sonar)
for(i=0;i<16;i++)
{
if((thgotten+angle_of_sonar[i])>360)//calculi of angle in the global plane
{
angle_temp=((double)(thgotten)+angle_of_sonar[i]-360)*(PI/180);//
}
else
{
angle_temp=((double)(thgotten)+angle_of_sonar[i])*(PI/180);
}
XS[i] = (double)(xgotten) + ((double)(sonar_read[i]) * cos(angle_temp));//x position (global)
YS[i] = (double)(ygotten) + ((double)(sonar_read[i]) * sin(angle_temp));//y position (global)
}

//laser readings: calculates all the laser obstacle positions.

for(i=0;i<numberOfLaserRanges;i++)
{
distance_laser_temp = myLaser->currentReadingPolar(startAngle[i],endAngle[i],&angle_laser_temp);
if((thgotten+angle_of_sonar[i])>360)
{
angle_temp=((double)(thgotten)+angle_laser_temp-360)*(PI/180);
}
else
{
angle_temp=((double)(thgotten)+angle_laser_temp)*(PI/180);
}
XL[i] = (double)(xgotten) + (distance_laser_temp * cos(angle_temp));
YL[i] = (double)(ygotten) + (distance_laser_temp * sin(angle_temp));
if(XL[i]>10000)
{
XL[i] = 0;
YL[i] = 0;
}
else if(YL[i]>10000)
{
XL[i] = 0;
YL[i] = 0;
}
}
// prints to file all positions, speed and sonar read and odometry and counter per line
fprintf(pFile, "%d,%d,%d,%d,%d,%d,",cont,xgotten,ygotten,thgotten,speed_right,speed_left);
fprintf(pFile,
"%d,%d,%d,%d,%d,%d,%d,%d,",sonar_read[0],sonar_read[1],sonar_read[2],sonar_read[3],sonar_read[4],sonar_read[5],sonar_read[6],sonar_read[7]);
fprintf(pFile,
"%d,%d,%d,%d,%d,%d,%d,%d,",sonar_read[8],sonar_read[9],sonar_read[10],sonar_read[11],sonar_read[12],sonar_read[13],sonar_read[14],sonar_read[1
5]);
fprintf(pFile, "%f,%f,%f,%f,%f,%f,%f,%f,",XS[0],YS[0],XS[1],YS[1],XS[2],YS[2],XS[3],YS[3]);
fprintf(pFile, "%f,%f,%f,%f,%f,%f,%f,%f,",XS[4],YS[4],XS[5],YS[5],XS[6],YS[6],XS[7],YS[7]);
fprintf(pFile, "%f,%f,%f,%f,%f,%f,%f,%f,",XS[8],YS[8],XS[9],YS[9],XS[10],YS[10],XS[11],YS[11]);
fprintf(pFile, "%f,%f,%f,%f,%f,%f,%f,%f,",XS[12],YS[12],XS[13],YS[13],XS[14],YS[14],XS[15],YS[15]);
fprintf(pFile, "%f,%f,%f,%f,%f,%f,%f,%f,",XL[0],YL[0],XL[1],YL[1],XL[2],YL[2],XL[3],YL[3]);
fprintf(pFile, "%f,%f,%f,%f,%f,%f,%f,%f,",XL[4],YL[4],XL[5],YL[5],XL[6],YL[6],XL[7],YL[7]);
fprintf(pFile, "%f,%f,%f,%f,%f,%f,%f,%f,",XL[8],YL[8],XL[9],YL[9],XL[10],YL[10],XL[11],YL[11]);
 fprintf(pFile, "%f,%f,%f,%f,%f,%f,%f,%f,",XL[12],YL[12],XL[13],YL[13],XL[14],YL[14],XL[15],YL[15]);
fprintf(pFile, "%f,%f,%f,%f,%f,%f,%f,%f\n",XL[16],YL[16],XL[17],YL[17],XL[18],YL[18],XL[19],YL[19]);
//console print...
printf("n= %d L=%d R=%d \r",cont,speed_left,speed_right);
cont++;//cnt increase (tick)

}
void laser(void) //This Function is defined on the update function with the comment "laser readings"
{
int i = 0;
i=14;//DO NOTHING! (This Function is defined on the update function with the comment "laser readings")
}

int main(int argc, char **argv)

{
Aria::init();
ArArgumentParser argParser(&argc, argv);
argParser.loadDefaultArguments();
ArRobotConnector robotConnector(&argParser, &robot);
ArLaserConnector laserConnector(&argParser, &robot, &robotConnector);
// Always try to connect to the first laser:
argParser.addDefaultArgument("-connectLaser");
if(!robotConnector.connectRobot())
{
ArLog::log(ArLog::Terse, "Could not connect to the robot.");
if(argParser.checkHelpAndWarnUnparsed())
{
// -help not given, just exit.
Aria::logOptions();
Aria::exit(1);
}
}
// Trigger argument parsing
if (!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
ArSonarDevice sonar;
robot.addRangeDevice(&sonar);
// try to connect to laser. if fail, warn but continue, using sonar only
if(!laserConnector.connectLasers())
{
ArLog::log(ArLog::Normal, "Warning: unable to connect to requested lasers, will wander using robot sonar only.");
}
ArPose space(3800, 3500, 268); // Initial robot's odometry.
robot.moveTo(space); //Moves the robot's idea of its position to this position.
//headers
fprintf (pFile, "n,X,Y,TH,Right Wheel,Left
Wheel,S0,S1,S2,S3,S4,S5,S6,S7,S8,S9,S10,S11,S12,S13,S14,S15,XS0,YS0,XS1,YS1,XS2,YS2,XS3,YS3,XS4,YS4,XS5,YS5,XS6");
fprintf (pFile,
",YS6,XS7,YS7,XS8,YS8,XS9,YS9,XS10,YS10,XS11,YS11,XS12,YS12,XS13,YS13,XS14,YS14,XS15,YS15,XL0,YL0,XL1,YL1,XL2,YL2,XL3,YL3,XL4,YL4,XL5,YL5,XL6,YL6,
XL7,YL7,");
fprintf (pFile, "XL8,YL8,XL9,YL9,XL10,YL10,XL11,YL11,XL12,YL12,XL13,YL13,XL14,YL14,XL15,YL15,XL16,YL16,XL17,YL17,XL18,YL18,XL19,YL19\n");
// turn on the motors, turn off amigobot sounds, turn on laser, add tasks for the OS
ArLaser *myLaser = robot.findLaser(1);
robot.enableMotors();
robot.addUserTask("update", 50, &updateCB);
robot.addUserTask("laser", 80, &laserCB);//not used
robot.setCycleTime (20);
robot.runAsync(true);
// wait for robot task loop to end before exiting the program
robot.waitForRunExit();
fclose (pFile);
Aria::exit(0);
}