/**
* 1 hinten-rechts
* 2 hinten-links
* 3 stelle rechts
* 4 stelle links
*/
uint8_t wenden(uint8_t richtung) {
static bool backdone = false;
//aktuallisierung des coprozessors
copro_update();
switch (richtung) {
case 1:
if (copro_ticks_l > wendeweg && copro_ticks_l > wendeweg / 2) {
copro_setSpeed(-20, -12);
} else {
return fahren;
}
;
break;
case 2:
if (copro_ticks_l > wendeweg / 2 && copro_ticks_l > wendeweg) {
copro_setSpeed(-12, -20);
} else {
return fahren;
}
break;
case 3:
if(copro_ticks_l > -50 && !backdone) {
copro_setSpeed(-20,-20);
}
else {
if(!backdone) {
copro_stop();
copro_resetOdometry(0, 0);
backdone = true;
}
if (copro_ticks_l < 35 && copro_ticks_r < 75) {
copro_setSpeed(12, -12);
} else {
copro_stop();
return 1;
}
}
break;
}
return rueckWende;
}
/*
* This function will drive forward by speed of 4 - 25 or backward by a speed of - 4 - (-25) and check
* wether objects are in the way of the
* nibo
*/
uint8_t drive(uint8_t speed) {
//aktuallisierung des coprozessors
copro_update();
if(copro_distance[0]/256 < irAbstand && copro_distance[1]/256 < irAbstand &&
copro_distance[2]/256 < irAbstand && copro_distance[3]/256 < irAbstand && copro_distance[4]/256 < 100 ) {
copro_setSpeed(speed,speed);
} else {
copro_stop();
return rueckWende;
}
return fahren;
}
/**
* @brief moves the nibo forwards if no obstacle blocks the path
* @param speed the speed the nibo moves in ticks
* @return drive_front_free(1) or drive_direction_blocked(0) if the path is blocked
*/
uint8_t drive(uint8_t speed){
uint8_t out = drive_front_free;
copro_update();
copro_resetOdometry(0, 0);
if(drive_isFrontFree()){
copro_setSpeed(speed,speed);
}else{
copro_stop();
out = drive_direction_blocked;
}
//transmit the moved distance and the ir sensor values
delay(100);
drive_sendTicks();
drive_sendIRDistance(0);
return out;
}
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.... */
}
