/*! * Initialisiere alle Sensoren */ void wheelencoder_init(void){ deactivate_output_bit(IO_SENS_ENCL); deactivate_output_bit(IO_SENS_ENCR); wheelencoder_l = 0; wheelencoder_r = 0; inc_l = FORWARDS; inc_r = FORWARDS; pid_left_cnt = 0; pid_right_cnt = 0; enc_l_cnt = ENC_ENTPRELL+1; enc_r_cnt = ENC_ENTPRELL+1; }
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(); }
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); } }
void native_ctbot_bot_init(void) { deactivate_output_bit(IO_SENS_ERROR); }
void native_ctbot_lightbarrier_init(void) { deactivate_output_bit(IO_SENS_SCHRANKE); }