Raharja Agie / Mbed 2 deprecated Mini-X

pid.h@0:d463d5c04541, 2011-08-16 (annotated)

Committer:
agiembed
Date:
Tue Aug 16 05:32:33 2011 +0000
Revision:
0:d463d5c04541

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


agiembed

0:d463d5c04541


     1

short alti_con, roll_con, yaw_con, pitch_con;





agiembed

0:d463d5c04541


     2

char yaw_state1 = 0, yaw_state2 = 0, yaw_cnt = 0;





agiembed

0:d463d5c04541


     3

#define PID_max 80





agiembed

0:d463d5c04541


     4

#define Int_max 100





agiembed

0:d463d5c04541


     5

#define Int_min -100





agiembed

0:d463d5c04541


     6







agiembed

0:d463d5c04541


     7

float PIDRoll, PIDPitch, PIDYaw, PIDAlti;





agiembed

0:d463d5c04541


     8

float Int_Roll, Int_Pitch;





agiembed

0:d463d5c04541


     9

float Roll_error, Roll_prev_error, Roll_ref = 0, Roll_int_error = 0;





agiembed

0:d463d5c04541


    10

float Pitch_error, Pitch_prev_error, Pitch_ref = 0, Pitch_int_error = 0;





agiembed

0:d463d5c04541


    11

float Yaw_error, Yaw_prev_error, Yaw_ref, Yaw_prev_ref;





agiembed

0:d463d5c04541


    12

float Alti_error, Alti_prev_error, Z_rate_error, Alti_ref, Alti_I_error;





agiembed

0:d463d5c04541


    13







agiembed

0:d463d5c04541


    14







agiembed

0:d463d5c04541


    15

void PID_Roll() {





agiembed

0:d463d5c04541


    16







agiembed

0:d463d5c04541


    17

    gainRoll.p = 0.65;//28;  //1.05;





agiembed

0:d463d5c04541


    18

    gainRoll.d = 0.5;





agiembed

0:d463d5c04541


    19

    gainRoll.i = 0.035;





agiembed

0:d463d5c04541


    20

    gainRoll.r = 0.175;//9;





agiembed

0:d463d5c04541


    21

    //if(fcc.US < 1000){





agiembed

0:d463d5c04541


    22

    //gainRoll.p = 1.05;   //1.05 Take-off Gain





agiembed

0:d463d5c04541


    23

    //gainRoll.r = 0.275;   //0.275 Take-off Gain





agiembed

0:d463d5c04541


    24

    //}





agiembed

0:d463d5c04541


    25







agiembed

0:d463d5c04541


    26

    Roll_ref = (RC.rolls / 7);





agiembed

0:d463d5c04541


    27

    if (Roll_ref>30) Roll_ref = 30;





agiembed

0:d463d5c04541


    28

    else if (Roll_ref<-30) Roll_ref = -30;





agiembed

0:d463d5c04541


    29

   





agiembed

0:d463d5c04541


    30

    Roll_error = fcc.roll -  Roll_ref;





agiembed

0:d463d5c04541


    31

    Roll_int_error += Roll_error;





agiembed

0:d463d5c04541


    32

    





agiembed

0:d463d5c04541


    33

    if (Roll_int_error > Int_max) Roll_int_error = Int_max;





agiembed

0:d463d5c04541


    34

    else if (Roll_int_error < Int_min) Roll_int_error = Int_min;





agiembed

0:d463d5c04541


    35

    Int_Roll = gainRoll.i * Roll_int_error;





agiembed

0:d463d5c04541


    36

    





agiembed

0:d463d5c04541


    37

    PIDRoll = gainRoll.p * Roll_error;





agiembed

0:d463d5c04541


    38

    PIDRoll += Int_Roll;





agiembed

0:d463d5c04541


    39

    PIDRoll += (gainRoll.d * (Roll_error - Roll_prev_error));





agiembed

0:d463d5c04541


    40

    PIDRoll += (gainRoll.r * fcc.g_roll);





agiembed

0:d463d5c04541


    41







agiembed

0:d463d5c04541


    42

    if (PIDRoll > PID_max) PIDRoll = PID_max;





agiembed

0:d463d5c04541


    43

    else if (PIDRoll < -PID_max) PIDRoll = -PID_max;





agiembed

0:d463d5c04541


    44







agiembed

0:d463d5c04541


    45

    Roll_prev_error = Roll_error;





agiembed

0:d463d5c04541


    46

    return;





agiembed

0:d463d5c04541


    47

}





agiembed

0:d463d5c04541


    48







agiembed

0:d463d5c04541


    49

void PID_Pitch() {





agiembed

0:d463d5c04541


    50







agiembed

0:d463d5c04541


    51

    gainPitch.p = gainRoll.p;//1.075;  //1.35    //1.05;





agiembed

0:d463d5c04541


    52

    gainPitch.d = gainRoll.d;//0.9635;





agiembed

0:d463d5c04541


    53

    gainPitch.i = gainRoll.i;//0.021;





agiembed

0:d463d5c04541


    54

    gainPitch.r = gainRoll.r;// 0.175;  //0.29





agiembed

0:d463d5c04541


    55

    //if(fcc.US < 1000){





agiembed

0:d463d5c04541


    56

    // gainPitch.p = 1.05;  //1.05 Take-off Gain





agiembed

0:d463d5c04541


    57

    // gainPitch.r = 0.275;  //0.275 Take-off Gain





agiembed

0:d463d5c04541


    58

    //}





agiembed

0:d463d5c04541


    59







agiembed

0:d463d5c04541


    60

    Pitch_ref = (-RC.pitchs / 6);





agiembed

0:d463d5c04541


    61

    if (Pitch_ref>30) Pitch_ref = 30;





agiembed

0:d463d5c04541


    62

    else if (Pitch_ref<-30) Pitch_ref = -30;





agiembed

0:d463d5c04541


    63

    





agiembed

0:d463d5c04541


    64

    Pitch_error = fcc.pitch - Pitch_ref;  





agiembed

0:d463d5c04541


    65

    Pitch_int_error += Pitch_error;   





agiembed

0:d463d5c04541


    66

        





agiembed

0:d463d5c04541


    67

    if (Pitch_int_error > Int_max) Pitch_int_error = Int_max;





agiembed

0:d463d5c04541


    68

    else if (Pitch_int_error < Int_min) Pitch_int_error = Int_min;





agiembed

0:d463d5c04541


    69

    Int_Pitch = gainPitch.i * Pitch_int_error;





agiembed

0:d463d5c04541


    70







agiembed

0:d463d5c04541


    71

    PIDPitch = (gainPitch.p * Pitch_error);





agiembed

0:d463d5c04541


    72

    PIDPitch += Int_Pitch;





agiembed

0:d463d5c04541


    73

    PIDPitch += (gainPitch.d * (Pitch_error - Pitch_prev_error));





agiembed

0:d463d5c04541


    74

    PIDPitch += (gainPitch.r * fcc.g_pitch);





agiembed

0:d463d5c04541


    75







agiembed

0:d463d5c04541


    76

    if (PIDPitch > PID_max) PIDPitch = PID_max;





agiembed

0:d463d5c04541


    77

    else if (PIDPitch < -PID_max) PIDPitch = -PID_max;





agiembed

0:d463d5c04541


    78







agiembed

0:d463d5c04541


    79

    Pitch_prev_error = Pitch_error;





agiembed

0:d463d5c04541


    80

    return;





agiembed

0:d463d5c04541


    81

}





agiembed

0:d463d5c04541


    82







agiembed

0:d463d5c04541


    83







agiembed

0:d463d5c04541


    84

void PID_Yaw() {





agiembed

0:d463d5c04541


    85







agiembed

0:d463d5c04541


    86

    gainYaw.p = 1.35;  //1.35 Take-off Gain   //1.15;





agiembed

0:d463d5c04541


    87

    //gainYaw.d = 0;





agiembed

0:d463d5c04541


    88

    gainYaw.r = 0.25;  //0.35 Take-off Gain





agiembed

0:d463d5c04541


    89







agiembed

0:d463d5c04541


    90

    if (RC.yaws>=50 || RC.yaws<=-50) yaw_state1 = 1;





agiembed

0:d463d5c04541


    91

    if (yaw_state1 == 1)if (RC.yaws>50 || RC.yaws<-50) yaw_state2 = 1;





agiembed

0:d463d5c04541


    92

    if (yaw_state1 == 1 && yaw_state2 == 1) yaw_cnt++;





agiembed

0:d463d5c04541


    93

    if (yaw_cnt == 10) {





agiembed

0:d463d5c04541


    94

        Yaw_ref = fcc.yaw;





agiembed

0:d463d5c04541


    95

        yaw_state1 = yaw_state2 = yaw_cnt = 0;





agiembed

0:d463d5c04541


    96

    }





agiembed

0:d463d5c04541


    97

    Yaw_error = Yaw_ref - fcc.yaw;





agiembed

0:d463d5c04541


    98

    if (Yaw_error>180) Yaw_error -= 360;





agiembed

0:d463d5c04541


    99

    if (Yaw_error<-180) Yaw_error += 360;





agiembed

0:d463d5c04541


   100







agiembed

0:d463d5c04541


   101

    PIDYaw = (gainYaw.p * Yaw_error);





agiembed

0:d463d5c04541


   102

    //PIDYaw += (gainYaw.i * Yaw_error);





agiembed

0:d463d5c04541


   103

    //PIDYaw += (gainYaw.d * (Yaw_error - Yaw_prev_error));





agiembed

0:d463d5c04541


   104

    PIDYaw -= (gainYaw.r * fcc.g_yaw);





agiembed

0:d463d5c04541


   105







agiembed

0:d463d5c04541


   106

    if (PIDYaw > PID_max) PIDYaw = PID_max;





agiembed

0:d463d5c04541


   107

    if (PIDYaw < -PID_max) PIDYaw = -PID_max;





agiembed

0:d463d5c04541


   108







agiembed

0:d463d5c04541


   109

    Yaw_prev_error = Yaw_error;





agiembed

0:d463d5c04541


   110

    return;





agiembed

0:d463d5c04541


   111

}





agiembed

0:d463d5c04541


   112







agiembed

0:d463d5c04541


   113







agiembed

0:d463d5c04541


   114







agiembed

0:d463d5c04541


   115

void pid() {





agiembed

0:d463d5c04541


   116







agiembed

0:d463d5c04541


   117

    alti_con = RC.throttles;





agiembed

0:d463d5c04541


   118







agiembed

0:d463d5c04541


   119







agiembed

0:d463d5c04541


   120

    PID_Roll();





agiembed

0:d463d5c04541


   121

    roll_con = - PIDRoll; //RC.rolls * 0.14 ;//





agiembed

0:d463d5c04541


   122







agiembed

0:d463d5c04541


   123

    PID_Pitch();





agiembed

0:d463d5c04541


   124

    pitch_con =  PIDPitch; //RC.pitchs  * 0.14; //





agiembed

0:d463d5c04541


   125







agiembed

0:d463d5c04541


   126

    PID_Yaw();





agiembed

0:d463d5c04541


   127

    yaw_con = (RC.yaws * 0.3) + PIDYaw;





agiembed

0:d463d5c04541


   128

    if (alti_con < 5) yaw_con = 0;





agiembed

0:d463d5c04541


   129







agiembed

0:d463d5c04541


   130

}