Öffentliche Projekte

#include <nibobee/iodefs.h>
#include <nibobee/motpwm.h>
#include <nibobee/analog.h>
#include <nibobee/line.h>
#include <nibobee/led.h>
#include <nibobee/delay.h>

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

int main() {
  activate_output_group(IO_LEDS);  // LED bits als Output
  motpwm_init();
  motpwm_setLeft(0);
  motpwm_setRight(0);
  analog_init(); 
  line_readPersistent(); // Kalibrierung für Liniensensoren laden
  set_output_group(IO_SENS); // interne Pull-up Widerstände aktivieren
  activate_output_bit(IO_LINE_EN);

int16_t speed_flt_l=0;
  int16_t speed_flt_r=0;

// Countdown: LEDs blinken lassen
  for (uint8_t i=0; i<10; ++i) {
    led_set(LED_L_RD, 1);
    led_set(LED_R_RD, 1);
    _delay_ms(10);
    led_set(LED_L_RD, 0);
    led_set(LED_R_RD, 0);
    _delay_ms(990);
  }
  led_set(LED_L_YE, 1);
  led_set(LED_R_YE, 1);
  _delay_ms(1000);
  led_set(LED_L_YE, 0);
  led_set(LED_R_YE, 0);

// Hauptschleife:
  while(1) {
    sei();
    _delay_ms(1);
    int16_t speed_l=0;
    int16_t speed_r=0;

// Sensorwerte aktualisiseren
    int16_t lval = line_get(LINE_L);
    int16_t cval = line_get(LINE_C);
    int16_t rval = line_get(LINE_R);

if (lval+cval+rval < 20) {
      // kein Boden (alle Werte schwarz!)
      led_set(LED_L_RD, 0);
      led_set(LED_R_RD, 0);
      speed_r=0, speed_l=0;
    } else if ((lval<cval) && (lval<rval)) {
      // Linie ist links (der linke Sensor hat den kleinsten Wert)
      led_set(LED_L_RD, 1);
      led_set(LED_R_RD, 0);
      // Rechter Motor schnell, linken Motor langsamer, in Abhängigkeit 
      // vom Unterschied zum mittleren Sensor besonders stark abbremsen
      speed_r=550, speed_l=450-1*(cval-lval);
    } else if ((rval<cval) && (rval<lval)) {
      // Linie ist rechts (der rechte Sensor hat den kleinsten Wert)
      led_set(LED_L_RD, 0);
      led_set(LED_R_RD, 1);
      // Linker Motor schnell, rechter Motor langsamer, in Abhängigkeit 
      // vom Unterschied zum mittleren Sensor besonders stark abbremsen
      speed_r=450-1*(cval-rval), speed_l=550;
    } else {
      // Linie ist mittig (der mittlere Sensor hat den kleinsten Wert)
      led_set(LED_L_RD, 1);
      led_set(LED_R_RD, 1);
      // Beide Motoren sehr schnell, je nach Unterschied zum mittleren
      // Sensor noch etwas schneller bzw. langsamer
      speed_r=750 + 1*(rval-cval), speed_l=750 + 1*(lval-cval);
    }

// Soll-Geschwindigkeit filtern: 75% alter Wert, 25% neuer Wert
    // alt*75/100 + neu*25/100 = (3*alt+1*neu)/4
    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;

// Soll Werte für Motoren setzen
    motpwm_setLeft(speed_flt_l);
    motpwm_setRight(speed_flt_r);

}
  return 0;
}

Öffentliche Projekte

Welcome

Projektverwaltung

Nachrichten

Neuigkeiten

Site-Statistic

nibobee_linienfolge/main.c [read only]

Werbung

Online