Programmcode für die Linienfolge aus dem Video:
Code:
#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();
set_output_group(IO_SENS); // Pull-ups 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;
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) {
led_set(LED_L_RD, 0);
led_set(LED_R_RD, 0);
speed_r=0, speed_l=0;
} else if ((lval<cval) && (lval<rval)) {
// lval is minimum
led_set(LED_L_RD, 1);
led_set(LED_R_RD, 0);
speed_r=550, speed_l=450-1*(cval-lval);
} else if ((rval<cval) && (rval<lval)) {
// rval is minimum
led_set(LED_L_RD, 0);
led_set(LED_R_RD, 1);
speed_r=450-1*(cval-rval), speed_l=550;
} else {
// cval is minimum
led_set(LED_L_RD, 1);
led_set(LED_R_RD, 1);
speed_r=750 + 1*(rval-cval), speed_l=750 + 1*(lval-cval);
}
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;
motpwm_setLeft(speed_flt_l);
motpwm_setRight(speed_flt_r);
}
return 0;
}
Zitieren
Lesezeichen