/*!
* 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);
}
