6. The robot's communication to the beacons

 

As we have to work with legOS in order to provide a quick and precise communication between the IR-ultrasonic receiver and the robot, one inconvenience is the difficulty to use the RCX-implemented IR-communication-system. We think having solved the problems by choosing the simplest way of RCX-IR-communication under legOS using the lnp_integrity_write and the lnp_integrity_set_handler commands (lnp.h and lnp-logical.h). Thanks to Paolo Masetti (legOS.sourceforge) for his prompt help!

 

Here the planned scenario:

  1. The robot(=master) sends a message -a simple identification number- to
    the beacon1 to fire. = 4 bytes =15ms
  2. all the beacon get the message and check the identification number. Only
    beacon1 must obey now. =2ms delay
  3. beacon1 fires synchronically the IR and ultrasonic 40kHz pulses
    =PING. = 2ms-5ms (we tested only 5ms yet)
  4. the receiver analyses and sends the distance information to the robot's
    brain =50ms
  5. some computation must be done to store the converted value =10ms

    ....  repeat the procedure for the other two beacons

   16.  computation to fix the exact position = 200ms
  
17.  the robot communicates its position to the three beacons = 50ms
  
18.  reset and start again = 1ms
__________________________
needed time: 3 * 82 + 251 = 497 ms

We get two positionings per second, which is largely enough.

The control-communication operates only with unilateral messages. Correct reception must not be confirmed. The beacons do not send any message, they just PING. The robot does not receive any message, it just analyses the PING.

Here a first test program on the nqc-platform. Every beacon is called to PING 3 times. The IR-communication between the RCXs disturbs the receiver-legOS-program in such a way, that the two first PINGs are misunderstood. We are still far away from 2 positionings per second!

// three beacon test program
// beacon1
task main ()
{
  int msg,idx;
  SetPower(OUT_A,7);
  idx=0;  
  msg=1;    //start with beacon1
    while(true) // repeat forever
     {
      until(msg==1)  
       {
         msg=Message();
       }
     
     while(idx<3)
      {  
         PlaySound(SOUND_CLICK);
         Wait(150);//wait a bit to update the robot's display
         OnFwd(OUT_A); //send IR-ultrasonic pulse for 10 ms
         Wait(1);
         Off(OUT_A);
         idx+=1;
      }
      Wait(200); //wait 2 sec    
      SendMessage(2); //activate beacon 2
      msg=0; //clear the value 
      idx=0;
      ClearMessage();
    } 
}
// three beacon test program
// beacon2
task main ()
{
  int msg,idx;
  SetPower(OUT_A,7);
  idx=0;  
  msg=0;    //clear
    while(true) // repeat forever
    {
      until(msg==2)  
       {
         msg=Message();
       }
     
      while(idx<3)
      {
         PlaySound(SOUND_CLICK);
         Wait(150);//wait a bit to update the robot's display
         OnFwd(OUT_A); //send IR-ultrasonic pulse for 10 ms
         Wait(1);
         Off(OUT_A);
         idx+=1;
      }
      Wait(200); //wait 2 sec    
      SendMessage(3); //activate beacon 3
      msg=0; //clear the value 
      idx=0;  
      ClearMessage();
    } 
}
// three beacon test program
// beacon3
task main ()
{
  int msg,idx;
  SetPower(OUT_A,7);
  idx=0;  
  msg=0;    //clear
    while(true) // repeat forever
    {
      until(msg==3)  
       {
         msg=Message();
       }
     
     while(idx<3)
      {
         PlaySound(SOUND_CLICK);
         Wait(150);//wait a bit to update the robot's display
         OnFwd(OUT_A); //send IR-ultrasonic pulse for 10 ms
         Wait(1);
         Off(OUT_A);
         idx+=1;
      }
      Wait(200); //wait 2 sec    
      SendMessage(1); //activate beacon 1
      msg=0; //clear the value
      idx=0; 
      ClearMessage();
    } 
}

Here now a legOS test of the system:

/* send_bea1.c; beacon should fire if it receives an IR-message from the robot*/
#include <conio.h>
#include <unistd.h>
#include <string.h>
#include <lnp.h>
#include <dmotor.h>
int go=0;
void my_integrity_handler(const unsigned char *data,unsigned char len)
{
    char  msg[7];
    int i;
   
    if(len>5) len=5;
    for(i=0;(i<len);i++)  msg[i]=data[i];
    cputs(msg);
    go=1;
 }
int main()
{
  
   lnp_integrity_set_handler(my_integrity_handler);
   while(!go) msleep(100);
   sleep(4);
   cputs("go");
    motor_a_speed(MAX_SPEED);
    motor_a_dir(fwd);
    msleep(50);
    motor_a_speed(0);
    motor_a_dir(off);
   return 0;
}

 

/*rec_bot1.c: should send a message to beacon in order to PING*/
#include <unistd.h>
#include <conio.h>
#include <dmotor.h>
#include <dsensor.h>
#include <string.h>
#include <lnp.h>
#include <lnp-logical.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*/

 int main(int argc, char *argv[])
{
   double raw_1,dist;
  int LOW_read,HIGH_read,LOW,HIGH,result,idx;
  long int SUM;
  char *s="send";
  lnp_logical_range(1); /*set long range*/
  lnp_integrity_write(s,strlen(s));
  msleep(100);
 
  /*start the IR / ultrasonic receiver*/
  motor_a_dir(fwd);
  motor_a_speed(MAX_SPEED);
  
  /*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;
}

 

The time-diagram is more explicit about the communication protocole. The go-variable in the beacon's internal programs could also be used later for telling all them to accept the robot's position if it sends a message P (instead of A,B, or C) together with the actual coordinates of the robot.

 

 

Note the error-condition which is generated at every RCX-IR-communication through the PIC. The beacons must wait the duration of this message before PINGing. This error-condition in the PIC-program has been reduced from 50 to 23 ms !

 

Note also the variable ultrasonic-delay (0,128 ... 32,64 ms). The first value corresponds to the minimum distance which is 4 cm, the second value is the system-overflow value at a distance of 11,2 m. Of course, this value will only be reached through an out-of-range position or an echo-condition. So, we will get a variable protocole-duration depending on the measured distance. This must of course be considered in the positioning-calculations.

Thus the whole procedure takes about 140 - 175 ms per beacon, e.a. 420 - 525 ms per session. Add the positioning calculations and the P-communication, and you get around 1 positioning per second. Unfortunately this is only half of our aim, but it can be overcome by using only slow- or medium-motion-robots.

 

Here the new test-programs:

/*rec_bot5.c: should send a message to each beacon in order to PING
Then calculate and display the values*/
#include <unistd.h>
#include <conio.h>
#include <dmotor.h>
#include <dsensor.h>
#include <string.h>
#include <lnp.h>
#include <lnp-logical.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*/
int LOW_read,HIGH_read,err;
void receive()
{
    int idx,wdt;
    long int SUM;
  /*initialize main variables*/
  LOW_read=0;
  HIGH_read=0;
  err=-88;
  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<-10) 
  {
    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<-10)
 {
     SUM=0;
     idx=0;  
    while(SENSOR_1>16000)
       {
          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<-10) 
  {
     wdt=0;  
     while(SENSOR_1<16001) 
      {
         wdt+=1;
         if (wdt>1000) { err=-4; break; }
      }
  } 
  /*while waiting until stop-message compute average*/ 
 if(err<-10)
 {
    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<-10)
 {
  wdt=0;  
  while(SENSOR_1<16001)  /*wait until NO_SIGNAL condition*/
     {
        wdt+=1;
        if (wdt>1000) { err=-6; break; }
     }
 }   
}
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=a*raw_1+b;
  LOW=raw_1;
  raw_1=100000/H;
  raw_1=a*raw_1+b;
  HIGH=raw_1; 
  /*calculate the distance*/
  dist=cycle*(16*HIGH+LOW)+.5;
  result=dist;      /*double to integer*/
  return(dist);
}
/**/
int main(int argc, char *argv[])
{
  int L1,L2,L3,H1,H2,H3,E1,E2,E3;
  char *s="X";
  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);
 
  while(1)
{
  
  s[0]='A';
  lnp_integrity_write(s,strlen(s));
  msleep(40);
  receive();
  L1=LOW_read;
  H1=HIGH_read;
  E1=err;
  msleep(25);
  
  s[0]='B';
  lnp_integrity_write(s,strlen(s));
  msleep(40);
  receive();
  L2=LOW_read;
  H2=HIGH_read; 
  E2=err;
  msleep(25);
 
  s[0]='C';
  lnp_integrity_write(s,strlen(s));
  msleep(40);
  receive();
  L3=LOW_read;
  H3=HIGH_read;
  E3=err;
  lcd_int(E1);
  sleep(1);
  lcd_int(distance(L1,H1));
  sleep(1);
  lcd_int(E2);
  sleep(1);
  lcd_int(distance(L2,H2));
  sleep(1);
  lcd_int(E3);
  sleep(1);
  lcd_int(distance(L3,H3));
  sleep(1);
 }
  /*to get rid of compiler warning*/
  return 0;
}
/* sender-beacon; beacon should fire if it receives an IR-message from the robo*/
#include <conio.h>
#include <unistd.h>
#include <string.h>
#include <lnp.h>
#include <dmotor.h>
int go=0;
void my_integrity_handler(const unsigned char *data,unsigned char len)
{    
    if (data[0]=='A')   go=1; /* use A  B  or C accordingly */
 }
int main()
{
  
   lnp_integrity_set_handler(my_integrity_handler);
   while(1)
     {
        while(!go) msleep(10);
        msleep(50);
        motor_a_speed(MAX_SPEED);
        motor_a_dir(fwd);
        msleep(35);
        motor_a_speed(0);
        motor_a_dir(off);
        go=0;
    }
   return 0;
}

RetourMain Page