Programmcode-Stabilisierung einer Achse (Quadrocopter)

[Jakob Rottmeir (Gast)]

Hallo,
(Schulprojekt Quadrocopter, Ziel Stabilisierung einer Achse)
ich versuche momentan eine Achse des Quadrocopters zu stabilisieren.
Dazu benötige ich erst mal nur den Nick Winkel, Gas an Motor1,2 lesen 
und schreiben.
Ich habe jetzt mal in C einen Code geschrieben, und bräuchte ein 
feedback ob der Code in die richtige Richtung geht. Was noch fehlt is 
die Auswertung der Sensoren, (die codes weiß ich noch nicht), was 
drinnen ist, ist ein PID regler der je nachdem wie der y ausfällt den 
hinteren oder vorderen Motor ansteuert.
(eine ganz simple regelung die natürlich nur für einen achse 
funktionieren soll)
Mein Vorwissen über die C programmierung ist gering. Also relativer 
Anfänger, wie man gleich sehen wird..
In den Code gehe ich davon aus das die waagrechte Lage also der Sollwert 
bei 400 mV Nick liegt. Eine Fusion bzw Offset umgehung ist noch nicht 
berücksichtigt. Die Paramter hab ich aus der Sprungantwort nach 
Ziegler/Nichols berechnet. Komm mir aber relativ klein vor.

Hier mal der Code:

            int ealt;
            int esum;
            int e; //Regelgröße
            int w;
            int x; //Istwert
            int p;
            int i;
            int d;
            int y;
            int G_Offset_V;
            int G_Offset_H;
            int iValVorne;
            int iValHinten;
double Kp; double Ki; double Ta; double Kd; double pdouble; double idouble; double ddouble;


 Kp = 0.135;
 Ki = 0.0432;
 Ta = 0.01; // Abtastzeit 10 ms/100 Hz
 Kd = 0.211; 
 
  
// Auslesen des Nick Winkels
// Auslesen/Schreiben GAS 0-255

 

 w = 400;
 
 e = w - x;

 // Proportianle Teil
 pdouble = e * Kp;

 //Integrativ
 esum = esum + e;
 idouble = Ki * Ta * esum;

 //Differential
 ddouble = Kd * (e - ealt)/Ta;
 ealt = e;

 //Build Sum

 p = checked((int)pdouble);
 i = checked((int)idouble);
 d = checked((int)ddouble);
 
 y = p + i + d;

 //Motor Vorne oder Motor Hinten
 if(y < 0) // vorne schneller // Waagrechte Lage Winkel Nick bei ca 400 mV
 {y = y * (-1);
 G_Offset_V = (int)y;
 G_Offset_H = 0;
 }else //hinten schneller
 {G_Offset_V = 0;
 G_Offset_H = (int)y;
 }

 //Einstellen der Motorwerte
 iValVorne = 20 + G_Offset_V;
 iValHinten = 20 + G_Offset_H;

Über konstruktive Anworten würd ich mich freuen. Danke soweit.

[Jakob Rottmeir (Gast)]

In Verwendung habe ich eineFlight-Ctrl V2.1 mit einem ATMEGA128P-AV.
Hierzu auch den passenden Schaltplan
http://mikrocontroller.com/files/Flight-Ctrl_ME_2_1f.pdf
ADC2/PA2 wäre der Eingang von den Nick Gyro

[jk (Gast)]

Bitte mal als code formatieren, sonst kann man nix lesen ;)

[Karl H. (kbuchegg)]

Hab die Forumsformatierzeichen mal eingefügt.

[Jakob Rottmeir (Gast)]

wusste ich nicht Danke.

[Mike M. (mikeii)]

Autoformat:

int ealt;
int esum;
int e; //Regelgröße
int w;
int x; //Istwert
int p;
int i;
int d;
int y;
int G_Offset_V;
int G_Offset_H;
int iValVorne;
int iValHinten;
double Kp;
double Ki;
double Ta;
double Kd;
double pdouble;
double idouble;
double ddouble;

Kp = 0.135;
Ki = 0.0432;
Ta = 0.01; // Abtastzeit 10 ms/100 Hz
Kd = 0.211;

// Auslesen des Nick Winkels
// Auslesen/Schreiben GAS 0-255

w = 400;

e = w - x;

// Proportianle Teil
pdouble = e * Kp;

//Integrativ
esum = esum + e;
idouble = Ki * Ta * esum;

//Differential
ddouble = Kd * (e - ealt)/Ta;
ealt = e;

//Build Sum

p = checked((int)pdouble);
i = checked((int)idouble);
d = checked((int)ddouble);

y = p + i + d;

//Motor Vorne oder Motor Hinten
if(y < 0) // vorne schneller // Waagrechte Lage Winkel Nick bei ca 400 mV
{
    y = y * (-1);
    G_Offset_V = (int)y;
    G_Offset_H = 0;
}
else  //hinten schneller
{
    G_Offset_V = 0;
    G_Offset_H = (int)y;
}

//Einstellen der Motorwerte
iValVorne = 20 + G_Offset_V;
iValHinten = 20 + G_Offset_H;

[Zapp (Gast)]

Das Wesen des Quadrocopter versuchs ist doch das Probieren...