main.c

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

          uart0SendString((unsigned char *)"##");
          uart0SendString((unsigned char *)svfr);   
          uart0SendChar((unsigned char)(jpegsize & 0xFF));
          uart0SendChar((unsigned char)((jpegsize/256) & 0xFF));
          uart0SendChar(0x00);
          uart0SendChar(0x00);
          for (jx=0; jx<(jpegsize/100)+1; jx++) {
            for (ix=(jx*100); (ix<((jx*100)+100)) && (ix<jpegsize); ix++)
              uart0SendChar(c328_buf[ix]);
            delayUs(1000);
          }
          break;
        case 'w':   // turn on "wander mode"
          decode_jpeg = TRUE;
          init_wander = 1;
          wander_motor = 1;
          uart0SendString((unsigned char *)"#w");
          break;
        case 'W':   // turn off "wander mode"
          lspeed = 0;
          rspeed = 0;
          wander_motor = 0;
          //decode_jpeg = FALSE;
          uart0SendString((unsigned char *)"#W");
          break;
        case 'm':   // turn on "wander mode", but disable autonomous wandering
          lspeed = 0;
          rspeed = 0;
          decode_jpeg = TRUE;
          init_wander = 1;
          wander_motor = 0;
          uart0SendString((unsigned char *)"#m");
          break;
        case 'M':   // grab 3 character motor command string (left, right, delay)
          lspeed = (int)((signed char)uart0GetCh());
          rspeed = (int)((signed char)uart0GetCh());
          delay = (unsigned int)uart0GetCh();
          setPwmDutyPercent(lspeed, rspeed);
          if (delay) {
            delayUs(delay * 10000);
            setPwmDutyPercent(0, 0);
            lspeed = 0;
            rspeed = 0;
          }
          uart0SendString((unsigned char *)"#M");
          break;
        case '+':   // increase base motor speed
          base_speed += 5;
          if (base_speed > 75)
            base_speed = 75;
          uart0SendString((unsigned char *)"#+");
          break;
        case '-':   // decrease base motor speed
          base_speed -= 5;
          if (base_speed < 35)
            base_speed = 35;
          uart0SendString((unsigned char *)"#-");
          break;
        case 'a':
          decode_jpeg = FALSE;  // no wander mode for IMJ1
          if (!camera_set_resolution('1'))
            goto restart;
          svfr[3] = '1';
          uart0SendString((unsigned char *)"#a");
          break;
        case 'b':
          if (!camera_set_resolution('3'))
            goto restart;
          svfr[3] = '3';
          uart0SendString((unsigned char *)"#b");
          break;
        case 'c':
          decode_jpeg = FALSE;  // no wander mode for IMJ5
          if (!camera_set_resolution('5'))
            goto restart;
          svfr[3] = '5';
          uart0SendString((unsigned char *)"#c");
          break;
        case '7':   // drift left
        case '8':   // forward
        case '9':   // drift right
        case '4':   // turn left
        case '5':   // stop
        case '6':   // turn right
        case '1':   // back left
        case '2':   // back
        case '3':   // back right
        case '0':   // counter clockwise turn
        case '.':   // clockwise turn
          set_motor(rxChar, base_speed, &lspeed, &rspeed);
          uart0SendChar('#');
          uart0SendChar(rxChar);
          break;
        default:
          break;
      }  

breakout:  
      // setPwmDutyPercent(0, 0);          
      while (uart0GetChar(&rxChar))  // do we need to stop motors briefly while flushing recv buffer ?
        continue;
    }

    if ((failsafe) && ((TIMER0_TC - t1) > 1000))  // if failsafe enabled, turn off motors if no signal 
      setPwmDutyPercent(0, 0);                     //     has been received for 1 sec
    else
      setPwmDutyPercent(lspeed, rspeed);

    if (xmit_beacon) {  // if "xmit beacon" mode on, send 0xFF minus id
      ch = 255 - (unsigned char)id;  // so if id=1, xmit char is 0xFE
      bounceIR(FORWARD, ch);
      bounceIR(LEFT, ch);
      bounceIR(BACK, ch);
      bounceIR(RIGHT, ch);
    }

    jpegsize = camera_grab_frame(c328_buf);

    switch (jpegsize) {
      case RESTART:
        goto restart;
      case RETRY:
        if (++retry_count > 6)
          goto restart;
        continue;
    }
    retry_count = 0;
    framecount++;
 
    if (decode_jpeg == FALSE) // just hold the captured jpeg, but don't decompress
      continue;
    
    //  if decode_jpeg is true, then decompress and dump Y pixels

    if ((c328_buf[0] != 0xFF) || (c328_buf[1] != 0xD8))
      continue;

    switch (svfr[3]) {
      case '1':   // don't decode IMR1, IMR5 or IMR7
      case '5':
      case '7':
        continue;
      case '3':
        ix = 160;
        iy = 128;
        break;
    }

    JPEG_DecompressImage(c328_buf, decode_buf, ix, iy);

    if (init_wander) {      // hardcoded for IMJ3 80x64 YUV
      init_wander = 0;
      vgrab(0, 20, 59, 0, 5);  // sets ymax[0], ymin[0], umax[0], umin[0], vmax[0], vmin[0]
    }

    vscan(0);
    
    for (ix=0; ix<7; ix++) {    // in groups of 20 columns, compute max, mean and # of zeros
      tmax[ix] = tmean[ix] = tzeros[ix] = 0;
      for (jx=0; jx<20; jx++) {
        itmp = tvect[(ix*10) + jx];
        if (tmax[ix] < itmp)
          tmax[ix] = itmp;
        tmean[ix] += itmp;
        if (itmp == 0)
          tzeros[ix]++;
      }
      tmean[ix] /= 20;
    }
    
    tflag = 0;      // flag the 7 open/blocked regions, and pack the flags into a single 7-bit word
    for (ix=0; ix<7; ix++)   // a table lookup (motor_cmd[]) is used to set robot action
      if (tzeros[ix] <8)
        tflag += (0x40 >> ix);
        
    if (locate_beacon) {  // lower 11 bits of listenIR returns 2 start bits, 8 data bits, 1 stop bit
      ix = listenIR(100);
      //printNumber(16, 8, FALSE, '0', ix);
      nforward = (ix >> 28) & 0x0000000F;
      nleft = (ix >> 24) & 0x0000000F;
      nback = (ix >> 20) & 0x0000000F;
      nright = (ix >> 16) & 0x0000000F;
      if ((nleft > nforward) && (nleft > nback) && (nleft > nright)) {
        set_motor('0', base_speed, &lspeed, &rspeed);  // turn left
        continue;
      }
      else if ((nright > nforward) && (nright > nleft) && (nright > nback)) {
        set_motor('.', base_speed, &lspeed, &rspeed);  // turn right
        continue;
      }
      else if ((nback > nforward) && (nback > nright) && (nback > nleft)) {
        set_motor('0', base_speed, &lspeed, &rspeed);  // turn left
        continue;
      }
    }

    if (wander_motor)
      set_motor(motor_cmd[tflag], base_speed, &lspeed, &rspeed);
  } 
}

void vgrab(unsigned int ii, unsigned int x1, unsigned int x2, unsigned int y1, unsigned int y2) {
  unsigned int xx, yy, y, u, v;
  
  ymax[ii] = 0;
  ymin[ii] = 255;
  umax[ii] = 0;
  umin[ii] = 255;
  vmax[ii] = 0;
  vmin[ii] = 255;

  for (yy=y1; yy<=y2; yy++) {   
    for (xx=x1; xx<=x2; xx++) {
      y = (unsigned int)decode_buf[index(xx,yy)];
      u = (unsigned int)decode_buf[index(xx,yy)+5120];
      v = (unsigned int)decode_buf[index(xx,yy)+10240];
      if (ymax[ii] < y)
        ymax[ii] = y;
      if (ymin[ii] > y)
        ymin[ii] = y;
      if (umax[ii] < u)
        umax[ii] = u;
      if (umin[ii] > u)
        umin[ii] = u;
      if (vmax[ii] < v)
        vmax[ii] = v;
      if (vmin[ii] > v)
        vmin[ii] = v;
    }
  }
}

void vscan(unsigned int ii) {
  unsigned int xx, yy, y, u, v;

  for (xx=0; xx<80; xx++) {   // for each column, measure distance to first mismatch from initial samples
    tvect[xx] = 64;            //      max distance will be 64 pixels (height of image)
    for (yy=0; yy<64; yy++) {
      y = (unsigned int)decode_buf[index(xx,yy)];
      u = (unsigned int)decode_buf[index(xx,yy)+5120];
      v = (unsigned int)decode_buf[index(xx,yy)+10240];
      if ((y < ymin[ii])
       || (y > ymax[ii]) 
       || (u < umin[ii]) 
       || (u > umax[ii]) 
       || (v < vmin[ii]) 
       || (v > vmax[ii])) {
        tvect[xx] = yy;
        break;
      }
    }
  }
}

void vblob(unsigned int ii) {
  unsigned int xx, yy, y, u, v, jj, count, bottom, top, tmp;

  for (jj=0; jj<MAX_BLOBS; jj++) {
    blobcnt[jj] = 0;
    blobx1[jj] = 80;
    blobx2[jj] = 0;
    bloby1[jj] = 64;
    bloby2[jj] = 0;
  }
    
  jj = 0;  // jj indicates the current blob being processed
  for (xx=0; xx<80; xx++) {
    count = 0;
    bottom = 64;
    top = 0;
    for (yy=0; yy<64; yy++) {
      y = (unsigned int)decode_buf[index(xx,yy)];
      u = (unsigned int)decode_buf[index(xx,yy)+5120];
      v = (unsigned int)decode_buf[index(xx,yy)+10240];
      if ((y >= ymin[ii])
       && (y <= ymax[ii]) 
       && (u >= umin[ii]) 
       && (u <= umax[ii]) 
       && (v >= vmin[ii]) 
       && (v <= vmax[ii])) {
        count++;
        if (bottom > yy)
          bottom = yy;
        if (top < yy)
          top = yy;
      }
    }
    if (count) {
      if (bloby1[jj] > bottom)
        bloby1[jj] = bottom;
      if (bloby2[jj] < top)
        bloby2[jj] = top;
      if (blobx1[jj] > xx)
        blobx1[jj] = xx;
      if (blobx2[jj] < xx)
        blobx2[jj] = xx;
      blobcnt[jj] += count;
    } else {
      if (blobcnt[jj])  // move to next blob if a gap is found
        jj++;
      if (jj > (MAX_BLOBS-1))
        goto blobbreak;
    }
  }
blobbreak:   // now sort blobs by size, largest to smallest pixel count
  for (xx=0; xx<=jj; xx++) {
    if (blobcnt[xx] == 0)  // no more blobs, so exit
      return;
    for (yy=xx; yy<=jj; yy++) {
      if (blobcnt[yy] == 0)
        break;
      if (blobcnt[xx] < blobcnt[yy]) {
        tmp = blobcnt[xx];
        blobcnt[xx] = blobcnt[yy];
        blobcnt[yy] = tmp;
        tmp = blobx1[xx];
        blobx1[xx] = blobx1[yy];
        blobx1[yy] = tmp;
        tmp = blobx2[xx];
        blobx2[xx] = blobx2[yy];
        blobx2[yy] = tmp;
        tmp = bloby1[xx];
        bloby1[xx] = bloby1[yy];
        bloby1[yy] = tmp;
        tmp = bloby2[xx];
        bloby2[xx] = bloby2[yy];
        bloby2[yy] = tmp;
      }
    }
  }
}

void set_motor(unsigned char ch, int speed, int *ls, int *rs)  {
  int left_speed, right_speed;
  
  left_speed = right_speed = 0;
  switch (ch) {
    case '7':   // drift left
      left_speed = speed-10;
      right_speed = speed+10;
      break;
    case '8':   // forward
      left_speed = speed; 
      right_speed = speed;
      break;
    case '9':   // drift right
      left_speed = speed+10;
      right_speed = speed-10;
      break;
    case '4':   // turn left
      left_speed = speed-20;
      right_speed = speed+20;
      break;
    case '5':    // stop
      left_speed = 0;
      right_speed = 0;
      break;
    case '6':   // turn right
      left_speed = speed+20;
      right_speed = speed-20;
      break;
    case '1':   // back left
      left_speed = -(speed-20);
      right_speed = -(speed+20);
      break;
    case '2':   // back
      left_speed = -speed;
      right_speed = -speed;
      break;
    case '3':   // back right
      left_speed = -(speed+20);
      right_speed = -(speed-20);
      break;
    case '0':   // counter clockwise turn
      setPwmDutyPercent(-speed, speed);
      delayUs(200000);
      setPwmDutyPercent(0, 0);
      left_speed = 0;
      right_speed = 0;
      break;
    case '.':   // clockwise turn
      setPwmDutyPercent(speed, -speed);
      delayUs(200000);
      setPwmDutyPercent(0, 0);
      left_speed = 0;
      right_speed = 0;
      break;
  }
  setPwmDutyPercent(left_speed, right_speed);
  *ls = left_speed;
  *rs = right_speed;
  return;
}

#ifdef _BASIC_

#define NUM_LAB 100
#define LAB_LEN 10 
#define FOR_NEST 25
#define SUB_NEST 25

#define DELIMITER  1
#define VARIABLE   2
#define NUMBER     3
#define COMMAND    4
#define STRING     5
#define QUOTE      6

#define PRINT      1
#define INPUT      2
#define IF         3
#define THEN       4
#define FOR        5
#define NEXT       6
#define TO         7
#define GOTO       8
#define EOL        9
#define FINISHED  10
#define GOSUB     11
#define RETURN    12
#define COMMENT   13
#define TIME      14
#define MOTORS    15
#define DELAY     16
#define IR        17
#define IMGCAP    18
#define COLORCAP  19
#define COLORSET  20
#define COLORGET  21
#define SCAN      22
#define BLOB      23
#define BITDIR    24
#define BITSET    25
#define BITGET    26
#define END       27

jmp_buf e_buf; /* hold environment for longjmp() */

struct cmds { /* keyword lookup table */
  char command[20];
  char tok;
} ccmd[] = {
  {"print", PRINT},
  {"input", INPUT},
  {"if", IF},
  {"then", THEN},
  {"goto", GOTO},
  {"for", FOR},
  {"next", NEXT},
  {"to", TO},
  {"gosub", GOSUB},
  {"return", RETURN},
  {"time", TIME},
  {"motors", MOTORS},
  {"delay", DELAY},
  {"ir", IR},
  {"imgcap", IMGCAP},
  {"colorcap", COLORCAP},
  {"colorset", COLORSET},
  {"colorget", COLORGET},
  {"scan", SCAN},
  {"blob", BLOB},
  {"bitdir", BITDIR},
  {"bitset", BITSET},
  {"bitget", BITGET},
  {"comment", COMMENT},
  {"end", END},
  {"", END}
};

int vvar[26]= {    /* 26 user variables,  A-Z */
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0
};

char token[80];
char token_type, tok;

struct label {
  char name[LAB_LEN];
  char *p;  /* points to place to go in source file*/
};
struct label label_ccmd[NUM_LAB];