int main(void) {
sei(); // enable interrupts
leds_init();
pwm_init();
bot_init();
i2c_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);
uint8_t pos=0;
uint8_t state=0;
gfx_term_print("Move 0\n");
nds3_move(0);
while (nds3_get_busy()) {
delay(1);
}
while (1) {
gfx_draw_mode(GFX_DM_JAM2);
check_voltage();
//gfx_term_print("Measure 90, 15\n");
delay(1);
nds3_measure(180, 1);
while (nds3_get_busy()) {
delay(1);
}
delay(1);
//gfx_term_print("Read -90, 15\n");
do_plot();
delay(1);
nds3_measure(0, 1);
while (nds3_get_busy()) {
delay(1);
}
delay(1);
//gfx_term_print("Read 90, 15\n");
nds3_read(0, 15);
do_plot();
}
}
void native_nibo_graphicdisplay_invoke(u08_t mref) {
if(mref == NATIVE_METHOD_clear) {
gfx_fill(0x00); // clear LCD
} else if(mref == NATIVE_METHOD_gotoXY) {
u08_t y = stack_pop();
u08_t x = stack_pop();
gfx_move(x, y);
} else if(mref == NATIVE_METHOD_print) {
char * str = stack_pop_addr();
#ifdef NVM_USE_EEPROM
if(NVMFILE_ISSET(str)) {
char chr;
while((chr = nvmfile_read08(str++)))
gfx_print_char(chr, proportional);
} else
#endif
while(*str)
gfx_print_char(*str++, proportional);
} else if(mref == NATIVE_METHOD_setProportional) {
proportional=stack_pop()?1:0;
} else if(mref == NATIVE_METHOD_getProportional) {
nvm_int_t val = proportional;
stack_push(val);
} 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 showPoints(uint8_t points[], uint8_t cnt){
char output[21] = "";
gfx_fill(0);
//Anzeige der der einzelnen Schritten
sprintf(output, "test1: %3i", points[1]);
gfx_move(0, 0);
gfx_print_text(output);
}
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 showParameter(uint8_t status, uint8_t richtung, uint8_t winkel, uint8_t distance){
char output[21] = "";
gfx_fill(0);
//Anzeige der der einzelnen Schritten
sprintf(output, "Ticks l: %3i r: %3i", copro_ticks_l, copro_ticks_r);
gfx_move(0, 0);
gfx_print_text(output);
//Anzeige der Geschwindigkeit
sprintf(output, "Speed l: %3i r: %3i", copro_speed_l, copro_speed_r);
gfx_move(0, 10);
gfx_print_text(output);
//Anzeige der Sensoren 1 , 2, 3, 4, 5
sprintf(output, "%3i %3i %3i %3i %3i", copro_distance[0] / 256,
copro_distance[1] / 256, copro_distance[2] / 256,
copro_distance[3] / 256, copro_distance[4] / 256);
gfx_move(0, 20);
gfx_print_text(output);
//Anzeige des Status
sprintf(output, "status: %1i", status);
gfx_move(0, 30);
gfx_print_text(output);
//Anzeige der belegten richtungen
sprintf(output, "belegt: %3i", richtung);
gfx_move(0, 40);
gfx_print_text(output);
//Anzeige Winkel und Dinstanz NDS
sprintf(output, "Winkel :%3i NDS: %3i ", winkel, distance);
gfx_move(0, 50);
gfx_print_text(output);
delay(100);
}
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(" ");
}
}
void native_nibo_graphicdisplay_init(void) {
display_init();
display_wait(0x00);
display_write(0x00, 0x3f);
display_wait(0x00);
display_write(0x00, 0xc0);
display_wait(0x00);
display_write(0x00, 0x40);
display_wait(0x00);
display_write(0x00, 0xb8);
display_wait(0x01);
display_write(0x01, 0x3f);
display_wait(0x01);
display_write(0x01, 0xc0);
display_wait(0x01);
display_write(0x01, 0x40);
display_wait(0x01);
display_write(0x01, 0xb8);
gfx_fill(0x00);
gfx_move(40, 0);
gfx_print_text("nibo 1.0", 1);
gfx_move(5, 12);
gfx_print_text("(c) 2007 by nicai-systems", 1);
uint8_t prop = 0;
// gfx_move(15, 16);
// gfx_print_text(" !\"#$%&\\()*+,-./", prop);
gfx_move(15, 24);
gfx_print_text("0123456789:;<=>?", prop);
gfx_move(15, 32);
gfx_print_text("@ABCDEFGHIJKLMNO", prop);
gfx_move(15, 40);
gfx_print_text("PQRSTUVWXYZ[\\]^_", prop);
gfx_move(15, 48);
gfx_print_text("`abcdefghijklmno", prop);
gfx_move(15, 56);
gfx_print_text("pqrstuvwxyz{|}~", prop);
gfx_print_char(127, prop);
/*
for (int16_t i=0; i<64; ++i)
{
display_wait(0x02);
display_write(0x02, i);
//display_wait(0x03);
//display_write(0x03, i);
}
display_wait(0x00);
display_write(0x00, 0xb9);
for (int16_t i=0; i<64; ++i)
{
display_wait(0x02);
display_write(0x02, i);
//display_wait(0x03);
//display_write(0x03, i);
}
*/
}
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);
}
}
