One-beacon and laser positioning
This is again an evolutive journal. The most recent informations are appended!!
(last change 31/10/01, very final stage, added picture of the improved sensor)
Journal
14/07/01
This project turns around the in-door robot positioning process, which is definitely our hobby, isn't it. The idea is to use only one of our beacons for distance-measurement and a laser-beam for finding the angle of the robot's position. Thus we'll determine the polar-coordinates of the robot.
The distance-finding has been sufficiently described at the beacon-pages. The only difference will be that there is only one beacon working. So the legOS-subroutines for operating the beacon-system have to be changed a bit.
For the measurement of the angle, the beacon is equipped with a vibrating laser. The vertical vibrations are necessary to realize a segment of laser-light. As the beacon should always turn itself towards the robot for best reception of the IR/ultrasonic communication, the laser is moved with the beacon in order to fix the robot horizontally.
We first experimented with the original LEGO light-sensor. But we found that the incorporated red LED-light goes out if the laser-beam falls directly on the sensor ! We do not want to dammage the sensor, so we build an own light-sensor.
For the first time we made a double-sided PC-board by the photographical technic. Here some hints:
We placed the finsihed sensor in a red-sprayed micro-cassette box with an aperture for the photo-transitor. We glued some LEGO plates to permit the communication to the LEGO-system. Then we added a transparent 2x2 brick, which central cylinder was filled with hot nylon (nylon-gluten). This gives a perfect diffraction of the laser-light, when exposed.
15/07/01
Tests of the sensor and building of the laser-beacon.
The first test is about the respons of the sensor (without transparent brick) to a vibrating laser-beam. The peak aren't too precise, are they, but this is due to the too slow sampling rate under the original firmware. This will obviously be largely improved under legOS.
Unfortunately the second test showed an important loss of information by using the transparent brick. So we will not use it any longer in this project.
Respons with the transparent brick:
The next test handles the frequency of the vertical laser-beam movement. With
rather weak batteries we get a frequency of 3.875 Hz. The rotation sensor shows a value of 1240 after 20 sec. So the rotation-speed is:
1240 / 16 /20 = 3.875 rot/sec
Thus the laser vibrates at a frequency of 3.875 Hz.
One cycle dures 258 ms.
Up to this stage the device works with a perfect linearity.
But as the LEGO-cam, which is used for producing the swinging of the laser, is
formed unsymetrically, the laser movement will not be sinusoidal. So the
sensor-respons will have a more complicated rythme.
The tests were made at a distance of 126 cm from the center of rotation. With an amplitude of 6cm, we have an angle of 5.4° =
2 * arctan ( 6 / 126 ).
If we consider the central beam-point, we may say that this point is reached twice during one cycle. The duration of one cycle is 258 ms. So, after 129ms the beam reaches the central point for the second time a.s.o.
The beam needs 258 / 4 = 64.5 ms for walking from the center to the maximum. So, the average exposure-time is 1,075ms per mm. Thus the 3mm phototransistor is exposed during about 3ms every time the beam passes. ( The exposure-time decreases in function of the distance from the laser to the sensor. ) This is very close to the sampling rate of the RCX under the original firmware. The sensor respons will be dramatically improved by switching to legOS !!
Here a picture of the beacon! Click to see the video.
The rotation-sensor has only been fixed to test the frequency. The laser and the vibration-motor have their own independent power-supply (unvisible on the picture).
Note that the main beacon CAD-drawings are to be found at the beacon-page.
 |
 |
 |
 |
 |
 |
 |
 |
 |
 |
16/07/01
After some reflection, we think that the transparent brick could be used nevertheless! We must test the respons of the sensor under legOS. For the tests we will have to establish a robust communication between the RCX and the PC. We will use the WINLNP-package.
We also thaught about replacing the LEGO-cam by some other part, which will be able to produce a true sinusoidal movement of the laser-beam. The best solution is of course an excenter disk.
There is no LEGO-part which fits the purpose. Also, after trying a homebrew plastic disk, the whole beacon-system started vibrating enormously, so we conclude that the sinusoidal movement at the desired frequency and amplitude produces some important resonance to the device. After all, the LEGO-cam remains a good solution.
17/07/01
 |
The initial idea was to produce a sinusoidal
laser-beam, which could reach the sensor (blue) at a certain moment. This
worked well when the beam was pointed directly on the sensor and when it
fulfilled only a vertical movement. But when we started sweeping it around
in ordre to seek the robot, or more exactly the 3mm
phototansistor, the probability of a recordable exposure decreased in
function of the robot's distance to the laser. So we must consider this
way as a DEAD-END of our research.
Conclusion: the vertical swinging laser-beam will no longer serve, neither does the too small laser-light-sensor.
|
Instead we'll try to use a relative new original LEGO part: the DACTA eLAB Solar-modul part number 9912. This photovoltaic element has the dimensions 11 x 7 cm and produces some volts if exposed to a light-source. Again we use Mike Gasperi's signal buffer to communicate the voltage to the RCX. The - pole of the module should be connected to the buffer's ground. The buffer is used like an active sensor. In percent-mode the values variate from 95 (no light) to 5 (strong light). The solar-modul has the advantage to present a large surface to our laser-beam. So it is much easier to be reached by the laser-beam. The element prouces a significant relative change of the returned values at any tested distance from the laser. Finally, we only need a horizontal sweeping laser-beam.
As we already pointed out, the use of the worm-screw with normal technic parts is somehow critical. If the sense of rotation is changed, the screw is displaced nearly one mm. The laser-beacon system needs a much more precise angle-reading than the 3-beacon system. So we designed a new support using the gear-box. The use of the large pulley allows a better motor-control.
 |
 |
 |
 |
 |
 |
 |
 |
 |
 |
17/09/01
We're back on stage again. As we pointed out in july, we are working now with the LEGO solar modul. The advantage is clear: the large surface makes it rather simple to be hit by the laser-beam. So, the idea is to point the solar modul in direction of the beacon with a generous tolerance. After the known distance-fixing through the IR/ultrasonic system, the laser-beacon will sweep during a while, until the laser-beam hits the solar-modul. The beacon's rotation-sensor notifies the angle. Now the beacon sends the information to the robot, so that it may compute its exact position. It is sufficient for the beacon to know the angle, so the robot mustn't send the position back to the beacon.
The first tests with the solar-modul reveal a considerable sensitivity to any changes of the environment light-level. The idea is now to pulse the laser by the beacon-RCX at a frequency of let's say 25Hz. This frequency seems to be sufficient. Then the robot's solar-modul-task has to filter the signal to obtain a clear distinction between pulsed and non-pulsed state.
A way to do this analyse is to compute at any moment the variance of a certain set of samples by the finite respons technique. Remember that the variance of a dataset is an indicator for the deviation of the data around their average-value.
The variances above are always calculated on a finite set of data-points. The graphics show the transition from pulse to non-pulse state. Several pulses make the transition, but the pulse and non-pulse state may be distingushed clearly. The advantage of the procedure is that we must not consider the absolute sensor-level, which is definitely not stable, but only the deviations from the average. The pictures show a noisy signal, which corresponds to real measurments under legOS. The finite respons technique makes quick calculations possible.
18/09/01
Here another experiment: the laser-RCX is working with the following Robolab program:
A second RCX equipped with the solar-modul runs with the datalogging program:
The result:
The variance-analysis with different data-sets using the finite respons technique.
The larger the observed sub-dataset, the smoother the curve!
--> We have to know the sampling rate under legOS and adjust the length of the sub-data-set accordingly.
NOTE: legOS allows no large arrays of unsigned integers!! The maximum is situated between 127 and 255 items. We are not sure about the exact value. The compiler doesn not return any error, but when we use our program with a one dimensional array of 255 elements, the RCX crashes in a non-recoverable state at the moment when the program tries to put a value into the array!!!. The RCX has to be re-booted. (Once we had even to remove all the batteries and wait for the internal capacitors to discharge, before we could re-use the RCX.) This complicates our plan!!
19/09/01
Some other problem occur:
---> The only solution seems to develop a mathematical filter for a 25 Hz signal ! But how to do that in a limited RCX-computer? We need a reflection-pause!
20/09/01
The solution is so simple! That's MINDSTORMS !!! You try, you make errors, and you retry, then perhaps you solve your problem. Seymour Papert calls this: bricolage, a French word whose nearest (but inadequate) translation might be "thinkering" (The children's machine, USA 1992, p.131)
We'll re-use all what we rejected so far: our homebrew laser-light-sensor AND the transparent LEGO brick AND the vibrating laser canon! AND we combine the whole system with our variance-analysis.
The result is quite amazing. Suddenly, we have a dramatic respons to very short laser-beam impacts. However. the laser-light-sensor with the transparent brick is nearly insensitive to environment light disturbances (even infra-red light), if not directly spot on the phototransistor.
Here a first legOS-program. Note that the lnp_integrity_write is disabled during the first tests. The finite respons part is high-lighted. (Don't forget to look to the IR/ultrasonic beacon pages for better understanding).
/*la_bea1.c (based upon rec_bot7.c): sends a message to the IR/ultrasonic-laser-beacon in order to PING Then calculates and displays the distance-value computes the variance of the laser-light-sensor-values multi-threading*/ #include <unistd.h> #include <conio.h> #include <dmotor.h> #include <dsensor.h> #include <string.h> #include <lnp.h> #include <lnp-logical.h> #define NUMBER_OF_SAMPLES 127 pid_t position_thread; pid_t pilot_thread; pid_t variance_thread; #define aa .0710843 /*linear function to transform raw_1 to real half-byte*/ #define bb -6.9802983 #define cycle 4.3904 /*=1.28E-4 * 343 * 100 result in cm*/ int LOW_read,HIGH_read; double variance; /*++++++++++++++++++begin receive routines+++++++++++++*/ int receive()
{
int idx,wdt,err;
long int SUM;
/*initialize main variables*/ LOW_read=0; HIGH_read=0; err=0; /*no error*/ wdt=0; /*watch-dog-timer*/ /*communication-protocole*/ while(SENSOR_1>16000) /*wait until start-message*/
{
wdt+=1;
if (wdt>3000) { err=+1; break; }
}
if(err==0) /*continue if no error*/
{
wdt=0;
while(SENSOR_1<16001) /*wait until low_byte*/
{
wdt+=1;
if (wdt>1000) { err=+2; break; }
}
}
/*while waiting until pause-message compute average*/
if(err==0) /*continue if no error*/
{
SUM=0;
idx=0;
while(SENSOR_1>16000) /*wait until high_byte*/
{
SUM+=SENSOR_1;
idx+=1;
if(idx>1000) { err=+3; break; }
}
LOW_read=SUM/64/idx; /*gives normal RCX raw-values 0..1023*/ } if(err==0) /*continue if no error*/
{
wdt=0;
while(SENSOR_1<16001)
{
wdt+=1;
if (wdt>1000) { err=+4; break; }
}
}
/*while waiting until stop-message compute average*/
if(err==0) /*continue if no error*/
{
SUM=0;
idx=0;
while(SENSOR_1>16000)
{
SUM+=SENSOR_1;
idx+=1;
if(idx>1000) { err=+5; break; }
}
HIGH_read=SUM/64/idx; /*gives normal RCX raw-values 0..1023*/ } if(err==0) /*continue if no error*/
{
wdt=0;
while(SENSOR_1<16001) /*wait until NO_SIGNAL condition*/
{
wdt+=1;
if (wdt>1000) { err=+6; break; }
}
}
return err;
}
int distance(int L, int H)
{
double raw_1,dist;
int LOW,HIGH,result;
/*convert raw sensor-values to half-byte values*/
raw_1=100000/L;
raw_1=aa*raw_1+bb;
LOW=raw_1;
raw_1=100000/H;
raw_1=aa*raw_1+bb;
HIGH=raw_1;
/*calculate the distance*/ dist=cycle*(16*HIGH+LOW)+.5; result=dist; /*double to integer*/ return(dist); } /*++++++++++++++++end receive routines+++++++++++++++*/ /*++++++++++++++begin positioning routines++++++++++++*/ /*++++++++++++++++++End Positioning routines++++++++++*/ /*+++++++++++++++Begin Positioning task+++++++++++++++*/ int position()
{
int L1,H1,E1,error;
char *s="X";
/*++++++++++++++++++ATTENTION LNP_INTEGRITY_WRITE NOT ACTIVE+++++++++++++*/
while(1) /*repeat forever*/
{
/*first check the distance*/
s[0]='A';
/*lnp_integrity_write(s,strlen(s));*/ /*command beacon A to ping*/
msleep(40); /*wait until PIC no more in error-condition*/
E1=receive(); /*get the error message*/
L1=LOW_read; /*get the byte*/
H1=HIGH_read;
msleep(25); /*recover-time*/ /*lcd_int(E1);
sleep(1);
lcd_int(distance(L1,H1));
sleep(1);*/
/*now show the laser-sensor-value*/
lcd_int(variance);
msleep(100);
} return 0; /*to get rid of the compiler messages*/ } /*++++++++++++++++End Positioning task++++++++++++++++*/ /*+++++++++++++++Begin Pilot routines+++++++++++++++++*/ /*++++++++++++++++End Pilot routines++++++++++++++++++*/ /*+++++++++++++++Begin Pilot task++++++++++++++++++++++*/
int pilot()
{
/*do nothing yet but loop*/
while(1) msleep(10);
return 0; } /*++++++++++++++++End Pilot task+++++++++++++++++++++++*/ /*+++++++++++++++++Begin Laser-sensor tasks+++++++++++++++*/ int calculate_variance()
{
int idx,idx2;
double temp;
unsigned int current;
long int sum,sum_squares;
double average,average_squares;
unsigned int samples[NUMBER_OF_SAMPLES];
ds_active(&SENSOR_3); /*Mike's buffer is an active sensor*/ sum=0; sum_squares=0; average=0; average_squares=0; variance=0; for(idx=0;idx<NUMBER_OF_SAMPLES;idx++) /*set the first n values*/
{
current=SENSOR_3/640; /*we only need values between 0 and 102*/
samples[idx]=current;
sum+=current;
sum_squares+=(current*current);
}
idx=0; /*reset the index*/
while(1)
{
average=sum/NUMBER_OF_SAMPLES; /*mean*/
average_squares=sum_squares/NUMBER_OF_SAMPLES; /*mean of squares*/
temp=average_squares-(average*average);
variance=temp;
current=SENSOR_3/640;
sum-=samples[idx];
sum_squares-=samples[idx]*samples[idx]; /*retrieve old values*/
samples[idx]=current; /*set new value*/
sum+=current;
sum_squares+=(current*current);
idx+=1; /*adjust index*/
if(idx==NUMBER_OF_SAMPLES) {idx=0;}
for(idx2=0;idx2<10;idx2++); /*wait a bit*/
} return 0; } /*++++++++++++++++++End Laser sensor tasks++++++++++++++++*/ /*MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN*/ int main(int argc, char *argv[])
{
/*don't forget sensor_1 -the IR/ultrasonic-receiver is a passive sensor*/ lnp_logical_range(1); /*set long range*/ /*start the IR / ultrasonic receiver*/ motor_a_dir(fwd); motor_a_speed(MAX_SPEED);
cputs("wait");
sleep(6);
position_thread=execi(&position,0,0,PRIO_NORMAL+5,DEFAULT_STACK_SIZE); pilot_thread=execi(&pilot,0,0,PRIO_NORMAL,DEFAULT_STACK_SIZE); variance_thread=execi(&calculate_variance,0,0,PRIO_NORMAL,DEFAULT_STACK_SIZE); return 0; /*to get rid of compiler warning*/ } |
21/09/01
Today we conceived the mobile base for the robot. There are several conditions the base should fulfill:
The following design took several hours of building. The most difficult task was keeping the light-sensor always pointed in direction of the laser. But with our direction-master we solved this problem.
Place the laser-light sensor upon the top of the worm-screw as shown on the photos. You have to put attention to turn the part in the right way! Otherwise the system won't work.
22/09/01
The new legOS-program produces an additional reaction if the laser-light-detector is hit by the laser-beam. We changed the stack_size parameter of the variance_thread. This prevents the program from crashing. So we've localized the main bug, which is NOT due to big arrays! Again the changes are hight-lighted.
/*la_bea2.c : IR/ultrasonic-laser-message still disabled; computes the variance of the laser-sensor-values; multi-threading react if the light-sensor registers any change; */ #include <unistd.h> #include <conio.h> #include <dmotor.h> #include <dsensor.h> #include <string.h> #include <lnp.h> #include <lnp-logical.h> #define NUMBER_OF_SAMPLES 127 #define THRESHOLD 4 pid_t position_thread; pid_t pilot_thread; pid_t variance_thread; LINES LINES LINES LINES.....(cf above)....positioning task+++++++++++++++*/ int position()
{
int L1,H1,E1,error;
char *s="X";
/*++++++++++++++++++ATTENTION LNP_INTEGRITY_WRITE NOT ACTIVE+++++++++++++*/
while(1) /*repeat forever*/
{
/*first check the distance*/
s[0]='A';
/*lnp_integrity_write(s,strlen(s));*/ /*command beacon A to ping*/
msleep(40); /*wait until PIC no more in error-condition*/
E1=receive(); /*get the error message*/
L1=LOW_read; /*get the byte*/
H1=HIGH_read;
msleep(25); /*recover-time*/ /*lcd_int(E1);
sleep(1);
lcd_int(distance(L1,H1));
sleep(1);*/
/*now show the laser-sensor-variance*/
lcd_int(variance);
if(variance>THRESHOLD)
{ /*wait until signal*/
motor_b_dir(fwd); /*short light signal*/
motor_b_speed(MAX_SPEED);
}
msleep(100);
motor_b_dir(off);
motor_b_speed(0);
} return 0; /*to get rid of the compiler messages*/ } LINES LINES LINES LINES ..... (cf above) /*++++++++++++++++++End Laser sensor tasks++++++++++++++++*/ /*MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN*/ int main(int argc, char *argv[])
{
/*don't forget sensor_1 -the IR/ultrasonic-receiver is a passive sensor*/ lnp_logical_range(1); /*set long range*/ /*start the IR / ultrasonic receiver*/ motor_a_dir(fwd); motor_a_speed(MAX_SPEED);
cputs("wait"); /*time to warm-up*/
sleep(6);
position_thread=execi(&position,0,0,PRIO_NORMAL+5,DEFAULT_STACK_SIZE); pilot_thread=execi(&pilot,0,0,PRIO_NORMAL,DEFAULT_STACK_SIZE); variance_thread=execi(&calculate_variance,0,0,PRIO_NORMAL,DEFAULT_STACK_SIZE+256); return 0; /*to get rid of compiler warning*/ } |
23/09/01
The following la_bea3.c program is NOT satisfying. It works together with the La_can.c, based on the A5.c from the IR/ultrasonic-beacon project. The implanting of the lnp_integrity_set_handler into the la_bea3.c makes either the variance-reading, either the distance-reading impossible. So, we must abandon the variance_thread. Instead, the main task of a next version will calculate the variance-values at the right moment.
/*la_bea3.c : computes the variance of the laser-sensor-values multi-threading react if the light-sensor registers any change: show small light signal and send message */ #include <unistd.h> #include <conio.h> #include <dmotor.h> #include <dsensor.h> #include <string.h> #include <lnp.h> #include <lnp-logical.h> #include <dsound.h> #define NUMBER_OF_SAMPLES 127 #define THRESHOLD 25 pid_t variance_thread; static const note_t click[] =
{
{ PITCH_E4,QUARTER},
{ PITCH_END,0}
} ;
int LOW_read,HIGH_read; int go=0; int hit=0; #define aa .0710843 /*linear function to transform raw_1 to real half-byte*/ #define bb -6.9802983 #define cycle 4.3904 /*=1.28E-4 * 343 * 100 result in cm*/ /*++++++++++++++++++begin receive routines+++++++++++*/ int receive()
{
int idx,wdt,err;
long int SUM;
/*initialize main variables*/ LOW_read=0; HIGH_read=0; err=0; /*no error*/ wdt=0; /*watch-dog-timer*/ /*communication-protocole*/ while(SENSOR_1>16000) /*wait until start-message*/
{
wdt+=1;
if (wdt>3000) { err=+1; break; }
}
if(err==0) /*continue if no error*/
{
wdt=0;
while(SENSOR_1<16001) /*wait until low_byte*/
{
wdt+=1;
if (wdt>1000) { err=+2; break; }
}
}
/*while waiting until pause-message compute average*/
if(err==0) /*continue if no error*/
{
SUM=0;
idx=0;
while(SENSOR_1>16000) /*wait until high_byte*/
{
SUM+=SENSOR_1;
idx+=1;
if(idx>1000) { err=+3; break; }
}
LOW_read=SUM/64/idx; /*gives normal RCX raw-values 0..1023*/ } if(err==0) /*continue if no error*/
{
wdt=0;
while(SENSOR_1<16001)
{
wdt+=1;
if (wdt>1000) { err=+4; break; }
}
}
/*while waiting until stop-message compute average*/
if(err==0) /*continue if no error*/
{
SUM=0;
idx=0;
while(SENSOR_1>16000)
{
SUM+=SENSOR_1;
idx+=1;
if(idx>1000) { err=+5; break; }
}
HIGH_read=SUM/64/idx; /*gives normal RCX raw-values 0..1023*/ } if(err==0) /*continue if no error*/
{
wdt=0;
while(SENSOR_1<16001) /*wait until NO_SIGNAL condition*/
{
wdt+=1;
if (wdt>1000) { err=+6; break; }
}
}
return err;
}
int distance(int L, int H)
{
double raw_1,dist;
int LOW,HIGH,result;
/*convert raw sensor-values to half-byte values*/
raw_1=100000/L;
raw_1=aa*raw_1+bb;
LOW=raw_1;
raw_1=100000/H;
raw_1=aa*raw_1+bb;
HIGH=raw_1;
/*calculate the distance*/ dist=cycle*(16*HIGH+LOW)+.5; result=dist; /*double to integer*/ return(dist); } /*++++++++++++++++end receive routines+++++++++++++++*/ void my_integrity_handler(const unsigned char *data,unsigned char len)
{
if (data[0]=='P') { go=1; }/* gets the angle */
}
/*+++++++++++++++Begin Variance task++++++++++++++++++++++*/
int calculate_variance()
{
/*variables for variance-calculation*/
int idx,idx2;
unsigned int current;
long int sum,sum_squares;
double variance,average,average_squares;
unsigned int samples[NUMBER_OF_SAMPLES];
/*initialize variance*/ ds_active(&SENSOR_3); /*Mike's buffer is an active sensor*/ sum=0; sum_squares=0; average=0; average_squares=0; variance=0; for(idx2=0;idx2<100;idx2++) /*do this several times*/
{
for(idx=0;idx<NUMBER_OF_SAMPLES;idx++) /*set the first n values*/
{
current=SENSOR_3/640; /*we only need values between 0 and 102*/
samples[idx]=current;
}
}
for(idx=0;idx<NUMBER_OF_SAMPLES;idx++) /*set the first n values*/
{
sum+=samples[idx];
sum_squares+=(samples[idx]*samples[idx]);
}
dsound_play(click);
wait_event(dsound_finished,0);
idx=0; /*reset the index*/
while(1) /*repeat forever*/
{
average=sum/NUMBER_OF_SAMPLES; /*mean*/
average_squares=sum_squares/NUMBER_OF_SAMPLES;
/*mean of squares*/
variance=average_squares-(average*average);
if(variance>THRESHOLD) {hit=1;}
current=SENSOR_3/640;
sum-=samples[idx];
sum_squares-=samples[idx]*samples[idx];
/*retrieve old values*/
samples[idx]=current; /*set new value*/
sum+=current;
sum_squares+=(current*current);
idx+=1; /*adjust index*/
if(idx==NUMBER_OF_SAMPLES) {idx=0;}
}
return 0;
}
/*++++++++++++++++End Variance task+++++++++++++++++++++++*/ /*MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN*/ int main(int argc, char *argv[])
{
int L1,H1,E1;
char *s="X";
/*set the beep-duration*/ dsound_set_duration(40); /*don't forget sensor_1 -the IR/ultrasonic-receiver is a passive sensor*/ lnp_logical_range(1); /*set long range*/ lnp_integrity_set_handler(my_integrity_handler); /*start the IR / ultrasonic receiver*/ motor_a_dir(fwd); motor_a_speed(MAX_SPEED); /*wait for warm_up*/
cputs("wait");
sleep(6);
variance_thread=execi(&calculate_variance,0,0,PRIO_NORMAL,DEFAULT_STACK_SIZE+256); while(1) /*repeat forever*/
{
hit=0; /*clear the hit flag*/
/*first check the distance*/ s[0]='A'; lnp_integrity_write(s,strlen(s)); /*command beacon A to ping*/ msleep(40); /*wait until PIC no more in error-condition*/ E1=receive(); /*get the error message*/ L1=LOW_read; /*get the byte*/ H1=HIGH_read; msleep(25); /*recover-time*/ lcd_int(E1);
sleep(1);
lcd_int(distance(L1,H1));
sleep(1);
/*now wait until hit by the laser-beam*/
while(hit==0) msleep(5);/*wait for laser_light*/
motor_b_dir(fwd); /*short light signal*/
motor_b_speed(MAX_SPEED);
msleep(100);
motor_b_dir(off);
motor_b_speed(0);
s[0]='H'; /*send the HIT message*/
lnp_integrity_write(s,strlen(s));
/*wait for angle message*/
while(go==0) msleep(10);
dsound_play(click);
wait_event(dsound_finished,0);
go=0;
}
return 0; /*to get rid of compiler warning*/
}
|
/*La_can.c derived from A5.c */ #include <unistd.h> #include <string.h> #include <lnp.h> #include <lnp-logical.h> #include <dmotor.h> #include <dsensor.h> #include <dsound.h> #define DEG_PER_ROT -1.5 /* 60 rotation pulses <=> 90 ° */ #define PING 1 #define HIT 2 /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ int go=0; void my_integrity_handler(const unsigned char *data,unsigned char len)
{
if (data[0]=='A') go=PING; /* order to ping */
if (data[0]=='H') go=HIT;/*HIT_message*/
}
static const note_t click[] =
{
{ PITCH_G4,QUARTER},
{ PITCH_END,0}
} ;
char angle()
{ double temp;
char temp2;
temp=ROTATION_3*DEG_PER_ROT;
if(temp>90) {temp=90;} /*security*/
if(temp<0) {temp=0; } /*idem*/
temp2=temp;
return temp2;
}
int main()
{
int sweep_angle,start_angle;
char *s="Ps";
lnp_logical_range(1); /*set long range*/ lnp_integrity_set_handler(my_integrity_handler); dsound_set_duration(40);
ds_active (&SENSOR_3);
ds_rotation_on(&SENSOR_3);
ds_rotation_set(&SENSOR_3,0);
sleep(1); /*warm_up*/
while(1)
{
while(go==0) msleep(10);
msleep(50);
motor_c_speed(MAX_SPEED); /*activate beacon*/
motor_c_dir(fwd);
msleep(35);
motor_c_speed(0);
motor_c_dir(off);
dsound_play(click);
wait_event(dsound_finished,0);
sweep_angle=10; /*start with 10 degrees*/
/*++++++*/
while(go!=HIT)
{
start_angle=angle();
motor_b_speed(MAX_SPEED/8); /*go slowly*/
motor_b_dir(fwd);
while(angle()-start_angle<sweep_angle && go!=HIT)
/*wait until angle too big or HIT*/
{
if(angle()==90) {break;}
}
if(go!=HIT)
{
motor_b_dir(rev);
start_angle=angle();
while(start_angle-angle()<sweep_angle && go!=HIT)
/*wait until angle too small or HIT*/
{
if(angle()==0) {break;}
}
}
if(go!=HIT)
{
sweep_angle+=10;
if(sweep_angle>90) {sweep_angle=10;}
}
}
motor_b_speed(0);
motor_b_dir(off);
msleep(150);
s[1]=angle(); /*send angle message*/
lnp_integrity_write(s,strlen(s));
msleep(100);
go=0;
} return 0; } |
24/09/01
The project comes to an end! As the most of our projects remain open-ended, we'll see, what follows next!
We've solved most of the problems now.
We had to increase the frequency of the vertical laser-movement. Now the laser swings at 10Hz. We use the 9V LEGO ungeared motor. We displaced the rubber-holder and we added a rubber as you can see on the picture.
We fixed the beacon-RCX in a vertical position, beacuse the direct infrared signals disturbed our laser-sensor. All this can be seen on the next photos.
 |
|
NOTES:
/*test*/ #include <unistd.h> #include <conio.h> #include <dsensor.h> #define NUMBER_OF_SAMPLES 127 pid_t variance_thread; double variance; /*+++++++++++++++++Begin Solar Modul tasks+++++++++++++++*/ int calculate_variance()
{
int idx,idx2;
double temp;
unsigned int current;
long int sum,sum_squares;
double average,average_squares;
unsigned int samples[NUMBER_OF_SAMPLES];
ds_active(&SENSOR_3); /*Mike's buffer is an active sensor*/ sum=0; sum_squares=0; average=0; average_squares=0; variance=0; for(idx=0;idx<NUMBER_OF_SAMPLES;idx++) /*set the first n values*/
{
current=SENSOR_3/640; /*we only need values between 0 and 102*/
samples[idx]=current;
sum+=current;
sum_squares+=(current*current);
}
idx=0; /*reset the index*/
while(1)
{
average=sum/NUMBER_OF_SAMPLES; /*mean*/
average_squares=sum_squares/NUMBER_OF_SAMPLES; /*mean of squares*/
temp=average_squares-(average*average);
variance=temp;
current=SENSOR_3/640;
sum-=samples[idx];
sum_squares-=samples[idx]*samples[idx]; /*retrieve old values*/
samples[idx]=current; /*set new value*/
sum+=current;
sum_squares+=(current*current);
idx+=1; /*adjust index*/
if(idx==NUMBER_OF_SAMPLES) {idx=0;}
for(idx2=0;idx2<10;idx2++); /*wait a bit*/
} return 0; } /*++++++++++++++++++End Solar Modul tasks++++++++++++++++*/ /*MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN*/ int main(int argc, char *argv[])
{
variance_thread=execi(&calculate_variance,0,0,PRIO_NORMAL,DEFAULT_STACK_SIZE); while(1)
{
lcd_int(variance);
msleep(5);
}
return 0; /*to get rid of compiler warning*/ } |
/*la_bea4.c : computes the variance of the laser-sensor-values multi-threading react if the light-sensor registers any change: show small light signal and send message then receive the angle-value from beacon */ #include <unistd.h> #include <conio.h> #include <dmotor.h> #include <dsensor.h> #include <string.h> #include <lnp.h> #include <lnp-logical.h> #include <dsound.h> #define NUMBER_OF_SAMPLES 127 #define THRESHOLD 20 static const note_t click[] =
{
{ PITCH_E4,QUARTER},
{ PITCH_END,0}
} ;
int LOW_read,HIGH_read; int go=0; int alpha=0; /*variables for variance-calculation, unfortunately must be global*/ int idx,idx2; unsigned int current; long int sum,sum_squares; double average,average_squares; unsigned int samples[NUMBER_OF_SAMPLES]; double variance; #define aa .0710843 /*linear function to transform raw_1 to real half-byte*/ #define bb -6.9802983 #define cycle 4.3904 /*=1.28E-4 * 343 * 100 result in cm*/ /*++++++++++++++++++begin receive routines+++++++++++*/ int receive()
{
int idx,wdt,err;
long int SUM;
/*initialize main variables*/ LOW_read=0; HIGH_read=0; err=0; /*no error*/ wdt=0; /*watch-dog-timer*/ /*communication-protocole*/ while(SENSOR_1>16000) /*wait until start-message*/
{
wdt+=1;
if (wdt>3000) { err=+1; break; }
}
if(err==0) /*continue if no error*/
{
wdt=0;
while(SENSOR_1<16001) /*wait until low_byte*/
{
wdt+=1;
if (wdt>1000) { err=+2; break; }
}
}
/*while waiting until pause-message compute average*/
if(err==0) /*continue if no error*/
{
SUM=0;
idx=0;
while(SENSOR_1>16000) /*wait until high_byte*/
{
SUM+=SENSOR_1;
idx+=1;
if(idx>1000) { err=+3; break; }
}
LOW_read=SUM/64/idx; /*gives normal RCX raw-values 0..1023*/ } if(err==0) /*continue if no error*/
{
wdt=0;
while(SENSOR_1<16001)
{
wdt+=1;
if (wdt>1000) { err=+4; break; }
}
}
/*while waiting until stop-message compute average*/
if(err==0) /*continue if no error*/
{
SUM=0;
idx=0;
while(SENSOR_1>16000)
{
SUM+=SENSOR_1;
idx+=1;
if(idx>1000) { err=+5; break; }
}
HIGH_read=SUM/64/idx; /*gives normal RCX raw-values 0..1023*/ } if(err==0) /*continue if no error*/
{
wdt=0;
while(SENSOR_1<16001) /*wait until NO_SIGNAL condition*/
{
wdt+=1;
if (wdt>1000) { err=+6; break; }
}
}
return err;
}
int distance(int L, int H)
{
double raw_1,dist;
int LOW,HIGH,result;
/*convert raw sensor-values to half-byte values*/
raw_1=100000/L;
raw_1=aa*raw_1+bb;
LOW=raw_1;
raw_1=100000/H;
raw_1=aa*raw_1+bb;
HIGH=raw_1;
/*calculate the distance*/ dist=cycle*(16*HIGH+LOW)+.5; result=dist; /*double to integer*/ return(dist); } /*++++++++++++++++end receive routines+++++++++++++++*/ void my_integrity_handler(const unsigned char *data,unsigned char len)
{
if (data[0]=='P')
{
go=1;
alpha=data[1]; /* get the angle */
}
}
void reset_array()
{
sum=0;
sum_squares=0;
average=0;
average_squares=0;
variance=0;
for(idx2=0;idx2<100;idx2++) /*do this several times*/
{
for(idx=0;idx<NUMBER_OF_SAMPLES;idx++) /*set the first n values*/
{
current=SENSOR_3/640; /*we only need values between 0 and 102*/
samples[idx]=current;
}
}
for(idx=0;idx<NUMBER_OF_SAMPLES;idx++) /*set the first n values*/
{
sum+=samples[idx];
sum_squares+=(samples[idx]*samples[idx]);
}
}
/*MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN*/ int main(int argc, char *argv[])
{
int L1,H1,E1;
char *s="X";
/*set the beep-duration*/ dsound_set_duration(40); /*don't forget sensor_1 -the IR/ultrasonic-receiver is a passive sensor*/ lnp_logical_range(1); /*set long range*/ lnp_integrity_set_handler(my_integrity_handler); /*start the IR / ultrasonic receiver*/ motor_a_dir(fwd); motor_a_speed(MAX_SPEED); /*initialize variance*/ ds_active(&SENSOR_3); /*Mike's buffer is an active sensor*/ reset_array(); /*wait for warm_up*/ cputs("wait");
sleep(5);
/*pilot_thread=execi(&pilot,0,0,PRIO_NORMAL,DEFAULT_STACK_SIZE);*/ while(1) /*repeat forever*/
{
/*first check the distance*/
s[0]='A';
lnp_integrity_write(s,strlen(s)); /*command beacon A to ping*/
msleep(40); /*wait until PIC no more in error-condition*/
E1=receive(); /*get the error message*/
L1=LOW_read; /*get the byte*/
H1=HIGH_read;
msleep(25); /*recover-time*/ lcd_int(E1);
sleep(1);
lcd_int(distance(L1,H1));
sleep(1);
/*now show the laser-light-value*/
idx=0; /*reset the index*/
while(variance<THRESHOLD) /*wait for laser_light*/
{
average=sum/NUMBER_OF_SAMPLES; /*mean*/
average_squares=sum_squares/NUMBER_OF_SAMPLES;
/*mean of squares*/
variance=average_squares-(average*average);
current=SENSOR_3/640;
sum-=samples[idx];
sum_squares-=samples[idx]*samples[idx];
/*retrieve old values*/
samples[idx]=current; /*set new value*/
sum+=current;
sum_squares+=(current*current);
idx+=1; /*adjust index*/
if(idx==NUMBER_OF_SAMPLES) {idx=0;}
}
lcd_int(variance);
motor_b_dir(fwd); /*short light signal*/
motor_b_speed(MAX_SPEED);
reset_array(); /*this needs a certain time, so is a good pause*/
motor_b_dir(off);
motor_b_speed(0);
s[0]='H'; /*send the HIT message*/
lnp_integrity_write(s,strlen(s));
/*wait for angle message*/
while(go==0) msleep(10);
dsound_play(click);
wait_event(dsound_finished,0);
lcd_int(alpha);
sleep(1);
go=0;
}
return 0; /*to get rid of compiler warning*/ } |
/*La_can3.c (version 2 isn't usable at all) derived from A5.c the sweep-direction changes after hit, sends the angle to the robot*/ #include <unistd.h> #include <string.h> #include <lnp.h> #include <lnp-logical.h> #include <dmotor.h> #include <dsensor.h> #include <dsound.h> #define DEG_PER_ROT -1.5 /* 60 rotation pulses <=> 90 ° */ #define PING 1 #define HIT 2 #define LEFT 0 #define RIGHT 1 /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ int go=0; void my_integrity_handler(const unsigned char *data,unsigned char len)
{
if (data[0]=='A') go=PING; /* order to ping */
if (data[0]=='H') go=HIT;/*HIT_message*/
}
static const note_t click[] =
{
{ PITCH_G4,QUARTER},
{ PITCH_END,0}
} ;
char angle()
{ double temp;
char temp2;
temp=ROTATION_3*DEG_PER_ROT;
if(temp>90) {temp=90;} /*security*/
if(temp<0) {temp=0; } /*idem*/
temp2=temp;
return temp2;
}
void sweep(int direction,int start,int angle_)
{
if(direction==LEFT)
{
motor_b_dir(fwd);
while(angle()-start<angle_ && go!=HIT)
/*wait until angle too big or HIT*/
{
if(angle()==90) {break;}
}
} else
{
motor_b_dir(rev);
while(start-angle()<angle_ && go!=HIT)
/*wait until angle too small or HIT*/
{
if(angle()==0) {break;}
}
}
} int main()
{
int sweep_angle,start_angle;
int sweep_direction=LEFT;
char *s="Ps";
lnp_logical_range(1); /*set long range*/ lnp_integrity_set_handler(my_integrity_handler); dsound_set_duration(40);
ds_active (&SENSOR_3);
ds_rotation_on(&SENSOR_3);
ds_rotation_set(&SENSOR_3,0);
sleep(1); /*warm_up*/
while(1)
{
while(go==0) msleep(10);
msleep(50);
motor_c_speed(MAX_SPEED); /*activate beacon*/
motor_c_dir(fwd);
msleep(35);
motor_c_speed(0);
motor_c_dir(off);
dsound_play(click);
wait_event(dsound_finished,0);
sweep_angle=15; /*start with 15 degrees*/
/*++++++*/
while(go!=HIT)
{
start_angle=angle();
motor_b_speed(MAX_SPEED/16); /*go slowly*/
sweep(sweep_direction,start_angle,sweep_angle);
sweep_direction=1-sweep_direction; /*reverse direction*/
if(go!=HIT)
{
sweep(sweep_direction,start_angle,sweep_angle);
sweep_direction=1-sweep_direction; /*reverse direction*/
}
if(go!=HIT)
{
sweep_angle+=15;
if(sweep_angle>90) {sweep_angle=10;}
}
}
motor_b_speed(0);
motor_b_dir(off);
msleep(150);
s[1]=angle(); /*send angle message*/
lnp_integrity_write(s,strlen(s));
msleep(100);
go=0;
}
return 0;
}
|
Conclusions:
This was a real complicated task. But, now the robot knows its position through the distance and the angle relative to the beacon-station.
The precision of the result is dependant of the distance as for any polar-coordinates system. We explained this at the plotter-pages. The robot gets a positioning every 5-10 seconds. This is really slow compared to our three beacon project, where we have 1-2 positionings per second.
This is not an energy-economic solution to the positioning task. The laser-movement needs a lot of current.
However the project is one of the best didactical stuff we've ever worked over. We learned about the transformation of a circular to a linear movement. Again we've seen the disadvantage in robotics of polar-coordinates (increasing error). We experienced the development and building of an own light-sensor. We considered once again our direction-master. We learned about the solar-modul. We saw the power of statistical maths with a sensor. We learned to manage complex things. We worked with ROBOLAB and LEGO VISION COMMAND, with legOS and LEOCAD. We wrote and designed this lab-web-journal, a perfect form of teaching and learning. And last but not least, we learned that if you get stuck and lost in a project, you just act as scouts do: you return to the place where you still knew , where you were!
So, again we see the power of the Mindstorms-project in an educational environment.
31/10/01
We changed the light-sensor: we took an old, dammaged 12V halogen-lamp with a parabolic screen; we broke out the lamp and fixed the screen to our sensor; the phototransistor was placed at the focal point. Now we have a highly directional light-sensor.
