Nach langer Zeit hab ich den nibobee wieder rausgekramt und ein wenig programmiert. Nachfolgendes Programm bewegt die Biene vorsichtig entlang der Dielenritzen. Ist sie blockiert oder stösst sie auf Hindernisse, so wendet sie. Start erfolgt durch Fühler.
Code:
/* Die Biene bewegt sich vorsichtig entlang der Dielenritzen.
Stößt sie an, wendet sie.
Fährt sie sich fest, versucht sie sich zu befreien */
#include <nibobee/iodefs.h>
#include <nibobee/sens.h>
#include <nibobee/motpwm.h>
#include <nibobee/led.h>
#include <nibobee/line.h>
#include <nibobee/delay.h>
#include <nibobee/odometry.h>
#include <avr/interrupt.h>
int odo_l = 0; /* Variablen für die Odometriezählwerte */
int odo_l_merk = 0;
int odo_r = 0;
int odo_r_merk = 0;
int loop = 0; /* Variable, die bei jedem Programmdurchlauf erhöht wird */
int motorblockt = 0; /* Variable, die von ISR gesetzt wird, wenn Motor steht */
/* Funktionen zur Steuerung der Fahrt
fahre(1) - im Uhrzeigersinn,
fahre(-1) - gegen die Uhr,
fahre(0) - rückwärts,
rote Lampe leuchtet auf der Kurven- Innen- Seite */
int fahre(int8_t richtung) {
switch(richtung) {
case 1: {
motpwm_setLeft(400); /* Kreis im Uhrzeigersinn */
motpwm_setRight(150);
led_set(LED_R_RD, 1);
led_set(LED_L_RD, 0);
break; }
case -1: { /* Kreis gegen die Uhr */
motpwm_setLeft(150);
motpwm_setRight(400);
led_set(LED_L_RD, 1);
led_set(LED_R_RD, 0);
break; }
case 0: { /* volle Kraft zurück */
motpwm_setLeft(-1024);
motpwm_setRight(-1024);
led_set(LED_L_RD, 1);
led_set(LED_R_RD, 1);
delay(500);
motpwm_setLeft(1000); /* und ein bischen drehen */
motpwm_setRight(-1000);
led_set(LED_L_RD, 0);
led_set(LED_R_RD, 0);
delay(300);
loop = 0; /* setze Schleifenzähler zurück */
break; }
}
return 0;
}
/* diese Routine wird beim Timerinterrupt abgearbeitet */
ISR(TIMER0_OVF_vect)
{
static uint8_t count=0;
count++;
loop ++;
if(count > 57)
{
count=0;
if ((odo_l == odo_l_merk) || (odo_r == odo_r_merk)) motorblockt = 1; /* Motor steht */
odo_l_merk = odo_l; /* merke den odo- Stand*/
odo_r_merk = odo_r;
}
}
/* Hauptprogramm */
int main() {
motpwm_init();
sens_init();
odometry_init();
led_init();
line_init();
/* warte, bis ein Fühler betätigt wird */
while((sens_getLeft()==0) && (sens_getRight()==0));
TCCR0 = (1<<CS02) | (1<<CS00); /* Zeitgeber initialisieren, Danke mic */
TIMSK |= (1<<TOIE0);
enable_interrupts();
/* jetzt gehts los! */
while(1==1) { /* Endlosschleife */
if (sens_getLeft() != 0) { /* linker Fühler */
fahre(0); /* bin angestoßen, also zurück */
}
if (sens_getRight() != 0) { /* rechter Fühler */
fahre(0); /* bin angestoßen, also zurück */
}
if (motorblockt == 1) /* Motor steht */
{
fahre(0); /* fahre rückwärts */
motorblockt = 0;
}
odo_l = odometry_getLeft(0); /* Odometrie einlesen */
odo_r = odometry_getRight(0);
/* bleib auf der Dielenritze */
if (line_get(LINE_L) > line_get(LINE_R)) fahre(1); else fahre(-1);
/* sehr lange nicht angestoßen, fahr einfach mal zurück */
if (loop > 2280) fahre(0);
}
return 0;
}
Lesezeichen