/*!
* Kuemmert sich um die Radencoder
* Das muss schneller gehen als die anderen Sensoren,
* daher Update per Timer-Interrupt und nicht per Polling
*/
void wheelencoder_isr(void){
// --------------------- links ----------------------------
//Rad-Encoder auswerten
uint8_t val = get_input_bit(IO_SENS_ENCL);
if (val != enc_l){ // uns interesieren nur Veraenderungen
enc_l=val; // neuen wert sichern
enc_l_cnt=0; // Counter zuruecksetzen
} else { // Zaehlen, wie lange Pegel bleibt
if (enc_l_cnt <= ENC_ENTPRELL) // Nur bis zur Entprell-Marke
enc_l_cnt++;
}
if (enc_l_cnt == ENC_ENTPRELL){// wenn lange genug konst
if (pwm_motor_l>0) // Drehrichtung beachten
inc_l=FORWARDS;
else if (pwm_motor_l<0)
inc_l=BACKWARDS;
if (inc_l!=BACKWARDS){
wheelencoder_l++;
pid_left_cnt++;
} else {
wheelencoder_l--;
pid_left_cnt--;
}
}
// --------------------- rechts ----------------------------
val = get_input_bit(IO_SENS_ENCR);
if (val != enc_r){ // uns interesieren nur Veraenderungen
enc_r=val; // neuen wert sichern
enc_r_cnt=0;
} else{ // Zaehlen, wie lange Pegel bleibt
if (enc_r_cnt <= ENC_ENTPRELL) // Nur bis zur Entprell-Marke
enc_r_cnt++;
}
if (enc_r_cnt == ENC_ENTPRELL){ // wenn lange genug konst
if (pwm_motor_r>0) // Drehrichtung beachten
inc_r=FORWARDS;
else if (pwm_motor_r<0)
inc_r=BACKWARDS;
if (inc_r!=BACKWARDS){
wheelencoder_r++;
pid_right_cnt++;
} else {
wheelencoder_r--;
pid_right_cnt--;
}
}
}
void schedule_behaviour() {
// called at 100 Hz / 10ms
if (behaviour == BEHAVIOUR_CALIB){
return;
}
if (!get_input_bit(IO_INPUT_1)) {
play_music();
}
copro_update();
update_ori();
uint8_t new_behaviour = behaviour;
switch (behaviour) {
case BEHAVIOUR_LINE: new_behaviour = behaviour_follow_line(); break;
case BEHAVIOUR_OBSTACLE: new_behaviour = behaviour_obstacle_avoidance(); break;
}
if (new_behaviour != behaviour) {
behaviour=new_behaviour;
switch (behaviour) {
case BEHAVIOUR_LINE: behaviour_follow_line_start(); break;
case BEHAVIOUR_OBSTACLE: behaviour_obstacle_avoidance_start(); break;
}
}
}
// the ctbot class
void native_ctbot_bot_invoke(u08_t mref) {
// JAVA: boolean getError()
if(mref == NATIVE_METHOD_getError) {
nvm_int_t val = get_input_bit(IO_SENS_ERROR)?1:0;
stack_push(val);
}
// JAVA: void setExtension1Enabled()
else if(mref == NATIVE_METHOD_setExtension1Enabled) {
if (stack_pop_int())
shift_reg_ena &= ~(uint8_t)ENA_ERW1;
else
shift_reg_ena |= ENA_ERW1;
shift_data(shift_reg_ena, SHIFT_REGISTER_ENA);
}
// JAVA: int getExtension1Enabled()
else if(mref == NATIVE_METHOD_getExtension1Enabled) {
nvm_int_t val = (shift_reg_ena & ENA_ERW1)?1:0;
stack_push(val);
}
// JAVA: int setExtension2Enabled()
else if(mref == NATIVE_METHOD_setExtension2Enabled) {
if (stack_pop_int())
shift_reg_ena &= ~(uint8_t)ENA_ERW2;
else
shift_reg_ena |= ENA_ERW2;
shift_data(shift_reg_ena, SHIFT_REGISTER_ENA);
}
// JAVA: int getExtension2Enabled()
else if(mref == NATIVE_METHOD_getExtension2Enabled) {
nvm_int_t val = (shift_reg_ena & ENA_ERW2)?1:0;
stack_push(val);
}
}
// the ctbot class
void native_ctbot_lightbarrier_invoke(u08_t mref) {
// JAVA: void getState()
if(mref == NATIVE_METHOD_getState) {
nvm_int_t val = get_input_bit(IO_SENS_SCHRANKE)?1:0;
stack_push(val);
}
// JAVA: void setEnabled()
else if(mref == NATIVE_METHOD_setEnabled) {
if (stack_pop_int())
shift_reg_ena &= ~(uint8_t)ENA_SCHRANKE;
else
shift_reg_ena |= ENA_SCHRANKE;
shift_data(shift_reg_ena, SHIFT_REGISTER_ENA);
}
// JAVA: boolean getEnabled()
else if(mref == NATIVE_METHOD_getEnabled) {
nvm_int_t val = (shift_reg_ena & ENA_SCHRANKE)?1:0;
stack_push(val);
}
}
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);
}
}
int main(int argc, char *argv[]) {
std::string ip_address = "";
int parse_ret = parse_command_line_options(argc, argv, ip_address);
if(parse_ret != 1)
return parse_ret;
if(ip_address.empty()) {
ip_address = DEFAULT_IP_ADDR;
}
print_intro();
modbus_t *mb;
mb = modbus_new_tcp(DEFAULT_IP_ADDR, 502);
if (mb == NULL) {
fprintf(stderr, "Unable to allocate libmodbus context\n");
return -1;
}
if (modbus_connect(mb) == -1) {
fprintf(stderr, "Connection failed: %s\n", modbus_strerror(errno));
modbus_free(mb);
return -1;
}
setup_interupt();
bool write_bool = true;
int write_num = 0;
unsigned int count = 0;
uint8_t outputs[16] = {0};
//variables for calculations
bool button_pressed = false;
try {
while (true) {
uint16_t input = 0;
if(read_inputs(mb, &input) == -1)
break;
button_pressed = get_input_bit(input, 9);
//solenoid = button xor photoeyen
bool sol_out = button_pressed ^ get_input_bit(input, 2);
outputs[0] = sol_out;
if(write_outputs(mb, outputs) == -1)
break;
print_io(input, outputs);
/* code for incrmenting IO
count++;
if(count%50==0){
outputs[write_num] = write_bool;
write_num++;
}
if(write_num==16){
write_bool = !write_bool;
write_num = 0;
}*/
usleep(1*1000); //Sleep 1ms
}
} catch(InterruptException& e) {
modbus_close(mb);
modbus_free(mb);
}
}
