Code:
#include <asuro.h>
unsigned char sw_data, sw0, sw1, sw2, sw3, i, tmp_char, step;
unsigned int data[2], j, tmp_int, loops, delay;
unsigned long akt, loop_count, loop_delay;
unsigned int speed_l, speed_r, power_l, power_r, count_l, count_r, imp_l, imp_r, p, p_l, p_r;
unsigned int speed_soll_l, speed_soll_r, speed_count_l, speed_count_r, speed_max_l, speed_max_r;
unsigned int odo_l, odo_old_l, odo_min_l, odo_max_l, odo_low_l, odo_high_l;
unsigned int odo_minmax_bit_l, odo_bit_l, o1_l, o2_l, o3_l;
unsigned int odo_r, odo_old_r, odo_min_r, odo_max_r, odo_low_r, odo_high_r;
unsigned int odo_minmax_bit_r, odo_bit_r, o1_r, o2_r, o3_r;
char getswitch(void) {
sw3=sw2; sw2=sw1; sw1=sw0; sw0=PollSwitch();
if ((sw0==sw1) && (sw0==sw2) && (sw0==sw3)) sw_data=sw0; else sw_data=0;
return(sw_data);
}
void count1(void){
OdometrieData(data); akt=loop_count;
if ((!odo_minmax_bit_l) && (data[0]<o1_l) && (o1_l<o2_l) && (o2_l<o3_l)) {
odo_minmax_bit_l=(1==1);
odo_max_l=data[0];
FrontLED(ON);
}
if ((odo_minmax_bit_l) && (data[0]>o1_l) && (o1_l>o2_l) && (o2_l>o3_l)) {
odo_minmax_bit_l=(1==0);
odo_min_l=data[0];
FrontLED(OFF);
}
if ((!odo_minmax_bit_r) && (data[1]<o1_r) && (o1_r<o2_r) && (o2_r<o3_r)) {
odo_minmax_bit_r=(1==1);
odo_max_r=data[1];
FrontLED(OFF);
}
if ((odo_minmax_bit_r) && (data[1]>o1_r) && (o1_r>o2_r) && (o2_r>o3_r)) {
odo_minmax_bit_r=(1==0);
odo_min_r=data[1];
FrontLED(ON);
}
o3_l=o2_l; o2_l=o1_l; o1_l=data[0];
o3_r=o2_r; o2_r=o1_r; o1_r=data[1];
odo_low_l=((odo_min_l + odo_max_l)/3); // je ein drittel high-nix-low
odo_high_l=((odo_min_l + odo_max_l)/3)*2;
odo_low_r=((odo_min_r + odo_max_r)/3); // je ein drittel high-nix-low
odo_high_r=((odo_min_r + odo_max_r)/3)*2;
if (!odo_bit_l & (data[0] > odo_high_l)) {
odo_bit_l=1; count_l ++; StatusLED(YELLOW);
speed_l=akt-speed_count_l; speed_count_l=akt;
}
if (odo_bit_l & (data[0] < odo_low_l)) {
odo_bit_l=0; count_l ++; StatusLED(OFF);
speed_l=akt-speed_count_l; speed_count_l=akt;
}
if (!odo_bit_r & (data[1] > odo_high_r)) {
odo_bit_r=1; count_r ++; StatusLED(RED);
speed_r=akt-speed_count_r; speed_count_r=akt;
}
if (odo_bit_r & (data[1] < odo_low_r)) {
odo_bit_r=0; count_r ++; StatusLED(OFF);
speed_r=akt-speed_count_r; speed_count_r=akt;
}
}
int drive(int weg_l, int weg_r, int v_l, int v_r) {
static char dstep=0;
switch (dstep) {
case(0): if ((weg_l==weg_r) && (v_l==v_r)) dstep=10; else dstep=10; break;
case(10): count_l=count_r=0; power_l=power_r=50; p_l=p_r=p; speed_soll_l=speed_soll_r=5; dstep++; break;
case(11): if (speed_l>speed_soll_l) power_l+=p_l; if (speed_l<speed_soll_l) power_l-=p_l;
if (speed_r>speed_soll_r) power_r+=p_r; if (speed_r<speed_soll_r) power_r-=p_r;
if (count_l>=1) p_l=3; if (count_r>=1) p_r=3;
if (count_l+count_r<=5) {power_l=count_r-count_l; power_r=count_l-count_r;}
//if (count_l>=50) speed_max_l=speed_l; if (count_r>=50) speed_max_r=speed_r;
dstep++; break;
case(12): MotorSpeed(power_l,power_r); if (count_l<weg_l) dstep--; else dstep++; break;
case(13): power_l-=p; power_r-=p; MotorSpeed(power_l,power_r); if (power_l<=50) dstep++; break;
case(14): MotorSpeed(0,0); dstep=0; break;
}
return(dstep>0);
}
int main(void) {
Init();
SerWrite("\nHallo\nBatterie: ",17); PrintInt(Batterie());
loop_count=0; loop_delay=0; loops=0; step=99; delay=1000;
OdometrieData(data);
odo_l=data[0]; odo_old_l=odo_l; odo_min_l=200; odo_max_l=500;
odo_r=data[1]; odo_old_r=odo_r; odo_min_r=200; odo_max_r=500;
speed_max_l=speed_max_r=100;
p=5;
do {
loop_count ++; sw_data=getswitch();
switch (sw_data) {
case(32): p=10; break;
case(16): p=5; break;
case (8): MotorDir(FWD,FWD); step=20; break;
case (4): MotorDir(FWD,RWD); step=10; break;
case (2): MotorDir(RWD,RWD); step=10; break;
case (1): MotorDir(FWD,FWD); step=10; break;
}
if (loop_count > loop_delay) switch (step) {
case(10): if (drive(300,300,5,5)) break; step=99; break;
case(11): if (drive(200,200,10,10)) break; step=99; break;
case(12): if (drive(50,50,15,15)) break; step=99; break;
case(20): if (drive(100,100,10,10)) break; MotorDir(RWD,FWD); step++; break;
case(21): if (drive(20,20,15,15)) break; MotorDir(FWD,FWD); step--; break;
case(99): break;
}
count1();
}while (1);
return 0;
}
Den Code muss man natürlich noch putzen, aber den Ansatz finde ich nett.
Lesezeichen