Projekte
BirgerT
burger9600bd
main.c
Projekte
Forum
Doku
Öffentliche Projekte
Startseite
Beispielprogramme
Projekte von anderen
Welcome
Username
Passwort
Eingeloggt bleiben
Zugangsdaten vergessen?
Registrieren
Projektverwaltung
⇨ Please choose! ⇦
——————————————————
✎ Create new project...
★ Browse existing projects...
——————————————————
⚬ MotorTest#1
⚬ C Tutorial 8#1
⚬ NIBO2 C Project#1
⚙ C Tutorial 15#1
⚬ 2010_11_18_el_test001#1
burger9600bd
main.c
Project details
Compiler settings
Nachrichten
Sie sind nicht eingeloggt.
Neuigkeiten
★
NiboRoboLib 3.6
2017-01-17: Neue Version 3.6
★
NiboRoboLib 3.4.1
2016-04-16: Neue Version 3.4.1
★
Coding Tutorial
2015-11-22: Jetzt auch für den NIBO burger!
Site-Statistic
7433 private projects
378 public projects
16180353 lines compiled
58212 builds
NIBO
@
burger9600bd/main.c [read only]
/* Programm zum ersten Testen des NIBObee Roboters */ #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> #include <avr/io.h> #include <avr/interrupt.h> #include <util/delay.h> //#define SPEED 500 #define SPEED 1023 #ifndef min #define min(a,b) \ ({ __typeof__ (a) _a = (a); \ __typeof__ (b) _b = (b); \ _a < _b ? _a : _b; }) #endif #define min3(a,b,c) min(min(a,b),c) uint8_t progmode = 0; enum { EVENT_NONE = 0, EVENT_KEY1 = 1, EVENT_KEY2 = 2, EVENT_KEY3 = 3, EVENT_TIMEOUT = 4, 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 }; uint8_t cops1[8] = { 0x10, 0x20, 0x40, 0x80, 0x00, 0x00, 0x00, 0x00 }; uint8_t cops2[8] = { 0x30, 0x30, 0x30, 0xc0, 0xc0, 0xc0, 0x00, 0x00 }; void setup_test0(); void setup_test1(); void setup_test2(); void setup_test3(); void loop_test0(); void loop_test1(); void loop_test2(); void loop_test3(); uint8_t key_getEvent(); 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); } void blink_led(uint8_t led, uint8_t count) { while (count--) { led_set(led, 1); delay(80); led_set(led, 0); delay(120); } } void blink_led_fast(uint8_t led, uint8_t count) { while (count--) { led_set(led, 1); delay(40); led_set(led, 0); delay(60); } } void set_led_byte(uint8_t byte) { IO_LEDS_PORT = (IO_LEDS_PORT & ~IO_LEDS_MASK) | ((byte << IO_LEDS_BIT_1) & IO_LEDS_MASK); } 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); } 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); } /* OBSTACLE */ 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; } /* KEY */ uint8_t key_getEvent() { static uint8_t key = 0; uint8_t act = key_get_state(); if (act==0) { act = key; key = 0; if (act==0x01) return EVENT_KEY1; if (act==0x02) return EVENT_KEY2; if (act==0x04) return EVENT_KEY3; return EVENT_NONE; } if (act!=KEY_STATE_INVALID) { key |= act; } return EVENT_NONE; } uint8_t getEvent() { uint8_t event = EVENT_NONE; event = key_getEvent(); if (event) return event; if (progmode==2) { event = obstacle_getEvent(); } else if (progmode==1) { event = follow_getEvent(); } return event; } 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; } 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); } 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); } void print_decimal2(uint8_t val) { uint8_t t=0; while (val>9) { t++; val-=10; } usart_putchar('0'+t); usart_putchar('0'+val); } 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); } 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); } void print_text (char * text) { char c; while ((c=*text++)) { while (usart_txfull()) { /* wait */ } usart_putchar(c); } } 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; } 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(); 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('.'); } int main() { activate_output_group(IO_LEDS); // LED bits als Output ///////////////////////////////////////////////////////////////////////////////////// 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(); motpwm_init(); motpid_init(); odometry_init(); //motpwm_setLeft(0); //motpwm_setRight(0); analog_init(); sei(); delay(100); // ????????????????????? diese Ausgabe kommt nicht print_hello(); usart_putchar('S'); usart_putchar('\n'); delay(100); delay(100); delay(100); delay(100); delay(100); print_hello(); usart_putchar('D'); usart_putchar('\n'); surface_init(); for (uint8_t i = 0; i<8; i++) { analog_setExtToggleMode(i, 0); } delay(100); uint8_t key = key_get_state(); if (key) { while(key_get_state()) { show_key(key); } show_key(0); } if (key==0x01) { setup_test1(); while(1) { loop_test1(); } } if (key==0x02) { setup_test2(); while(1) { loop_test2(); } } if (key==0x04) { setup_calibrate(); while(1) { loop_calibrate(); } } 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() { analog_setExtToggleMode(ANALOG_FL, 1); analog_setExtToggleMode(ANALOG_FR, 1); analog_setExtToggleMode(ANALOG_FLL, 1); analog_setExtToggleMode(ANALOG_FRR, 1); blink_led(1, 4); } void loop_test1() { uint8_t key=key_get_state(); if (key && (key!=KEY_STATE_INVALID)) { while (key_get_state()) { delay(10); } progmode = progmode?0:1; 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); 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) { 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 { 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(); 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) { 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); } } } }
Compiler results:
Werbung
Online
danischilling2152
Marinavuc
warrenhinder6456