Kurskorrektur mittels Kompass

Hallo RN-Gemeinde!

Ich bin gerade dabei meinen Roboter von der Steuerung mittels ATmega328 (Arduino UNO) auf RN-Control mit ATmega644 umzurüsten.

Soweit funktioniert alles schon ganz gut, allerdings ist mir unklar wie ich in die Hauptschleife eine Kurskorrektur implementieren könnte. Die Daten aus dem HMC5883L Kompassmodul zu lesen bereitet keine Porbleme, auch die Umwandlung in einen Kurs klappt prima. Alles was mir im Moment probleme bereitet ist diesen Kurs in der Hauptschleife weiterzuverarbeiten.

Hier ist die Hauptschleife:

Code:

---

int main(void)
{
    init_SRF05();
    init_USART();
    init_Servo();
    init_Timer1();
   
    i2c_init();
    init_HMC5883L();
   
   
    distL = SRF05_getDistance(left);
    distM = SRF05_getDistance(middle);
    distR = SRF05_getDistance(right);
   
   
    Mlinksvor();
    Mrechtsvor();
    accelerate(10);
   
    while(1)
    {
       
        while(distM > threshold){
            distL = SRF05_getDistance(left);
            distM = SRF05_getDistance(middle);
            distR = SRF05_getDistance(right); 
        }
       
        if(distM < threshold && distR > threshold){
           
            while(distM < threshold){
                setPWMlinks(maxspeed + 100);
                setPWMrechts(maxspeed - 100);
                distM = SRF05_getDistance(middle);
            }           
           
            setPWMlinks(maxspeed);
            setPWMrechts(maxspeed);
        }
        else if(distM < threshold && distL > threshold){
   
            while(distM < threshold){
                setPWMlinks(maxspeed - 100);
                setPWMrechts(maxspeed + 100);
                distM = SRF05_getDistance(middle);
            }   
           
            setPWMlinks(maxspeed);
            setPWMrechts(maxspeed);
        }
       
    }
}

---

Hier die Funktion zum Auslesen des Kompasses:

Code:

---

void getHeading(void){
   
    sendUSART("Start reading...");
   
    i2c_start_wait(HMC5883L_WRITE);
    i2c_write(0x03); // pointer auf X-Achse MSB setzen
    i2c_stop();
   
    i2c_rep_start(HMC5883L_READ);

    raw_x = ((uint8_t)i2c_readAck())<<8;
    raw_x |= i2c_readAck();
   
    raw_z = ((uint8_t)i2c_readAck())<<8;
    raw_z |= i2c_readAck();
   
    raw_y = ((uint8_t)i2c_readAck())<<8;
    raw_y |= i2c_readNak();
   
    i2c_stop();
   
    headingDegrees = atan2((double)raw_y,(double)raw_x) * 180 / 3.141592654 + 180;
   
    sendUSART("complete!");
    sendUSART("\r\n");
}

---

Meine Idee wäre einfach den Kurs beim Start/ nach asuweichen eines Hindernisses abzufragen und wenn eine bestimmte Abweichung vom Kurs erreicht ist den Kurs mittels Beschleunigung einer Kette wieder zu korrigieren. Währenddessen wird der Kurs abgefragt und wenn der ursprüngliche Kurs erreicht ist wird weitergefahren.

Leider habe ich keine konkrete Lösung im Kopf, da mir der Kurs sorgen bereitet. Woher soll der Bot wissen in welche Richtung er vom Kurs abgewichen ist, wenn er Werte zwischen 0 und 360 erhält?

MfG

Torrentula

Das ist relatif einfach : du gibt eine "Soll" Kurse ein, das ist die Richtung das Robby eigentlich folgen soll. Das Kompass gibt naturlich eine "Ist" Kurse aus. Jetzt einfach das Differenz bilden, und schon haben wir die Abweichung. Diese Abweichung wird dan mit eine bestimmte "Regelparameter" multipliziert, und diesen Zahl verwenden sie jetzt um die Geschwindigkeit /PWM Wert von L und R Motor zu regelen (Die eine wird Erhoht, die andere Gemindert). Das ganse wird dan eine P-regelung genennt (Proportional). Wichtig ist die richtige Werte von diese "P Regelparameter" zu bestimmen. Zu hoch : das Robby oszilliert durch den Raum. Zu niedrig : die Kurse wird nicht genau angehalten. Google auch mal nach "PID" Regelung.

Gäbe es da nicht das Problem mit der Einschwingzeit der ganzen Funktion?

Wenn ein Hindernis im Weg ist, dann bricht ja sowieso die Kurskorrektur ab. Nachdem vorne wieder frei wird, fängt das ganze wieder an einzuschwingen usw. ich erhalte also keine gescheite Kurskorrektur weil er mir durch den Raum eiert, bis er auf das nächste Hindernis stößt.

Sowieso habe ich noch nicht ganz durch die ganzen Regelungen durchgeblickt. Proportional ist klar, Derivative - Ableitung naja und Integral --> also mehr als dass man damit die Fläche unter einem Graphen ausrechnen kann weiß ich nicht :D

MfG

Torrentula