asuro lernt geregelt fahren

**radbruch** · 16.01.2007, 00:32

Hallo

nachdem ich nun 4 Wochen mit meinem "Baby" verbracht habe, kenne ich einige seiner Macken. Aber immer noch erliege ich seinem Scharm. *grins*

Oki, was ich euch eigentlich zeigen wollte: Mir ist es nun endlich gelungen die Räder meines asuros in der Geschwindigkeit zu regeln. Ein Grund die Korken knallen zu lassen, denn der Weg bis hierher war mehr als nur steinig und steil..

Code:

/* Einfache Geschwindigkeitsregelung beider Motoren (Einfache Impulszahl mit 4er-Scheibe)

   Über den den zeitlichen Abstand des Hell/Dunkel-Wechsels der ODO-Scheibe wird die Geschwindigkeit
   ermittelt und geregelt. Jedes Rad für sich ohne Beeinflussung der anderen Seite.

   15.1.2007
   mic

	Funktion der Tasten:

	T6:                      mehr Speed
	T5: Fährt endlos vor mit mittlerer Speed
	T4:                      Weniger Speed

	T3:                       grosser Geschwindigkeit
	T2: kurze Strecke vor mit mittlerer Geschwindigkeit
	T1:                       kleiner Geschwindigkeit

	Anhalten nach T5 über T1/T2/T3

	Wenn die Motoren stehen werden die ODO-Daten mit der StatusLED 
	angezeigt. 

	Die Segmentwechsel der ODO-Scheiben werden mit der StatusLED angezeigt. 
 
*/


#include <asuro.h>

unsigned char sw_data, sw0, sw1, sw2, sw3, i, tmp_char, step, power;
unsigned int data[2], j, tmp_int, loops, strecke, 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;
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_bit_l, o1_l, o2_l, o3_l;
unsigned int odo_r, odo_old_r, odo_min_r, odo_max_r, odo_bit_r, o1_r, o2_r, o3_r;
unsigned int odo_impbit_l, odo_impbit_r, odo_implevel_l, odo_implevel_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 count(void) {

    OdometrieData(data); akt=loop_count;
    if ((data[0]<o1_l) && (o1_l<o2_l) && (o2_l<o3_l)) {
        if (!odo_bit_l) {
            count_l ++; odo_bit_l=(1==1); StatusLED(YELLOW);
            odo_max_l=data[0];
            imp_l ++; odo_implevel_l=(odo_max_l-odo_min_l)/4;
            speed_l=akt-speed_count_l; speed_count_l=akt;
            //if (speed_l<speed_max_l) speed_max_l=speed_l;
        }
        if (odo_impbit_l && (data[0] < odo_implevel_l)) {
            imp_l ++; odo_impbit_l=(1==0);
        }
    }
    if ((data[0]>o1_l) && (o1_l>o2_l) && (o2_l>o3_l)) {
        if (odo_bit_l) {
            count_l ++; odo_bit_l=(1==0); StatusLED(OFF);
            odo_min_l=data[0];
            imp_l ++; odo_implevel_l=((odo_max_l-odo_min_l)/4)*3;
            speed_l=akt-speed_count_l; speed_count_l=akt;
            //if (speed_l<speed_max_l) speed_max_l=speed_l;
        }
        if (!odo_impbit_l && (data[0] > odo_implevel_l)) {
            imp_l ++;  odo_impbit_l=(1==1);
        }
    }

    if ((data[1]<o1_r) && (o1_r<o2_r) && (o2_r<o3_r)) {
        if (!odo_bit_r) {
            count_r ++; odo_bit_r=(1==1); StatusLED(RED);
            odo_max_r=data[1];
            imp_r ++; odo_implevel_r=(odo_max_r-odo_min_r)/4;
            speed_r=akt-speed_count_r; speed_count_r=akt;
            //if (speed_r<speed_max_r) speed_max_r=speed_r;
        }
        if (odo_impbit_r && (data[1] < odo_implevel_r)) {
            imp_r ++; odo_impbit_r=(1==0);
        }
    }
    if ((data[1]>o1_r) && (o1_r>o2_r) && (o2_r>o3_r)) {
        if (odo_bit_r) {
            count_r ++; odo_bit_r=(1==0); StatusLED(OFF);
            odo_min_r=data[1];
            imp_r ++; odo_implevel_r=((odo_max_r-odo_min_r)/4)*3;
            speed_r=akt-speed_count_r; speed_count_r=akt;
            //if (speed_r<speed_max_r) speed_max_r=speed_r;
        }
        if (!odo_impbit_r && (data[1] > odo_implevel_r)) {
            imp_r ++;  odo_impbit_r=(1==1);
        }
    }

    o3_l=o2_l; o2_l=o1_l; o1_l=data[0];
    o3_r=o2_r; o2_r=o1_r; o1_r=data[1];
}

int main(void) {
Init();
SerWrite("\nHallo\nBatterie: ",17);
PrintInt(Batterie());
loop_count=0; loop_delay=0; loops=0; step=0; delay=1000; strecke=80; power=125;

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;

power=1;

do {
    loop_count ++; sw_data=getswitch();

    switch (sw_data) {
    case(32): speed_soll_l=speed_soll_r+=1; step=20; break;
    case(16): speed_soll_l=speed_soll_r=15; power_l=power_r=50; step=20; break;
    case (8): speed_soll_l=speed_soll_r-=1; step=20; break;
    case (4): speed_soll_l=speed_soll_r=5; power_l=power_r=50; count_l=count_r=0; step=10; break;
    case (2): speed_soll_l=speed_soll_r=10; power_l=power_r=50; count_l=count_r=0; step=10; break;
    case (1): speed_soll_l=speed_soll_r=15; power_l=power_r=50; count_l=count_r=0; step=10; break;
    }

    if (loop_count > loop_delay) switch (step) {

    case(10): if (speed_l>speed_soll_l) power_l+=power; if (speed_r>speed_soll_r) power_r+=power;
              if (speed_l<speed_soll_l) power_l-=power; if (speed_r<speed_soll_r) power_r-=power;
              step++; break;

    case(11): MotorSpeed(power_l,power_r); if ((count_l+count_r)<500) step--; else step++; break;
    case(12): MotorSpeed(0,0); step++; break;
    case(13): SerWrite("+",1); PrintInt(speed_max_l); step++; break;
    case(14): SerWrite("-",1); PrintInt(speed_max_r); step=99; break;

    case(20): if (speed_l>speed_soll_l) power_l+=power; if (speed_r>speed_soll_r) power_r+=power;
              if (speed_l<speed_soll_l) power_l-=power; if (speed_r<speed_soll_r) power_r-=power;
              step++; break;
    case(21): MotorSpeed(power_l,power_r); step--; break;
    case(99): StatusLED(GREEN); break;
    }
    count();
}while (1);
return 0;
}

(Der Code ist die orginale Version!)

Das zuckelt und ruckelt und sieht eher wie torkeln aus. Ursache ist die extrem kleine Pulszahl der 4er-ODO-Scheibe des asuro. Ich habe in diesen Code nun noch rechnerisch eine zweite Flanke eingebaut und mit den selben Parametern sieht das nun so aus:

Code:

/* Einfache Geschwindigkeitsregelung beider Motoren (Doppelte(!) Impulszahl mit 4er-Scheibe)

   Über den den zeitlichen Abstand des Hell/Dunkel-Wechsels der ODO-Scheibe wird die Geschwindigkeit
   ermittelt und geregelt. Jedes Rad für sich ohne Beeinflussung der anderen Seite.

   Es wird zusätzlich auch die zweite Flanke der ODO-Scheibe erkannt.

   15.1.2007
   mic

	Funktion der Tasten:

	T6:                      mehr Speed
	T5: Fährt endlos vor mit mittlerer Speed
	T4:                      Weniger Speed

	T3:                       grosser Geschwindigkeit
	T2: kurze Strecke vor mit mittlerer Geschwindigkeit
	T1:                       kleiner Geschwindigkeit

	Anhalten nach T5 über T1/T2/T3

	Die Segmentwechsel der ODO-Scheiben werden mit der StatusLED angezeigt. 

 
*/
#include <asuro.h>

unsigned char sw_data, sw0, sw1, sw2, sw3, i, tmp_char, step, power;
unsigned int data[2], j, tmp_int, loops, strecke, 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;
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_bit_l, o1_l, o2_l, o3_l;
unsigned int odo_r, odo_old_r, odo_min_r, odo_max_r, odo_bit_r, o1_r, o2_r, o3_r;
unsigned int odo_impbit_l, odo_impbit_r, odo_implevel_l, odo_implevel_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 count(void) {

    OdometrieData(data); akt=loop_count;
    if ((data[0]<o1_l) && (o1_l<o2_l) && (o2_l<o3_l)) {
        if (!odo_bit_l) {
            count_l ++; odo_bit_l=(1==1); StatusLED(YELLOW);
            odo_max_l=data[0];
            imp_l ++; odo_implevel_l=(odo_max_l-odo_min_l)/4;
            speed_l=akt-speed_count_l; speed_count_l=akt;
            //if (speed_l<speed_max_l) speed_max_l=speed_l;        }
        }
        if (odo_impbit_l && (data[0] < odo_implevel_l)) {
            imp_l ++; odo_impbit_l=(1==0);
            speed_l=akt-speed_count_l; speed_count_l=akt;
        }
    }
    if ((data[0]>o1_l) && (o1_l>o2_l) && (o2_l>o3_l)) {
        if (odo_bit_l) {
            count_l ++; odo_bit_l=(1==0); StatusLED(OFF);
            odo_min_l=data[0];
            imp_l ++; odo_implevel_l=((odo_max_l-odo_min_l)/4)*3;
            speed_l=akt-speed_count_l; speed_count_l=akt;
            //if (speed_l<speed_max_l) speed_max_l=speed_l;        }
        }
        if (!odo_impbit_l && (data[0] > odo_implevel_l)) {
            imp_l ++;  odo_impbit_l=(1==1);
            speed_l=akt-speed_count_l; speed_count_l=akt;
        }
    }

    if ((data[1]<o1_r) && (o1_r<o2_r) && (o2_r<o3_r)) {
        if (!odo_bit_r) {
            count_r ++; odo_bit_r=(1==1); StatusLED(RED);
            odo_max_r=data[1];
            imp_r ++; odo_implevel_r=(odo_max_r-odo_min_r)/4;
            speed_r=akt-speed_count_r; speed_count_r=akt;
            //if (speed_r<speed_max_r) speed_max_r=speed_r;        }
        }
        if (odo_impbit_r && (data[1] < odo_implevel_r)) {
            imp_r ++; odo_impbit_r=(1==0);
            speed_r=akt-speed_count_r; speed_count_r=akt;
        }
    }
    if ((data[1]>o1_r) && (o1_r>o2_r) && (o2_r>o3_r)) {
        if (odo_bit_r) {
            count_r ++; odo_bit_r=(1==0); StatusLED(OFF);
            odo_min_r=data[1];
            imp_r ++; odo_implevel_r=((odo_max_r-odo_min_r)/4)*3;
            speed_r=akt-speed_count_r; speed_count_r=akt;
            //if (speed_r<speed_max_r) speed_max_r=speed_r;
        }
        if (!odo_impbit_r && (data[1] > odo_implevel_r)) {
            imp_r ++;  odo_impbit_r=(1==1);
            speed_r=akt-speed_count_r; speed_count_r=akt;
        }
    }

    o3_l=o2_l; o2_l=o1_l; o1_l=data[0];
    o3_r=o2_r; o2_r=o1_r; o1_r=data[1];
}

int main(void) {
Init();
SerWrite("\nHallo\nBatterie: ",17);
PrintInt(Batterie());
loop_count=0; loop_delay=0; loops=0; step=0; delay=1000; strecke=80; power=125;

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;

power=1;

do {
    loop_count ++; sw_data=getswitch();

    switch (sw_data) {
    case(32): speed_soll_l=speed_soll_r+=1; step=20; break;
    case(16): speed_soll_l=speed_soll_r=15; power_l=power_r=50; step=20; break;
    case (8): speed_soll_l=speed_soll_r-=1; step=20; break;
    case (4): speed_soll_l=speed_soll_r=5; power_l=power_r=50; count_l=count_r=0; step=10; break;
    case (2): speed_soll_l=speed_soll_r=10; power_l=power_r=50; count_l=count_r=0; step=10; break;
    case (1): speed_soll_l=speed_soll_r=15; power_l=power_r=50; count_l=count_r=0; step=10; break;
    }

    if (loop_count > loop_delay) switch (step) {

    case(10): if (speed_l>speed_soll_l) power_l+=power; if (speed_r>speed_soll_r) power_r+=power;
              if (speed_l<speed_soll_l) power_l-=power; if (speed_r<speed_soll_r) power_r-=power;
              step++; break;

    case(11): MotorSpeed(power_l,power_r); if ((count_l+count_r)<500) step--; else step++; break;
    case(12): MotorSpeed(0,0); step++; break;
    case(13): SerWrite("+",1); PrintInt(speed_max_l); step++; break;
    case(14): SerWrite("-",1); PrintInt(speed_max_r); step=99; break;

    case(20): if (speed_l>speed_soll_l) power_l+=power; if (speed_r>speed_soll_r) power_r+=power;
              if (speed_l<speed_soll_l) power_l-=power; if (speed_r<speed_soll_r) power_r-=power;
              step++; break;
    case(21): MotorSpeed(power_l,power_r); step--; break;
    case(99): StatusLED(GREEN); break;
    }
    count();
}while (1);
return 0;
}

Sieht schon besser aus. Gelegentlich spinnt das ganze zwar noch etwas, aber als Ansatz finde ich das schon recht nett.

Gruß

mic