dann poste ich auch mal einen für den NIBObee^^
Code:
#include <nibobee/iodefs.h>
#include <nibobee/motpwm.h>
#include <nibobee/delay.h>
#include <nibobee/sens.h>
enum {
MODE_STOP,
MODE_DRIVE,
MODE_BACK,
MODE_STEER_R,
MODE_STEER_L,
MODE_AVOID_R,
MODE_AVOID_L
};
int16_t speed_l;
int16_t speed_r;
uint16_t counter_ms;
uint8_t perform_check(uint8_t mode);
uint8_t do_stop();
uint8_t do_drive();
uint8_t do_back();
uint8_t do_steer_r();
uint8_t do_steer_l();
uint8_t do_avoid_r();
uint8_t do_avoid_l();
int main() {
motpwm_init();
sens_init();
uint8_t mode;
while(1==1) {
enable_interrupts();
delay(1);
mode = perform_check(mode);
switch (mode) {
case MODE_STOP: mode = do_stop(); break;
case MODE_DRIVE: mode = do_drive(); break;
case MODE_BACK: mode = do_back(); break;
case MODE_STEER_R: mode = do_steer_r(); break;
case MODE_STEER_L: mode = do_steer_l(); break;
case MODE_AVOID_R: mode = do_avoid_r(); break;
case MODE_AVOID_L: mode = do_avoid_l(); break;
}
switch (mode) {
case MODE_STOP: speed_l = 0; speed_r = 0;
break;
case MODE_DRIVE: speed_l = 500; speed_r = 500;
break;
case MODE_BACK: speed_l = -500; speed_r = -500;
break;
case MODE_STEER_R: speed_l = 600; speed_r = 400;
break;
case MODE_STEER_L: speed_l = 400; speed_r = 600;
break;
case MODE_AVOID_R: speed_l = -400; speed_r = -600;
break;
case MODE_AVOID_L: speed_l = -600; speed_r = -400;
break;
}
motpwm_setLeft(speed_l);
motpwm_setRight(speed_r);
}
return 0;
}
uint8_t perform_check(uint8_t mode) {
if (sens_getLeft() && sens_getRight()) {
if ((sens_getLeft()==-1) && (sens_getRight()==-1)) {
mode = MODE_BACK;
} else {
mode = MODE_STOP;
}
}
return mode;
}
uint8_t do_stop() {
if ((sens_getLeft()==0) && (sens_getRight()==0)) {
return MODE_DRIVE;
}
return MODE_STOP;
}
uint8_t do_back() {
if (sens_getLeft()==0) {
return MODE_AVOID_L;
}
if (sens_getRight()==0) {
return MODE_AVOID_R;
}
return MODE_BACK;
}
uint8_t do_drive() {
if (sens_getRight()==1) {
return MODE_STEER_L;
}
if (sens_getLeft()==1) {
return MODE_STEER_R;
}
if (sens_getRight()==-1) {
return MODE_AVOID_L;
}
if (sens_getLeft()==-1) {
return MODE_AVOID_R;
}
return MODE_DRIVE;
}
uint8_t do_steer_r() {
if (sens_getLeft()==0) {
return MODE_DRIVE;
}
return MODE_STEER_R;
}
uint8_t do_steer_l() {
if (sens_getRight()==0) {
return MODE_DRIVE;
}
return MODE_STEER_L;
}
uint8_t do_avoid_r() {
if (counter_ms==0) {
counter_ms=1000;
} else {
counter_ms--;
}
if (counter_ms) {
return MODE_AVOID_R;
} else {
return MODE_DRIVE;
}
}
uint8_t do_avoid_l() {
if (counter_ms==0) {
counter_ms=1000;
} else {
counter_ms--;
}
if (counter_ms) {
return MODE_AVOID_L;
} else {
return MODE_DRIVE;
}
}
Lesezeichen