Dead Reckoning Techniques for the RoboCube
Project report for Techniques of the AI 2
by Kristof Van Laerhoven
Index INTRODUCTION Dead Reckoning * Odometry * Objectives * THE ALGORITHMS Overview of the architecture framework * The queue module * The dead reckoning module * Miscellaneous * THE RESULTS The UMBmark Benchmark * The results * Remarks * References *
|
Introduction
Description of dead reckoning and odometry
The term dead reckoning comes from the fifteenth, sixteenth, and seventeenth centuries, where the sea captains relied on this method to reach their destination. They estimated their position (and, more important, their destination heading) by calculations using the speed of their ship and its heading. Using crude tools and factoring in the effects of ocean currents, fickle winds and errors in judgement, the technique of dead reckoning too often marked him for a dead man [3].
Subsequent vectors can be calculated if the speed is known (used to determine the length of the vector) and the heading of the movement (used for the angle of the vector). Given the starting coordinates, and these two components, it is theoretically possible to calculate the new coordinates after movement has occurred.
This method can also be applied in the positioning system of mobile robots. The motion of a robot in a specific coordinate system can be described with the kinematics of its differential drive. More concrete, the position and orientation of the robot can be calculated from the speed that was sent to the motors.
One way to figure out what distance was traveled by the robot is the use of shaft- encoders. These are little devices that measure the wheel rotation; [1] gives a good introduction, as well as an overview on the variations on the shaft-encoder. Usually, it is connected on the motor-side as the motor turns faster than the wheel and thus gives a better resolution of measurement.
Odometry provides easily accessible real-time positioning information ( cartesian x and y coordinates, orientation, …) from periodic absolute measurements. The frequency at which this measurement (which is usually time-consuming) is performed depends largely on the accuracy of the odometry system.
A well-known disadvantage of odometry is its inaccuracy with an accumulation of errors. Errors will become so large that it is only a matter of time before the robot’s internal position estimate is completely wrong. Therefore, it is common to estimate the position uncertainty, which can be visualized by an ellipse around the robot that grows larger with travel distance.
Two categories of errors exist in odometry: systematic and non-systematic errors. Systematic errors are caused by imperfections in the design of the robot, like unequal wheel diameters, misalignment of wheels, limited encoder resolution and limited encoder sampling rate. These errors are particularly serious because they accumulate constantly. Non-systematic errors deal with uneven floors, unexpected objects on the floor and wheel slippage due to over-acceleration, fast turning (skidding) and slippery or dirty floors.
This project aims at using dead reckoning methods to guide small robots that are equipped with the RoboCube processing unit as the robot’s kernel. Commands should be sent to them like "go to position (45,14)" or "turn 12 degrees". A camera that is placed above the robot arena can act as a monitoring device that is able to give the directions and can –if necessary- improve or correct the robot’s internal position coordinates. The target of this project is to research methods that enable an internal positioning system for the robot without too much interference and correction from the camera.
The aim of this report is to elaborate on the techniques that were used and implemented in this project. The most important sections of the source code will be discussed, along with the motives why it was inserted and why possible other methods were ignored. Finally, the system will be tested using an independent benchmark, UMBmark.
The algorithms
A discussion of the different implemented algorithms
Overview of the architecture framework
Two distinct modules form the whole dead reckoning system. One handles incoming commands in a FIFO (first in – first out) queue, so it is called the Queue Module. The other –which is called the Dead Reckoning Module- obtains the commands from the queue and executes them. The commands that can be placed in the queue are represented by a ID number, which is only defined in the latter module; the Queue Module manipulates only records with short integers and has no idea of their meaning.
The Dead Reckoning Module is able to execute commands like "turn", "go forward", "go to position", "shoot" and so on. Most of these commands need additional parameters, the turn position needs for instance something that represents how much the robot has to turn and in which direction. In the picture above the "turn" command is encoded by the command ID 2, and the x parameter contains the centidegrees that the robot has to turn. Positive angles make the robot turn to the right, negative ones make it turn to the left.
So far, only the ability to command the robot what to do was discussed. This project goes further than that, however. The Dead Reckoning Module has algorithms that keep track of the robot’s position (in x-y coordinates), heading and traveled distance. These values can be accessed and modified by a second system to enable corrections from an external source (e.g. a birds-view camera overlooking the scene).
The part of the program that manages the queue is very simple in structure. It is actually a FIFO (first in - first out) queue, implemented by a pointer-list of simple structures containing three numbers: a command_id which represents the command that has been send (e.g. "turn"), and two parameters: x and y. These parameters are not always necessary, though.
| struct command { signed int command_id; signed int x; signed int y; struct command *next; }; |
The command structure.
To manage this queue, a few functions were written to execute the most basic tasks: queue_add adds a command to the queue, queue_get takes the first command of the queue, queue_flush deletes all commands in the queue. Two other functions were written for inspection of the queue: queue_length returns the length of the queue (number of commands) and queue_show prints a list of the commands plus their parameters to the terminal screen. To summarize, the queue-related functions are:
The bodies of the functions can be found in the appendix.
The command_id's are recognized by the dead reckoning module and the links from id numbers to the actual meanings are stored in the empty_queue function:
This function gets the commands one by one from the queue and executes them. The commands that were implemented, along with their id numbers are summed up in the next table:
command_id |
command using X and Y as parameters |
1 |
Go forward for X pulses |
2 |
Turn X pulses |
3 |
Shoot |
4 |
Goto position (X,Y) |
Additional commands should be added to this table, which is implemented as a switch statement in the empty_queue function. The shoot command will be clarified in the miscellaneous section.
The most important part of the program is the dead reckoning part, however. This module is responsible for executing the commands from the queue and calculating the position of the robot. The functions that compute the position, direction and distance are: get_dist which takes the number of pulses from both motors’ shaft-encoders and returns the distance traveled so far. get_heading takes the number of pulses from both motors as well and returns the heading of the robot. Finally getxy takes also the number of pulses from both motors and returns the x-y position. The getxy function uses the two previous functions. The core odometry functions therefore are:
This function uses the two parameters in the next function:
where WR is the radius of the wheels, GR is the gear ratio and P360 is the number of pulses per turn.
Similar to the previous function this function calculates:
where WR, GR and P360 have the same meaning as in the previous function, and WD represents the distance between the wheels.
This function uses the two previous functions to obtain the distance and heading, and -given these two values- calculates the x and y positions as follows:
The cosine and sine functions were implemented with look-up tables to speed up the processing and shrink the size of the download (program-)file.
Remark: the mathematical background of these formulas can be found in [1].
The functions that control the speed of both motors are very straightforward: go activates both motors in the right direction until a certain distance is travelled, turn does the same, but this time for turning (so the speed of one motor is the reverse of the other). To recapitulate, the two functions for controlling the motors are:
The implementation of these functions can be found in the Appendix. Both go and turn functions use several techniques to enhance the performance of the robot:
Active breaking: when the robot comes to a full stop, one wheel usually stops before the other, resulting in a different (and false) heading. The higher the speed of the motors (and/or the more abrupt the stop), the higher the fault of the changed heading will be. It gets even worse when the wheel keeps rolling for a moment. Not only a false heading will be reached, but also the distance will be too high (since the wheels don't stop at once). The solution is to block the wheels by giving the motors that drive them a small negative speed. It is of course imperative that this blocking happens simultaneously on both wheels.
This function first calculates the Euclidean distance (in millimeters) between the current position and the target position:
After this, the number of pulses that are required to make the robot travel this distance are calculated from the formula in the get_dist function:
where GR, P360 and WR have the same meaning as in previous functions.
This function first processes the angle (in centidegrees) that has to be turned by the robot:
Then, the number of pulses that are needed to obtain this angle (in centidegrees) are processed from the formula in the get_heading function:
To facilitate the testing of the algorithms, a menu was implemented for inspection and command purposes. As a result, changes in parameters can be made while the program keeps running and new tests can directly be made without the programmer having to reset the robot, recompile the program and resend the whole program into the RoboCube memory. A menu was implemented for both the queue and the dead reckoning modules.
| ******************************************************************************* |
| X:120 Y:-23 (201-785) Heading: -6770 Distance: 172 |
| ******************************************************************************* |
| 1. go pulses |
| 2. turn pulses |
| 3. manage the queue |
| 4. change # of distance pulses |
| 5. change # of degree turning pulses |
| 6. let the robot do pre-prepared moves |
| 7. reset position to (0,0) |
| 8. test shooter. |
| 9. go to position (X,Y) (in mms) |
| 10. set new X and Y (in mms) |
| >What shall I do? |
The menu for the dead reckoning modules.
| 1. flush the queue |
| 2. add a command to the queue |
| 3. get a command from the queue |
| 4. get the length of the queue |
| 5. show the contents of the queue |
| 6. execute all |
| What shall I do? |
The menu for the queue.
Apart from these menus, the ability for the robot to activate its shooting mechanism was also implemented, in the function:
This function first activates the motor to do the actual shooting, then it retracts its 'foot'. The shooting mechanism enables the robot to play soccer-like games.
The results
Outcome of the UMBmark Benchmark test and conclusions
Borenstein [2] proposes a benchmark test for the quantitative measurement of odometry errors in mobile robot navigation. It assures that different dead reckoning errors don’t compensate for each other, as may occur in other odometry tests. The procedure yields a single numeric value, Emax,sys, that represents a measure of the robots’ systematic odometry errors. The procedure can be summarized as follows:
The tests in both clockwise and counterclockwise directions are necessary since it is possible that faulty angles and faulty forward driving compensate each other, resulting in the end-position to be very near the start-position. A test in the other direction will eliminate this.
The UMBmark paper suggests that the robot should move as slow as possible to prevent errors due to slipping or skimming to obtain the best results. However, some applications (like the RoboCup event for example) require that the robot moves as fast as possible. Two tests were executed to evaluate the performance of the dead reckoning: one with high turning speed (50) and high forward speed (100) and one with low speeds (30 for turning, 50 for driving forward). The results, as depicted in the following figures, speak for themselves: a trade-off emerges between speed and precision. It should also be pointed out that the size of the square path in the tests was somewhat smaller than that suggested for the UMBmark benchmark. In stead of four meters, sixty centimeters was chosen since it was hard (impossible) to find an open space of more than four by four meters.
The first graph has large errors, that reside in the same region for both the clockwise squares and the counterclockwise ones. The large amount of dust on the floor of the AI lab may be responsible, but only for a small part. A more realistic explanation is the high speed, since the robot never stops on time due to a high velocity. This problem still appears in the low-speed test, but this time in a less dramatic way. The results are a very good performance for the clockwise squares, and a not-so-good performance for the counterclockwise square path. The results are however comparable with those mentioned in the paper by Borenstein [2].
As was experienced in this project, small things often make a big difference. Here is a short list of things that should be kept in mind:
The source
/**************************************************************************/
/* */
/* Dead Reckoning & Odometry routines by Kristof Van Laerhoven */
/* */
/**************************************************************************/
#include "\robot\pdl\v2.0\smb2pdl1.h"
#include "\robot\pdl\v2.0\smb2io.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h> /* for sqrt and atan */
#define PI 3.141592 /* pi */
#define RADS 57.2958 /* degrees <-> radians conversion factor */
#define WHEEL_RADIUS 17.5 /* radius of the wheels in mms */
#define GEAR_RATIO 16.58 /* gear ratio */
#define PULSES_PER_TURN 16 /* pulses per motor turn */
#define WHEELS_DISTANCE 112 /* distance between wheels in mms */
#define MAX_TURN_SPEED 40 /* maximum motor speed value for turning */
#define MAX_GO_SPEED 100 /* maximum value for driving forwards */
#define HEAD_ERROR 1.18 /* experimentally verified heading error */
#define DIST_ERROR 0.90 /* ... and travelled distance error */
#define TURN_ERROR 100 /* precision in turning (in pulses) */
#define GO_ERROR 100 /* precision in driving forward (" ") */
#define sign(x) ((x)>=0?1:-1)
/* used with the Sin lookup table to determine the heading */
/* PDL quantities */
quantity LeftSpeed, LeftAcceleration, RightSpeed, RightAcceleration;
quantity LeftMotor, RightMotor, Shooting;
/* lookup tables for sin and cos */
const short Cos[180] = { 10000, 9998, 9994, 9986, 9976, 9962, 9945, 9925, 9903, 9877, 9848, 9816, 9781, 9744,
9703, 9659, 9613, 9563, 9511, 9455, 9397, 9336, 9272, 9205, 9135, 9063, 8988, 8910,
8829, 8746, 8660, 8572, 8480, 8387, 8290, 8192, 8090, 7986, 7880, 7771, 7660, 7547,
7431, 7314, 7193, 7071, 6947, 6820, 6691, 6561, 6428, 6293, 6157, 6018, 5878, 5736,
5592, 5446, 5299, 5150, 5000, 4848, 4695, 4540, 4384, 4226, 4067, 3907, 3746, 3584,
3420, 3256, 3090, 2924, 2756, 2588, 2419, 2250, 2079, 1908, 1736, 1564, 1392, 1219,
1045, 872, 698, 523, 349, 175, 0, -175, -349, -523, -698, -872,-1045,-1219,
-1392,-1564,-1736,-1908,-2079,-2250,-2419,-2588,-2756,-2924,-3090,-3256,-3420,-3584,
-3746,-3907,-4067,-4226,-4384,-4540,-4695,-4848,-5000,-5150,-5299,-5446,-5592,-5736,
-5878,-6018,-6157,-6293,-6428,-6561,-6691,-6820,-6947,-7071,-7193,-7314,-7431,-7547,
-7660,-7771,-7880,-7986,-8090,-8192,-8290,-8387,-8480,-8572,-8660,-8746,-8829,-8910,
-8988,-9063,-9135,-9205,-9272,-9336,-9397,-9455,-9511,-9563,-9613,-9659,-9703,-9744,
-9781,-9816,-9848,-9877,-9903,-9925,-9945,-9962,-9976,-9986,-9994,-9998};
const short Sin[180] = { 0, 175, 349, 523, 698, 872, 1045, 1219, 1392, 1564, 1736, 1908, 2079, 2250,
2419, 2588, 2756, 2924, 3090, 3256, 3420, 3584, 3746, 3907, 4067, 4226, 4384, 4540,
4695, 4848, 5000, 5150, 5299, 5446, 5592, 5736, 5878, 6018, 6157, 6293, 6428, 6561,
6691, 6820, 6947, 7071, 7193, 7314, 7431, 7547, 7660, 7771, 7880, 7986, 8090, 8192,
8290, 8387, 8480, 8572, 8660, 8746, 8829, 8910, 8988, 9063, 9135, 9205, 9272, 9336,
9397, 9455, 9511, 9563, 9613, 9659, 9703, 9744, 9781, 9816, 9848, 9877, 9903, 9925,
9945, 9962, 9976, 9986, 9994, 9998,10000, 9998, 9994, 9986, 9976, 9962, 9945, 9925,
9903, 9877, 9848, 9816, 9781, 9744, 9703, 9659, 9613, 9563, 9511, 9455, 9397, 9336,
9272, 9205, 9135, 9063, 8988, 8910, 8829, 8746, 8660, 8572, 8480, 8387, 8290, 8192,
8090, 7986, 7880, 7771, 7660, 7547, 7431, 7314, 7193, 7071, 6947, 6820, 6691, 6561,
6428, 6293, 6157, 6018, 5878, 5736, 5592, 5446, 5299, 5150, 5000, 4848, 4695, 4540,
4384, 4226, 4067, 3907, 3746, 3584, 3420, 3256, 3090, 2924, 2756, 2588, 2419, 2250,
2079, 1908, 1736, 1564, 1392, 1219, 1045, 872, 698, 523, 349, 175};
struct command {
signed int command_id;
signed int x;
signed int y;
struct command *next;
};
struct command *queue, *current;
signed short x_pos, y_pos;
signed short x_tar, y_tar;
int dist, dgr; /* distance from position */
signed short heading;
signed short distance;
char turn_ok, go_ok, queue_ok, shoot_ok, shoot_id, goto_ok;
int countleft;
int countright;
/***************************************************************************/
/* queue_add: add a command to the queue */
char queue_add(struct command com) {
struct command *newptr;
newptr = (struct command*) malloc (sizeof (struct command));
if (newptr == NULL) return 1; /* no more memory available.. */
if (queue == (struct command*)NULL)
queue = current = newptr; /*.. if this is the first command*/
else { /* else... */
current = queue;
while (current->next != (struct command*)NULL)
current = current->next;
current->next = newptr;
current = newptr;
}
/*current->command_id = com.command_id;
current->x = com.x;
current->y = com.y; */
*current = com; /* add at the end of list */
current->next = (struct command*)NULL; /* this is the new end-of-list */
return 0; /* no errors here ! */
}
/* queue_get: get the first command in the queue and delete it */
struct command queue_get(void) {
struct command com;
current = queue;
if (current == (struct command*)NULL) {
com.command_id = 0; /*0: no more commands in queue !*/
return com;
}
else {
/* com.command_id = current->command_id;
com.x = current->x;
com.y = current->y; */
com = *current;
queue = current->next;
}
free(current);
return com;
}
/* queue_show: show all commands in the queue */
void queue_show(void) {
if (queue == (struct command*)NULL) {
printf("\Empty queue.\n\r");
return;
}
current = queue;
do {
printf(" Command_id:%d",current->command_id);
printf(", X parameter: %d",current->x);
printf(", Y parameter: %d \n\r",current->y);
current = current->next;
} while (current != (struct command*)NULL);
}
/* queue_flush: delete everything in the queue */
char queue_flush(void) {
if (queue == (struct command*)NULL) {
return 0; /* there was nothing to flush! */
}
do {
current = queue;
queue = current->next;
free(current);
} while (queue != (struct command*)NULL);
return 0;
}
/* queue_length: returns the length of the queue */
int queue_length(void) {
unsigned int n;
if (queue == (struct command*)NULL) {
return 0; /* length = 0 */
}
current = queue;
do {
current = current->next;
n++;
} while (current != (struct command*)NULL);
return n;
}
/* monitor routine */
int printmenu(void) {
int n;
printf("\n\r 1. flush the queue\n\r");
printf(" 2. add a command to the queue\n\r");
printf(" 3. get a command from the queue\n\r");
printf(" 4. get the length of the queue\n\r");
printf(" 5. show the contents of the queue\n\r");
printf(" 6. execute all!\n\r");
printf("What shall I do?\n\r");
scanf("%d",&n);
return n;
}
/* monitor routine */
struct command getcommand(void) {
struct command com;
printf("\n\r*** new command ***\n\r");
printf(" What command id? ");
scanf("%d",&com.command_id);
printf("\n\r What X value? ");
scanf("%d",&com.x);
printf("\n\r What Y value? ");
scanf("%d",&com.y);
return com;
}
/* monitor routine */
void handle_queue(void) {
/****** queue related functions **************************/
char queue_add(struct command com);
struct command queue_get(void);
void queue_show(void);
char queue_flush(void);
int queue_length(void);
/****** menu stuff for tests *****************************/
int printmenu(void);
struct command getcommand(void);
struct command temp;
int n;
do {
switch (n = printmenu()) {
case 1: queue_flush(); break;
case 2: queue_add(getcommand()); break;
case 3: printf("\n\r*******************************\n\r");
temp = queue_get();
printf(" command = %d\n\r",temp.command_id);
printf(" x = %d\n\r",temp.x);
printf(" y = %d\n\r",temp.y);
printf("*******************************\n\r");
break;
case 4: printf("\n\r*******************************\n\r");
printf(" length = %d\n\r",queue_length());
printf("*******************************\n\r");
break;
case 5: printf("\n\r***************************************************\n\r");
queue_show();
printf("***************************************************\n\r");
break;
case 6: printf(" ok!"); break;
}
} while (n!=6);
}
/*************************************************************************/
/* calculate the distance */
signed short get_dist(signed short left_pulses, signed short right_pulses) {
return ((left_pulses+right_pulses)*PI*WHEEL_RADIUS)/
(GEAR_RATIO*PULSES_PER_TURN);
}
/* calculate the heading */
signed short get_heading(signed short left_pulses, signed short right_pulses) {
return ((left_pulses-right_pulses)*36000*WHEEL_RADIUS)/
(GEAR_RATIO*PULSES_PER_TURN*WHEELS_DISTANCE);
}
/* calculate the new (x,y) position given the left- & right pulses */
void getxy(signed short left_ps, signed short right_ps) {
distance = get_dist(left_ps, right_ps);
heading += get_heading(left_ps, right_ps);
/* x_pos += cos((heading/100)/RADS)*(distance/10);
y_pos += sin((heading/100)/RADS)*(distance/10);
*/
/* reduce the centidegrees to -18000 to 18000 */
if (abs(heading)>18000) {
if ((heading/18000)%2!=0)
heading=-heading;
heading = sign(heading)*18000-(heading%18000);
}
/* if the look-up tables are used: */
x_pos += Cos[abs(heading%18000/100)]*distance/10000; /* cos and sin don't */
y_pos += (-1)*sign(heading)*Sin[abs(heading%18000/100)]*distance/10000; /* use centidegrees ! */
}
/* calculate the dist value to go forwards */
void get_dst(void) {
dist = sqrt((x_tar-x_pos)*(x_tar-x_pos) + (y_tar-y_pos)*(y_tar-y_pos));
dist *= (GEAR_RATIO*PULSES_PER_TURN)/(2*PI*WHEEL_RADIUS);
}
/* calculate the dgr value to turn */
void get_dgr(void) {
dgr = atan((y_tar-y_pos+0.0001)/(x_tar-x_pos+0.0001))*100*RADS;
dgr *=((GEAR_RATIO*PULSES_PER_TURN*WHEELS_DISTANCE)/(2*36000*WHEEL_RADIUS));
}
/************************************************************************/
/* activate the shooter */
void shoot(void) {
shoot_id++;
if (shoot_id<10) add_value(Shooting, 100);
else if (shoot_id<25) add_value(Shooting, -80);
else { add_value(Shooting, -value(Shooting));
if (shoot_id==30) queue_ok=1;
};
}
/* turn the robot for dgr pulses */
void turn(void)
{
int M;
int th; /*threshold*/
th = dgr*40/100;
countleft += sign(dgr)*value(LeftSpeed);
countright+= sign(dgr)*value(RightSpeed)*(-1);
/*printf("%f %f\n\r",value(LeftMotor),value(RightMotor));*/
if ((abs(countleft) < abs(dgr))&&(abs(countright) < abs(dgr)))
M = (-1)*sign(dgr)*MAX_TURN_SPEED;
else {
/*decrease the speeds to zero, using active braking: */
M = 0+sign(dgr)*(value(LeftSpeed))/2;
/*****************************************************/
if (value(LeftSpeed)==0 && value(RightSpeed)==0) {
/*did motors actually halt?*/
turn_ok=0; /* do not turn anymore */
queue_ok=1; /* get next command from queue */
}
}
if (M>MAX_TURN_SPEED) M=MAX_TURN_SPEED;
add_value(RightMotor, -value(RightMotor) + M );
add_value(LeftMotor, -value(LeftMotor ) + M );
}
/* go forwards for dist pulses */
void go(void)
{
int LM, RM;
int th; /*threshold*/
th=dist*1/4;
countleft += sign(dist) * value(LeftSpeed);
countright += sign(dist) * value(RightSpeed);
if ( abs(countleft) < abs(dist)) LM = sign(dist)*(abs(dist-countleft)/2);
if ( abs(countright) < abs(dist)) RM = sign(dist)*(abs(dist-countright)/2);
if (RM > MAX_GO_SPEED) RM=MAX_GO_SPEED;
if (LM > MAX_GO_SPEED) LM=MAX_GO_SPEED;
/* adjust both speeds a bit (only necessary for the longer distances) */
if (abs(countleft) > abs(countright)) {
RM+= 5 *sign(dist);
LM-= 5 *sign(dist);
}
else if (abs(countleft) < abs(countright)) {
LM+= 5 *sign(dist);
RM-= 5 *sign(dist);
}
if ((abs(countleft) < abs(dist))||(abs(countright) < abs(dist))) {}
else {
/* decrease both speeds, using active braking: */
LM = 0-sign(dist)*value(LeftSpeed)/2;
RM = 0-sign(dist)*value(RightSpeed)/2;
/***********************************************/
if (value(LeftSpeed)==0 && value(RightSpeed)==0) {
/* did the robot actually halt? */
queue_ok=1; /* get the next command from queue */
go_ok=0; /* stop */
}
}
add_value(RightMotor, -value(RightMotor) + RM );
add_value(LeftMotor, -value(LeftMotor ) - LM );
}
/* gets a command from the queue, executes it, and gets another, and so on... */
void empty_queue(void) {
struct command dummy;
dummy = queue_get();
switch (dummy.command_id) {
case 1 : go_ok=1; dist=dummy.x; break;
case 2 : turn_ok=1; dgr=dummy.x; break;
case 3 : shoot_ok=1; shoot_id=0; break;
case 4 : goto_ok=1; x_tar=dummy.x; y_tar=dummy.y; break;
}
if (dummy.command_id) {
countleft=0; /* reset the counters */
countright=0;
}
/***********************************************/
/* command_id * function (using X and Y) */
/***********************************************/
/* 1 * drive straight for X pulses */
/* 2 * turn left for X pulses */
/* 3 * shoot !!! */
/* 4 * go to position (X,Y) */
/* ... * ... (more to come?) */
/***********************************************/
}
/* turn to (xpos,ypos) and drive towards that position */
void gotopos(void) {
struct command c;
/* turn to the right angle ... */
get_dgr();
printf("*");
c.command_id=2; /* enable turning */
c.x=dgr;
queue_add(c);
/* ... and travel the right distance */
get_dst();
c.command_id=1;
c.x=dist;
queue_add(c);
printf("-> %d %d <-\n\r",dist,dgr);
queue_ok=1; /* empty the queue */
goto_ok=0;
}
/* procedure to test the queue */
void test(void) {
struct command c;
int i;
/* small square + shoot (cfr. UMBmark) */
for (i=0; i<20; i++) {
c.command_id=1;
c.x=1000;
queue_add(c);
c.command_id=3;
queue_add(c);
c.command_id=2;
c.x=100;
queue_add(c);
c.x=-200;
queue_add(c);
}
}
/***************************************************************************/
void menu(void){
int n;
if ((!turn_ok) && (!go_ok) && (!queue_ok) && (!shoot_ok)) {
/* information is only shown in case of an empty queue */
getxy(countleft,countright);
printf("\n\r*******************************************************************************\n\r");
printf(" X:%d Y:%d (%d-%d)", x_pos, y_pos, countleft, countright);
printf(" Heading: %d Distance: %d", heading, distance);
printf("\n\r*******************************************************************************\n\r");
countleft=0; /* reset the counters */
countright=0;
printf(" 1. go pulses\n\r");
printf(" 2. turn pulses\n\r");
printf(" 3. manage the queue\n\r");
printf(" 4. change # of distance pulses\n\r");
printf(" 5. change # of degree turning pulses\n\r");
printf(" 6. let the robot do pre-prepared moves\n\r");
printf(" 7. reset position to (0,0)\n\r");
printf(" 8. test shooter.\n\r");
printf(" 9. go to position (X,Y) (in mms)\n\r");
printf(">What shall I do?\n\r");
scanf("%d",&n);
switch (n) {
case 1: go_ok=1; break;
case 2: turn_ok=1;break;
case 3: handle_queue(); queue_ok=1; break;
case 4: printf("value? ");scanf("%d",&dist); break;
case 5: printf("value? ");scanf("%d",&dgr); break;
case 6: test(); queue_ok=1; break;
case 7: x_pos=0; y_pos=0; heading=0; distance=0;break;
case 8: x_tar = dgr; y_tar = dist; shoot_id=0; shoot_ok=1; break;
case 9: goto_ok = 1; break;
}
}
if (goto_ok) gotopos();
if (queue_ok) { empty_queue(); queue_ok=0;}
if (go_ok) go();
if (turn_ok) turn();
if (shoot_ok) { shoot(); if ((shoot_id++)>30) { shoot_ok=0; queue_ok=1;} }
/* shoot requires a 30 timesteps pause to retract the 'foot'*/
}
/***************************************************************************/
void main(void) {
/* these are the two main modules: */
void handle_queue(void);
/* handle_queue manages the queue of incoming commands */
void getxy(signed short left_ps, signed short right_ps);
/* getxy calculates the position of the robot */
int dummy;
printf("Starting Execution...\n\r");
init_pdl();
LeftMotor = add_quantity("LeftMotor",100.0,-100.0,0.0);
RightMotor = add_quantity("RightMotor",100.0,-100.0,0.0);
Shooting = add_quantity("Shooting",100.0, -100.0,0.0);
LeftSpeed = add_quantity("LeftSpeed",255.0,0.0,0.0);
LeftAcceleration = add_quantity("LeftAcceleration",127.0,-127.0,0.0);
RightSpeed = add_quantity("RightSpeed",255.0,0.0,0.0);
RightAcceleration = add_quantity("RightAcceleration",127.0,-127.0,0.0);
connect_sensor(SID_PAC1_SP,LeftSpeed);
connect_sensor(SID_PAC1_AC,LeftAcceleration);
connect_sensor(SID_PAC2_SP,RightSpeed);
connect_sensor(SID_PAC2_AC,RightAcceleration);
connect_actuator(AID_MOTOR1,LeftMotor);
connect_actuator(AID_MOTOR2,RightMotor);
connect_actuator(AID_MOTOR3,Shooting);
dgr = 100;
dist= 1000;
x_tar = -100;
y_tar = 0;
add_process("menu",menu);
run_pdl(-1L);
}