utils.c

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

void 
initIR()
{
  // initialize IR pins for GPIO
  PINSEL1 &=0x0000ffff;

  IODIR0 = IR_SEND;
  IOCLR0 = IR_SEND;  // set low should turn off IR LED
}


/*****************************************************************************
 *
 * Description:
 *    read IR - return direction counts in upper 16 bits, data in lower 8 bits
 *
 ****************************************************************************/
unsigned int
listenIR(unsigned int max_wait)
{
  unsigned int ix, jx, kx, t0, count;
  unsigned int nleft, nright, nforward, nback;
  unsigned int start[11];
 
  t0 = TIMER0_TC; 
  jx = 0;      // accumulator for received bits
  kx = 0;      // bit counter  -  11 bits per character
  nleft = 0;   // nleft, nright, nforward, nback won't exceed 11
  nright = 0;
  nforward = 0;
  nback = 0;
  count = 0;
  for (ix=0; ix<11; ix++)
    start[ix] = 0;

  while (((TIMER0_TC - t0) < max_wait) && (kx < 11))
  {
    count++;
    delayUs(28);
    ix = IOPIN0 & IR_RECV;
    if (ix != IR_RECV)
    {
      start[kx] = count;
      if ((ix & IR_R_FORWARD) == 0)
        nforward++;
      if ((ix & IR_R_LEFT) == 0)
        nleft++;
      if ((ix & IR_R_BACK) == 0)
        nback++;
      if ((ix & IR_R_RIGHT) == 0)
        nright++;
      while ((IOPIN0 & IR_RECV) != IR_RECV)
      {
        count++;
        delayUs(28);
      }
      kx++;
    }
  }
  
  jx = 0x00000400;   // highest bit is always set, since that's how we located the signal
  t0 = start[0];     // note: there are 40 * 28usec pulse periods in each bit
  for (ix=1; ix<kx; ix++)
  {
    start[ix] -= t0;
    //printNumber(10, 6, FALSE, ' ', start[ix]);
    if ((start[ix] > 30) && (start[ix] < 440)) 
      jx |= 0x00000400 >> ((start[ix]+10)/40);
  }  

  jx |= (nforward << 28) | (nleft << 24) | (nback << 20) | (nright << 16);

  return jx;
}



/*****************************************************************************
 *
 * Description:
 *    bounce IR light and look for return
 *
 ****************************************************************************/
unsigned int
bounceIR(unsigned int direction, unsigned char cc)
{
  unsigned int jx, kx, count, xbit, rbit, itmp;
  unsigned char ch;
  
  IOCLR0 = IR_SEND;  // turn off IR LED
  count = 0;     
  itmp = 0;          // itmp doesn't do anything except help balance the timing 
  ch = cc;
  
  switch (direction)
  {
    case 0x08:   // FORWARD
      xbit = IR_X_FORWARD;
      rbit = IR_R_FORWARD;
      break;
    case 0x04:   // LEFT
      xbit = IR_X_LEFT;
      rbit = IR_R_LEFT;
      break;
    case 0x02:   // BACK
      xbit = IR_X_BACK;
      rbit = IR_R_BACK;
      break;
    case 0x01:   // RIGHT
      xbit = IR_X_RIGHT;
      rbit = IR_R_RIGHT;
      break;
  }
  
rbit = IR_RECV;
  
  for (jx=0; jx<2; jx++)  // send 2 start bits
  {
    for (kx=0; kx<20; kx++) 
    {
      IOSET0 = xbit;  // send pulse 20 times
      delayUs(14);
      IOCLR0 = xbit;
      delayUs(14);
      if ((IOPIN0 & rbit) == 0)
        count++;
      else
        itmp++;
    }
    delayUs(560);      // 20 cycle deadband
  }

  for (jx=0; jx<8; jx++)  // now send 8 data bits
  {
    if (ch & 0x80) 
    {
      for (kx=0; kx<20; kx++)
      {
        IOSET0 = xbit;  // send pulse 20 times
        delayUs(14);
        IOCLR0 = xbit;
        delayUs(14);
        if ((IOPIN0 & rbit) == 0)
          count++;
        else
          itmp++;
      }
      delayUs(560);      // 20 cycle deadband
    }
    else
    {
      IOCLR0 = xbit;  // turn off IR LED
      delayUs(1120);
    }
    ch = ch << 1;      
  }

  for (kx=0; kx<20; kx++)    // finally, send 1 stop bit
  {
    IOSET0 = xbit;  // send pulse 20 times
    delayUs(14);
    IOCLR0 = xbit;
    delayUs(14);
    if ((IOPIN0 & rbit) == 0)
      count++;
    else
      itmp++;
  }
  delayUs(560);

  IOCLR0 = IR_SEND;  // turn off IR LEDs
  return count;
}

void testIR()
{
  unsigned int jx, ix;
  
  jx = (unsigned int) listenIR(100);   // listen for 100 milliseconds
  printNumber(16, 8, FALSE, '0', jx);
  
  ix = bounceIR(FORWARD, 0xFF);
  printNumber(16, 8, FALSE, ' ', ix);

  ix = bounceIR(LEFT, 0xFF);
  printNumber(16, 8, FALSE, ' ', ix);

  ix = bounceIR(BACK, 0xFF);
  printNumber(16, 8, FALSE, ' ', ix);

  ix = bounceIR(RIGHT, 0xFF);
  printNumber(16, 8, FALSE, ' ', ix);

  return;
}

/*****************************************************************************
 *
 * Description:
 *    convert ASCII character to hex nibble
 *
 ****************************************************************************/
unsigned char atoh(unsigned char ch)  
{
  if ((ch >= '0') && (ch <= '9'))
    return (ch & 0x0F);
  else
    return ((ch & 0x0F) + 9);
}

/*****************************************************************************
 *
 * Description:
 *    read pins E5 - E8 and assign robot ID
 *
 ****************************************************************************/
unsigned char read_id_pins()
{
  PINSEL0 &= ~0x0FF00000;  //  clear pins P0.13 0.12 0.11 0.10 corresponding to E8 E7 E6 E5
  return (unsigned char)((IOPIN0 & 0x00003C00) >> 10);
}

/*****************************************************************************
 *
 * Description:
 *    set direction of i/o bits on E9 E10 E11 E12 E13 (p0.16 - 0.20)
 *
 ****************************************************************************/
void bits_dir(unsigned char bits)
{
  PINSEL1 &= 0xFFFFFC00;  // set p0.16 - 0.20 (E13 - E9) for GPIO
  if (bits & 0x01)
    IODIR0 |= 0x00010000;
    
  if (bits & 0x02)
    IODIR0 |= 0x00020000;

  if (bits & 0x04)
    IODIR0 |= 0x00040000;

  if (bits & 0x08)
    IODIR0 |= 0x00080000;

  if (bits & 0x10)
    IODIR0 |= 0x00100000;
}
  
  /*****************************************************************************
 *
 * Description:
 *    set i/o bits on E9 E10 E11 E12 E13 (p0.16 - 0.20)
 *
 ****************************************************************************/
void bits_write(unsigned char bits)
{
  if (bits & 0x01)
    IOSET0 = 0x00010000;
  else
    IOCLR0 = 0x00010000;
    
  if (bits & 0x02)
    IOSET0 = 0x00020000;
  else
    IOCLR0 = 0x00020000;

  if (bits & 0x04)
    IOSET0 = 0x00040000;
  else
    IOCLR0 = 0x00040000;

  if (bits & 0x08)
    IOSET0 = 0x00080000;
  else
    IOCLR0 = 0x00080000;

  if (bits & 0x10)
    IOSET0 = 0x00100000;
  else
    IOCLR0 = 0x00100000;
}
  
/*****************************************************************************
 *
 * Description:
 *    read i/o bits on E9 E10 E11 E12 E13 (p0.16 - 0.20)
 *
 ****************************************************************************/
unsigned char bits_read()
{
  return (unsigned char)((IOPIN0 & 0x001F0000) >> 16);
}