Robots & Small-C
by Pete Gray


Listing One

  ldist[0] = LMIN;
  rdist[0] = RMIN;
  fdist[0] = FMIN;
  do_sonar();               // take initial sonar reading
  lspeed = SMAX;            // default to "full speed ahead"

  rspeed = SMIN;
  while (1) {           // loop forever
    do_sonar();
// object too close in front ?
    if (fdist[0] < FMIN) {
      if (rdist[0] <= ldist[0])
        turn (LEFT90);
      else
        turn (RIGHT90);
      do_sonar();
    }
    else {
// too close, both sides ?
      if (rdist[0] < RMIN && ldist[0] < LMIN) {
        if (rdist[0] <= ldist[0])
          turn (LEFT180);
        else
          turn (RIGHT180);
        do_sonar();
      }
      else {
// too close, either side ?
        if (rdist[0] < RMIN || ldist[0] < LMIN) {
// object too close to right ?
          if (rdist[0] < RMIN)
            adjust (LEFT);                
          else
// no, object must be too close to left
            adjust (RIGHT);
        }
        else {
// no objects too close, but are any getting closer ?
          if (rmcloser == 1 && lmcloser == 0)
// closer to right
            adjust (LEFT);
          else
            if (lmcloser == 1 && rmcloser == 0)
// closer to left
              adjust (RIGHT);
            else {
// no object too close, and none getting closer
              lspeed = SMAX;
              rspeed = SMIN;
            }
        }
      }
    }
    do_tracks();
// set LEDs
// green = left closer. red = right closer. both = forward closer.
    val = 0;
    if (rdist[0] < ldist[0] && rdist[0] < fdist[0]) val = 0x0004;
    if (ldist[0] < rdist[0] && ldist[0] < fdist[0]) val = 0x0008;
    if (fdist[0] < ldist[0] && fdist[0] < rdist[0]) val = 0x000C;
    *PEDR = val;
  }


Listing Two

// slight turn - decelerate inner track if possible, else accelerate outer
adjust (direct)
int direct;
{
  if (direct == RIGHT) {
    if (rspeed == SMIN)
      rspeed += SINC;
    else
      lspeed += SINC;
  }
  if (direct == LEFT) {
    if (rspeed == SMIN)
      lspeed -= SINC;
    else
      rspeed -= SINC;
  }
  if (lspeed < SMIN) lspeed = SMIN;
  if (lspeed > SMAX) lspeed = SMAX;
  if (rspeed < SMIN) rspeed = SMIN;
  if (rspeed > SMAX) rspeed = SMAX;
}


Listing Three

turn (direct)
int direct;
{
  int t;
// set servo speeds ready for rotation
  if (direct==LEFT90 || direct==LEFT180) {
    lspeed = SMIN;
    rspeed = SMIN;
  }
  if (direct==RIGHT90 || direct==RIGHT180) {
    lspeed = SMAX;
    rspeed = SMAX;
  }
  if (direct==LEFT90 || direct==LEFT180) turn90d (LEFT);
  if (direct==LEFT180) turn90d (LEFT);
  if (direct==RIGHT90 || direct==RIGHT180) turn90d (RIGHT);
  if (direct==RIGHT180) turn90d (RIGHT);
// juggle the distance arrays
  if (direct==LEFT90) {
    rdist[1] = fdist[1];
    fdist[1] = ldist[1];
    ldist[1] = 100;
  }
  if (direct==RIGHT90) {
    ldist[1] = fdist[1];
    fdist[1] = rdist[1];
    rdist[1] = 100;
  }
  if (direct==RIGHT180 || direct==LEFT180) {
    t = rdist[1];
    rdist[1] = ldist[1];
    ldist[1] = t;
    fdist[1] = 100;
  }
// default to "full speed ahead"

  lspeed = SMAX;
  rspeed = SMIN;
}


Listing Four

do_sonar()
{
  ldist[1] = ldist[0];      // save previous readings
  rdist[1] = rdist[0];
  fdist[1] = fdist[0];

  *E08 = TLEFT;                 // rotate left
  do_pwm();
  wfs();                        // wait for servo
  ping();
  ldist[0] = pingres;           // save reading (current)
  *E08 = TMID;                  // rotate forward
  do_pwm();
  wfs();                        // wait for servo
  ping();
  fdist[0] = pingres;           // save reading (current)
  *E08 = TRIGHT;                // rotate right
  do_pwm();
  wfs();                        // wait for servo
  ping();
  rdist[0] = pingres;           // save reading (current)
  *E08 = TMID;                  // rotate forward, ready to start again
  do_pwm();
  wfs();                        // wait for servo

  if (ldist[0]<ldist[1])        // set lmcloser
    lmcloser = 1;
  else
    lmcloser = 0;
  if (rdist[0]<rdist[1])        // set rmcloser
    rmcloser = 1;
  else
    rmcloser = 0;
}

Listing Five

  tmrcnt=0;
  do {
    reset_timers();
    while ((*TMRSCR1 & 0x0800) != 0x0800) {
      tmrcnt++;
      if (tmrcnt==2000) {
        tmrcnt = 0;
        reset_timers();
      }
    }
  } while ((*TMRSCR1 & 0x0800) != 0x0800);
  p1 = *TMRCAP1;                 // get capture value
  p1 /= 183;                     // convert to inches







4



