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();
}
void leds_init(void) {
#ifdef _NIBO_2_
activate_output_bit(IO_LED_RG);
set_output_bit(IO_LED_RG);
#endif
activate_output_bit(IO_LED_WHITE);
activate_output_bit(IO_DISP_LIGHT);
IO_LEDS_RED_DDR = IO_LEDS_RED_MASK;
IO_LEDS_GREEN_DDR = IO_LEDS_GREEN_MASK;
}
void line_init() {
if (!(nibobee_initialization & NIBOBEE_ANALOG_INITIALIZED)) {
analog_init();
}
line_readPersistent();
activate_output_bit(IO_LINE_EN);
}
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 analog_init() {
nibobee_initialization |= NIBOBEE_ANALOG_INITIALIZED;
ADCSRA = _BV(ADPS2) // prescale faktor = 128 ADC laeuft
| _BV(ADPS1) // mit 15 MHz / 128 = 120 kHz
| _BV(ADPS0)
| _BV(ADEN) // ADC an
| _BV(ADATE) // auto trigger
| _BV(ADIE) // enable interrupt
| _BV(ADSC); // Beginne mit der Konvertierung
activate_output_bit(IO_LINE_EN);
}
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);
}
// 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 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 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();
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_nibo_leds_init(void) {
activate_output_bit(IO_LED_WHITE);
activate_output_bit(IO_DISP_LIGHT);
IO_LEDS_RED_DDR = IO_LEDS_RED_MASK;
IO_LEDS_GREEN_DDR = IO_LEDS_GREEN_MASK;
}
void sound_init() {
activate_output_bit(IO_AUDIO);
}
