5. The robot's communication to the receiver
|
// beacon1 // first test of the beacon // the low-byte of the time delay between IR and ultrasonic impact // is stored in variable LOW // the high-byte is stored in variable HIGH int LOW,HIGH;task main(){ SetPower(OUT_A,7); //power-supply for the IR-Ultrasonic receiver On(OUT_A); //turn device on SetSensorType(SENSOR_1,SENSOR_TYPE_TOUCH); //must be passive input 5V SetSensorMode(SENSOR_1,SENSOR_MODE_RAW); //device sends raw-values
until( SENSOR_1 < 1000){ } //wait until device output activewhile( SENSOR_1 < 100) { } //wait as long as start-messageLOW = SENSOR_1; //NOPLOW = SENSOR_1;LOW = SENSOR_1; //store low-byteuntil( SENSOR_1 < 100) { } //wait until pause-messagewhile( SENSOR_1 < 100) { } //wait as long as pause-messageHIGH = SENSOR_1;HIGH = SENSOR_1;HIGH = SENSOR_1; //store high-byte} |
So, you may read 431 as low-byte, and 614 as high-byte.
This simply means that the delay has the value:
431 --> 1001
614 --> 0100
delay = B'01001001' = 0x49 = 4*16+9=73
the time is 0.128 * 73 = 9.344 msec
the distance is 0.009344 * 343 = 3.2m
// beacon2 // second test of the beacon-communication to the RCX // the converted low-byte of the time delay between IR and ultrasonic impact // is stored in variable LOW // the converted high-byte is stored in variable HIGH // added error-messages: ERROR // added watchdogtimer to prevent the RCX getting lost in loops // the watchdog, if enabled, has to be calmed regularly // global variables for dubugging phase #define NET SENSOR_1 #define WATCHDOG 0 #define NONE 0 #define NO_START_SIGNAL 1 #define OUT_OF_RANGE 2 #define NO_LOW_BYTE 3 #define NO_PAUSE_SIGNAL 4 #define NO_HIGH_BYTE 5 int Low, High, Low_raw, High_raw, Error, Err, Delay, Distance; //the delay is expressed in PIC-TMR0-cycles of 0.128msec //the distance is expressed in cm void half_byte(int x,int &y) //not elegant, but the fastest I could do
//has anyone a better idea? The stack permits it?
//Don't know the H8 possibilities
{
if (x<319)
{
y=15;
}
else
{
if (x<330)
{
y=14;
}
else
{
if (x<349)
{
y=13;
}
else
{
if (x<372)
{
y=12;
}
else
{
if (x<399)
{
y=11;
}
else
{
if (x<422)
{
y=10;
}
else
{
if (x<458)
{
y=9;
}
else
{
if (x<472)
{
y=8;
}
else
{
if (x<502)
{
y=7;
}
else
{
if (x<545)
{
y=6;
}
else
{
if (x<600)
{
y=5;
}
else
{
if (x<675)
{
y=4;
}
else
{
if (x<750)
{
y=3;
}
else
{
if (x<820)
{
y=2;
}
else
{
if (x<950)
{
y=1;
}
else
{
y=0;
}
} } } } } } } } } } } } } } } task main()
{
SetPower(OUT_A,7); //power-supply for the IR-Ultrasonic receiver
On(OUT_A); //turn device on
SetSensorType(NET,SENSOR_TYPE_TOUCH); //must be passive input 5V
SetSensorMode(NET,SENSOR_MODE_RAW); //device sends raw-values
start receive;
start errors;
}
task receive()
{
Error=NONE;
until (NET < 200) {} //wait until device output active
start WDT; //start the watchdog-timer
if (NET > 90) //device out of range
{
Error = OUT_OF_RANGE;
}
else //now surely we have a start-message
{
Err = NO_LOW_BYTE; //needed if the watchdog gives an error
ClearTimer(WATCHDOG); //reset the timer
while (NET < 200) {} //wait as long as start-message
//too long in this loop will wake up the watchdog
//now we have the low_byte raw value
Low_raw = NET;
Err = NO_PAUSE_SIGNAL;
ClearTimer(WATCHDOG);
until (NET < 200) {} //wait until pause message
Err = NO_HIGH_BYTE;
ClearTimer(WATCHDOG);
while (NET < 200) {} //wait as long as pause-message
//now we have the raw value of high_byte
High_raw = NET;
stop WDT;
//now determine the real low and high-byte
half_byte(Low_raw,Low);
half_byte(High_raw,High);
Delay=16*High+Low;
Distance=(44*Delay+5)/10;
}
} task WDT()
{
while(Timer(WATCHDOG) < 2)
{
Error = Err;
}
}
task errors()
{
while (true)
{
while (Error == NONE){ }
//an error condition has been detected
//the following lines are not needed in the test
stop WDT; //disable the watchdog-timer
stop receive;
Low_raw = 32000;
High_raw = 30000;
}
}
|
As you perhaps could guess this program causes some major problems. Often it will return an overflow-error where no error from the PIC was sent! This problem has not been present in the first test program. What changed? First of all, some lines have been added between the whiles and untils. Second, this time the program works in multitasking. In fact, a real time problem occured. Executing program lines needs a certain amount of time. Especially during multitasking, the CPU has to store and retrieve much information to and from the stack, such as the program counter, several flags and so on. The central unit switches between all the active tasks, which slows down the process of each task. Even if the sensor sampling rate is not directly affected as it is probably controlled by interrupts in the H8-controller, the sampling through the program might get a chaotic frequency. So there will surely be moments of frequent and others of scarce sampling.
Another problem which must be considered in dealing with our special data-transfer is the slope of the curves. No computer is able to change a state without time. So there will always be interim-states where the information is undefined. Normally, this difficulty is solved using thresholds.
A further consequence of both problems is the possibility for the RCX to read a certain sensor value, but when, at a given threshold, the program executes the command to store the sensor value in a variable, the sensor value may have changed to another state, so that the resulting information will be garbage.
These problems can be reduced by increasing the sensor sampling rate, which is not possible with the standard firmware. Using legOS, the RCX works at a higher sampling rate.
Note that the PIC-program uses an information interval of 10 ms! These pictures are only illustrations of the sampling problem.
According to C.Shannon (1949): fsampling>=2*finformation
In order to test the whole data transfer with a better sampling rate and the use of the RCX-display, we created this third program on the legOS platform. Note that the conversion from raw-value to half-byte is done by a linear function.
Testing a slightly altered version of this program under simple multitasking conditions show that the same problems may occur as enumerated above. But this central communication-routine MUST function without ambiguity. The solution is to set the priority of the receiving task a little bit higher. Here the better version:
/* beacon3bis.c*/ #include <unistd.h> #include <conio.h> #include <dmotor.h> #include <dsensor.h> #define a .0710843 /*linear function to transform raw_1 to real half-byte*/ #define b -6.9802983 #define cycle 4.3904 /*=1.28E-4 * 343 * 100 result in cm*/ pid_t receive_thread; pid_t do_something_thread; int receive(int argc, char *argv[])
{
double raw_1,dist; int LOW_read,HIGH_read,LOW,HIGH,result,idx; long int SUM; /*start the IR / ultrasonic receiver*/ motor_a_dir(fwd); motor_a_speed(MAX_SPEED); while(1)
{
/*initialize main variables*/ LOW_read=0; HIGH_read=0; /*communication-protocole*/ while(SENSOR_1>16000) ; /*wait until start-message*/
while(SENSOR_1<16001); /*wait until low_byte*/ /*while waiting until pause-message compute average*/
SUM=0;
idx=0;
while(SENSOR_1>16000)
{
SUM+=SENSOR_1;
idx+=1;
}
LOW_read=SUM/64/idx; /*gives normal RCX raw-values 0..1023*/ while(SENSOR_1<16001) ; /*while waiting until stop-message compute average*/
SUM=0;
idx=0;
while(SENSOR_1>16000)
{
SUM+=SENSOR_1;
idx+=1;
}
HIGH_read=SUM/64/idx; /*gives normal RCX raw-values 0..1023*/ while(SENSOR_1<16001) ; /*wait until NO_SIGNAL condition*/ /*convert raw sensor-values to half-byte values*/ raw_1=100000/LOW_read; raw_1=a*raw_1+b; LOW=raw_1; raw_1=100000/HIGH_read; raw_1=a*raw_1+b; HIGH=raw_1; /*calculate the distance*/ dist=cycle*(16*HIGH+LOW)+.5; result=dist; lcd_int(result); sleep(1); } /*to get rid of compiler warning*/ return 0; } int do_something(int argc,char *argv[])
{
int x;
while(1)
{
x=0; /*only for test of multitasking*/
}
return 0;
}
int main(int argc, char *argv[])
{
receive_thread=execi(&receive,0,0,PRIO_NORMAL+5,DEFAULT_STACK_SIZE);
do_something_thread=execi(&do_something,0,0,PRIO_NORMAL,DEFAULT_STACK_SIZE);
return 0;
}
|
Conclusion:
We have a perfect signal receiving up to a maximum distance of 7 meters. But we have to stay in the -6dB beam given by the transducer's caracteristics. We could increase the signal by adding a reflector at the top of the receiving ultrasonic transducer. We measured an error of only 4.5 cm, as expected.
An old funnel cut to be an open cone serves as ultrasonic reflector. Try to find the best height ! The waves arrive from the top, then they are reflected by the cone.
After having found another interesting publication around the subject, we changed the reflector a little bit. By the way, the authors Luis E. Navarro-Serment et al from the Carnegie Mellon University operate their robot positioning with a RF/ultrasonic mixt beacon system in the document A Beacon System for the Localization of Distributed Robotic Teams. They have created nice robots called Millibot.
The better reflector. Much more efficient than the first one ! The waves may arrive in a small angle, so the beacons mustn't be placed very high.
