![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Werkend voor onze motor
Dependencies: Encoder MODSERIAL QEI mbed
Fork of feed_forward_PI_copy by
main.cpp@4:840ce3b21767, 2017-11-01 (annotated)
- Committer:
- RoyvZ
- Date:
- Wed Nov 01 15:09:54 2017 +0000
- Revision:
- 4:840ce3b21767
- Parent:
- 3:91e4ed7b6748
Prima;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
RoyvZ | 0:331597250051 | 1 | #include "mbed.h" |
RoyvZ | 0:331597250051 | 2 | #include "MODSERIAL.h" |
RoyvZ | 0:331597250051 | 3 | #include "math.h" |
DiondeGreef | 2:68de33d10c67 | 4 | #include "encoder.h" |
RoyvZ | 0:331597250051 | 5 | |
RoyvZ | 0:331597250051 | 6 | DigitalOut motorDirection(D4); |
DiondeGreef | 1:92a60278860a | 7 | PwmOut motorSpeed(D5); |
DiondeGreef | 2:68de33d10c67 | 8 | AnalogIn potMeterIn1(A1); |
DiondeGreef | 2:68de33d10c67 | 9 | AnalogIn potMeterIn2(A2); |
RoyvZ | 3:91e4ed7b6748 | 10 | InterruptIn button(D11); |
DiondeGreef | 2:68de33d10c67 | 11 | Ticker m1_Ticker; |
RoyvZ | 3:91e4ed7b6748 | 12 | Ticker SP_m1_Ticker; |
RoyvZ | 4:840ce3b21767 | 13 | Encoder encoder1(D9,D8); |
RoyvZ | 3:91e4ed7b6748 | 14 | float M1_KP = 2.5; |
RoyvZ | 3:91e4ed7b6748 | 15 | float M1_KI = 1.0; |
RoyvZ | 3:91e4ed7b6748 | 16 | const float M1_TS = 0.01; |
RoyvZ | 3:91e4ed7b6748 | 17 | const float SP_TS = 0.01; |
RoyvZ | 3:91e4ed7b6748 | 18 | const float RAD_PER_PULSE = 0.000119; |
RoyvZ | 3:91e4ed7b6748 | 19 | float m1_err_int = 0; |
DiondeGreef | 2:68de33d10c67 | 20 | int motorD = 0; |
RoyvZ | 3:91e4ed7b6748 | 21 | float motor1 = 0; |
RoyvZ | 3:91e4ed7b6748 | 22 | float reference = 0; |
RoyvZ | 3:91e4ed7b6748 | 23 | float position = 0; |
RoyvZ | 3:91e4ed7b6748 | 24 | int outOfEncod = 0; |
RoyvZ | 3:91e4ed7b6748 | 25 | int KP_changer = 1; |
RoyvZ | 3:91e4ed7b6748 | 26 | float RotSpeed = 0; |
RoyvZ | 3:91e4ed7b6748 | 27 | |
RoyvZ | 3:91e4ed7b6748 | 28 | int outOfEncod_SP_1 = 0; |
RoyvZ | 3:91e4ed7b6748 | 29 | int outOfEncod_SP_2 = 0; |
RoyvZ | 3:91e4ed7b6748 | 30 | |
RoyvZ | 0:331597250051 | 31 | |
RoyvZ | 0:331597250051 | 32 | MODSERIAL pc(USBTX, USBRX); |
RoyvZ | 0:331597250051 | 33 | |
RoyvZ | 0:331597250051 | 34 | |
DiondeGreef | 2:68de33d10c67 | 35 | // Reusable PI controller |
RoyvZ | 3:91e4ed7b6748 | 36 | float PI( float e, const float Kp, const float Ki, float Ts, float &e_int ){ |
RoyvZ | 3:91e4ed7b6748 | 37 | e_int += Ts * e; // e_int is changed globally because it’s ’by reference’ (&) |
RoyvZ | 3:91e4ed7b6748 | 38 | return Kp * e + Ki * e_int; |
RoyvZ | 3:91e4ed7b6748 | 39 | } |
RoyvZ | 3:91e4ed7b6748 | 40 | |
RoyvZ | 3:91e4ed7b6748 | 41 | void Change() |
RoyvZ | 3:91e4ed7b6748 | 42 | { |
RoyvZ | 3:91e4ed7b6748 | 43 | if(KP_changer == 60){ |
RoyvZ | 3:91e4ed7b6748 | 44 | KP_changer = 0; |
RoyvZ | 3:91e4ed7b6748 | 45 | } |
RoyvZ | 3:91e4ed7b6748 | 46 | else{ |
RoyvZ | 3:91e4ed7b6748 | 47 | KP_changer++; |
RoyvZ | 3:91e4ed7b6748 | 48 | } |
RoyvZ | 3:91e4ed7b6748 | 49 | } |
RoyvZ | 3:91e4ed7b6748 | 50 | |
RoyvZ | 3:91e4ed7b6748 | 51 | void m1_Velocity(){ |
RoyvZ | 3:91e4ed7b6748 | 52 | outOfEncod_SP_1 = encoder1.getPosition(); |
RoyvZ | 3:91e4ed7b6748 | 53 | float RotDiff = (outOfEncod_SP_1 - outOfEncod_SP_2)*RAD_PER_PULSE; |
RoyvZ | 3:91e4ed7b6748 | 54 | outOfEncod_SP_2 = outOfEncod_SP_1; |
RoyvZ | 3:91e4ed7b6748 | 55 | RotSpeed = RotDiff/SP_TS; |
RoyvZ | 0:331597250051 | 56 | } |
DiondeGreef | 2:68de33d10c67 | 57 | // Next task, measure the error and apply the output to the plant |
DiondeGreef | 2:68de33d10c67 | 58 | void m1_Controller() { |
RoyvZ | 3:91e4ed7b6748 | 59 | reference = 5 * potMeterIn1.read(); |
RoyvZ | 3:91e4ed7b6748 | 60 | M1_KI = 2 * potMeterIn2.read(); |
RoyvZ | 3:91e4ed7b6748 | 61 | M1_KP = KP_changer*0.1; |
RoyvZ | 3:91e4ed7b6748 | 62 | outOfEncod = encoder1.getPosition(); |
RoyvZ | 3:91e4ed7b6748 | 63 | position = RAD_PER_PULSE * outOfEncod; |
RoyvZ | 3:91e4ed7b6748 | 64 | float ref_pos = reference - position; // Don’t use magic numbers! |
RoyvZ | 4:840ce3b21767 | 65 | if (ref_pos <= 0) // Don’t use magic numbers! |
RoyvZ | 4:840ce3b21767 | 66 | { |
RoyvZ | 4:840ce3b21767 | 67 | motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int ); |
RoyvZ | 4:840ce3b21767 | 68 | } |
RoyvZ | 4:840ce3b21767 | 69 | else |
RoyvZ | 4:840ce3b21767 | 70 | { |
RoyvZ | 4:840ce3b21767 | 71 | motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int ); |
RoyvZ | 4:840ce3b21767 | 72 | } |
RoyvZ | 3:91e4ed7b6748 | 73 | if (ref_pos <= 0) |
RoyvZ | 0:331597250051 | 74 | { |
RoyvZ | 0:331597250051 | 75 | //float motor1DirectionPin1 = 1; |
DiondeGreef | 2:68de33d10c67 | 76 | motorD=1; |
RoyvZ | 0:331597250051 | 77 | } |
RoyvZ | 0:331597250051 | 78 | else |
RoyvZ | 0:331597250051 | 79 | { |
RoyvZ | 0:331597250051 | 80 | //float motor1DirectionPin1 = 0; |
DiondeGreef | 2:68de33d10c67 | 81 | motorD=0; |
RoyvZ | 0:331597250051 | 82 | } |
RoyvZ | 3:91e4ed7b6748 | 83 | motorDirection.write(motorD); |
RoyvZ | 3:91e4ed7b6748 | 84 | motorSpeed.write(motor1); |
RoyvZ | 0:331597250051 | 85 | } |
RoyvZ | 0:331597250051 | 86 | |
RoyvZ | 0:331597250051 | 87 | int main() { |
RoyvZ | 3:91e4ed7b6748 | 88 | m1_Ticker.attach( &m1_Controller, M1_TS ); |
RoyvZ | 3:91e4ed7b6748 | 89 | SP_m1_Ticker.attach( &m1_Velocity, SP_TS ); // 100 Hz |
RoyvZ | 3:91e4ed7b6748 | 90 | pc.baud(115200); |
RoyvZ | 3:91e4ed7b6748 | 91 | pc.printf("Fuck you pc"); |
RoyvZ | 3:91e4ed7b6748 | 92 | m1_Controller(); |
RoyvZ | 3:91e4ed7b6748 | 93 | button.rise(&Change); |
RoyvZ | 3:91e4ed7b6748 | 94 | while(true) { |
RoyvZ | 3:91e4ed7b6748 | 95 | pc.printf("%f \t", RotSpeed); |
RoyvZ | 3:91e4ed7b6748 | 96 | pc.printf("%f \t", M1_KI); |
RoyvZ | 3:91e4ed7b6748 | 97 | pc.printf("%f \t", M1_KP); |
RoyvZ | 3:91e4ed7b6748 | 98 | pc.printf("%f \t", position); |
RoyvZ | 3:91e4ed7b6748 | 99 | pc.printf("%f \r\n", reference); |
RoyvZ | 3:91e4ed7b6748 | 100 | //wait(0.5); |
RoyvZ | 0:331597250051 | 101 | } |
RoyvZ | 0:331597250051 | 102 | } |
RoyvZ | 3:91e4ed7b6748 | 103 | |
RoyvZ | 0:331597250051 | 104 |