void behaviour_obstacle_avoidance_start() { copro_ir_startMeasure(); for (uint8_t dir = 0; dir < 32; dir++) { distances[dir] = 32; } ori_opt = ori; distances[(32-1+ori_opt)%32] = 0; distances[(32+0+ori_opt)%32] = 0; distances[(32+1+ori_opt)%32] = 0; mode=MODE_STOP; abyss_counter=0; line_detect_counter_l=0; line_detect_counter_r=0; }
int main() { sei(); // enable interrupts bot_init(); spi_init(); display_init(); /* Displayansteuerung initialisieren */ gfx_init(); /* Grafikdisplay initialisieren */ leds_init(); floor_init(); gfx_move(62, 0); gfx_set_proportional(1); gfx_print_text("floor stop"); gfx_set_proportional(0); delay(50); copro_ir_startMeasure(); copro_setSpeedParameters(5, 6, 7); copro_update(); uint8_t go = 0; while (1) { sei(); _delay_ms(1); floor_update(); if (go==0) { if ((floor_relative[FLOOR_LEFT]>THRESHOLD) && (floor_relative[FLOOR_RIGHT]>THRESHOLD)) { // Boden links und rechts vorhanden -> losfahren! go=1; copro_setSpeed(40, 40); IO_LEDS_RED_PORT=0x00; IO_LEDS_GREEN_PORT=0x84; } } else { if ((floor_relative[FLOOR_LEFT]<THRESHOLD) || (floor_relative[FLOOR_RIGHT]<THRESHOLD)) { // Boden links oder rechts verloren -> sofort stoppen! go=0; copro_stopImmediate(); IO_LEDS_RED_PORT=0x84; IO_LEDS_GREEN_PORT=0x00; } } } return 0; }
void native_nibo_distsensor_invoke(u08_t mref) { // JAVA: void update() if(mref == NATIVE_METHOD_update) { copro_update_dist(); } // JAVA: int getSensor(int id) else if(mref == NATIVE_METHOD_getSensor) { nvm_int_t id = stack_pop_int(); if ((id<0) || (id>4)) error(ERROR_NATIVE_ILLEGAL_ARGUMENT); nvm_int_t val = copro_distance[id]/256; stack_push(val); } // JAVA: void setEnabled(boolean enabled) else if(mref == NATIVE_METHOD_setEnabled) { if (stack_pop_int()) { enabled=1; copro_ir_startMeasure(); } else { enabled=0; copro_ir_stop(); } } // JAVA: boolean getEnabled() else if(mref == NATIVE_METHOD_getEnabled) { stack_push(enabled); } // ERROR else { error(ERROR_NATIVE_UNKNOWN_METHOD); } }
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(" "); } }
uint8_t behaviour_obstacle_avoidance() { copro_ir_startMeasure(); update_distances(); dist_l = (dist_l<250)? (dist_l+5):255; dist_r = (dist_r<250)? (dist_r+5):255; dist_f = (dist_f>5)? (dist_f-5):0; dist_max_ls = (dist_l>dist_fl)?dist_l:dist_fl; dist_max_rs = (dist_r>dist_fr)?dist_r:dist_fr; dist_max_fs = (dist_fr>dist_fl)?dist_fr:dist_fl; if (dist_f>dist_max_fs) dist_max_fs = dist_f; uint8_t emergency = 0; int16_t speed_l = 0; int16_t speed_r = 0; int16_t speed = 0; uint8_t ledr = 0x00; uint8_t ledg = 0x00; uint16_t sum = dist_r + dist_fr + dist_f + dist_fl + dist_l; sum /= 5; uint8_t dmax = (sum<0xe0)?(sum+0x20):0xff; uint8_t dmin = (sum>0x20)?(sum-0x20):0x00; ledr=0; ledg=0; stress = (stress>10)?(stress-10):(0); stress += (copro_current_l>20)?(copro_current_l-5):0; stress += (copro_current_r>20)?(copro_current_r-5):0; uint8_t ori_corr = (32+ori-ori_opt)%32; //gfx_set_proportional(0); //gfx_move(100, 40); //print_hex(ori_corr); uint8_t proposed_mode = mode; static uint8_t candidate_mode = 0; static uint8_t candidate_cnt = 0; switch (mode) { case MODE_STOP: if (check_le(dist_l, 0, DIST_ALL)) proposed_mode = MODE_ROTATE_LEFT; if (check_le(dist_r, 0, DIST_ALL)) proposed_mode = MODE_ROTATE_RIGHT; if (check_le(dist_fl, 0, DIST_ALL)) proposed_mode = MODE_TURN_LEFT; if (check_le(dist_fr, 0, DIST_ALL)) proposed_mode = MODE_TURN_RIGHT; if (check_le(dist_f, 0, DIST_ALL)) proposed_mode = MODE_STRAIGHTON; break; case MODE_STRAIGHTON: if (check_le(dist_fl, 0, DIST_FL|DIST_F|DIST_FR)) proposed_mode = MODE_TURN_LEFT; if (check_le(dist_fr, 0, DIST_FL|DIST_F|DIST_FR)) proposed_mode = MODE_TURN_RIGHT; if (check_le(dist_f, -2, DIST_FL|DIST_F|DIST_FR)) proposed_mode = MODE_STRAIGHTON; if ((3<=ori_corr) && (ori_corr<=15)) proposed_mode = MODE_TURN_LEFT; if ((29>=ori_corr) && (ori_corr>=17)) proposed_mode = MODE_TURN_RIGHT; if (!check_le(dist_max_fs, -40, DIST_ALL)) { if (dist_max_rs>dist_max_ls) proposed_mode = MODE_ROTATE_LEFT; else if (dist_max_ls>dist_max_rs) proposed_mode = MODE_ROTATE_RIGHT; else proposed_mode = MODE_STOP; } if (dist_max_fs>0xa0) proposed_mode = MODE_BACK; break; case MODE_TURN_LEFT: if ((ori_corr>29) || (ori_corr<3)) proposed_mode = MODE_STRAIGHTON; if (check_le(dist_l, 0, DIST_L|DIST_FL|DIST_F)) proposed_mode = MODE_ROTATE_LEFT; if (check_le(dist_fl, -3, DIST_L|DIST_FL|DIST_F)) proposed_mode = MODE_TURN_LEFT; if (check_le(dist_f, 0, DIST_L|DIST_FL|DIST_F)) proposed_mode = MODE_STRAIGHTON; //if (!check_le(dist_max_fs, -40, DIST_ALL)) // proposed_mode = MODE_STOP; if (dist_max_fs>0xa0) proposed_mode = MODE_BACK; break; case MODE_TURN_RIGHT: if ((ori_corr>29) || (ori_corr<3)) proposed_mode = MODE_STRAIGHTON; if (check_le(dist_r, 0, DIST_R|DIST_FR|DIST_F)) proposed_mode = MODE_ROTATE_RIGHT; if (check_le(dist_fr, -3, DIST_R|DIST_FR|DIST_F)) proposed_mode = MODE_TURN_RIGHT; if (check_le(dist_f, 0, DIST_R|DIST_FR|DIST_F)) proposed_mode = MODE_STRAIGHTON; //if (!check_le(dist_max_fs, -40, DIST_ALL)) // proposed_mode = MODE_STOP; if (dist_max_fs>0xa0) proposed_mode = MODE_BACK; break; case MODE_ROTATE_LEFT: if (check_le(dist_l, 0, DIST_L|DIST_FL|DIST_F)) proposed_mode = MODE_ROTATE_LEFT; if (check_le(dist_fl, -1, DIST_L|DIST_FL|DIST_F)) proposed_mode = MODE_TURN_LEFT; if (check_le(dist_f, -1, DIST_L|DIST_FL|DIST_F)) proposed_mode = MODE_TURN_LEFT; if (!check_le(dist_max_ls, -40, DIST_ALL)) proposed_mode = MODE_BACK; break; case MODE_ROTATE_RIGHT: if (check_le(dist_r, 0, DIST_R|DIST_FR|DIST_F)) proposed_mode = MODE_ROTATE_RIGHT; if (check_le(dist_fr, -1, DIST_R|DIST_FR|DIST_F)) proposed_mode = MODE_TURN_RIGHT; if (check_le(dist_f, -1, DIST_R|DIST_FR|DIST_F)) proposed_mode = MODE_TURN_RIGHT; if (!check_le(dist_max_rs, -40, DIST_ALL)) proposed_mode = MODE_BACK; break; case MODE_BACK: proposed_mode = MODE_STOP; break; } if (mode_time<255) { mode_time++; if (mode_time>=minmodetime[mode]) { mode_time=255; } } if (mode_time==255) { if (mode!=proposed_mode) { if (proposed_mode==candidate_mode) { candidate_cnt++; if (candidate_cnt>5) { mode = proposed_mode; mode_time = 0; } } else { candidate_cnt = 0; candidate_mode = proposed_mode; } } } if (stress>200) { if (mode != MODE_BACK) { mode = MODE_BACK; distances[(32-3+ori)%32]=255; distances[(32-2+ori)%32]=255; distances[(32-1+ori)%32]=255; distances[(32+0+ori)%32]=255; distances[(32+1+ori)%32]=255; distances[(32+2+ori)%32]=255; distances[(32+3+ori)%32]=255; } else { mode = MODE_STOP; distances[(16-3+ori)%32]=255; distances[(16-2+ori)%32]=255; distances[(16-1+ori)%32]=255; distances[(16+0+ori)%32]=255; distances[(16+1+ori)%32]=255; distances[(16+2+ori)%32]=255; distances[(16+3+ori)%32]=255; } emergency = 1; stress = 0; } if (mode!=last_mode) { mode_cnt+=2; if (mode_cnt>80) { mode = MODE_BACK; emergency = 1; } last_mode=mode; } else { if (mode_cnt>0) { mode_cnt--; } } //gfx_move(100, 0); //print_hex(mode); uint8_t turn_factor; switch (mode) { case MODE_STOP: speed_l=0, speed_r=0, speed=0, ledg=0x00, ledr=0x84; turn_factor = 0; emergency = 1; break; case MODE_STRAIGHTON: headlight_target=10; // speed_l=100, speed_r=100, speed=50, ledg= 0x30; speed_l=100, speed_r=100, speed=80, ledg= 0x30; turn_factor = 60; break; case MODE_TURN_LEFT: headlight_target=10; // speed_l=70, speed_r=100, speed=40, ledg= 0x08; speed_l=70, speed_r=100, speed=60, ledg= 0x08; turn_factor = 60; break; case MODE_TURN_RIGHT: headlight_target=10; // speed_l=100, speed_r=70, speed=40, ledg= 0x40; speed_l=100, speed_r=70, speed=60, ledg= 0x40; turn_factor = 60; break; case MODE_ROTATE_LEFT: headlight_target=0; speed_l=-100, speed_r=100, speed=20, ledg= 0x04; turn_factor = 30; break; case MODE_ROTATE_RIGHT: headlight_target=0; speed_l=100, speed_r=-100, speed=20, ledg= 0x80; turn_factor = 30; break; case MODE_BACK: headlight_target=0; speed_l=-100, speed_r=-100, speed=20, ledg= 0x03, ledr=0x00; turn_factor = 20; break; } IO_LEDS_RED_PORT = ledr; IO_LEDS_GREEN_PORT = ledg; if (headlight_act<headlight_target) headlight_act++; if (headlight_act>headlight_target) headlight_act--; if (headlight_act) leds_set_headlights(1<<(headlight_act-1)); else leds_set_headlights(0); if (mode==MODE_STRAIGHTON) { int16_t lval = 2*dist_fl+dist_l; int16_t rval = 2*dist_fr+dist_r; int16_t delta = lval-rval; if (abs(delta)>5) { if (lval>rval) speed_r-=10; // rechts mehr Platz if (rval>lval) speed_l-=10; // links mehr Platz } else { if (lval>rval) speed_r-=5; // rechts mehr Platz if (rval>lval) speed_l-=5; // links mehr Platz } } else if (mode==MODE_BACK) { uint16_t lval = dist_fl+2*dist_l; uint16_t rval = dist_fr+2*dist_r; if (lval>rval) speed_r-=15; // rechts mehr Platz if (rval>lval) speed_l-=15; // links mehr Platz } else { uint16_t lval = dist_fl+dist_l; uint16_t rval = dist_fr+dist_r; if (lval>rval) speed_r-=5; // rechts mehr Platz if (rval>lval) speed_l-=5; // links mehr Platz } if (ori_corr == 0) { // BESTER WEG LIEGT GERADEAUS } else if (ori_corr < 16) { // BESTER WEG LIEGT LINKS // ori_corr [1 ... 8] speed_l -= (turn_factor * (uint16_t)ori_corr)/8; speed_r += (turn_factor * (uint16_t)ori_corr)/8; speed = (speed * (16 - ori_corr))/16; } else { // BESTER WEG LIEGT RECHTS // ori_corr [24 ...31] ori_corr = 32-ori_corr; speed_l += (turn_factor * (uint16_t)ori_corr)/8; speed_r -= (turn_factor * (uint16_t)ori_corr)/8; speed = (speed * (16 - ori_corr))/16; } speed_l = (speed_l*speed)/128; speed_r = (speed_r*speed)/128; #if 0 if (abyss_counter<500) { // 5 sec if ((acquisition_getFloorState(AN_FLOOR_L)==FLOOR_LINE) || (acquisition_getFloorState(AN_FLOOR_R)==FLOOR_LINE) || (acquisition_getFloorState(AN_LINE_L)==FLOOR_LINE) || (acquisition_getFloorState(AN_LINE_R)==FLOOR_LINE) ){ // still in area abyss_counter=0; } else { abyss_counter++; } } else { if ((acquisition_getDiff(AN_FLOOR_L)<0x02)||(acquisition_getDiff(AN_FLOOR_R)<0x02)) { speed_l=0; speed_r=0; emergency = 1; } // if ((acquisition_getFloorState(AN_FLOOR_R)!=FLOOR_ABYSS) || (acquisition_getFloorState(AN_FLOOR_L)!=FLOOR_ABYSS)) { // speed_l=0; // speed_r=0; // emergency = 1; // } if (line_detect_counter_l) { line_detect_counter_l--; } else if (acquisition_getFloorState(AN_FLOOR_L)!=FLOOR_ABYSS) { line_detect_counter_l = 300; if (line_detect_counter_r==0) { line_detect_pos_l = copro_ticks_l; line_detect_pos_r = copro_ticks_r; } } if (line_detect_counter_r) { line_detect_counter_r--; } else if (acquisition_getFloorState(AN_FLOOR_R)!=FLOOR_ABYSS) { line_detect_counter_r = 300; if (line_detect_counter_l==0) { line_detect_pos_l = copro_ticks_l; line_detect_pos_r = copro_ticks_r; } } if (line_detect_counter_l && line_detect_counter_r) { speed_l=0; speed_r=0; emergency = 1; line_detect_dir = line_detect_counter_l-line_detect_counter_r; line_detect_pos_l = copro_ticks_l-line_detect_pos_l; line_detect_pos_r = copro_ticks_r-line_detect_pos_r; return BEHAVIOUR_LINE; } } #endif 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; copro_stop(); } else { tspeed_l = (3*tspeed_l+speed_l)/4; tspeed_r = (3*tspeed_r+speed_r)/4; if ((speed_l>0)&&(speed_l<tspeed_l)) { tspeed_l=speed_l; } if ((speed_r>0)&&(speed_r<tspeed_r)) { tspeed_r=speed_r; } copro_setSpeed(tspeed_l, tspeed_r); } return BEHAVIOUR_OBSTACLE; }
int main() { sei(); // enable interrupts bot_init(); spi_init(); display_init(); /* Displayansteuerung initialisieren */ gfx_init(); /* Grafikdisplay initialisieren */ gfx_move(62, 0); gfx_set_proportional(1); gfx_print_text("motion"); gfx_set_proportional(0); gfx_move(5, 0); gfx_print_char('R'); gfx_move(118, 0); gfx_print_char('L'); delay(50); copro_ir_startMeasure(); copro_setSpeedParameters(5, 6, 7); int counter=0; while (1) { // IR Controller delay(10); char text[20]="RC5: ----"; if (copro_update()) { sprintf(text, "RC5: %04x", (int)copro_rc5_cmd); } gfx_move(35, 45); gfx_print_text(text); // Spannung bot_update(); float volt = 0.0166 * bot_supply - 1.19; sprintf(text, "%2.1fV ", (double)volt); gfx_move(25, 0); gfx_print_text(text); switch(++counter) { case 200: copro_setSpeed(20, 20); break; case 400: copro_stop(); break; case 600: copro_setSpeed(-20, -20); break; case 800: copro_stop(); counter=0; break; } } return 0; /* Fertig.... */ }