void play_music() { cli(); SPCR = 0; set_output_bit(IO_DISPLAY_RST); activate_output_bit(IO_DISPLAY_RST); clear_output_bit(IO_RESET_CO); activate_output_bit(IO_AUDIO); make_sound(127/4, 15000); delay_ms(10); make_sound(191/4, 15000); delay_ms(10); make_sound(255/4, 10000); delay_ms(10); make_sound(255/4, 10000); delay_ms(10); make_sound(191/4, 15000); delay_ms(1); sei(); bot_init(); deactivate_output_bit(IO_DISPLAY_RST); clear_output_bit(IO_DISPLAY_RST); spi_init(); }
// the ctbot class void native_nibo_edgedetector_invoke(u08_t mref) { // JAVA: void update() if(mref == NATIVE_METHOD_update) { activate_output_bit(IO_LINE_FLOOR_EN); clear_output_bit(IO_LINE_FLOOR_EN); delay_us(25); edge_value_off_l = 1023-adc_read(AN_FLOOR_L); edge_value_off_r = 1023-adc_read(AN_FLOOR_R); edge_value_off_r += 1023-adc_read(AN_FLOOR_R); edge_value_off_l += 1023-adc_read(AN_FLOOR_L); set_output_bit(IO_LINE_FLOOR_EN); delay_us(25); edge_value_on_l = 1023-adc_read(AN_FLOOR_L); edge_value_on_r = 1023-adc_read(AN_FLOOR_R); edge_value_on_r += 1023-adc_read(AN_FLOOR_R); edge_value_on_l += 1023-adc_read(AN_FLOOR_L); clear_output_bit(IO_LINE_FLOOR_EN); } // JAVA: int getLeftRel() else if(mref == NATIVE_METHOD_getLeftRel) { nvm_int_t val = edge_value_on_l-edge_value_off_l; stack_push(val); } // JAVA: int getRightRel() else if(mref == NATIVE_METHOD_getRightRel) { nvm_int_t val = edge_value_on_r-edge_value_off_r; stack_push(val); } // JAVA: int getLeftAbs() else if(mref == NATIVE_METHOD_getLeftAbs) { nvm_int_t val = edge_value_on_l; stack_push(val); } // JAVA: int getRightAbs() else if(mref == NATIVE_METHOD_getRightAbs) { nvm_int_t val = edge_value_on_r; stack_push(val); } // ERROR else error(ERROR_NATIVE_UNKNOWN_METHOD); }
void check_voltage() { bot_update(); char text[20]; static float volt_flt = 10.0; float volt = 0.0160 * bot_supply; // filter volt_flt = 0.9*volt_flt+0.1*volt; if (volt_flt<8.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); } } }
void bot_init(void) { // Reset controllers #ifdef _NIBO_2_ activate_output_bit(IO_RESET_CO); clear_output_bit(IO_RESET_CO); delay(10); set_output_bit(IO_RESET_CO); delay(100); #else activate_output_bit(IO_RESET_5); activate_output_bit(IO_RESET_3); clear_output_bit(IO_RESET_5); clear_output_bit(IO_RESET_3); delay(10); set_output_bit(IO_RESET_5); set_output_bit(IO_RESET_3); delay(100); #endif }
void native_nibo_bot_init(void) { adc_init(AN_VBAT); // Reset controllers activate_output_bit(IO_RESET_CO); clear_output_bit(IO_RESET_CO); delay(10); set_output_bit(IO_RESET_CO); // Initialize communication spi_init(); delay(10); }
void acquisition_setup() { ADCSRA = _BV(ADPS2) // prescale faktor = 128 ADC laeuft | _BV(ADPS1) // mit 16 MHz / 128 = 125 kHz | _BV(ADPS0) | _BV(ADEN) // ADC an | _BV(ADFR) // free running | _BV(ADIE) // enable interrupt | _BV(ADSC); // Beginne mit der Konvertierung adc_init(0); adc_init(1); adc_init(2); adc_init(3); adc_init(7); clear_output_bit(IO_LINE_FLOOR_EN); activate_output_bit(IO_LINE_FLOOR_EN); }
void show_recharge_screen() { gfx_fill(0x00); print_prop_text(25, 20, "Please recharge"); print_prop_text(35, 30, "batteries!"); gfx_set_proportional(0); while (1) { leds_set_headlights(0); clear_output_bit(IO_RESET_CO); leds_set_displaylight(500); IO_LEDS_RED_PORT = 0xaa; IO_LEDS_GREEN_PORT = 0; delay(500); leds_set_displaylight(0); IO_LEDS_RED_PORT = 0x55; IO_LEDS_GREEN_PORT = 0; delay(500); } }
void make_sound(int interval, int time) { while (time) { int i=interval; clear_output_bit(IO_AUDIO); while (time && i) { time--; i--; delay_us(10); } i=interval; set_output_bit(IO_AUDIO); while (time && i) { time--; i--; delay_us(10); } } }
/** * Spielt einen Ton ab * Vor der ersten Benutzung einmal sound_init() aufrufen (deklariert in nibo/sound.h) * frequency ist die Frequenz des Tons in Hz (nicht hoeher als 5000 uebergeben!) * duration gibt die Wiedergabedauer des Tons an */ int tone(int frequency, int duration) { int interval = (50000 / frequency); duration = duration * 100; while (duration) { int i = interval; clear_output_bit(IO_AUDIO); while (duration && i) { duration--; i--; delay_us(10); } i = interval; set_output_bit(IO_AUDIO); while (duration && i) { duration--; i--; delay_us(10); } } return 1; }
void native_nibo_edgedetector_init(void) { clear_output_bit(IO_LINE_FLOOR_EN); activate_output_bit(IO_LINE_FLOOR_EN); adc_init(AN_FLOOR_L); adc_init(AN_FLOOR_R); }
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(" "); } }
int main(void) { sei(); // enable interrupts leds_init(); pwm_init(); bot_init(); spi_init(); acquisition_setup(); xbee_set_baudrate(9600); xbee_enable(); if (display_init()!=0) { leds_set_displaylight(1024); if (display_type==2) { gfx_init(); } } eeprom_read_block (acquisition_calibration, persistant.calibration, sizeof(acquisition_calibration)); gfx_fill(0x00); print_prop_text(15, 0, "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 stress = 0; //Setup Jumper... deactivate_output_bit(IO_INPUT_2); activate_output_bit(IO_INPUT_3); set_output_bit(IO_INPUT_2); clear_output_bit(IO_INPUT_3); if (!get_input_bit(IO_INPUT_1)) { behaviour = BEHAVIOUR_CALIB; acquisition_calibration[0]=0x20; acquisition_calibration[1]=0x20; acquisition_calibration[2]=0x20; acquisition_calibration[3]=0x20; print_prop_text(21, 30, "CALIBRATION"); delay(100); // Floor uint16_t values[4]; values[0]=0; values[1]=0; values[2]=0; values[3]=0; for (uint8_t i = 0; i<5; i++) { for (uint8_t j = 0; j<10; j++) { if (j>2) { values[0] += acquisition_getValue(0); values[1] += acquisition_getValue(1); values[2] += acquisition_getValue(2); values[3] += acquisition_getValue(3); } gfx_set_proportional(0); gfx_draw_mode(GFX_DM_JAM2); gfx_move(31, 41); print_hex(acquisition_getValue(AN_FLOOR_R)); gfx_print_char(' '); print_hex(acquisition_getValue(AN_LINE_R)); gfx_print_char(' '); print_hex(acquisition_getValue(AN_LINE_L)); gfx_print_char(' '); print_hex(acquisition_getValue(AN_FLOOR_L)); delay(100); } gfx_move(81+4*i, 30); gfx_print_char('.'); } acquisition_calibration[0] = values[0]/71; acquisition_calibration[1] = values[1]/71; acquisition_calibration[2] = values[2]/71; acquisition_calibration[3] = values[3]/71; eeprom_write_block (acquisition_calibration, persistant.calibration, sizeof(acquisition_calibration)); delay(100); gfx_print_text(" OK"); IO_LEDS_GREEN_PORT = 0xff; while (1) { // Floor gfx_set_proportional(0); gfx_draw_mode(GFX_DM_JAM2); gfx_move(31, 49); print_hex(acquisition_getValue(AN_FLOOR_R)); gfx_print_char(' '); print_hex(acquisition_getValue(AN_LINE_R)); gfx_print_char(' '); print_hex(acquisition_getValue(AN_LINE_L)); gfx_print_char(' '); print_hex(acquisition_getValue(AN_FLOOR_L)); delay(10); } } while (1) { //while (!uart0_rxempty() && !uart0_txfull()) { // uart0_putchar(uart0_getchar()); //} gfx_draw_mode(GFX_DM_JAM2); //* if (xbee_tx_idle()) { transmit_update1(); } //delay(50); //*/ delay(10); // Spannung //bot_update(); char text[20]; float volt = 0.0160 * acquisition_getValue(AN_VBAT); 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<8.0) { show_recharge_screen(); } // Ori gfx_move(0, 25); sprintf(text, "or:%3i ", (int)ori_deg); gfx_print_text(text); sprintf(text, "op:%2i/%2i ", (int)ori_opt, (int)ori); gfx_print_text(text); sprintf(text, "m:%1i", (int)mode); gfx_print_text(text); // Floor gfx_move(31, 49); print_hex(acquisition_getDiff(AN_FLOOR_R)); gfx_print_char(' '); print_hex(acquisition_getDiff(AN_LINE_R)); gfx_print_char(' '); print_hex(acquisition_getDiff(AN_LINE_L)); gfx_print_char(' '); print_hex(acquisition_getDiff(AN_FLOOR_L)); // Distance gfx_move(4, 57); gfx_print_text("R "); print_hex(dist_r); gfx_print_char(' '); print_hex(dist_fr); gfx_print_char(' '); print_hex(dist_f); gfx_print_char(' '); print_hex(dist_fl); gfx_print_char(' '); print_hex(dist_l); gfx_print_text(" L"); //////////////////////////// print_hex16(0, 33, copro_ticks_r); print_hex16(105, 33, copro_ticks_l); print_hex16(0, 41, copro_speed_r); print_hex16(105, 41, copro_speed_l); print_hex16(0, 49, copro_current_r); print_hex16(105, 49, copro_current_l); } }