Öffentliche Projekte

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

#include <niboburger/iodefs.h>
#include <niboburger/motpwm.h>
#include <niboburger/motpid.h>
#include <niboburger/delay.h>
#include <niboburger/key.h>
#include <niboburger/usart.h>
#include <niboburger/led.h>
#include <niboburger/surface.h>
#include <niboburger/odometry.h>
#include <niboburger/base.h>
#include <niboburger/storage.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 baudrate_writePersistent(uint32_t val) {
  storage_write(STORAGE_SYS_UART0_BAUDRATE, &val);
}

uint32_t baudrate_readPersistent() {
  uint32_t val;
  if (storage_read(STORAGE_SYS_UART0_BAUDRATE, &val)) {
    return val;
  }
  return 9600;
}

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] = nibo_getMillivolt ();
  }
  
  if (WRITE_REG(NSPREG_LEDS)) {
    uint8_t val = nsp_registers[NSPREG_LEDS];
    led_set(LED_1, val&0x01);
    led_set(LED_2, val&0x02);
    led_set(LED_3, val&0x04);
    led_set(LED_4, val&0x08);
  }
  
  if (READ_REG(NSPREG_SENS)) {
    uint8_t val=0;
    int8_t l = 0; //TODO! sens_getLeft ();
    int8_t r = 0; //TODO! 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] = surface_get(SURFACE_C);
  }
  
  if (READ_REG(NSPREG_LINEL)) {
    nsp_registers[NSPREG_LINEL] = surface_get(SURFACE_CL);
  }

if (READ_REG(NSPREG_LINER)) {
    nsp_registers[NSPREG_LINER] = surface_get(SURFACE_CR);
  }
  
}

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();
  surface_init();
  odometry_init();
  usart_setbaudrate(baudrate_readPersistent());
  usart_enable();
  mode = 1;
  usart_putchar('N');
  usart_putchar('I');
  pos=0;
  
  usart_putchar('B'); // TODO: NIBOburger
  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

nibo_burger_terminal/main.c [read only]

Werbung

Online