adsfadsfa
Dependencies: Encoder MODSERIAL feed_forward mbed
Fork of feed_forward by
main.cpp@2:68de33d10c67, 2017-10-11 (annotated)
- Committer:
- DiondeGreef
- Date:
- Wed Oct 11 09:49:09 2017 +0000
- Revision:
- 2:68de33d10c67
- Parent:
- 1:92a60278860a
alkdfjakljasdfladksfj;
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 gpo(D0); |
RoyvZ | 0:331597250051 | 7 | DigitalOut motorDirection(D4); |
DiondeGreef | 1:92a60278860a | 8 | PwmOut motorSpeed(D5); |
DiondeGreef | 2:68de33d10c67 | 9 | AnalogIn potMeterIn1(A1); |
DiondeGreef | 2:68de33d10c67 | 10 | AnalogIn potMeterIn2(A2); |
DiondeGreef | 2:68de33d10c67 | 11 | //InterruptIn button1(D3); |
DiondeGreef | 2:68de33d10c67 | 12 | Ticker m1_Ticker; |
DiondeGreef | 2:68de33d10c67 | 13 | Encoder encoder1(D13,D12); |
DiondeGreef | 2:68de33d10c67 | 14 | const double M1_KP = 2.5, M1_KI = 1.0; |
DiondeGreef | 2:68de33d10c67 | 15 | const double M1_TS = 0.01; |
DiondeGreef | 2:68de33d10c67 | 16 | const double RAD_PER_PULSE = 0.002991; |
DiondeGreef | 2:68de33d10c67 | 17 | double m1_err_int = 0; |
DiondeGreef | 2:68de33d10c67 | 18 | int motorD = 0; |
DiondeGreef | 2:68de33d10c67 | 19 | double motor1 = 0; |
RoyvZ | 0:331597250051 | 20 | |
RoyvZ | 0:331597250051 | 21 | MODSERIAL pc(USBTX, USBRX); |
RoyvZ | 0:331597250051 | 22 | |
RoyvZ | 0:331597250051 | 23 | |
DiondeGreef | 2:68de33d10c67 | 24 | // Reusable PI controller |
DiondeGreef | 2:68de33d10c67 | 25 | double PI( double e, const double Kp, const double Ki, double Ts, double &e_int ){ |
DiondeGreef | 2:68de33d10c67 | 26 | e_int += Ts ∗ e; // e_int is changed globally because it’s ’by reference’ (&) |
DiondeGreef | 2:68de33d10c67 | 27 | return Kp ∗ e + Ki ∗ e_int; |
RoyvZ | 0:331597250051 | 28 | } |
DiondeGreef | 2:68de33d10c67 | 29 | // Next task, measure the error and apply the output to the plant |
DiondeGreef | 2:68de33d10c67 | 30 | void m1_Controller() { |
DiondeGreef | 2:68de33d10c67 | 31 | double reference = potMeterIn1; |
DiondeGreef | 2:68de33d10c67 | 32 | |
DiondeGreef | 2:68de33d10c67 | 33 | |
DiondeGreef | 2:68de33d10c67 | 34 | double position = RAD_PER_PULSE∗encoder1; // Don’t use magic numbers! |
DiondeGreef | 2:68de33d10c67 | 35 | motor1 = PI( reference − position, M1_KP, M1_KI, M1_TS, m1_err_int ); |
DiondeGreef | 2:68de33d10c67 | 36 | if ( reference − position >= 0) |
RoyvZ | 0:331597250051 | 37 | { |
RoyvZ | 0:331597250051 | 38 | //float motor1DirectionPin1 = 1; |
DiondeGreef | 2:68de33d10c67 | 39 | motorD=1; |
RoyvZ | 0:331597250051 | 40 | } |
RoyvZ | 0:331597250051 | 41 | else |
RoyvZ | 0:331597250051 | 42 | { |
RoyvZ | 0:331597250051 | 43 | //float motor1DirectionPin1 = 0; |
DiondeGreef | 2:68de33d10c67 | 44 | motorD=0; |
RoyvZ | 0:331597250051 | 45 | } |
RoyvZ | 0:331597250051 | 46 | } |
RoyvZ | 0:331597250051 | 47 | |
RoyvZ | 0:331597250051 | 48 | int main() { |
DiondeGreef | 2:68de33d10c67 | 49 | m1_Ticker.attach( &m1_Controller, M1_TS ); // 100 Hz |
DiondeGreef | 2:68de33d10c67 | 50 | while( 1 ) { |
DiondeGreef | 2:68de33d10c67 | 51 | motorDirection.write(motorD); |
DiondeGreef | 2:68de33d10c67 | 52 | motorSpeed.write(motor1); |
RoyvZ | 0:331597250051 | 53 | } |
RoyvZ | 0:331597250051 | 54 | } |
DiondeGreef | 2:68de33d10c67 | 55 | } |
RoyvZ | 0:331597250051 | 56 |