Intelligent Rescue Robot Maze Mission Example Code

#define SONAR_READ_CM (analog(5) * 100) / 4095
#define SPD 45
#define kpsonar 7
#define kpspin_t 350
#define kpifr 1500
#define kpfreq 500
#define kpstop_t 300
#include <POP32.h>
void fdSONAR(int cm, int freq, int ao_t) {
  fd2(SPD - 2, SPD);
  motor(2, SPD);
  while (SONAR_READ_CM > cm);
  ao();
  sound(freq, ao_t);
}
void slMAZE(int sl_t, int ao_t) {
  sl(SPD);
  delay(sl_t);
  ao();
  delay(ao_t);
}
void srMAZE(int sr_t, int ao_t) {
  sr(SPD);
  delay(sr_t);
  ao();
  delay(ao_t);
}
void bkSW(int freq, int ao_t) {
  while (1) {
    if (!in(22) && !in(23)) {
      bk(SPD);
      sound(freq, ao_t);
      ao();
      delay(ao_t);
      break;
    } else if (in(22) && !in(23)) {
      sl(SPD);
    } else if (!in(22) && in(23)) {
      sr(SPD);
    } else {
      bk(SPD);
    }
  }
}
void stopline(int ifr, int freq, int ao_t) {
  fd2(SPD - 2, SPD);
  while (analog(2) > ifr);
  ao();
  sound(freq, ao_t);
}
void setup() {
  // put your setup code here, to run once:
}
void loop() {
  // put your main code here, to run repeatedly:
  waitSW_OK_bmp();
  fdSONAR(kpsonar, kpfreq, kpstop_t);
  srMAZE(kpspin_t, kpstop_t);
  bkSW(kpfreq, kpstop_t);
  fdSONAR(kpsonar, kpfreq, kpstop_t);
  slMAZE(kpspin_t, kpstop_t);
  bkSW(kpfreq, kpstop_t);
  fdSONAR(kpsonar, kpfreq, kpstop_t);
  slMAZE(kpspin_t, kpstop_t);
  bkSW(kpfreq, kpstop_t);
  fdSONAR(kpsonar, kpfreq, kpstop_t);
  slMAZE(kpspin_t, kpstop_t);
  bkSW(kpfreq, kpstop_t);
  fdSONAR(kpsonar, kpfreq, kpstop_t);
  srMAZE(kpspin_t, kpstop_t);
  bkSW(kpfreq, kpstop_t);
  fdSONAR(kpsonar, kpfreq, kpstop_t);
  srMAZE(kpspin_t, kpstop_t);
  bkSW(kpfreq, kpstop_t);
  stopline(kpifr, kpfreq, kpstop_t);
}