robotdo.c

Go to the documentation of this file.

//
// C Implementation: robotdo
//
// Description: "do" funktions from robot - this determines the behaviour
//
//
// Author: Leonhard Klein <leoklein@gmx.net>, (C) 2004
#include "robotdo.h"
#include "robotu.h"
#include <avr/eeprom.h>
#include <avr/interrupt.h>


void do_boredom() {
#ifdef BEHAVIOUR
  static u08 sendval = 0;
  if (d_pos > 300) modus = flee;
  if (needpower) modus = searching;
  if (light_value < 5) {
    start_sound(go_sleep,1);
    modus = sleep;
  }
  if (modus != connected) {
    modustime = 0;
    l_to=positionl;
    r_to=positionr;
  }
  speed = 100;
  if (irpos_value > 50) direction = 127; else direction = 0;
#else
  if (time == 3000) SETMODUS(ircont);
#endif
}
void do_sleep() {
#ifdef BEHAVIOUR
  RESETP;
  if ((!soundplaying) & (modustime < 5)) {
    modustime++;
    start_sound(snore,0);
  }
  if (light_value > 70) {
    modus = boredom;
    start_sound(wake_up,1);
  }
  if (needpower) modus = searching;
  if (modus != sleep) {
    modustime = 0;
    l_to = positionl;
    r_to = positionr;
  }
#endif
}
void do_connected() {
  static u16 delayt;
  static u08 conmodus;
  static u16 write_counter;
  static u08 sendbuf[IRMSLENGTH];
  irbittime = 12;
  direction = 0;
  speed = 0;
  switch (modustime) {
  case 0:                                           /* only at new connection */
    modustime = 4;
    sendbuf[2]= 0;
    if (needpower)
      sendbuf[0] = sendbuf[1]= 'E';
    else
      sendbuf[0] = sendbuf[1]= 'F';
    ir_string_send(sendbuf);
    break;
  case 1:
    if ((vcc_value > BATFULL) | (!needpower)) {
      sendbuf[0]='F';sendbuf[1]=0;
      ir_string_send(sendbuf);
      modustime = 5;
      needpower = 0;
    } else {
      modustime = 2;
      delayt = 0;
    }
    break;
  case 2:
    speed = 20;
    if (delayt++ == 2000) {
      sendbuf[0]=1;
      ir_string_send(sendbuf);
      SETMODUS(searching);
    }
    if (vcco_value > 100) {
      modustime = 3;
      delayt = 0;
    }
    break;
  case 3:
    if (delayt++ > 65000) modustime = 4;
    if (vcco_value < 100) speed = 20; else RESETP;
    break;
  case 4:
    speed = -20;
    if (vcco_value < 100) {
      modustime = 1;
    }
    break;
  case 5:
    if (vcco_value > 100) speed = -20; else RESETP;
    if (needpower) modustime = 0;
    break;
  }
#ifdef PROGRAMMING
  if (stringvalid) {
    switch (irrstring[1]) {
    case 'D':
      ir_int_send(write_counter);
      break;
    case 'W':
      conmodus = 2;
      write_counter = 102;
      break;
    case 'Q':
      conmodus = 0;
      eeprom_write_byte((u08*)(write_counter),0xFF);
      ir_int_send(write_counter);
      break;
    case 'I':
      conmodus = 1;
      write_counter = 1;
      break;
    case 'R':
      write_counter -= 2;
      break;
    case 'V':
      write_counter += 2;
      break;
    case 'S':
      SETMODUS (script);
      break;

    default:
      if (conmodus) {
        if ((irrstring[1]>='a')&&(irrstring[1]<='z')) {
          eeprom_write_byte((u08*)(write_counter),irrstring[1]);
          eeprom_write_byte((u08*)(write_counter+1),atoi((u08*)(irrstring+3)));
          write_counter += 2;
        } else if (((irrstring[1]>='0')&&(irrstring[1]<='9'))||((irrstring[1]>='A')&&(irrstring[1]<='F'))) {
          eeprom_write_byte((u08*)write_counter,atoi((u08*)(irrstring+1)));
          write_counter += 1;
        }
      }
    }
    if (conmodus != 1) ir_string_send(irrstring+1);
    stringvalid = 0;
    irrstring[0]=0;
  }
#endif
}
void do_searching() {
  static s16 ir_value_max, ir_value_min;
  u16 headingdiff;
  static u16 headingdiffold, headingmax, mtime;

  switch (modustime) {
  case 0: /* start modus */
    start_sound(go_hungry, 1);
    RESETP;
    mtime = 0;
    direction = 0;
    speed = -40;
    modustime = 1;
    break;
  case 1: /* going back to avoid walls */
    if (mtime++ > 3000) {
      ir_value_max = 0;
      ir_value_min = 32000;
      headingdiffold = -500;
      headingmax = (heading + 1127) % POS360;/*POS360*95/100*/
      modustime = 2;
      mtime = 0;
    }
    break;
  case 2:
    direction = 127; speed = 20;
    play(irstation_value);
    if (irstation_value > ir_value_max) {
      ir_value_max = irstation_value;
      headingmax = (heading + POS360) % POS360;
      headingdiffold = 0;
    }
    if (irstation_value < ir_value_min) ir_value_min = irstation_value;
    headingdiff = (POS360 + heading - headingmax) % POS360;
    if (mtime < 4000) mtime++;
    if ((mtime >=4000) & (headingdiffold > headingdiff + (POS360/3))) {
      play(0);
      ir_value_max = irstation_value;
      ir_value_min = (ir_value_max + ir_value_min)>>1;
      mtime = 0;
      modustime = 4;
    }
    headingdiffold = headingdiff;
    break;
  case 4:  /* drive to station */
    speed = 80;
    direction = 0;
    if (((irstation_value < ir_value_min) & (ir_value_max - ir_value_min > 4)) | (mtime++ > 2000*3) | (d_pos > 300)) {
      modustime = 0;
    }
    break;
  }
  if (vcco_value > 100) {  /* detect increase in voltage */
    RESETP;
    SETMODUS (connected);
    start_sound(get_connected,1);
  }
}
void do_empty() {
  START_BEEP(3);
  if (vcco_value > 100) {
    modus = connected;
    vcc_value = BATLOW - 1;
  }
}
void do_flee() {
#ifdef BEHAVIOUR
#endif
}
void do_script() {
#ifdef SCRIPT
  static u16 script_counter;
  static u16 wait;
  u16 x_unclipped;
  static u08 x_script;
  u08 command, para;
  if (!modustime) {
    script_counter = INITSKRIPT;
    direction = 0;
    speed = 0;
    wait = 0;
    modustime = 1;
  }
  if (wait>0) {
    wait--;
    return;
  }
  command = eeprom_read_byte(script_counter);
  para = eeprom_read_byte(script_counter + 1);
  script_counter += 2;
  switch (command) {
  case 'r': drive_to_pos(0, (para - 128) * 10); break;
  case 'l': drive_to_pos((para - 128) * 10, 0); break;
  case 's': speed = para - 128; break;
  case 'd': direction = para - 128; break;
  case 'w': wait = para * 146; break;
  case 'p': playt(para); break;
  case 'm': SETMODUS(para); break;
  case 'g': script_counter = INITSKRIPT + 0*(para / 2); break;
#ifdef SCRIPT2
  case 'h': switch (para) {
    case 0: x_unclipped = light_value; break;
    case 1: x_unclipped = radar_value>>8; break;
    case 2: x_unclipped = irpos_value>>8; break;
    case 3: x_unclipped = d_pos; break;
    }
    x_script=(x_unclipped>255)?255:x_unclipped;
    break;
  case 'i': if (x_script >= para) script_counter +=2; break;
#endif
  default: START_BEEP(1);
    speed = 0;
    SETMODUS(connected);
    break;
  }
  if (needpower) SETMODUS(searching);
#endif
}
void do_ircont() {
#ifdef IRCONT
  static u16 stop;
  u08 i, imax, imaxn, i2;
  u08 ircmp[IRKEYS];
  if (modustime == 0) {
    modustime++;
    irbittime = eeprom_read_byte(IRKEYS*20+1);
    for (i = 0; i < IRKEYS; i++)
      for (i2 = 0; i2 < IRMLENGTH; i2++)
        cmp_target[i][i2] = eeprom_read_byte(1+i*IRMLENGTH+i2);
  }
  if (stop++ > 2000) speed = 0;
  if (pulsevalid) {
    pulsevalid = 0;
    imax = 0;
    imaxn= 0xff;
    for (i=0; i < IRKEYS; i++) {
      ircmp[i] = ir_comp(cmp_target[i], irrbuf);
      if (imax < ircmp[i]) {
        imax = ircmp[i];
        imaxn = i;
        stop = 0;
      }
    }
    if (imax < 130) return;
    speed = 40;
    direction = 0;
    switch (imaxn) {
    case 0:
      speed = 127;
      break;
    case 1:
      direction = 127;
      break;
    case 2:
      direction = -127;
      break;
    case 3:
      speed = -40;
      break;
    }
  }
  if (vcco_value > 100) {  /* detect increase in voltage */
    speed = 0;
    SETMODUS (connected);
    start_sound(get_connected,1);
  }
#endif
}

---