void schedule_behaviour() {
// called at 100 Hz / 10ms
if (behaviour == BEHAVIOUR_CALIB){
return;
}
if (!get_input_bit(IO_INPUT_1)) {
play_music();
}
copro_update();
update_ori();
uint8_t new_behaviour = behaviour;
switch (behaviour) {
case BEHAVIOUR_LINE: new_behaviour = behaviour_follow_line(); break;
case BEHAVIOUR_OBSTACLE: new_behaviour = behaviour_obstacle_avoidance(); break;
}
if (new_behaviour != behaviour) {
behaviour=new_behaviour;
switch (behaviour) {
case BEHAVIOUR_LINE: behaviour_follow_line_start(); break;
case BEHAVIOUR_OBSTACLE: behaviour_obstacle_avoidance_start(); break;
}
}
}
/**
* @brief sends the moved distance of nibo via xbee
*/
void drive_sendTicks(){
copro_update();
if(copro_ticks_l > 0){
niboCom_putDistance(copro_ticks_l);
}
copro_resetOdometry(0, 0);
}
/**
* 1 hinten-rechts
* 2 hinten-links
* 3 stelle rechts
* 4 stelle links
*/
uint8_t wenden(uint8_t richtung) {
static bool backdone = false;
//aktuallisierung des coprozessors
copro_update();
switch (richtung) {
case 1:
if (copro_ticks_l > wendeweg && copro_ticks_l > wendeweg / 2) {
copro_setSpeed(-20, -12);
} else {
return fahren;
}
;
break;
case 2:
if (copro_ticks_l > wendeweg / 2 && copro_ticks_l > wendeweg) {
copro_setSpeed(-12, -20);
} else {
return fahren;
}
break;
case 3:
if(copro_ticks_l > -50 && !backdone) {
copro_setSpeed(-20,-20);
}
else {
if(!backdone) {
copro_stop();
copro_resetOdometry(0, 0);
backdone = true;
}
if (copro_ticks_l < 35 && copro_ticks_r < 75) {
copro_setSpeed(12, -12);
} else {
copro_stop();
return 1;
}
}
break;
}
return rueckWende;
}
/*
* This function will drive forward by speed of 4 - 25 or backward by a speed of - 4 - (-25) and check
* wether objects are in the way of the
* nibo
*/
uint8_t drive(uint8_t speed) {
//aktuallisierung des coprozessors
copro_update();
if(copro_distance[0]/256 < irAbstand && copro_distance[1]/256 < irAbstand &&
copro_distance[2]/256 < irAbstand && copro_distance[3]/256 < irAbstand && copro_distance[4]/256 < 100 ) {
copro_setSpeed(speed,speed);
} else {
copro_stop();
return rueckWende;
}
return fahren;
}
/**
* @brief returns a non-blocked direction for the nibo
* @return drive_front_free(1) or drive_right_free(2) or
* drive_left_free(3) or drive_front_all_blocked(4).
*/
uint8_t drive_getFreePath(){
copro_update();
if(drive_isFrontFree()){
return drive_front_free;
}
else if(drive_isRightFree()){
return drive_right_free;
}
else if(drive_isLeftFree()){
return drive_left_free;
}
else{
return drive_all_blocked;
}
}
int main() {
sei(); // enable interrupts
bot_init();
spi_init();
display_init(); /* Displayansteuerung initialisieren */
gfx_init(); /* Grafikdisplay initialisieren */
leds_init();
floor_init();
gfx_move(62, 0);
gfx_set_proportional(1);
gfx_print_text("floor stop");
gfx_set_proportional(0);
delay(50);
copro_ir_startMeasure();
copro_setSpeedParameters(5, 6, 7);
copro_update();
uint8_t go = 0;
while (1) {
sei();
_delay_ms(1);
floor_update();
if (go==0) {
if ((floor_relative[FLOOR_LEFT]>THRESHOLD) && (floor_relative[FLOOR_RIGHT]>THRESHOLD)) {
// Boden links und rechts vorhanden -> losfahren!
go=1;
copro_setSpeed(40, 40);
IO_LEDS_RED_PORT=0x00;
IO_LEDS_GREEN_PORT=0x84;
}
} else {
if ((floor_relative[FLOOR_LEFT]<THRESHOLD) || (floor_relative[FLOOR_RIGHT]<THRESHOLD)) {
// Boden links oder rechts verloren -> sofort stoppen!
go=0;
copro_stopImmediate();
IO_LEDS_RED_PORT=0x84;
IO_LEDS_GREEN_PORT=0x00;
}
}
}
return 0;
}
/**
* @brief moves the nibo forwards if no obstacle blocks the path
* @param speed the speed the nibo moves in ticks
* @return drive_front_free(1) or drive_direction_blocked(0) if the path is blocked
*/
uint8_t drive(uint8_t speed){
uint8_t out = drive_front_free;
copro_update();
copro_resetOdometry(0, 0);
if(drive_isFrontFree()){
copro_setSpeed(speed,speed);
}else{
copro_stop();
out = drive_direction_blocked;
}
//transmit the moved distance and the ir sensor values
delay(100);
drive_sendTicks();
drive_sendIRDistance(0);
return out;
}
/**
* @brief scans the surrounding of the nibo with the nds3
* @param degree the angle resolution of the nds3 scan, defines the number of values returned -> (180/degree)+1
* @return the nds measurement
*/
uint8_t *ndsScan(uint8_t degree) {
uint8_t winkel = 0;
static uint8_t points[180] = {};
for(winkel = 0; winkel < 180; winkel = winkel + degree){
nds3_move(winkel);
//delay for the movement of the servo, more for the first move
if(winkel == 0){
delay(900);
}else {
delay(100);
}
//updating the coprozessor, to get the current value
copro_update();
points[winkel / degree] = nds3_get_dist();
}
nds3_move(90);
return points;
}
uint8_t checkDriveDirection() {
//coprozessors update
copro_update();
if(copro_distance[0]/256 < irAbstand && copro_distance[1]/256 < irAbstand &&
copro_distance[2]/256 < irAbstand && copro_distance[3]/256 < irAbstand && copro_distance[4]/256 < 100 )
{
return 3;
}
else {
//rechts && rechtsoben
if (copro_distance[0] / 256 < irAbstand && copro_distance[1] / 256 < irAbstand) {
return 1;
}
//links && linksoben
else if (copro_distance[4] / 256 < irAbstand && copro_distance[3] / 256 < irAbstand) {
return 2;
}
else {
return 0;
}
}
}
int main(void) {
sei(); // enable interrupts
leds_init();
pwm_init();
bot_init();
spi_init();
floor_init();
sound_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
sound_tone(127/4, 15000);
delay_ms(10);
sound_tone(191/4, 15000);
delay_ms(10);
sound_tone(255/4, 10000);
delay_ms(10);
sound_tone(255/4, 10000);
delay_ms(10);
sound_tone(191/4, 15000);
while (1)
{
delay(20);
// Spannung
bot_update();
char text[20];
float volt = 0.0160 * bot_supply;
sprintf(text, "%2.1fV ", (double)volt);
gfx_move(45, 0);
gfx_print_text(text);
volt_flt = 0.9*volt_flt+0.1*volt;
if (volt_flt<9.0) {
gfx_fill(0x00);
gfx_move(25, 20);
gfx_set_proportional(1);
gfx_print_text("Please recharge");
gfx_move(35, 30);
gfx_print_text("batteries!");
gfx_set_proportional(0);
while(1) {
leds_set_headlights(0);
clear_output_bit(IO_RESET_CO);
//motco_stop();
leds_set_displaylight(500);
IO_LEDS_RED_PORT = 0xaa;
IO_LEDS_GREEN_PORT = 0;
delay(500);
//motco_setSpeed(0, 0);
leds_set_displaylight(0);
IO_LEDS_RED_PORT = 0x55;
IO_LEDS_GREEN_PORT = 0;
delay(500);
}
}
// Analog values:
floor_disable_ir();
delay(1);
floor_update();
int16_t value_fl = -floor_l;
int16_t value_fr = -floor_r;
int16_t value_ll = -line_l;
int16_t value_lr = -line_r;
int16_t value_nfl = floor_l/16;
int16_t value_nfr = floor_r/16;
int16_t value_nll = line_l/16;
int16_t value_nlr = line_r/16;
floor_enable_ir();
delay(1);
floor_update();
int16_t value_pfl = floor_l/16;
int16_t value_pfr = floor_r/16;
int16_t value_pll = line_l/16;
int16_t value_plr = line_r/16;
value_fl += floor_l;
value_fr += floor_r;
value_ll += line_l;
value_lr += line_r;
//floor_disable_ir();
if (value_fl<0) value_fl=0;
if (value_fr<0) value_fr=0;
if (value_ll<0) value_ll=0;
if (value_lr<0) value_lr=0;
if (value_fl>255) value_fl=255;
if (value_fr>255) value_fr=255;
if (value_ll>255) value_ll=255;
if (value_lr>255) value_lr=255;
gfx_move(30, 10);
print_hex(value_nfr);
gfx_print_char(' ');
print_hex(value_nlr);
gfx_print_char(' ');
print_hex(value_nll);
gfx_print_char(' ');
print_hex(value_nfl);
gfx_move(30, 20);
print_hex(value_pfr);
gfx_print_char(' ');
print_hex(value_plr);
gfx_print_char(' ');
print_hex(value_pll);
gfx_print_char(' ');
print_hex(value_pfl);
gfx_move(30, 30);
print_hex(value_fr);
gfx_print_char(' ');
print_hex(value_lr);
gfx_print_char(' ');
print_hex(value_ll);
gfx_print_char(' ');
print_hex(value_fl);
// Distance
int16_t speed_l = 0;
int16_t speed_r = 0;
// Request distance data
if (!copro_update())
{
gfx_move(10, 10);
gfx_set_proportional(1);
gfx_print_text("IRCO Error ");
gfx_set_proportional(0);
continue;
}
gfx_move(76, 0);
gfx_set_proportional(1);
gfx_print_text("RC5: ");
gfx_set_proportional(0);
print_hex(HIBYTE(copro_rc5_cmd));
print_hex(LOBYTE(copro_rc5_cmd));
if (copro_rc5_cmd!=old_rc5_cmd) {
old_rc5_cmd = copro_rc5_cmd;
if ((HIBYTE(copro_rc5_cmd)&0x07)==0x02) {
switch(LOBYTE(copro_rc5_cmd)) {
//case 0xad: floor_calibrate(); break;
case 0xac: emergency = emergency?0:1;
}
}
}
uint8_t is_line_l = linear_trans(value_ll, 0x16, 0x02);
uint8_t is_line_r = linear_trans(value_lr, 0x16, 0x02);
uint8_t is_floor_l = linear_trans(value_ll, 0x10, 0x20);
uint8_t is_floor_r = linear_trans(value_lr, 0x10, 0x20);
uint8_t go_l = or_poss(is_line_r, and_poss(is_floor_l, 0x40));
uint8_t go_r = or_poss(is_line_l, and_poss(is_floor_r, 0x40));
if ((is_line_l>0x80) && (line_ori<+5)) line_ori++;
if ((is_line_r>0x80) && (line_ori>-5)) line_ori--;
if ((is_line_l>0x80) && (is_line_r>0x80)) line_ori=0;
if ((is_line_l<0x20) && (is_line_r<0x20)) {
//line was lost
if (line_ori>0) {
go_l = -20;
go_r = 160;
} else if (line_ori<0) {
go_l = 160;
go_r = -20;
}
}
uint8_t ledr = 0x00;
uint8_t ledg = 0x00;
if (is_line_l>0x80) ledg |= 0x10;
if (is_line_r>0x80) ledg |= 0x20;
if (is_floor_l>0x80) ledr |= 0x10;
if (is_floor_r>0x80) ledr |= 0x20;
IO_LEDS_RED_PORT = ledr;
IO_LEDS_GREEN_PORT = ledg;
speed_l = go_l/20;
speed_r = go_r/20;
if (((tspeed_l<0) && (speed_l>0)) ||
((tspeed_l>0) && (speed_l<0)) ||
((tspeed_r<0) && (speed_r>0)) ||
((tspeed_r>0) && (speed_r<0)) ||
emergency)
{
tspeed_l = 0;
tspeed_r = 0;
if (!copro_stop())
{
gfx_move(10, 10);
gfx_set_proportional(1);
gfx_print_text("MOTCO Error");
gfx_set_proportional(0);
continue;
}
}
else
{
tspeed_l = (3*tspeed_l+speed_l)/4;
tspeed_r = (3*tspeed_r+speed_r)/4;
if (!copro_setSpeed(tspeed_l, tspeed_r))
{
gfx_move(10, 10);
gfx_set_proportional(1);
gfx_print_text("MOTCO_Error");
gfx_set_proportional(0);
continue;
}
}
gfx_move(25, 40);
gfx_set_proportional(1);
gfx_print_text("r:");
gfx_set_proportional(0);
print_hex(HIBYTE(tspeed_r));
print_hex(LOBYTE(tspeed_r));
gfx_set_proportional(1);
gfx_print_text(" l:");
gfx_set_proportional(0);
print_hex(HIBYTE(tspeed_l));
print_hex(LOBYTE(tspeed_l));
delay(10);
if (!copro_update())
{
gfx_move(10, 10);
gfx_set_proportional(1);
gfx_print_text("MOTCO-Error");
gfx_set_proportional(0);
continue;
}
gfx_move(25, 50);
gfx_set_proportional(1);
gfx_print_text("r:");
gfx_set_proportional(0);
print_hex(HIBYTE(copro_speed_r));
print_hex(LOBYTE(copro_speed_r));
gfx_set_proportional(1);
gfx_print_text(" l:");
gfx_set_proportional(0);
print_hex(HIBYTE(copro_speed_l));
print_hex(LOBYTE(copro_speed_l));
//gfx_move(10, 10);
//gfx_print_text(" ");
}
}
/**
* @brief sends the IR-sensor values via xbee
* @param ms delay before collection IR-sensor values
*/
void drive_sendIRDistance(int ms){
delay(ms);
copro_resetOdometry(0, 0);
copro_update();
niboCom_putIRDistance(copro_distance);
}
int main()
{
sei(); // enable interrupts
bot_init();
spi_init();
display_init(); /* Displayansteuerung initialisieren */
gfx_init(); /* Grafikdisplay initialisieren */
gfx_move(62, 0);
gfx_set_proportional(1);
gfx_print_text("motion");
gfx_set_proportional(0);
gfx_move(5, 0);
gfx_print_char('R');
gfx_move(118, 0);
gfx_print_char('L');
delay(50);
copro_ir_startMeasure();
copro_setSpeedParameters(5, 6, 7);
int counter=0;
while (1)
{
// IR Controller
delay(10);
char text[20]="RC5: ----";
if (copro_update())
{
sprintf(text, "RC5: %04x", (int)copro_rc5_cmd);
}
gfx_move(35, 45);
gfx_print_text(text);
// Spannung
bot_update();
float volt = 0.0166 * bot_supply - 1.19;
sprintf(text, "%2.1fV ", (double)volt);
gfx_move(25, 0);
gfx_print_text(text);
switch(++counter)
{
case 200:
copro_setSpeed(20, 20);
break;
case 400:
copro_stop();
break;
case 600:
copro_setSpeed(-20, -20);
break;
case 800:
copro_stop();
counter=0;
break;
}
}
return 0; /* Fertig.... */
}
