Öffentliche Projekte

/*
  Programm zum ersten Testen des NIBObee Roboters
  dokumentiert von BirgerT 
*/

//
// hier werden zuerst die Header Dateien der NiboRoboLib eingebunden
// diese Dateien verraten dem Compiler welche Funktionen in den 
// Bibliotheken zur Verfügung stehen. Damit der Compiler weiss, dass 
// die Funktion in einer fertig compilierten Bibliothek zu finden ist, 
// stehen die includes in spitzen Klammern
//
#include <niboburger/iodefs.h>
#include <niboburger/usart.h>
#include <niboburger/motpwm.h>
#include <niboburger/motpid.h>
#include <niboburger/analog.h>
#include <niboburger/odometry.h>
#include <niboburger/led.h>
#include <niboburger/key.h>
#include <niboburger/delay.h>
#include <niboburger/surface.h>
#include <niboburger/color.h>

//
// die hier eingebundenen Bibliotheken wurden bereits mit den o.a. 
// NIBOburger Header Dateien verwurschtelt, also hier doppelt und unnütz
// 
//#include <avr/io.h>
//#include <avr/interrupt.h>
//#include <util/delay.h>

//#define SPEED 500 // unbenutzte defines
//#define SPEED 1023

//
// wenn das Makro "min" noch nicht definiert ist "#ifndef min"
//
#ifndef min
#define min(a,b) \
   ({ __typeof__ (a) _a = (a); \
       __typeof__ (b) _b = (b); \
     _a < _b ? _a : _b; })
#endif                               // gehört zu #ifndef min

#define min3(a,b,c) min(min(a,b),c)

//
// warum diese Variable progmode heisst ?!
// Hier wird in verschiedenen Funktion der Status (Statemachine) gespeichert 
//
uint8_t progmode = 0;

//
// Der Status einer Statemachine wird als Zahl in einer Variablen gespeichert.
// Damit der Programmierer sich die Zahlen nicht merken muss, können diesen mit
// enum{}; symbolische Namen zugeordnet werden 
// (bzw. umgekehrt - einem Symbolnamen wird eine Nummer zugeordnet)
//
enum {
  EVENT_NONE              =  0,
  EVENT_KEY1              =  1,
  EVENT_KEY2              =  2,
  EVENT_KEY3              =  3,
//  EVENT_TIMEOUT           =  4, // unbenutzt
// die nachfolgenden States werden in unbenutzten Programmteilen verwendet 
  EVENT_OBSTACLE_DETECTED = 16,
  EVENT_OBSTACLE_CLEAR    = 17,
  
  EVENT_NO_HAND           = 18,
  EVENT_TO_CLOSE          = 19,
  EVENT_HAND_C            = 20,
  EVENT_HAND_L            = 21,
  EVENT_HAND_R            = 22,  
  EVENT_HAND_LL           = 23,
  EVENT_HAND_RR           = 24  
  
};
//
// OMG dieser Code wird ja gar nicht benutzt..
#if 0
- genauso wie alle anderen Zeilen, die zwischen #if 0 und #endif stehen
#endif
  
  #if 0
//
// Hier habe ich dann auch aufgegeben - WTF bedeutet 'cops'?
// Mittlerweile weiss ich, dass es sich hier um Bitmasken und 
// Blinkmuster handelt, die von blink_byte() verarbeitet werden
// jeweils 4 Bit eines Bytes bestimmen welche LEDs leuchten

uint8_t cops1[8] = {
  0x10,		// LEDs 1..4 aus, dann LED1 ein
  0x20,		// LEDs 1..4 aus, dann LED2 ein
  0x40,		// LEDs 1..4 aus, dann LED3 ein
  0x80,		// LEDs 1..4 aus, dann LED4 ein
  0x00,		// LEDs 1..4 aus, dann LEDs 1..4 aus,
  0x00,
  0x00,
  0x00
};

uint8_t cops2[8] = {
  0x30,		// LEDs 1..4 aus, dann LED1 und LED2 ein
  0x30,
  0x30,
  0xc0,		// LEDs 1..4 aus, dann LED3 und LED4 ein
  0xc0,
  0xc0,
  0x00,		// LEDs 1..4 aus, dann LEDs 1..4 aus,
  0x00
};
#endif
//
// Prototypen der Testfunktionen
//

void setup_test0();		// TESTPROG 0 - LED + KEY
void setup_test1();		// TESTPROG 1 - Sensorbricks
void setup_test2();		// TESTPROG 2 - Motor + Odo
//void setup_test3();	// sollte wohl Kalibrierung werden..hat aber einen eigenen Protypen weiter unten

void loop_test0();
void loop_test1();
void loop_test2();
//void loop_test3();

//
// die Funktion key_getEvent() ist erst weiter unten implementiert
// damit sie aber vorher nutzbar ist - in do_cops().. musste hier
// schon mal der Prototyp hin..
//
uint8_t key_getEvent();

// ------------------------------------------------------------------ LED ANSTEUERUNG
//
// key_get_state() liefert den "Gedrückt"-Status der 3 Tasten des 
// NIBOburger als Bitmaske zurück, die Bitposition entspricht der 
// gedrückten Taste(n); 
// hier werden die Tastendrücke mit den LEDs 1..3 angezeigt
//
// Diese Funktion wird einmal in der main() verwendet, ansonsten
// in loop_test_0(); 
// Was passiert, wenn mehrere Tasten gleichzeitig gedrückt werden..
//
// Eigentlich ist diese Funktion redundant, weiter unten gibt's
// set_led_byte(). ein "set_led_byte(key_getState());" sollte
// genau so funktionieren..
//
static void show_key(uint8_t key) {
  led_set(1, key&0x01);
  led_set(2, key&0x02);
  led_set(3, key&0x04);
  led_set(4, 0);
}
//
// Die Blinkinfo über das gestartete Testprogramm
//

void blink_led(uint8_t led, uint8_t count) {
  while (count--) {
    led_set(led, 1);
    delay(80);
    led_set(led, 0);
    delay(120);
  }
}

//
// Schnelles Blinken nach dem Kalibrieren
//
void blink_led_fast(uint8_t led, uint8_t count) {
  while (count--) {
    led_set(led, 1);
    delay(40);
    led_set(led, 0);
    delay(60);
  }
}
//
// Aufgemerkt: Diese Funktion ist leider (noch) nicht in der niborobolib vorhanden
// Wäre m.E. sinnvoll, sie als led_setMask(uint8_t mask) zu implemntieren.
//
// Mit der einer vorhandenen Lib Funktion sähe das so aus:
// led_setall(byte & 1, byte & 2, byte & 4, byte & 8);
//
void set_led_byte(uint8_t byte) {
  IO_LEDS_PORT = (IO_LEDS_PORT & ~IO_LEDS_MASK) | ((byte << IO_LEDS_BIT_1) & IO_LEDS_MASK);
}

//
// Hier werden jetzt die Bytes aus den o.a. copsX-Arrays auf die LEDs ausgegeben
// IO_LEDS_PORT, IO_LEDS_MASK, IO_LEDS_BIT_1 sind defines aus iodefs_niboburger.h
// die oben über #include <niboburger/iodefs.h> eingebunden wird.
//
// Das ist übrigens BASIC IO Programmierung, die jeder Anfänger als erstes beherrschen sollte
//
void blink_byte(uint8_t byte) {
  IO_LEDS_PORT = (IO_LEDS_PORT & ~IO_LEDS_MASK) | ((byte << IO_LEDS_BIT_1) & IO_LEDS_MASK);
  delay(50);  
  IO_LEDS_PORT = (IO_LEDS_PORT & ~IO_LEDS_MASK) | ((byte >> (4-IO_LEDS_BIT_1)) & IO_LEDS_MASK);
  delay(50);
}

// ----------------------------------------------------------------- ungenutzter Code
#if 0
uint8_t do_cops1() {
  uint8_t i, e;
  for (i=0; i<4; i++) {
    blink_byte(cops1[i]);
    e = key_getEvent();
    if ((e>=EVENT_KEY1)&&(e<=EVENT_KEY3)) {
      blink_byte(0);
      return e;
    }
  }
  return EVENT_NONE;
}

uint8_t do_cops2() {
  uint8_t i, e;
  for (i=0; i<7; i++) {
    blink_byte(cops2[i]);
    e = key_getEvent();
    if ((e>=EVENT_KEY1)&&(e<=EVENT_KEY3)) {
      blink_byte(0);
      return e;
    }
  }
  return EVENT_NONE;
}
void do_cops() {
  uint8_t i;
  while (1) {
    for (i=0; i<2; i++) {
      if (do_cops2() != EVENT_NONE) {
        blink_byte(0);
        return;
      }
    }
    for (i=0; i<3; i++) {
      if (do_cops1() != EVENT_NONE) {
        blink_byte(0);
        return;
      }
    }
  }
  blink_byte(0);
}
#endif
// -------------------------------------------------------------------------------------

// -------------------------------------------------- irrelevanter und ungenutzer Code 
//
// würde bei einer Bedingung (progmode == 2) aufgerufen werden, die in diesem Programmm
// aber nie erfüllt sein würde.

/* OBSTACLE */
#if 0
int8_t obstacle_pos;
int8_t obstacle_val;

uint8_t follow_getEvent() {
  static uint8_t state = 0;
  uint8_t nstate = 0;
  int8_t l = analog_getValueExt(ANALOG_FL, 2)>>3;
  int8_t r = analog_getValueExt(ANALOG_FR, 2)>>3;
  int8_t ll = analog_getValueExt(ANALOG_FLL, 2)>>3;
  int8_t rr = analog_getValueExt(ANALOG_FRR, 2)>>3;
  int8_t cc = l + r;
  ll += l;
  rr += r;
 
  obstacle_val = (ll>rr)?ll:rr;
  if (cc>obstacle_val) {
    obstacle_val = cc;
  }
  
  if ((obstacle_val==cc) || (ll==rr)) {
    if (ll>rr) {
      obstacle_pos = -1;
      nstate = EVENT_HAND_L;
    } else if (ll<rr) {
      obstacle_pos = +1;
      nstate = EVENT_HAND_R;
    } else {
      obstacle_pos = 0;
      nstate = EVENT_HAND_C;
    }
  } else if (ll>rr) {
    obstacle_pos = -2;
    nstate = EVENT_HAND_LL;
  } else {
    obstacle_pos = +2;
    nstate = EVENT_HAND_RR;
  }

if (obstacle_val>8) {
    nstate = EVENT_TO_CLOSE;
  } else if (obstacle_val<3) {
    nstate = EVENT_NO_HAND;
  }

if (state!=nstate) {
    state = nstate;
    return state;
  }
 
  return EVENT_NONE;
}

uint8_t obstacle_getEvent() {
  static uint8_t state = 0;
  int8_t l = analog_getValueExt(ANALOG_FL, 2)>>3;
  int8_t r = analog_getValueExt(ANALOG_FR, 2)>>3;
  int8_t ll = analog_getValueExt(ANALOG_FLL, 2)>>3;
  int8_t rr = analog_getValueExt(ANALOG_FRR, 2)>>3;
  int8_t cc = l + r;
  ll += l;
  rr += r;
 
  obstacle_val = (ll>rr)?ll:rr;
  if (cc>obstacle_val) {
    obstacle_val = cc;
  }
  
  if ((obstacle_val==cc) || (ll==rr)) {
    if (ll>rr) {
      obstacle_pos = -1;
    } else if (ll<rr) {
      obstacle_pos = +1;
    } else {
      obstacle_pos = 0;
    }
  } else if (ll>rr) {
    obstacle_pos = -2;
  } else {
    obstacle_pos = +2;
  }

if ((obstacle_val>6) && (state!=1)) {
    state=1;
    return EVENT_OBSTACLE_DETECTED;
  }
  
  if ((obstacle_val<5) && (state!=0)) {
    state=0;
    return EVENT_OBSTACLE_CLEAR;
  }  
  
  return EVENT_NONE;
}
#endif

// ----------------------------------------------------------------- Tastenabfrage
// wird in calibrate() aufgerufen, und scheint ein Vorgänger der 
// Bibliotheksfunktion key_getChar() zu sein:
//
//	switch(key_get_char){
//	case 'a': return EVENT_KEY1;
//	case 'b': return EVENT_KEY2;
//	case 'c': return EVENT_KEY3;
//	}
//	return EVENT_NONE;
//
// Damit das Programm bei einem Tastendruck eine Aktion nur einmal ausführt, muss
// man den letzten Zustand des Tasters mit dem aktuellen Zustand vergleichen.
// Das ist eine sogenannte Flankenauswertung; 
// es gibt eine (steigende) Flanke, wenn der Eingang von 0 auf 1 wechselt, 
// und eine (fallende) Flanke beim Signalwechsel von 1 auf 0.
//
/* KEY */

uint8_t key_getEvent() {
  static uint8_t key = 0;				// Flankenmerker (key_state nach letztem Aufruf)
  uint8_t act = key_get_state();		// aktuellen key_state abfragen
  if (act==0) {							// Wenn jetzt keine Taste gedrückt ist..
    act = key;							// ..den letzten Status erinnern
    key = 0;
    if (act==0x01) return EVENT_KEY1;	// war vorher also Taste 1 gedrückt..
    if (act==0x02) return EVENT_KEY2;	// oder Taste 2..
    if (act==0x04) return EVENT_KEY3;   // oder Taste 3..
    return EVENT_NONE;					// oder keine -> dann gibt's auch nix Neues
  }
  										// Aktuell ist eine Taste gedrückt:
  if (act!=KEY_STATE_INVALID) {			// Wenn das ein gültiger Tastenstatus ist
    key |= act;  						// merken, bis keine Taste mehr gedrückt ist
  }
  return EVENT_NONE;					// aktuell gedrückte Taste -> nix Neues
}

//
// Sensorik abfragen und dem Aufrufer als State zurückmelden
//
uint8_t getEvent() {
  uint8_t event = EVENT_NONE;
  event = key_getEvent();
  if (event) return event;

// diesen Code braucht's nur in den beiden anderen Beispielprogrammen
// Fraidy Cat und Follow Me
#if 0
  if (progmode==2) {
    event = obstacle_getEvent();
  } else if (progmode==1) {
    event = follow_getEvent();
  }
#endif
  return event;
}

// ----------------------------------------------------------------- ungenutzter Code
#if 0
int8_t decode_l (uint8_t val) {
  if (val==0x01) return -1;
  if (val==0x02) return 1;
  return 0;
}

int8_t decode_r (uint8_t val) {
  if (val==0x01) return -1;
  if (val==0x02) return 1;
  return 0;
}
#endif

// --------------------------------------------------- Print Funktionen über den UART
//
// Byte in zwei ASCII Zeichen umwandeln und am UART ausgeben
//
void print_hex (uint8_t val) {
  char c1=val/16;
  char c2=val%16;
  if (c1<10) c1+='0'; else c1+='a'-10;
  if (c2<10) c2+='0'; else c2+='a'-10;
  usart_putchar(c1);
  usart_putchar(c2);
}
//
// drei Byte aus einer 32Bit Variablen als Hexcode ausgeben
//
void print_color_rgb (uint32_t rgb) {
  usart_putchar('#');
  print_hex((rgb>>16)&0xff);
  print_hex((rgb>> 8)&0xff);
  print_hex((rgb>> 0)&0xff);
}

void print_color_hsv (uint32_t hsv) {
  usart_putchar('$');
  print_hex((hsv>>16)&0xff);
  print_hex((hsv>> 8)&0xff);
  print_hex((hsv>> 0)&0xff);
}

//
// Byte 0..99 als 2stellige Dezimalzahl über UART ausgeben
//
void print_decimal2(uint8_t val) {
  uint8_t t=0;
  while (val>9) {
    t++;
    val-=10;
  }
  usart_putchar('0'+t);
  usart_putchar('0'+val);
}
//
// Byte 0..255 als 3stellige Dezimalzahl 000..255 über UART ausgeben
//

void print_decimal(uint8_t val) {
  uint8_t e=0, z=0, h=0;
  while (val>99) {
    h++;
    val-=100;
  }
  while (val>9) {
    z++;
    val-=10;
  }
  e=val;
  if (h) usart_putchar('0'+h);
  if (h||z)usart_putchar('0'+z);
  usart_putchar('0'+e);
}

//
// drei Byte aus einer 32Bit Variablen als Dezimalzahlen ausgeben
//

void print_color_hsv2 (uint32_t color) {
  uint8_t h = (uint8_t)((color >> 16) & 0xff); // [0..255]
  uint8_t s = (uint8_t)((color >>  8) & 0xff); // [0..255]
  uint8_t v = (uint8_t)((color >>  0) & 0xff); // [0..255]

usart_putchar('!');
  print_decimal(h);
  usart_putchar('.');
  print_decimal(s);
  usart_putchar('.');
  print_decimal(v);
}

//
// eine Zeichenkette (String/Text) über den UART senden
// char str_Hello[] = "hello ";
// print_text(str_Hello);
// print_text("world");
//
// ----------------------------------- wird in diesem Programm nicht genutzt
#if 0
void print_text (char * text) {
  char c;
  while ((c=*text++)) {
    while (usart_txfull()) {
      /* wait */
      ; // ich vermute mal, dass das Oroginal nicht funktioniert, weil diese Semikolon fehlt?!
    }
    usart_putchar(c);
  }
}
#endif

#if 0
// ----------------------------------- wird in diesem Programm nicht genutzt
uint16_t get_sensor(uint8_t s) {
  int16_t val = analog_getValueExt(s, 1);
  val -= analog_getValueExt(s, 0);
  if (val<0) val = 0;
  if (val>1023) val = 1023;
  return val;
}
#endif

//
// und hier noch ein paar Variablen und Prototypen
// die unbenutzten sind auskommentiert ;-)
//
//uint8_t run = 0;
//uint16_t counter = 0;

//void setup_follow_me();
//void setup_avoid_obst();
void setup_calibrate();

//void loop_startup();
//void loop_follow_me();
//void loop_avoid_obst();
void loop_calibrate();
//
// Hiermit soll sich der NIBOburger an einem Terminal/seriellen Monitor melden
// aber leider sind das zuviele Zeichen, sie werden verschluckt, 
//
void print_hello() {
  
  usart_putchar('N');
  usart_putchar('I');
  usart_putchar('B');
  usart_putchar('O');
  usart_putchar(' ');
 /* 
  usart_putchar('B');
  usart_putchar('u');
  usart_putchar('r');
  usart_putchar('g');
  usart_putchar('e');
  usart_putchar('r');
  usart_putchar(' ');
*/
  usart_putchar('V');
  usart_putchar('0');
  usart_putchar('2');
  usart_putchar(' ');
  usart_putchar(' ');
  usart_putchar('.');

}

//
// **************************************************************** MAIN()
// Hier geht das eigentliche Programm los
//
int main() {
  activate_output_group(IO_LEDS);  // LED bits als Output -> led_init();
  
  ///////////////////////////////////////////////////////////////////////////////////// Birger
  
  usart_setbaudrate(9600);
  //usart_setbaudrate(38400);

//       Register     ---- 8 Datenbits -----   ---- no Parity -----    - 1 Stop -
 // UCSRC = (1<<URSEL) | (1<<UCSZ1) | (1<<UCSZ0) | (0<<UPM1) | (0<<UPM0) | (0<<USBS) ;
  
  usart_enable();
 
  // Die üblichen Initialisierungen - damit sollte jede main() eines Niboburger Programms beginnen
  motpwm_init();		// Start des Timers für die Zeitmessung, und PWM Setup
  motpid_init();		// 
  odometry_init();
  //motpwm_setLeft(0);
  //motpwm_setRight(0);
  analog_init();		// Den ADC anwerfen, und die automatische Analogwertmessung starten
  sei();				// Wichtig: Interrupts freigeben
  
// hier begannen meine erste Tests, damals noch mit dem XBee Modul, 
// das nach dem Einschalten des Burgers aber auch seine Zeit zum "Hochfahren" braucht
  delay(100);
  
// ????????????????????? diese Ausgabe kommt nicht
  print_hello();
  usart_putchar('S');
  usart_putchar('\n');

delay(100); // Warten bis XBee (o. Blue Modul) bereit sein müssten
  delay(100);
  delay(100);
  delay(100);
  delay(100);

print_hello();
  usart_putchar('D');
  usart_putchar('\n');
  
// noch eine Initialisierung für die Bodensensor Bricks
  surface_init();
  
//
// mit analog_init() wird die automatische Differenzmessung (Togglemode) aktiviert, d.h.
// je Brick werden zwei ADC Messungen durchgeführt, einmal ohne IR- oder Farb-LED
// und einmal mit eingeschalteter "Beleuchtung".
// 
// hier wird der Togglemode wieder deaktiviert, IR- und Bodensensoren liefern nur
// passives Signal ohne aktive Reflexion.
//
  for (uint8_t i = 0; i<8; i++) {
    analog_setExtToggleMode(i, 0);
  }
  
// no comment  
  delay(100);
  
  
  //
  // Welche Taste ist denn beim Einschalten gedrückt?
  uint8_t key = key_get_state();
  if (key) {
    while(key_get_state()) {	// solange eine Taste gedrückt ist, geht's nicht weiter
      show_key(key);
    }
    show_key(0);
  }
  
  if (key==0x01) {
    setup_test1();				// Testprogramm Initialisierungscode
    while(1) {
      loop_test1();				// Dauerschleife ohne Rückkehr
    }    
  }
  
  if (key==0x02) {
    setup_test2();
    while(1) {
      loop_test2();
    }
  }
  
  if (key==0x04) {
    setup_calibrate();
    while(1) {
      loop_calibrate();
    }    
  }
  //				Beim Einschalten war keine Taste gedrückt
  setup_test0();
  while(1) {
    loop_test0();
  }
  
}

/////////////////////////////////////////////////////////// CALIBRATION

void setup_calibrate() {
  blink_led(3, 4);
  for (uint8_t i = 0; i<8; i++) {
    analog_setExtToggleMode(i, 1);
  }
  progmode = 3;
}

void loop_calibrate() {
  analog_wait_update();
  uint8_t event = getEvent();
  
  if (event==EVENT_KEY1) {
    blink_byte(0);
    surface_calibrateBlack();
    surface_writePersistent();
    blink_led_fast(1, 10);
  }

if (event==EVENT_KEY2) {
    blink_byte(0);
    surface_calibrateWhite();
    surface_writePersistent();
    blink_led_fast(2, 10);
  }

uint8_t leds = 0;
  
  uint32_t rgb = surface_getColorRGB();
  uint32_t hsv = surface_getColorHSV();
  
  uint16_t dcol_bk = color_diff_hsv_bal(hsv, COLOR_HSV_CAL_BLACK, 20);
  uint16_t dcol_wt = color_diff_hsv_bal(hsv, COLOR_HSV_CAL_WHITE, 20);
  uint16_t dcol_rd = color_diff_hsv_bal(hsv, COLOR_HSV_CAL_RED, 20);
  uint16_t dcol_bl = color_diff_hsv_bal(hsv, COLOR_HSV_CAL_BLUE, 20);
  uint16_t dcol_gn = color_diff_hsv_bal(hsv, COLOR_HSV_CAL_GREEN, 20);
  uint16_t dcol_ye = color_diff_hsv_bal(hsv, COLOR_HSV_CAL_YELLOW, 20);
  
  uint16_t dcol_min = min3(min(dcol_bk, dcol_wt), min(dcol_rd, dcol_bl), min(dcol_gn, dcol_ye));  
  
  if (dcol_min<900) {
    if (dcol_min==dcol_bk) {
      leds = 0x01;
    } else if (dcol_min==dcol_wt) {
      leds = 0x02;
    } else if (dcol_min==dcol_rd) {
      leds = 0x09;
    } else if (dcol_min==dcol_bl) {
      leds = 0x06;
    } else if (dcol_min==dcol_gn) {
      leds = 0x03;
    } else if (dcol_min==dcol_ye) {
      leds = 0x0c;
    }
  }
  
  set_led_byte(leds);

if (event==EVENT_KEY3) {
    usart_putchar(' ');
    print_color_rgb(rgb);
    print_color_hsv(hsv);
    //usart_putchar('\n');
    delay(100);
  }
}

//////////////////////////////////////////////////////////////////////// TESTPROG 0 - LED + KEY

void setup_test0() {
}

void loop_test0() {
  static uint16_t cnt=20;

uint8_t key=key_get_state();
  if (key) {
    show_key(key);
    cnt = 400;
  } else {
    switch (cnt++) {
      case 400: led_set(1, 0); led_set(2, 0); led_set(3, 0); led_set(4, 0);break;
      case 500: led_set(1, 1); break;
      case 600: led_set(1, 0); break;
      case 700: led_set(2, 1); break;
      case 800: led_set(2, 0); break;
      case 900: led_set(3, 1); break;
      case 1000: led_set(3, 0); break;
      case 1100: led_set(4, 1); break;
      case 1200: led_set(4, 0); break;
      case 1300:
  // ----------------------------------------------- Birger
      
   // ?????????????????????????????????????? UART Ausgabe erfolgt erst nach erstem Schleifendurchlauf   
      			print_hello(); 
		        usart_putchar('L');
				usart_putchar('\n');
				cnt=400; break;
    }
  }
  delay(10);
}

//////////////////////////////////////////////////////////////// TESTPROG 1 - Sensor
void setup_test1() {
  // Distanzsensoren Togglemode aktivieren (IR Leds zuschalten)
  analog_setExtToggleMode(ANALOG_FL,  1);
  analog_setExtToggleMode(ANALOG_FR,  1);
  analog_setExtToggleMode(ANALOG_FLL, 1);
  analog_setExtToggleMode(ANALOG_FRR, 1);
  blink_led(1, 4);							// LED 1 4x blinken lassen
}

void loop_test1() {
  uint8_t key=key_get_state();
  if (key && (key!=KEY_STATE_INVALID)) {	// Auch hier eine Warteschleife bis
    while (key_get_state()) {				// bis keine Taste mehr gedrückt ist
      delay(10);
    }
    progmode = progmode?0:1;				// Toggle Testmode 0->1 oder 1->0

// Entweder Bodensensor Bricks LEDs für Reflexionsmessung aktivieren..
    analog_setExtToggleMode(ANALOG_BL,  progmode?1:0);
    analog_setExtToggleMode(ANALOG_BC,  progmode?1:0);
    analog_setExtToggleMode(ANALOG_BR,  progmode?1:0);
    analog_setExtToggleMode(ANALOG_BCL, progmode?1:0);
 	// ..oder Frontsensor Bricks IR - LEDs für Reflexionsmessung aktivieren
    analog_setExtToggleMode(ANALOG_FL,  progmode?0:1);
    analog_setExtToggleMode(ANALOG_FR,  progmode?0:1);
    analog_setExtToggleMode(ANALOG_FLL, progmode?0:1);
    analog_setExtToggleMode(ANALOG_FRR, progmode?0:1);
    
    delay(100);
  }
  if (progmode) {
    // analog Differenzwert zwischen aktiver und passiver Messung
    // der Bodensensoren > 25, dann entsprechende LED ein
    uint8_t bl = analog_getValueExt(ANALOG_BL, 2)>25;
    uint8_t br = analog_getValueExt(ANALOG_BR, 2)>25;
    uint8_t bc = analog_getValueExt(ANALOG_BC, 2)>25;
    led_set(1, bl);
    led_set(2, bc);
    led_set(3, bc);
    led_set(4, br);
  } else {
    // und hier der Frontsensoren > 25, dann entsprechende LED ein
    uint8_t fl = analog_getValueExt(ANALOG_FL, 2)>25;
    uint8_t fr = analog_getValueExt(ANALOG_FR, 2)>25;
    uint8_t fll = analog_getValueExt(ANALOG_FLL, 2)>25;
    uint8_t frr = analog_getValueExt(ANALOG_FRR, 2)>25;
    led_set(1, fll);
    led_set(2, fl);
    led_set(3, fr);
    led_set(4, frr);
  }
}

///////////////////////////////////////////////// TESTPROG 2 - Motor + Odo
void setup_test2() {
  blink_led(2, 4);
}

void loop_test2() {
  //analog_wait_update();
  // Zustand der Odometrie Lichtschranke auf die LEDs ausgeben
  led_set(2, IO_ODO_PIN & _BV(IO_ODO_BIT_LEFT));
  led_set(3, IO_ODO_PIN & _BV(IO_ODO_BIT_RIGHT));
  led_set(1, !(IO_ODO_PIN & _BV(IO_ODO_BIT_LEFT)));
  led_set(4, !(IO_ODO_PIN & _BV(IO_ODO_BIT_RIGHT)));
 
  if (analog_has_update()) {
    static int8_t speed=0;
    uint8_t key=key_get_char();
    
    if (key) {
      // Hier gab es jetzt schon die Funktion key_get_char() ?! ;-)
      // Aber die Doku war schon geschrieben, und deshalb blieb dort
      // unerwähnt, dass die Motorengeschwindigkeit mit den Tastern 
      // in 2 Stufen vorwärts und rückwärts geschaltet werden kann
      if (key=='A') {
        if (speed<2) speed++;
      } else if (key=='B') {
        speed=0;
      } else if (key=='C') {
        if (speed>-2) speed--;
      }
  
      if (speed==-2) {
        motpwm_setLeft(-1000);
        motpwm_setRight(-1000);
      } else if (speed==-1) {
        motpwm_setLeft(-500);
        motpwm_setRight(-500);            
      } else if (speed==0) {
        motpwm_setLeft(0);
        motpwm_setRight(0);
      } else if (speed==1) {
        motpwm_setLeft(500);
        motpwm_setRight(500);    
      } else if (speed==2) {
        motpwm_setLeft(1000);
        motpwm_setRight(1000);
      }
    }
  }
  
}
// The End

Öffentliche Projekte

Welcome

Projektverwaltung

Nachrichten

Neuigkeiten

Site-Statistic

burger_first_explained/main.c [read only]

Werbung

Online