asuro lernt geregelt fahren

[radbruch]

Hallo

Mit optimierter Odometrie klappts nun deutlich besser:

Taste 1: Strecke vorwärtes
Taste 2: Strecke rückwärts
Taste 3: Drehen
Taste 4: Endlos vor und drehen
Taste 5: Regelverstärkung klein
Taste 6: Regelverstärkung gross
(Taste 1 ist die rechte Taste)

Die StatusLED zeigt die Hell/Dunkel-Bereiche der Codierscheibe an:

Gelb - Hell links
Rot - Hell rechts
Aus - Dunkel links/rechts

Die FrontLED leuchtet, wenn eine aufsteigende Flanke erkannt wurde, bei einer erkannten absteigenden Flanke wird die FrontLED ausgeschaltet. Beide Seiten werden zusammen angezeigt. Sehr nett anzuschauen wenn man bei stehenden Motoren an den Rädern rumfummelt.

Ein kleiner, aber interessanter Bug in diesem Program ist die Tatsache, dass der asuro jetzt erst losfährt, wenn sich an einem Rad ein Bit ändert. Das muss natürlich noch beseitigt werden, aber zur Demo passt dieses Verhalten sehr gut:

#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.

Gruss

mic