main.c

来自「一个机器人开发的相关嵌入式开发源码」· C语言 代码 · 共 1,936 行 · 第 1/4 页

char *gstack[SUB_NEST];    /* stack for gosub */
int gtos;  /* index to top of GOSUB stack */
int ftos;  /* index to top of FOR stack */

struct for_stack {
  int var; /* counter variable */
  int target;  /* target value */
  char *loc;
} fstack[FOR_NEST]; /* stack for FOR/NEXT loop */

char *prog;  /* holds expression to be analyzed */

/*  End of Global Defines for BASIC Interpreter */


int basic(char *p_buf)
{

  if(setjmp(e_buf)) return(1); /* initialize the long jump buffer */

  prog = p_buf;
  scan_labels(); /* find the labels in the program */
  ftos = 0; /* initialize the FOR stack index */
  gtos = 0; /* initialize the GOSUB stack index */
  do {
    token_type = get_token();
    /* check for assignment statement */
    if(token_type==VARIABLE) {
      putback(); /* return the var to the input stream */
      assignment(); /* must be assignment statement */
    }
    else /* is command */
      switch(tok) {
        case PRINT:
          print();
          break;
        case GOTO:
          exec_goto();
          break;
        case IF:
          exec_if();
          break;
        case FOR:
          exec_for();
          break;
        case NEXT:
          next();
          break;
          case INPUT:
          input();
          break;
        case GOSUB:
          gosub();
          break;
        case TIME:
          time();
          break;
        case DELAY:
          delay();
          break;
        case IR:
          ir();
          break;
        case MOTORS:
          motors();
          break;
        case IMGCAP:
          imgcap();
          break;
        case COLORCAP:
          colorcap();
          break;
        case COLORSET:
          colorset();
          break;
        case COLORGET:
          colorget();
          break;
        case SCAN:
          scan();
          break;
        case BLOB:
          blob();
          break;
        case BITDIR:
          bitdir();
          break;
        case BITSET:
          bitset();
          break;
        case BITGET:
          bitget();
          break;
        case COMMENT:
          comment();
          break;
        case RETURN:
          greturn();
          break;
        case END:
          return(0);
      }
  } while (tok != FINISHED);
}


/* Assign a variable a value. */
void assignment()
{
  int v1, val;

  /* get the variable name */
  get_token();
  if(!isalpha(*token)) {
    serror(4);
  }

  v1 = toupper(*token)-'A';
 
  /* get the equals sign */
  get_token();
  if(*token != '=') {
    serror(3);
  }

  /* get the value to assign to var */
  get_exp(&val);

  /* assign the value */
  vvar[v1] = val;
}

/* Execute a simple version of the BASIC PRINT statement */
void print()
{
  int answer;
  int len=0, spaces;
  char last_delim;

  do {
    get_token(); /* get next list item */
    if(tok==EOL || tok==FINISHED) break;
    if(token_type==QUOTE) { /* is string */
      uart0SendString(token);
      len += strlen(token);
      get_token();
    }
    else { /* is expression */
      putback();
      get_exp(&answer);
      get_token();
      printNumber(10, 8, FALSE, ' ', answer);

      len += 8;
    }
    last_delim = *token; 

    if(*token==';') {
      /* compute number of spaces to move to next tab */
      spaces = 8 - (len % 8); 
      len += spaces; /* add in the tabbing position */
      while(spaces) { 
        uart0SendChar(' ');
        spaces--;
      }
    }
    else if(*token==',') /* do nothing */;
    else if(tok!=EOL && tok!=FINISHED) serror(0); 
  } while (*token==';' || *token==',');

  if(tok==EOL || tok==FINISHED) {
    if(last_delim != ';' && last_delim!=',') uart0SendChar('\n');
  }
  else serror(0); /* error is not , or ; */

}

void time()
{
  vvar[19] = TIMER0_TC;  // set T = current time
}


void delay()
{
  int del;
  
  get_exp(&del);
  if ((del < 0) || (del > 1000000))
    serror(14);
  delayUs(del * 1000);
}

void ir()
{
  int dd, dir;
  
  get_exp(&dd);
  switch (dd) {
    case 1:
      dir = FORWARD;
      break;
    case 2:
      dir = LEFT;
      break;
    case 3:
      dir = BACK;
      break;
    case 4:
      dir = RIGHT;
      break;
    default:
      serror(14);  // invalid selection
  }
  vvar[23] = bounceIR(dir, 0xFF);  // return data in 'X' variable
}

void imgcap()
{
  unsigned int retry;

  retry = 0;
  while (camera_grab_frame(c328_buf) <= 0)  {
    retry++;
    if (retry > 6) {
      camera_reset("IMJ3");
      // uart0SendString("reset\n");
      retry = 0;
    }
  }

  if ((c328_buf[0] == 0xFF) && (c328_buf[1] == 0xD8)) {
    JPEG_DecompressImage(c328_buf, decode_buf, 160, 128);
    // uart0SendString("imgcap ok\n");
  }
}

void colorcap()  // sample colors from bounded region and 
                  //    show results in Y, U, V, with range in R, S, T
{
  int ix, x1, x2, y1, y2;
 
  get_exp(&ix);
  if ((ix < 0) || (ix > 15))
    serror(14);
  get_exp(&x1);
  if ((x1 < 0) || (x1 > 255))
    serror(14);
  get_exp(&x2);
  if ((x2 < 0) || (x2 > 255))
    serror(14);
  get_exp(&y1);
  if ((y1 < 0) || (y1 > 255))
    serror(14);
  get_exp(&y2);
  if ((y2 < 0) || (y2 > 255))
    serror(14);

  vgrab(ix, x1, x2, y1, y2);
  vvar[24] = (ymax[ix] + ymin[ix]) / 2;  // set avg Y to 'Y'  
  vvar[20] = (umax[ix] + umin[ix]) / 2;  // set avg U to 'U'  
  vvar[21] = (vmax[ix] + vmin[ix]) / 2;  // set avg V to 'V'  
  vvar[17] = ymax[ix] - ymin[ix] + 1;  // set Y range to 'R'  
  vvar[18] = umax[ix] - umin[ix] + 1;  // set U range to 'S'  
  vvar[19] = vmax[ix] - vmin[ix] + 1;  // set V range to 'T'  
}

void colorset() // set color - color#  y-avg u-avg v-avg y-range u-range vrange
{
  int ix, y, u, v, yr, ur, vr;
  
  get_exp(&ix);  // get color index
  if ((ix < 0) || (ix > 15))
    serror(14);
  get_exp(&y);  // get avg Y
  if ((y < 0) || (y > 255))
    serror(14);
  get_exp(&u);  // get avg U
  if ((u < 0) || (u > 255))
    serror(14);
  get_exp(&v);  // get avg V
  if ((v < 0) || (v > 255))
    serror(14);
  get_exp(&yr);  // get Y range
  if ((yr < 0) || (yr > 255))
    serror(14);
  get_exp(&ur);  // get U range
  if ((ur < 0) || (ur > 255))
    serror(14);
  get_exp(&vr);  // get V range
  if ((vr < 0) || (vr > 255))
    serror(14);

  ymin[ix] = y - yr/2;
  umin[ix] = u - ur/2;
  vmin[ix] = v - vr/2;
  ymax[ix] = y + yr/2;
  umax[ix] = u + ur/2;
  vmax[ix] = v + vr/2;
}

void colorget()  // dump color data into in Y, U, V, with range in R, S, T
{
  int ix;
 
  get_exp(&ix);
  if ((ix < 0) || (ix > 15))
    serror(14);

  vvar[24] = (ymax[ix] + ymin[ix]) / 2;  // set avg Y to 'Y'  
  vvar[20] = (umax[ix] + umin[ix]) / 2;  // set avg U to 'U'  
  vvar[21] = (vmax[ix] + vmin[ix]) / 2;  // set avg V to 'V'  
  vvar[17] = ymax[ix] - ymin[ix] + 1;  // set Y range to 'R'  
  vvar[18] = umax[ix] - umin[ix] + 1;  // set U range to 'S'  
  vvar[19] = vmax[ix] - vmin[ix] + 1;  // set V range to 'T'  
}

void scan()  // divides field of view into 3 overlapping groups of 40 columns
              //   output 3 column average to 'X', 'Y', 'Z'
{
  int ix, jx;
  unsigned int itmp, tmax[3], tmean[3], tzeros[3];

  get_exp(&ix);
  if ((ix < 0) || (ix > 15))
    serror(14);

  vscan(ix);
    
  for (ix=0; ix<3; ix++) {    // in groups of 40 columns, compute max, mean and # of zeros
    tmax[ix] = tmean[ix] = tzeros[ix] = 0;
    for (jx=0; jx<40; jx++) {
      itmp = tvect[(ix*20) + jx];
      if (tmax[ix] < itmp)
        tmax[ix] = itmp;
      tmean[ix] += itmp;
      if (itmp == 0)
        tzeros[ix]++;
    }
    if (tzeros[ix] < 15)
      tmean[ix] /= 40;
    else
      tmean[ix] = 0;
  }
  vvar[23] = tmean[0];  // 'X'
  vvar[24] = tmean[1];  // 'Y'
  vvar[25] = tmean[2];  // 'Z'
}

void blob()  //  search for blob by color, return center point X,Y and width Z
{
  int ix, ii;

  get_exp(&ix);
  if ((ix < 0) || (ix > 15))
    serror(14);
    
  vblob(ix);
  vvar[23] = (blobx1[0] + blobx2[0]) / 2;  // set 'X' with (x1+x2)/2
  vvar[24] = (bloby1[0] + bloby2[0]) / 2;  // set 'Y' with (y1+y2)/2
  ii = blobx2[0] - blobx1[0] + 1;
  if (blobcnt[0] == 0) 
    ii = 0;
  vvar[25] = ii;  // set 'Z' with blobcnt
}

void bitdir()  // set direction of i/o pins E9 E10 E11 E12 E13
{
  int ix;
  
  get_exp(&ix);  // note that the input is decimal
  if ((ix < 0) || (ix > 63))
    serror(14);
  bits_dir((unsigned char)(ix & 0x000000FF));
}

void bitset()  // write to i/o pins E9 E10 E11 E12 E13
{
  int ix;
  
  get_exp(&ix);  // note that the input is decimal
  if ((ix < 0) || (ix > 63))
    serror(14);
  bits_write((unsigned char)(ix & 0x000000FF));
}

void bitget()  // read i/o pins E9 E10 E11 E12 E13, store in variable 'X'
{
  vvar[23] = (unsigned int)bits_read();
}

void comment()  // don't process this line - it's just a comment
{
  while(*prog++ != '\n') {
    if (*prog == 0)
      serror(0);
  }
}

void motors()
{
  int lspeed, rspeed;
  
  get_exp(&lspeed);
  if ((lspeed < -100) || (lspeed > 100))
    serror(14);
  get_exp(&rspeed);
  if ((rspeed < -100) || (rspeed > 100))
    serror(14);
  setPwmDutyPercent(lspeed, rspeed);
}

/* Find all labels. */
void scan_labels()
{
  int addr;
  char *temp;

  label_init();  /* zero all labels */
  temp = prog;   /* save pointer to top of program */

  /* if the first token in the file is a label */
  get_token();
  if(token_type==NUMBER) {
    strcpy(label_ccmd[0].name,token);
    label_ccmd[0].p=prog;
  }

  find_eol();
  do {     
    get_token();
    if(token_type==NUMBER) {
      addr = get_next_label(token);
      if(addr==-1 || addr==-2) {
          (addr==-1) ?serror(5):serror(6);
      }
      strcpy(label_ccmd[addr].name, token);
      label_ccmd[addr].p = prog;  /* current point in program */
    }
    /* if not on a blank line, find next line */
    if(tok!=EOL) find_eol();
  } while(tok!=FINISHED);
  prog = temp;  /* restore to original */
}

/* Find the start of the next line. */
void find_eol()
{
  while(*prog!='\n'  && *prog!='\0') ++prog;
  if(*prog) prog++;
}

/* Return index of next free position in label array. 
   A -1 is returned if the array is full.
   A -2 is returned when duplicate label is found.
*/
int get_next_label(char *s)
{
  register int t;

  for(t=0;t<NUM_LAB;++t) {
    if(label_ccmd[t].name[0]==0) return t;
    if(!strcmp(label_ccmd[t].name,s)) return -2; /* dup */
  }

  return -1;
}

/* Find location of given label.  A null is returned if
   label is not found; otherwise a pointer to the position
   of the label is returned.
*/
char *find_label(char *s)
{
  int t;

  for(t=0; t<NUM_LAB; ++t) 
    if(!strcmp(label_ccmd[t].name,s)) return label_ccmd[t].p;
  return '\0'; /* error condition */
}

/* Execute a GOTO statement. */
void exec_goto()