![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
motor met PID controller, aangestuurd via pc.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of frdm_Motor_V2 by
main.cpp@12:69ab81cf5b7d, 2015-09-29 (annotated)
- Committer:
- Rvs94
- Date:
- Tue Sep 29 16:28:55 2015 +0000
- Revision:
- 12:69ab81cf5b7d
- Parent:
- 11:0793a78109a2
- Child:
- 13:a6770307a5d2
PID controller, nog niet getest, biquad vars nog niet gevonden.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Margreeth95 | 0:284ed397e046 | 1 | #include "mbed.h" |
Margreeth95 | 0:284ed397e046 | 2 | #include "MODSERIAL.h" |
Margreeth95 | 0:284ed397e046 | 3 | #include "HIDScope.h" |
Margreeth95 | 0:284ed397e046 | 4 | #include "QEI.h" |
Rvs94 | 12:69ab81cf5b7d | 5 | #include "biquadFilter.h" |
Margreeth95 | 0:284ed397e046 | 6 | |
Margreeth95 | 0:284ed397e046 | 7 | Serial pc(USBTX, USBRX); // tx, rx |
Margreeth95 | 0:284ed397e046 | 8 | DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield) |
Margreeth95 | 0:284ed397e046 | 9 | PwmOut motor2speed(D5); |
Rvs94 | 9:774fc3c6a39e | 10 | AnalogIn potmeter2(A5); |
Rvs94 | 5:455773cf460b | 11 | QEI Encoder(D3, D2, NC, 128); |
Rvs94 | 1:48aba8d5610a | 12 | HIDScope scope(3); |
Rvs94 | 12:69ab81cf5b7d | 13 | |
Margreeth95 | 0:284ed397e046 | 14 | Ticker ScopeTime; |
Rvs94 | 9:774fc3c6a39e | 15 | Ticker myControllerTicker; |
Rvs94 | 7:67b50d4fb03c | 16 | |
Rvs94 | 7:67b50d4fb03c | 17 | double Aantal_Degs; |
Rvs94 | 7:67b50d4fb03c | 18 | double Aantal_pulses; |
Rvs94 | 10:80fe931a71e4 | 19 | double reference; |
Rvs94 | 10:80fe931a71e4 | 20 | double position; |
Rvs94 | 2:099da0fc31b6 | 21 | |
Margreeth95 | 0:284ed397e046 | 22 | void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt |
Margreeth95 | 0:284ed397e046 | 23 | { |
Margreeth95 | 0:284ed397e046 | 24 | scope.set(0, motor2direction.read()); |
Margreeth95 | 0:284ed397e046 | 25 | scope.set(1, motor2speed.read()); |
Rvs94 | 1:48aba8d5610a | 26 | scope.set(2, Aantal_Degs); |
Rvs94 | 10:80fe931a71e4 | 27 | Aantal_Degs = Encoder.getPulses()*360/(0.5*128*131); |
Rvs94 | 4:0d4aff8b57b3 | 28 | |
Margreeth95 | 0:284ed397e046 | 29 | scope.send(); |
Rvs94 | 1:48aba8d5610a | 30 | |
Margreeth95 | 0:284ed397e046 | 31 | } |
Rvs94 | 12:69ab81cf5b7d | 32 | // Sample time (motor2-step) |
Rvs94 | 12:69ab81cf5b7d | 33 | const double m2_Ts = 0.01; |
Rvs94 | 12:69ab81cf5b7d | 34 | |
Rvs94 | 9:774fc3c6a39e | 35 | // Controller gain |
Rvs94 | 12:69ab81cf5b7d | 36 | const double m2_Kp = 0.07,m2_Ki = 0.005, m2_Kd = 0.005; |
Rvs94 | 12:69ab81cf5b7d | 37 | double m2_err_int = 0, m1_prev_err = 0; |
Rvs94 | 12:69ab81cf5b7d | 38 | |
Rvs94 | 12:69ab81cf5b7d | 39 | //Derivative filter coeffs |
Rvs94 | 12:69ab81cf5b7d | 40 | const double m2_f_a1 = 1.0, m2_f_a2 = 1.0, m2_f_b0 = 1.0, m2_f_b1 = 1.0, m2_f_b2 = 1.0; |
Rvs94 | 12:69ab81cf5b7d | 41 | // Filter variables |
Rvs94 | 12:69ab81cf5b7d | 42 | double m2_f_v1 = 0, m2_f_v2 = 0; |
Rvs94 | 10:80fe931a71e4 | 43 | |
Rvs94 | 12:69ab81cf5b7d | 44 | // Biquad filter |
Rvs94 | 12:69ab81cf5b7d | 45 | double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 ) |
Rvs94 | 9:774fc3c6a39e | 46 | { |
Rvs94 | 12:69ab81cf5b7d | 47 | double v = u − a1∗v1 − a2∗v2; |
Rvs94 | 12:69ab81cf5b7d | 48 | double y = b0∗v + b1∗v1 + b2∗v2; |
Rvs94 | 12:69ab81cf5b7d | 49 | v2 = v1; v1 = v; |
Rvs94 | 12:69ab81cf5b7d | 50 | return y; |
Rvs94 | 12:69ab81cf5b7d | 51 | } |
Rvs94 | 12:69ab81cf5b7d | 52 | |
Rvs94 | 12:69ab81cf5b7d | 53 | |
Rvs94 | 12:69ab81cf5b7d | 54 | // Reusable PID controller |
Rvs94 | 12:69ab81cf5b7d | 55 | double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2, |
Rvs94 | 12:69ab81cf5b7d | 56 | const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b3) |
Rvs94 | 12:69ab81cf5b7d | 57 | { |
Rvs94 | 12:69ab81cf5b7d | 58 | // Derivative |
Rvs94 | 12:69ab81cf5b7d | 59 | double e_der = (e-e_prev)/Ts; |
Rvs94 | 12:69ab81cf5b7d | 60 | e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2); |
Rvs94 | 12:69ab81cf5b7d | 61 | e_prev = e |
Rvs94 | 12:69ab81cf5b7d | 62 | // Integral |
Rvs94 | 12:69ab81cf5b7d | 63 | e_int = e_int + Ts*e; |
Rvs94 | 12:69ab81cf5b7d | 64 | // PID |
Rvs94 | 12:69ab81cf5b7d | 65 | return Kp * e + Ki*e_int + Kd*e_der; |
Rvs94 | 9:774fc3c6a39e | 66 | } |
Margreeth95 | 0:284ed397e046 | 67 | |
Rvs94 | 12:69ab81cf5b7d | 68 | // Motor control |
Rvs94 | 11:0793a78109a2 | 69 | void motor2_Controller() |
Rvs94 | 9:774fc3c6a39e | 70 | { |
Rvs94 | 10:80fe931a71e4 | 71 | reference = potmeter2.read()*360; |
Rvs94 | 10:80fe931a71e4 | 72 | position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs |
Rvs94 | 12:69ab81cf5b7d | 73 | double P2 = PID( reference − position, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, |
Rvs94 | 12:69ab81cf5b7d | 74 | m2_f_b0, m2_f_b1, m2_f_b2); |
Rvs94 | 9:774fc3c6a39e | 75 | motor2speed = abs(P2); |
Rvs94 | 9:774fc3c6a39e | 76 | if(P2 > 0) |
Rvs94 | 9:774fc3c6a39e | 77 | { |
Rvs94 | 9:774fc3c6a39e | 78 | motor2direction = 0; |
Rvs94 | 9:774fc3c6a39e | 79 | } |
Rvs94 | 9:774fc3c6a39e | 80 | else |
Rvs94 | 9:774fc3c6a39e | 81 | { |
Rvs94 | 9:774fc3c6a39e | 82 | motor2direction = 1; |
Rvs94 | 9:774fc3c6a39e | 83 | } |
Rvs94 | 9:774fc3c6a39e | 84 | |
Rvs94 | 9:774fc3c6a39e | 85 | } |
Rvs94 | 3:687729d7996e | 86 | |
Margreeth95 | 0:284ed397e046 | 87 | int main() |
Rvs94 | 9:774fc3c6a39e | 88 | { |
Margreeth95 | 0:284ed397e046 | 89 | pc.baud(115200); |
Rvs94 | 3:687729d7996e | 90 | pc.printf("Tot aan loop werkt\n"); |
Rvs94 | 9:774fc3c6a39e | 91 | |
Margreeth95 | 0:284ed397e046 | 92 | ScopeTime.attach_us(&ScopeSend, 10e4); |
Rvs94 | 11:0793a78109a2 | 93 | myControllerTicker.attach( &motor2_Controller, 0.01f ); // 100 Hz |
Rvs94 | 9:774fc3c6a39e | 94 | while(true) |
Margreeth95 | 0:284ed397e046 | 95 | { |
Rvs94 | 10:80fe931a71e4 | 96 | pc.printf("position = %f aantal degs = %f \n",reference,position); |
Rvs94 | 10:80fe931a71e4 | 97 | wait(0.2f); |
Margreeth95 | 0:284ed397e046 | 98 | } |
Rvs94 | 9:774fc3c6a39e | 99 | |
Margreeth95 | 0:284ed397e046 | 100 | } |