Öffentliche Projekte

#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;
}

Öffentliche Projekte

Welcome

Projektverwaltung

Nachrichten

Neuigkeiten

Site-Statistic

nibo2_fuzzylinie2/main.c [read only]

Werbung

Online