![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Motor programma
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of frdm_Motor_V2_3 by
main.cpp@20:f5091e29cd26, 2015-10-02 (annotated)
- Committer:
- Rvs94
- Date:
- Fri Oct 02 17:22:17 2015 +0000
- Revision:
- 20:f5091e29cd26
- Parent:
- 19:9417d2011e8b
- Child:
- 21:cd7eb62183da
- Child:
- 22:56c3a5918bfc
- Child:
- 24:d0af4b2be295
Programma selectie werkend, alleen is het script instabiel (vermoedelijk door de hoeveelheid tickers). Volgende stap is het installeren van Go flags.
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 |
Rvs94 | 20:f5091e29cd26 | 8 | QEI Encoder(D3, D2, NC, 128); |
Rvs94 | 20:f5091e29cd26 | 9 | HIDScope scope(3); |
Rvs94 | 20:f5091e29cd26 | 10 | |
Rvs94 | 20:f5091e29cd26 | 11 | //Ledjes |
Margreeth95 | 19:9417d2011e8b | 12 | DigitalOut LedR(LED_RED); |
Margreeth95 | 19:9417d2011e8b | 13 | DigitalOut LedG(LED_GREEN); |
Margreeth95 | 19:9417d2011e8b | 14 | DigitalOut LedB(LED_BLUE); |
Rvs94 | 20:f5091e29cd26 | 15 | |
Rvs94 | 20:f5091e29cd26 | 16 | //Motor |
Margreeth95 | 0:284ed397e046 | 17 | DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield) |
Margreeth95 | 0:284ed397e046 | 18 | PwmOut motor2speed(D5); |
Rvs94 | 12:69ab81cf5b7d | 19 | |
Rvs94 | 20:f5091e29cd26 | 20 | //Tickers |
Margreeth95 | 0:284ed397e046 | 21 | Ticker ScopeTime; |
Rvs94 | 9:774fc3c6a39e | 22 | Ticker myControllerTicker; |
Rvs94 | 20:f5091e29cd26 | 23 | Ticker PrintTime; |
Rvs94 | 7:67b50d4fb03c | 24 | |
Rvs94 | 20:f5091e29cd26 | 25 | //Startwaarden |
Rvs94 | 10:80fe931a71e4 | 26 | double reference; |
Rvs94 | 10:80fe931a71e4 | 27 | double position; |
Rvs94 | 15:7fbee317af2d | 28 | double m2_ref = 0; |
Margreeth95 | 19:9417d2011e8b | 29 | int count = 0; |
Rvs94 | 2:099da0fc31b6 | 30 | |
Rvs94 | 20:f5091e29cd26 | 31 | //Sample time (motor2-step) |
Rvs94 | 20:f5091e29cd26 | 32 | const double m2_Ts = 0.01; |
Rvs94 | 20:f5091e29cd26 | 33 | |
Rvs94 | 20:f5091e29cd26 | 34 | //Controller gain Motor 2 |
Rvs94 | 20:f5091e29cd26 | 35 | const double m2_Kp = 0.5,m2_Ki = 0.005, m2_Kd = 0.5; |
Rvs94 | 20:f5091e29cd26 | 36 | double m2_err_int = 0, m2_prev_err = 0; |
Rvs94 | 20:f5091e29cd26 | 37 | |
Rvs94 | 20:f5091e29cd26 | 38 | //Derivative filter coeffs Motor 2 |
Rvs94 | 20:f5091e29cd26 | 39 | const double BiGain = 0.016955; |
Rvs94 | 20:f5091e29cd26 | 40 | const double m2_f_a1 = -0.96608908283*BiGain, m2_f_a2 = 0.0*BiGain, m2_f_b0 = 1.0*BiGain, m2_f_b1 = 1.0*BiGain, m2_f_b2 = 0.0*BiGain; |
Rvs94 | 20:f5091e29cd26 | 41 | |
Rvs94 | 20:f5091e29cd26 | 42 | // Filter variables |
Rvs94 | 20:f5091e29cd26 | 43 | double m2_f_v1 = 0, m2_f_v2 = 0; |
Rvs94 | 20:f5091e29cd26 | 44 | |
Rvs94 | 20:f5091e29cd26 | 45 | |
Rvs94 | 20:f5091e29cd26 | 46 | //HIDScope |
Margreeth95 | 0:284ed397e046 | 47 | void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt |
Margreeth95 | 0:284ed397e046 | 48 | { |
Margreeth95 | 0:284ed397e046 | 49 | scope.set(0, motor2direction.read()); |
Margreeth95 | 0:284ed397e046 | 50 | scope.set(1, motor2speed.read()); |
Rvs94 | 15:7fbee317af2d | 51 | scope.set(2, position); |
Rvs94 | 4:0d4aff8b57b3 | 52 | |
Margreeth95 | 0:284ed397e046 | 53 | scope.send(); |
Rvs94 | 1:48aba8d5610a | 54 | |
Margreeth95 | 0:284ed397e046 | 55 | } |
Rvs94 | 12:69ab81cf5b7d | 56 | |
Rvs94 | 20:f5091e29cd26 | 57 | // PC Printf |
Rvs94 | 20:f5091e29cd26 | 58 | void PCPrintf() |
Rvs94 | 20:f5091e29cd26 | 59 | { |
Rvs94 | 20:f5091e29cd26 | 60 | pc.printf("position = %f aantal degs = %f, count = %i\n",reference,position,count); |
Rvs94 | 20:f5091e29cd26 | 61 | } |
Rvs94 | 10:80fe931a71e4 | 62 | |
Rvs94 | 12:69ab81cf5b7d | 63 | // Biquad filter |
Rvs94 | 12:69ab81cf5b7d | 64 | 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 | 65 | { |
Rvs94 | 13:a6770307a5d2 | 66 | double v = u - a1*v1 - a2*v2; |
Rvs94 | 13:a6770307a5d2 | 67 | double y = b0*v + b1*v1 + b2*v2; |
Rvs94 | 12:69ab81cf5b7d | 68 | v2 = v1; v1 = v; |
Rvs94 | 12:69ab81cf5b7d | 69 | return y; |
Rvs94 | 12:69ab81cf5b7d | 70 | } |
Rvs94 | 12:69ab81cf5b7d | 71 | |
Rvs94 | 12:69ab81cf5b7d | 72 | |
Rvs94 | 12:69ab81cf5b7d | 73 | // Reusable PID controller |
Rvs94 | 12:69ab81cf5b7d | 74 | 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 | 13:a6770307a5d2 | 75 | const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2) |
Rvs94 | 12:69ab81cf5b7d | 76 | { |
Rvs94 | 12:69ab81cf5b7d | 77 | // Derivative |
Rvs94 | 12:69ab81cf5b7d | 78 | double e_der = (e-e_prev)/Ts; |
Rvs94 | 12:69ab81cf5b7d | 79 | e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2); |
Rvs94 | 13:a6770307a5d2 | 80 | e_prev = e; |
Rvs94 | 12:69ab81cf5b7d | 81 | // Integral |
Rvs94 | 12:69ab81cf5b7d | 82 | e_int = e_int + Ts*e; |
Rvs94 | 12:69ab81cf5b7d | 83 | // PID |
Rvs94 | 12:69ab81cf5b7d | 84 | return Kp * e + Ki*e_int + Kd*e_der; |
Rvs94 | 9:774fc3c6a39e | 85 | } |
Margreeth95 | 0:284ed397e046 | 86 | |
Margreeth95 | 18:6f71bb91b8bd | 87 | // Motor2 control |
Rvs94 | 11:0793a78109a2 | 88 | void motor2_Controller() |
Rvs94 | 9:774fc3c6a39e | 89 | { |
Rvs94 | 15:7fbee317af2d | 90 | reference = m2_ref; // Setpoint |
Margreeth95 | 18:6f71bb91b8bd | 91 | double pulses = Encoder.getPulses(); |
Margreeth95 | 19:9417d2011e8b | 92 | position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs |
Rvs94 | 13:a6770307a5d2 | 93 | 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 | 94 | m2_f_b0, m2_f_b1, m2_f_b2); |
Rvs94 | 15:7fbee317af2d | 95 | motor2speed = abs(P2); // Speed control |
Rvs94 | 15:7fbee317af2d | 96 | if(P2 > 0) // Direction control |
Rvs94 | 9:774fc3c6a39e | 97 | { |
Rvs94 | 9:774fc3c6a39e | 98 | motor2direction = 0; |
Rvs94 | 9:774fc3c6a39e | 99 | } |
Rvs94 | 9:774fc3c6a39e | 100 | else |
Rvs94 | 9:774fc3c6a39e | 101 | { |
Rvs94 | 9:774fc3c6a39e | 102 | motor2direction = 1; |
Rvs94 | 9:774fc3c6a39e | 103 | } |
Rvs94 | 20:f5091e29cd26 | 104 | |
Rvs94 | 9:774fc3c6a39e | 105 | } |
Rvs94 | 3:687729d7996e | 106 | |
Margreeth95 | 0:284ed397e046 | 107 | int main() |
Rvs94 | 9:774fc3c6a39e | 108 | { |
Rvs94 | 20:f5091e29cd26 | 109 | LedR.write(1); |
Rvs94 | 20:f5091e29cd26 | 110 | LedB.write(1); |
Rvs94 | 20:f5091e29cd26 | 111 | LedG.write(1); |
Margreeth95 | 0:284ed397e046 | 112 | pc.baud(115200); |
Rvs94 | 3:687729d7996e | 113 | pc.printf("Tot aan loop werkt\n"); |
Rvs94 | 9:774fc3c6a39e | 114 | |
Margreeth95 | 0:284ed397e046 | 115 | ScopeTime.attach_us(&ScopeSend, 10e4); |
Rvs94 | 11:0793a78109a2 | 116 | myControllerTicker.attach( &motor2_Controller, 0.01f ); // 100 Hz |
Rvs94 | 20:f5091e29cd26 | 117 | PrintTime.attach(&PCPrintf,0.1f); |
Rvs94 | 9:774fc3c6a39e | 118 | while(true) |
Rvs94 | 20:f5091e29cd26 | 119 | { |
Rvs94 | 20:f5091e29cd26 | 120 | char c = pc.getc(); |
Rvs94 | 20:f5091e29cd26 | 121 | if(c == 'e') //Ga 1 programma omhoog |
Rvs94 | 20:f5091e29cd26 | 122 | { |
Rvs94 | 20:f5091e29cd26 | 123 | count = count + 1; |
Rvs94 | 20:f5091e29cd26 | 124 | if(count > 2) |
Rvs94 | 20:f5091e29cd26 | 125 | { |
Rvs94 | 20:f5091e29cd26 | 126 | count = 2; |
Rvs94 | 20:f5091e29cd26 | 127 | } |
Rvs94 | 20:f5091e29cd26 | 128 | |
Rvs94 | 20:f5091e29cd26 | 129 | } |
Rvs94 | 20:f5091e29cd26 | 130 | if(c == 'd') //Ga 1 programma omlaag |
Rvs94 | 20:f5091e29cd26 | 131 | { |
Rvs94 | 20:f5091e29cd26 | 132 | count = count - 1; |
Rvs94 | 20:f5091e29cd26 | 133 | if(count < 0) |
Rvs94 | 20:f5091e29cd26 | 134 | { |
Rvs94 | 20:f5091e29cd26 | 135 | count = 0; |
Rvs94 | 20:f5091e29cd26 | 136 | } |
Rvs94 | 20:f5091e29cd26 | 137 | } |
Rvs94 | 20:f5091e29cd26 | 138 | if(count == 0) //Motor 1 control |
Rvs94 | 20:f5091e29cd26 | 139 | { |
Rvs94 | 20:f5091e29cd26 | 140 | |
Rvs94 | 20:f5091e29cd26 | 141 | LedR = LedB = 1; |
Rvs94 | 20:f5091e29cd26 | 142 | LedG = 0; |
Margreeth95 | 19:9417d2011e8b | 143 | if(c == 'r') |
Margreeth95 | 19:9417d2011e8b | 144 | { |
Margreeth95 | 19:9417d2011e8b | 145 | m2_ref = m2_ref + 5; |
Margreeth95 | 19:9417d2011e8b | 146 | if (m2_ref > 90) |
Margreeth95 | 19:9417d2011e8b | 147 | { |
Margreeth95 | 19:9417d2011e8b | 148 | m2_ref = 90; |
Margreeth95 | 19:9417d2011e8b | 149 | } |
Margreeth95 | 19:9417d2011e8b | 150 | } |
Margreeth95 | 19:9417d2011e8b | 151 | if(c == 'f') |
Margreeth95 | 19:9417d2011e8b | 152 | { |
Margreeth95 | 19:9417d2011e8b | 153 | m2_ref = m2_ref - 5; |
Margreeth95 | 19:9417d2011e8b | 154 | if (m2_ref < -90) |
Margreeth95 | 19:9417d2011e8b | 155 | { |
Margreeth95 | 19:9417d2011e8b | 156 | m2_ref = -90; |
Margreeth95 | 19:9417d2011e8b | 157 | } |
Margreeth95 | 19:9417d2011e8b | 158 | } |
Rvs94 | 20:f5091e29cd26 | 159 | } |
Rvs94 | 20:f5091e29cd26 | 160 | if(count == 1) //Motor 2 control |
Rvs94 | 20:f5091e29cd26 | 161 | { |
Rvs94 | 20:f5091e29cd26 | 162 | LedG = LedB = 1; |
Rvs94 | 20:f5091e29cd26 | 163 | LedR = 0; |
Rvs94 | 20:f5091e29cd26 | 164 | } |
Rvs94 | 20:f5091e29cd26 | 165 | if(count == 2) //Vuur mechanisme |
Rvs94 | 20:f5091e29cd26 | 166 | { |
Rvs94 | 20:f5091e29cd26 | 167 | |
Rvs94 | 20:f5091e29cd26 | 168 | LedR = LedG = 1; |
Rvs94 | 20:f5091e29cd26 | 169 | LedB = 0; |
Rvs94 | 20:f5091e29cd26 | 170 | } |
Margreeth95 | 0:284ed397e046 | 171 | } |
Rvs94 | 9:774fc3c6a39e | 172 | |
Margreeth95 | 0:284ed397e046 | 173 | } |