Öffentliche Projekte

#include <niboburger/robomain.h>

// Linienfolge für schwarze Linie auf hellem Boden

// mittel-grau:
//#define FLOOR_VALUE 30

// hell-grau:
#define FLOOR_VALUE 160

// x > FLOOR_VALUE : Boden
// x < FLOOR_VALUE : Linie

/* maroon SHIELD defines - BEGIN */
#define MAROON_CLEAR() "\33c"
#define MAROON_BRIGHT(x) "\33b" #x
#define MAROON_DIM(x) "\33d" #x
#define MAROON_TXBACK(c) "\33t" c
#define MAROON_LOAD() "\33l"

#define MAROON_BAR(bar) "\33B" bar " "
#define MAROON_GFX(gfx) "\33G" gfx " "
#define MAROON_PAUSE(x) "\33P" #x " "
#define MAROON_STIME(x) "\33S" #x " "
#define MAROON_DTIME(x) "\33D" #x " "
#define MAROON_TRANSITION(n, x) "\33T" #n #x " "

#define MAROON_IMM_CLEAR() "\20c"
#define MAROON_IMM_DIM(x) "\20d" #x
/* maroon SHIELD defines - END */

char maroon_gfxdata[] = MAROON_LOAD() MAROON_BAR("00000000");
#define MAROON_BAR_OFFSET 4

volatile int8_t state;

/* diese Funktion wird automatisch aufgerufen. Der Aufruf erfolgt 
   immer nachdem alle analogen Kanäle einen neuen Wert gemessen
   haben. Da die Funktion so häufig aufgerufen wird, sollte sie 
   möglichst kurz sein und keine Wartezeiten (delay(), etc...)
   oder ausgiebige Berechnungen enthalten!
*/
extern "C"
void analog_irq_hook() {
  uint16_t bcl = analog_getValueExt(ANALOG_BCL,2);
  uint16_t bc = analog_getValueExt(ANALOG_BC,2);
  uint16_t bcr = analog_getValueExt(ANALOG_BCR,2);
  
  if (bc<FLOOR_VALUE) {
    // Linie detektiert
    if (bcl<bcr)  state=-1; // Linie liegt links
    if (bcl>bcr)  state=+1; // Linie liegt rechts
    if (bcl==bcr) state= 0; // Linie liegt mittig
  } else {
    // Linie verloren
    if (state==-1) state=-2; // Linie lag vorher links
    if (state==0)  state=+2; // Linie lag vorher mittig -> Raten
    if (state==+1) state=+2; // Linie lag vorher rechts
  }

switch (state) {
    case -2: led_setall(1,0,0,0); break;
    case -1: led_setall(0,1,0,0); break;
    case  0: led_setall(0,1,1,0); break;
    case +1: led_setall(0,0,1,0); break;
    case +2: led_setall(0,0,0,1); break;
  }
}

/* Zeichen für das maroon SHIELD, abhängig vom aktuellen Sensorwert */
char getSensorChar(int16_t val) {
  if (val<5)   return 'a'; // 0
  if (val<10)  return 'b'; // 1
  if (val<15)  return 'c'; // 2
  if (val<25)  return 'd'; // 3
  if (val<35)  return 'e'; // 4
  if (val<50)  return 'f'; // 5
  if (val<70)  return 'g'; // 6
  if (val<100) return 'h'; // 7
               return 'i'; // 8
}

/* 8 Bars für das maroon SHIELD zusammenbauen */
void show_sensors(uint16_t v0, uint16_t v1, uint16_t v2, uint16_t v3, uint16_t v4, uint16_t v5, uint16_t v6, uint16_t v7) {
  maroon_gfxdata[MAROON_BAR_OFFSET+0]= getSensorChar(v0);
  maroon_gfxdata[MAROON_BAR_OFFSET+1]= getSensorChar(v1);
  maroon_gfxdata[MAROON_BAR_OFFSET+2]= getSensorChar(v2);
  maroon_gfxdata[MAROON_BAR_OFFSET+3]= getSensorChar(v3);
  maroon_gfxdata[MAROON_BAR_OFFSET+4]= getSensorChar(v4);
  maroon_gfxdata[MAROON_BAR_OFFSET+5]= getSensorChar(v5);
  maroon_gfxdata[MAROON_BAR_OFFSET+6]= getSensorChar(v6);
  maroon_gfxdata[MAROON_BAR_OFFSET+7]= getSensorChar(v7);
  usart_write(maroon_gfxdata);
}

/* alle aktuellen Werte auf dem maroon SHIELD ausgeben */
void updateDisplay() {
  if (usart_txempty()) {
    uint16_t bl = analog_getValueExt(ANALOG_BL,2)/2;
    uint16_t bc = analog_getValueExt(ANALOG_BC,2)/2;
    uint16_t br = analog_getValueExt(ANALOG_BR,2)/2;
    uint16_t bcl = analog_getValueExt(ANALOG_BCL,2);
    uint16_t bcr = analog_getValueExt(ANALOG_BCR,2);
    show_sensors(br,br,bcr,bc,bc,bcl,bl,bl);
  }
}

void setup() {
  led_init();
  analog_init();
  motpwm_init();
  usart_setbaudrate(38400);
  usart_enable();
  // für Linienfolge grüne Seitensensoren einschalten (Licht vom mittleren 
  // Sensor (BC), Messwerte von den äußeren Sensoren (BL+BR))
  analog_setExtToggleMode(ANALOG_BCL, 1);
  analog_setExtToggleMode(ANALOG_BCR, 1);
  delay(500);
  
  // auf einen Tastendruck warten...
  while (!key_get_char()) {}
  
}

uint8_t display_cnt = 50;

uint16_t loose_cnt = 550;

void loop() {
  char key = key_get_char();
  bool pause = false;
  if(key == 'A'){
    pause = true;
  	
  }
  while(pause == true){
    motpwm_setLeft(0);
    motpwm_setRight(0);
    key = key_get_char();
    if(key == 'c'){
    pause = false;
    }  
    
  }
  analog_wait_update();
  // nach jedem 25. Durchlauf, das Display aktualisieren
  if (display_cnt-- == 0) {
    display_cnt = 25;
    updateDisplay();
  }
  
  int16_t left=0, right=0;

// Geschwindigkeiten in Abhängigkeit vom aktuellen Linienstatus setzen
  switch (state) {
    case -2: left=   0; right=1000; break;
    case -1: left= 500; right=1000; loose_cnt = 550; break; // loose_cnt zurücksetzen
    case  0: left=1000; right=1000; loose_cnt = 550; break; // loose_cnt zurücksetzen
    case +1: left=1000; right= 500; loose_cnt = 550; break; // loose_cnt zurücksetzen
    case +2: left=1000; right=   0; break;
  }
  
  // Wenn die Linie verloren wurde, kurz warten und danach eine Zeit
  // lang den Roboter drehen, um die Linie wiederzufinden.
  if (loose_cnt>500) {
    // warten...
    loose_cnt--;
  } else if (loose_cnt) {
    // auf der Stelle drehen...
    loose_cnt--;    
    if (state>0) {left=+800; right=-800;}
    if (state<0) {left=-800; right=+800;}
  } else {
    // Linie endgültig verloren :-(
    left = right = 0;
  }
  
  // Leistung der Motoren setzen:
  motpwm_setLeft(left); 
  motpwm_setRight(right);
}

Öffentliche Projekte

Welcome

Projektverwaltung

Nachrichten

Neuigkeiten

Site-Statistic

linienfolge/main.cpp [read only]

Werbung

Online