Home > Robots > Half Life > Slave Code
Slave Code

/* Slave control program for LEGO RoboGladiators */
/* by Dan Danknick 1999 */

int NoSignal, L, R, Lset, Rset, M;
int Ldir, Ltime, Lspeed, Rdir, Rtime, Rspeed;


#define TIMEOUT 200
#define tNEUT 30
#define tEND 60
#define eSPEED 1

#define DONE 0
#define FWD 1
#define REV 2

task ActivateL {
// Each swich motion is OUT_FULL for 5, and then OUT eSPEED for the time

  if(Ldir == FWD)
    Fwd(OUT_A, OUT_FULL);
  else
    Rev(OUT_A, OUT_FULL);
  Sleep(5);
  
  if(Ldir == FWD)
    Fwd(OUT_A, Lspeed);
  else
    Rev(OUT_A, Lspeed);
  Sleep(Ltime);
  Off(OUT_A);
  Ldir = DONE;
}
  
task ActivateR {
// Each swich motion is OUT_FULL for 5, and then OUT eSPEED for the time

  if(Rdir == FWD)
    Fwd(OUT_C, OUT_FULL);
  else
    Rev(OUT_C, OUT_FULL);
  Sleep(5);
  
  if(Rdir == FWD)
    Fwd(OUT_C, Rspeed);
  else
    Rev(OUT_C, Rspeed);
  Sleep(Rtime);
  Off(OUT_C);
  Rdir = DONE;
}

sub UpdateSwitches {
// Maybe there isn't anything to do
//
  if((L == Lset) && (R == Rset))
    return;

// Calculate direction and magnitude of the movement
// L, Lset have the format 0 = rev, 1 = neutral and 2 = fwd
//
  if(L != Lset){
    if(L > Lset)
      Ldir = REV;
    else
      Ldir = FWD;

// Approach neutral at cal speed
//
    if(L == 1){
      Ltime = tNEUT;
      Lspeed = OUT_HALF;
    }
    else {
      Ltime = tEND;
      Lspeed = eSPEED;
    }
  }
  
  if(R != Rset){
    if(R > Rset)
      Rdir = REV;
    else
      Rdir = FWD;
// Approach neutral at cal speed
//
    if(R == 1){
      Rtime = tNEUT;
      Rspeed = OUT_HALF;
    }
    else {
      Rtime = tEND;
      Rspeed = eSPEED;
    }
  }
  
 
// Now fire up the tasks simultaneously to move the switch motors
//            
  if(L != Lset) {
    start ActivateL;
    Lset = L;
  }
    
  if(R != Rset) {
    start ActivateR;
    Rset = R;
  }
}
  


task main
{
  PlayNote(1800,10);
  PlayNote(1500,10);

// Calibrate the switches to zero
   Fwd(OUT_A+OUT_C, eSPEED);
   Sleep(tEND*3);
   Off(OUT_A+OUT_C);

   Rev(OUT_A+OUT_C, OUT_FULL);
   Sleep(5);
   Rev(OUT_A+OUT_C, OUT_HALF);
   Sleep(tNEUT);
   Off(OUT_A+OUT_C);
   Lset = 1;
   Rset = 1;
   Rdir = DONE;
   Ldir = DONE;     

   while (true) {
    ClearMessage();

    while(Message() == 0) {
      NoSignal += 1;
      if(NoSignal > TIMEOUT) {
        L = 1;
        R = 1;
        UpdateSwitches();
        NoSignal = 0;
      }
    }
    
    NoSignal = 0;

    M = Message();
    
    if (M > 100) {
       M -= 10;
       Fwd(OUT_B, OUT_FULL);
    }
    else
      Off(OUT_B);
       

    if (M == 91) {
       L = 1;
       R = 1;
    }

    if (M == 92) {
       L = 2;
       R = 2;
    }

    if (M == 93) {
       L = 0;
       R = 0;
    }
  
    if (M == 94) {
       L = 0;
       R = 2;
   }
       
    if (M == 95) {
       L = 2;
       R = 0;
    }

    // Now wait for tasks to complete before continuing
    UpdateSwitches();
    wait((Rdir == DONE) && (Ldir == DONE));
  }

}