robotu.c

Go to the documentation of this file.

//
// C Implementation: robotu
//
// Description: helper routines for robot.c
//
//
// Author: Leonhard Klein <leoklein@gmx.net>, (C) 2004

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include "robotu.h"


const s08 graytab[]= {0,-1,1,0, 1,0,0,-1, -1,0,0,1, 0,1,-1,0};

#define IRON     (PORTB |=  _BV(PB4))  
#define IROFF    (PORTB &= ~_BV(PB4))  
#define IRTOGGLE (PORTB ^=  _BV(PB4))  
u16 irtime;
s16 irbit;

irdatatype irbuf;

#ifdef SINUS
const s08 __ATTR_PROGMEM__ sinetab[] = {
                                         0,3,6,9,12,16,19,22,25,28,31,34,37,40,43,46,49,51,54,57,60,63,65,68,71,
                                         73,76,78,81,83,85,88,90,92,94,96,98,100,102,104,106,107,109,110,112,113,
                                         115,116,117,118,120,121,122,122,123,124,125,125,126,126,126,127,127,127
                                       }
                                       ;  /* sine_tab [0..63]-> [0..127] */

s08 sine(u08 in) {
  if (in < 64) return pgm_read_byte_near(sinetab[in]);
  else if (in < 128) return pgm_read_byte_near(sinetab[127-in]);
  else if (in < 192) return -pgm_read_byte_near(sinetab[in-128]);
  else return -pgm_read_byte_near(sinetab[255-in]);
}
#endif

void decode_position() {                   /* decode angle sensors */
  static s16 posoldl, posoldr, temppos;
  static u08 positioncount, positionl_code, positionr_code;
  positionl_code = positionl_code >> 2;
  if (PIND & _BV(PD2)) positionl_code |=4;
  if (PIND & _BV(PD3)) positionl_code |=8;
  positionl -= graytab[positionl_code];
  positionr_code = positionr_code >> 2;
  if (PIND & _BV(PD0)) positionr_code |=4;
  if (PIND & _BV(PD1)) positionr_code |=8;
  positionr += graytab[positionr_code];
  heading = (positionr-positionl);
  while (heading < 0) heading+=POS360;
  heading = heading % POS360;
  if (!positioncount++) {
    /*        temppos = ((positionl-posoldl)+(positionr-posoldr)) >> 1;
            posx += temppos * sine(heading*255/POS360);
            posy += temppos * COSINE(heading*255/POS360);       not used */
    if ((positionl>10000)|(positionl<-10000)|(positionl>10000)|(positionl<-10000)) {
      l_to -= positionl;
      r_to -= positionr - heading;
      positionr = heading;
      positionl = 0;
    }
    posoldl = positionl;
    posoldr = positionr;
  }
}
u16 abs(s16 val) {
  if (val<0) return -val;
  else return val;
}
void drive() {                             /* set PWM according to l_to, r_to and calculate speed and direction */
  static s16 tempspeed;
  s16 l,r;
  if (direction > 0) {
    l_to += (tempspeed * (64-direction)) / DIVSPEED;
    r_to += (tempspeed * 64) / DIVSPEED;
  } else {
    r_to += (tempspeed * (64+direction)) / DIVSPEED;
    l_to += (tempspeed * 64) / DIVSPEED;
  }
  l = (l_to - positionl);
  r = (r_to - positionr);
  d_pos = abs(l)+abs(r);
  if ((l > 255)|(l < -255)|(r > 255)|(r < -255)) {
    if (l>255) l=255;   if (l<-255) l=-255;
    if (r>255) r=255;   if (r<-255) r=-255;
    tempspeed = 0;
  } else tempspeed = speed;
  if ((l < MINSPEED)&(l>-MINSPEED)) l=0;
  if ((r < MINSPEED)&(r>-MINSPEED)) r=0;   /* TODO: Delay to avoid sliding */
  if (l>=0) {
    PORTB &= ~_BV(PB0);
    OCR1A = l;
  } else {
    PORTB |= _BV(PB0);
    OCR1A = 255+l;
  }
  if (r>=0) {
    OCR1B = r;
    PORTD &= ~_BV(PD7);
  } else {
    PORTD |= _BV(PD7);
    OCR1B = 255+r;
  }
}
void drive_to_pos(s16 l2, s16 r2) {       /* not additive! */
  l_to = positionl + l2;
  r_to = positionr + r2;
}

void start_radar() {
  oldmodus = modus;                                /* save old modus */
  modus = radar;
  radar_value = -1400;                             /* delay before INIT */
  TCCR0 = _BV(CS00);                               /* set TIMER0 frequency to 15,6kHz 1Tick = 1,92cm */
  PORTD &= ~_BV(PD6);                              /* switch radar on */
}
void start_ad(enum admodtype adm) {
  if (admodus == none) {
    admodus = adm;
    switch (adm) {
    case irpos:
      ADMUX = _BV(REFS0) + 1;                                     /* external AREF Channel 1 */
      ADCSRA = _BV(ADEN)|_BV(ADSC)|_BV(ADFR)|_BV(ADIE) + 6;       /* enable freerunning /64 */
      return;
    case irstation:
      ADMUX = _BV(REFS0) + 1;                                     /* external AREF Channel 1 */
      break;
    case light:
      ADMUX = _BV(REFS0) + 0;                                     /* light sensor */
      break;
    case vcc:
      ADMUX = _BV(REFS0) + 5;                                     /* internal bandgap 1.23V */
      break;
    case vcco:
      ADMUX = _BV(REFS0) + 4;                                     /* voltage of "feeler" */
      break;
    }
    ADCSRA = _BV(ADEN)|_BV(ADSC)|_BV(ADIE) + 3;                   /* enable single mode */
  } else START_BEEP(5);
}
u08 atoi(u08 *str) {
  u08 i,i2;
  i = str[0];
  i2= str[1];
  if ((i2 == ' ')|(i2 == 0)) {
    i2 = i;
    i = '0';
  }
  if (i>='A') i -= 'A'-'9'-1;
  if (i2>='A') i2 -= 'A'-'9'-1;
  return ((i-'0') << 4) + (i2-'0');
}
void ir_int_send(u16 val) {
  static u08 intstr[6];
  u08 i;
  i = 0;
  while (i++ < 5) {
    intstr[5-i] = val%10 + '0';
    val /= 10;
  }
  intstr[5] = 0;
  ir_string_send(intstr);
}

---