Öffentliche Projekte

#include <avr/interrupt.h>
#include <avr/pgmspace.h>

#include <nibobee/iodefs.h>
#include <nibobee/motpwm.h>
#include <nibobee/motpid.h>
#include <nibobee/delay.h>
#include <nibobee/sens.h>
#include <nibobee/usart.h>
#include <nibobee/led.h>
#include <nibobee/line.h>
#include <nibobee/odometry.h>
#include <nibobee/base.h>
#include "niboprot.h"

static char buffer[256];
static uint16_t pos;
static uint16_t len;
static uint8_t mode;
static uint8_t echo;

#define READ_REG(x) nsp_register_flags[x]&(NSP_FLAG_REQUESTED|NSP_FLAG_REPORTED)
#define WRITE_REG(x) nsp_register_flags[x]&NSP_FLAG_MODIFIED_REMOTE

void update() {
  
  
  if (READ_REG(NSPREG_BOTID)) {
    //nsp_registers[NSPREG_BOTID] = 0x4e32;   // N2 NIBO 2
    nsp_registers[NSPREG_BOTID] = 0x4e62; // Nb NIBObee
  }

if (READ_REG(NSPREG_VERSION)) {
    nsp_registers[NSPREG_VERSION] = 2;
  }
  
  if (READ_REG(NSPREG_VSUPPLY)) {
    nsp_registers[NSPREG_VSUPPLY] = nibobee_getMillivolt ();
  }
  
  if (WRITE_REG(NSPREG_LEDS)) {
    uint8_t val = nsp_registers[NSPREG_LEDS];
    led_set(LED_L_YE, val&0x01);
    led_set(LED_L_RD, val&0x02);
    led_set(LED_R_RD, val&0x04);
    led_set(LED_R_YE, val&0x08);
  }
  
  if (READ_REG(NSPREG_SENS)) {
    uint8_t val=0;
    int8_t l = sens_getLeft ();
    int8_t r = sens_getRight ();
    if (l<0) val |= 0x01;
    if (l>0) val |= 0x02;
    if (r<0) val |= 0x10;
    if (r>0) val |= 0x20;
    nsp_registers[NSPREG_SENS] = val;
  }

if (WRITE_REG(NSPREG_MOTL) || WRITE_REG(NSPREG_MOTR) || WRITE_REG(NSPREG_MOTMODE) ) {
    static uint8_t oldmode = 99;
    uint8_t mode = nsp_registers[NSPREG_MOTMODE];
    if (mode != oldmode) {
      if (mode==2) {
        motpid_release();
      } else {
        motpid_init();
      }
    }
    
    switch (nsp_registers[NSPREG_MOTMODE]) {
      case 0: motpid_stop(1); break;
      case 1: motpid_stop(0); break;
      case 2: motpwm_setLeft (nsp_registers[NSPREG_MOTL]); 
              motpwm_setRight(nsp_registers[NSPREG_MOTR]); break;
      case 3: motpid_setSpeed(nsp_registers[NSPREG_MOTL], nsp_registers[NSPREG_MOTR]); break;
    } 
  }

if (READ_REG(NSPREG_ODOL)) {
    nsp_registers[NSPREG_ODOL] = odometry_getLeft(0);
  }

if (READ_REG(NSPREG_ODOR)) {
    nsp_registers[NSPREG_ODOR] = odometry_getRight(0);
  }

if (READ_REG(NSPREG_LINEC)) {
    nsp_registers[NSPREG_LINEC] = line_get(LINE_C);
  }
  
  if (READ_REG(NSPREG_LINEL)) {
    nsp_registers[NSPREG_LINEL] = line_get(LINE_L);
  }

if (READ_REG(NSPREG_LINER)) {
    nsp_registers[NSPREG_LINER] = line_get(LINE_R);
  }
  
  
}

static void send_help() {
  PGM_P text = PSTR(NSP_HELP_TEXT NSP_REGISTER_TEXT);
  char c=1;
  while (c) {
    if (!usart_txfull()) {
      c=pgm_read_byte(text++);
      usart_putchar(c);
    }
  }
  
}

int main(int argc, char * argv[]) {
  sei(); // enable interrupts
  led_init();
  line_init();
  sens_init();
  odometry_init();
  usart_setbaudrate(9600);
  usart_enable();
  mode = 1;
  usart_putchar('N');
  usart_putchar('I');
  pos=0;
  
  usart_putchar('B');
  usart_putchar('O');
  usart_putchar('b');
  usart_putchar('e');
  usart_putchar('e');
  usart_putchar('\r');
  usart_putchar('\n');
  motpwm_init();
  
  while (1) {
    if (mode==1) {
      if (!usart_rxempty()) {
        char c = usart_getchar();
        if (pos==0) {
          echo = (c!='$');
        }
        if (echo) {
          usart_putchar(c);
        }
        if (c!='\r') {
          buffer[pos++] = c;
        }
        if (c=='\n') {
          nsp_request(buffer, pos-1);
          update();
          if (nsp_show_help()) {
            send_help();
          }
          len = nsp_reply(buffer, sizeof(buffer));
          pos = 0;
          mode = 2;
        }
      }
    }
    if (mode==2) {
      if (pos<len) {
        if (!usart_txfull()) {
          usart_putchar(buffer[pos++]);
        }
      } else {
        pos = 0;
        len = 0;
        mode = 1;
      }
    }
  }
  
  return 0;
}

Öffentliche Projekte

Welcome

Projektverwaltung

Nachrichten

Neuigkeiten

Site-Statistic

nibobee_terminal/main.c [read only]

Werbung

Online