Projekte
workwind
NIBO2 - Fuzzy Linienfolge
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
NIBO2 - F...nienfolge
fuzzy.c
fuzzy.h
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
7426 private projects
385 public projects
16180353 lines compiled
58212 builds
NIBO
@
nibo2_fuzzylinie/main.c [read only]
#include "nibo/niboconfig.h" #include "nibo/iodefs.h" #include "nibo/delay.h" #include "nibo/display.h" #include "nibo/gfx.h" #include "nibo/spi.h" #include "nibo/copro.h" #include "nibo/adc.h" #include "nibo/floor.h" #include "nibo/leds.h" #include "nibo/pwm.h" #include "nibo/bot.h" #include "nibo/sound.h" #include "fuzzy.h" #include <avr/interrupt.h> #include <avr/pgmspace.h> #include <stdio.h> #define LOBYTE(x) (uint8_t)((uint16_t)x) #define HIBYTE(x) (uint8_t)(((uint16_t)x)>>8) #define MAKE_WORD(hi,lo) ((hi*0x100)+lo) 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; gfx_print_char(c1); gfx_print_char(c2); } int main() { sei(); // enable interrupts leds_init(); pwm_init(); bot_init(); spi_init(); floor_init(); if (display_init()!=0) { leds_set_displaylight(50); if (display_type==2) { gfx_init(); } } gfx_fill(0x00); gfx_move(15, 0); gfx_set_proportional(1); gfx_print_text("nibo"); gfx_set_proportional(0); copro_ir_startMeasure(); delay(10); //motco_setSpeedParameters(5, 4, 6); // ki, kp, kd copro_setSpeedParameters(15, 20, 10); // ki, kp, kd int16_t speed_flt_l=0; int16_t speed_flt_r=0; uint8_t last_r=0; uint8_t last_l=0; while(1) { sei(); _delay_ms(1); int16_t speed_l=0; int16_t speed_r=0; floor_update(); char text[20]="-- -- -- -- --"; // Werte der Bodensensoren aktualisiseren und ausgeben floor_update(); sprintf(text, "%03x %03x %03x %03x", (uint16_t)(floor_relative[FLOOR_RIGHT]), (uint16_t)(floor_relative[LINE_RIGHT]), (uint16_t)(floor_relative[LINE_LEFT]), (uint16_t)(floor_relative[FLOOR_LEFT])); gfx_move(16, 20); gfx_print_text(text); uint8_t line_l0 = FUZZYFY_HL(0x20, 0xb0, floor_relative[LINE_LEFT]); uint8_t line_r0 = FUZZYFY_HL(0x20, 0xb0, floor_relative[LINE_RIGHT]); uint8_t line_l1 = FUZZYFY_HL(0x20, 0xb0, floor_relative[FLOOR_LEFT]); uint8_t line_r1 = FUZZYFY_HL(0x20, 0xb0, floor_relative[FLOOR_RIGHT]); sprintf(text, "%02x %02x %02x %02x", (uint16_t)line_r1, (uint16_t)line_r0, (uint16_t)line_l0, (uint16_t)line_l1); gfx_move(22, 30); gfx_print_text(text); uint8_t go_all = FUZZY_OR(FUZZY_NOT(line_l1), FUZZY_NOT(line_r1)); uint8_t go_l_0 = FUZZY_AND(line_r1, FUZZY_NOT(line_l1)); uint8_t go_r_0 = FUZZY_AND(line_l1, FUZZY_NOT(line_r1)); uint8_t go_l_1 = line_r0; uint8_t go_r_1 = line_l0; last_l = FUZZY_AND(FUZZY_NOT(line_r1), FUZZY_OR(last_l, line_l1)); last_r = FUZZY_AND(FUZZY_NOT(line_l1), FUZZY_OR(last_r, line_r1)); uint8_t go_l = FUZZY_MEAN4(go_all, last_r, go_l_0, go_l_1); uint8_t go_r = FUZZY_MEAN4(go_all, last_l, go_r_0, go_r_1); sprintf(text, "%02x %02x %02x %02x", (uint16_t)go_r, (uint16_t)last_r, (uint16_t)last_l, (uint16_t)go_l); gfx_move(22, 40); gfx_print_text(text); // Defuzzifizieren: speed_l = (800/8)*(int16_t)go_l/32-200; speed_r = (800/8)*(int16_t)go_r/32-200; int16_t sum = speed_l+speed_r; int16_t boost = (sum<600)?((600-sum)/2):0; if (speed_l<0) speed_l=0; else speed_l+=boost; if (speed_r<0) speed_r=0; else speed_r+=boost; // Geschwindigkeit filtern: 75% alte Geschwindigkeit + 25% neue Geschwindigkeit speed_flt_l*=3; speed_flt_l+=speed_l; speed_flt_l/=4; speed_flt_r*=3; speed_flt_r+=speed_r; speed_flt_r/=4; if (!copro_setPWM(speed_flt_l, speed_flt_r)) { gfx_move(10, 10); gfx_set_proportional(1); gfx_print_text("COPRO_Error"); gfx_set_proportional(0); continue; } gfx_move(25, 50); gfx_set_proportional(1); gfx_print_text("r:"); gfx_set_proportional(0); print_hex(HIBYTE(speed_flt_r)); print_hex(LOBYTE(speed_flt_r)); gfx_set_proportional(1); gfx_print_text(" l:"); gfx_set_proportional(0); print_hex(HIBYTE(speed_flt_l)); print_hex(LOBYTE(speed_flt_l)); delay(2); if (!copro_update()) { gfx_move(10, 10); gfx_set_proportional(1); gfx_print_text("COPRO-Error"); gfx_set_proportional(0); continue; } /* gfx_move(25, 50); gfx_set_proportional(1); gfx_print_text("r:"); gfx_set_proportional(0); print_hex(HIBYTE(copro_speed_r)); print_hex(LOBYTE(copro_speed_r)); gfx_set_proportional(1); gfx_print_text(" l:"); gfx_set_proportional(0); print_hex(HIBYTE(copro_speed_l)); print_hex(LOBYTE(copro_speed_l)); */ } return 0; }
Compiler results:
Werbung
Online
adriannelabonte7
agrohimxdv
Juclinc
lavondax4578413909105
tresaautry51023212776
warrenhinder6456