Projekte
workwind
NIBO2 - Fuzzy Linienfolge2
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...ienfolge2
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
7433 private projects
378 public projects
16179466 lines compiled
58199 builds
NIBO
@
nibo2_fuzzylinie2/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 "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) #define LINE_POS_MAX 1000 #define LINE_POS_SUB 100 #define LINE_POS_LOST 100 #define LINE_POS_BONUS 100 #define LINE_LOST_INC 3 // Fuzzy variables: uint8_t lost_left, line_left, line_center, line_right, lost_right; uint8_t last_lowest_line; // value of the last lowest line value uint8_t last_lowest_sens; // which sensor gives us last_lowest_line int16_t line_pos; uint8_t line_lost; int16_t speed_l; int16_t speed_r; void clip_floor_relative() { for (uint8_t i=0; i<4; ++i) { if (floor_relative[i]<0) floor_relative[i]=0; if (floor_relative[i]>0xff) floor_relative[i]=0xff; } } void updateLineVariables() { if (last_lowest_line<0x80) { last_lowest_line++; // forget historic values } for (uint8_t i=0; i<4; ++i) { if (floor_relative[i]<last_lowest_line) { last_lowest_line = floor_relative[i]; last_lowest_sens = i; } } uint8_t line_mean = floor_relative[LINE_RIGHT]/2+floor_relative[LINE_LEFT]/2; if (line_mean < 0xd0) { line_lost = 0; if (line_pos<-LINE_POS_SUB) { line_pos += LINE_POS_SUB; } else if (line_pos>LINE_POS_SUB) { line_pos -= LINE_POS_SUB; } else { line_pos = 0; } // left is negative, right is positive line_pos += floor_relative[LINE_LEFT] - floor_relative[LINE_RIGHT]; if ((floor_relative[LINE_LEFT]<0x20) && (line_pos>0)) { line_pos -= LINE_POS_BONUS; } if ((floor_relative[LINE_RIGHT]<0x20)&& (line_pos<0)) { line_pos += LINE_POS_BONUS; } } else { // line seems to be lost.... switch (last_lowest_sens) { case FLOOR_LEFT: case LINE_LEFT: line_pos -= LINE_POS_LOST; break; case FLOOR_RIGHT: case LINE_RIGHT: line_pos += LINE_POS_LOST; break; } if (line_lost<(255-LINE_LOST_INC)) { line_lost += LINE_LOST_INC; } else { line_lost = 255; } } if (line_pos<-LINE_POS_MAX) { line_pos = -LINE_POS_MAX; } if (line_pos>LINE_POS_MAX) { line_pos = LINE_POS_MAX; } } void calculateFuzzyVars() { line_left = FUZZYFY_HL( -500, 200, line_pos); line_center = FUZZYFY_LHL( -600, -100, 100, 600, line_pos); line_right = FUZZYFY_LH( -200, 500 , line_pos); lost_left = FUZZY_AND(line_left, line_lost); lost_right = FUZZY_AND(line_right, line_lost); line_left = FUZZY_AND(line_left, FUZZY_NOT(line_lost)); line_center = FUZZY_AND(line_center, FUZZY_NOT(line_lost)); line_right = FUZZY_AND(line_right, FUZZY_NOT(line_lost)); } void calculateSpeed() { speed_l = 0; speed_r = 0; //-------------- speed_l -= 8*(uint16_t)lost_left; speed_r -= 2*(uint16_t)lost_left; speed_l += 3*(uint16_t)line_left; speed_r += 9*(uint16_t)line_left; speed_l += 6*(uint16_t)line_center; speed_r += 6*(uint16_t)line_center; speed_l += 9*(uint16_t)line_right; speed_r += 3*(uint16_t)line_right; speed_l -= 2*(uint16_t)lost_right; speed_r -= 8*(uint16_t)lost_right; //-------------- speed_l /= 256; speed_r /= 256; } 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); floor_update(); char text[20]="-- -- -- -- --"; // Werte der Bodensensoren aktualisiseren und ausgeben floor_update(); clip_floor_relative(); sprintf(text, "%02x %02x %02x %02x", (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(22, 20); gfx_print_text(text); updateLineVariables(); sprintf(text, "p: %5i ll: %3i", line_pos, line_lost); gfx_move(10, 30); gfx_print_text(text); calculateFuzzyVars(); sprintf(text, "%02x %02x %02x %02x %02x", (uint16_t)lost_right, (uint16_t)line_right, (uint16_t)line_center, (uint16_t)line_left, (uint16_t)lost_left); gfx_move(22, 40); gfx_print_text(text); calculateSpeed(); if (!copro_setSpeed(speed_l, speed_r)) { gfx_move(10, 10); gfx_set_proportional(1); gfx_print_text("COPRO_Error"); gfx_set_proportional(0); continue; } sprintf(text, "r: %2i l:%2i ", (uint16_t)speed_r, (uint16_t)speed_l); gfx_move(20, 50); gfx_print_text(text); delay(2); if (!copro_update()) { gfx_move(10, 10); gfx_set_proportional(1); gfx_print_text("COPRO-Error"); gfx_set_proportional(0); continue; } } return 0; }
Compiler results:
Werbung
Online
christig679944810001
lavinatompson081925
LucaBeinlich
NAnakaka
valariebumgarner
warrenhinder6456