Programmcode-Stabilisierung einer Achse (Quadrocopter)

Jakob Rottmeir Gast #2996107
Lesenswert?
• ▲
▼ 
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:
1
            int ealt;

2
            int esum;

3
            int e; //Regelgröße

4
            int w;

5
            int x; //Istwert

6
            int p;

7
            int i;

8
            int d;

9
            int y;

10
            int G_Offset_V;

11
            int G_Offset_H;

12
            int iValVorne;

13
            int iValHinten;

14
double Kp; double Ki; double Ta; double Kd; double pdouble; double idouble; double ddouble;

15


16


17
 Kp = 0.135;

18
 Ki = 0.0432;

19
 Ta = 0.01; // Abtastzeit 10 ms/100 Hz

20
 Kd = 0.211; 

21
 

22
  

23
// Auslesen des Nick Winkels

24
// Auslesen/Schreiben GAS 0-255

25


26
 

27


28
 w = 400;

29
 

30
 e = w - x;

31


32
 // Proportianle Teil

33
 pdouble = e * Kp;

34


35
 //Integrativ

36
 esum = esum + e;

37
 idouble = Ki * Ta * esum;

38


39
 //Differential

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

41
 ealt = e;

42


43
 //Build Sum

44


45
 p = checked((int)pdouble);

46
 i = checked((int)idouble);

47
 d = checked((int)ddouble);

48
 

49
 y = p + i + d;

50


51
 //Motor Vorne oder Motor Hinten

52
 if(y < 0) // vorne schneller // Waagrechte Lage Winkel Nick bei ca 400 mV

53
 {y = y * (-1);

54
 G_Offset_V = (int)y;

55
 G_Offset_H = 0;

56
 }else //hinten schneller

57
 {G_Offset_V = 0;

58
 G_Offset_H = (int)y;

59
 }

60


61
 //Einstellen der Motorwerte

62
 iValVorne = 20 + G_Offset_V;

63
 iValHinten = 20 + G_Offset_H;

Über konstruktive Anworten würd ich mich freuen. Danke soweit.
Antwort Zitieren Bearbeiten Thread verschieben Thread sperren Anmeldepflicht aktivieren Anpinnen Thread löschen Thread mit anderem zusammenführen Beitrag melden
Mike M. #2996139
Lesenswert?
• ▲
▼ 
Autoformat:
1
int ealt;

2
int esum;

3
int e; //Regelgröße

4
int w;

5
int x; //Istwert

6
int p;

7
int i;

8
int d;

9
int y;

10
int G_Offset_V;

11
int G_Offset_H;

12
int iValVorne;

13
int iValHinten;

14
double Kp;

15
double Ki;

16
double Ta;

17
double Kd;

18
double pdouble;

19
double idouble;

20
double ddouble;

21


22
Kp = 0.135;

23
Ki = 0.0432;

24
Ta = 0.01; // Abtastzeit 10 ms/100 Hz

25
Kd = 0.211;

26


27
// Auslesen des Nick Winkels

28
// Auslesen/Schreiben GAS 0-255

29


30
w = 400;

31


32
e = w - x;

33


34
// Proportianle Teil

35
pdouble = e * Kp;

36


37
//Integrativ

38
esum = esum + e;

39
idouble = Ki * Ta * esum;

40


41
//Differential

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

43
ealt = e;

44


45
//Build Sum

46


47
p = checked((int)pdouble);

48
i = checked((int)idouble);

49
d = checked((int)ddouble);

50


51
y = p + i + d;

52


53
//Motor Vorne oder Motor Hinten

54
if(y < 0) // vorne schneller // Waagrechte Lage Winkel Nick bei ca 400 mV

55
{

56
    y = y * (-1);

57
    G_Offset_V = (int)y;

58
    G_Offset_H = 0;

59
}

60
else  //hinten schneller

61
{

62
    G_Offset_V = 0;

63
    G_Offset_H = (int)y;

64
}

65


66
//Einstellen der Motorwerte

67
iValVorne = 20 + G_Offset_V;

68
iValHinten = 20 + G_Offset_H;
Antwort Zitieren Bearbeiten Löschen Beitrag melden
Thread beobachten

Antwort schreiben

Bitte melde dich an, um einen Beitrag zu schreiben.

Bestehender Account

oder

Mit Google-Account einloggen

Noch kein Account?

Die Registrierung ist kostenlos und dauert nur eine Minute.

Jetzt registrieren