groep 6
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of frdm_Motor_V2_2 by
main.cpp@11:0793a78109a2, 2015-09-29 (annotated)
- Committer:
- Rvs94
- Date:
- Tue Sep 29 14:01:47 2015 +0000
- Revision:
- 11:0793a78109a2
- Parent:
- 10:80fe931a71e4
- Child:
- 12:69ab81cf5b7d
Werkende PI controller, zonder filtering. Volgende stap is filtering en implementatie PID controller. Kp = 0.07, Ki = 0.005
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" |
Margreeth95 | 0:284ed397e046 | 5 | |
Margreeth95 | 0:284ed397e046 | 6 | Serial pc(USBTX, USBRX); // tx, rx |
Margreeth95 | 0:284ed397e046 | 7 | DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield) |
Margreeth95 | 0:284ed397e046 | 8 | PwmOut motor2speed(D5); |
Margreeth95 | 0:284ed397e046 | 9 | DigitalIn button1(SW3); |
Rvs94 | 9:774fc3c6a39e | 10 | DigitalIn encoderA(D3); |
Rvs94 | 9:774fc3c6a39e | 11 | DigitalIn encoderB(D2); |
Rvs94 | 9:774fc3c6a39e | 12 | AnalogIn potmeter2(A5); |
Rvs94 | 5:455773cf460b | 13 | QEI Encoder(D3, D2, NC, 128); |
Rvs94 | 1:48aba8d5610a | 14 | HIDScope scope(3); |
Margreeth95 | 0:284ed397e046 | 15 | Ticker ScopeTime; |
Rvs94 | 9:774fc3c6a39e | 16 | Ticker myControllerTicker; |
Rvs94 | 7:67b50d4fb03c | 17 | |
Rvs94 | 7:67b50d4fb03c | 18 | double Aantal_Degs; |
Rvs94 | 7:67b50d4fb03c | 19 | double Aantal_pulses; |
Rvs94 | 10:80fe931a71e4 | 20 | double reference; |
Rvs94 | 10:80fe931a71e4 | 21 | double position; |
Rvs94 | 2:099da0fc31b6 | 22 | |
Margreeth95 | 0:284ed397e046 | 23 | void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt |
Margreeth95 | 0:284ed397e046 | 24 | { |
Margreeth95 | 0:284ed397e046 | 25 | scope.set(0, motor2direction.read()); |
Margreeth95 | 0:284ed397e046 | 26 | scope.set(1, motor2speed.read()); |
Rvs94 | 1:48aba8d5610a | 27 | scope.set(2, Aantal_Degs); |
Rvs94 | 10:80fe931a71e4 | 28 | Aantal_Degs = Encoder.getPulses()*360/(0.5*128*131); |
Rvs94 | 4:0d4aff8b57b3 | 29 | |
Margreeth95 | 0:284ed397e046 | 30 | scope.send(); |
Rvs94 | 1:48aba8d5610a | 31 | |
Margreeth95 | 0:284ed397e046 | 32 | } |
Rvs94 | 9:774fc3c6a39e | 33 | // Controller gain |
Rvs94 | 11:0793a78109a2 | 34 | const double m2_Kp = 0.07,m2_Ki = 0.005; |
Rvs94 | 11:0793a78109a2 | 35 | const double m2_Ts = 0.01; |
Rvs94 | 11:0793a78109a2 | 36 | double m2_err_int = 0; |
Rvs94 | 10:80fe931a71e4 | 37 | |
Rvs94 | 11:0793a78109a2 | 38 | // Reusable PI controller |
Rvs94 | 11:0793a78109a2 | 39 | double P( double error, const double Kp, const double Ki, double Ts, double &e_int) |
Rvs94 | 9:774fc3c6a39e | 40 | { |
Rvs94 | 11:0793a78109a2 | 41 | e_int = e_int + Ts*error; |
Rvs94 | 11:0793a78109a2 | 42 | return Kp * error + Ki*e_int; |
Rvs94 | 9:774fc3c6a39e | 43 | } |
Margreeth95 | 0:284ed397e046 | 44 | |
Rvs94 | 9:774fc3c6a39e | 45 | // Next task, measure the error and apply the output to the plant |
Rvs94 | 11:0793a78109a2 | 46 | void motor2_Controller() |
Rvs94 | 9:774fc3c6a39e | 47 | { |
Rvs94 | 10:80fe931a71e4 | 48 | reference = potmeter2.read()*360; |
Rvs94 | 10:80fe931a71e4 | 49 | position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs |
Rvs94 | 11:0793a78109a2 | 50 | double P2 = P( reference - position, m2_Kp, m2_Ki, m2_Ts, m2_err_int); |
Rvs94 | 9:774fc3c6a39e | 51 | motor2speed = abs(P2); |
Rvs94 | 9:774fc3c6a39e | 52 | if(P2 > 0) |
Rvs94 | 9:774fc3c6a39e | 53 | { |
Rvs94 | 9:774fc3c6a39e | 54 | motor2direction = 0; |
Rvs94 | 9:774fc3c6a39e | 55 | } |
Rvs94 | 9:774fc3c6a39e | 56 | else |
Rvs94 | 9:774fc3c6a39e | 57 | { |
Rvs94 | 9:774fc3c6a39e | 58 | motor2direction = 1; |
Rvs94 | 9:774fc3c6a39e | 59 | } |
Rvs94 | 9:774fc3c6a39e | 60 | |
Rvs94 | 9:774fc3c6a39e | 61 | } |
Rvs94 | 3:687729d7996e | 62 | |
Margreeth95 | 0:284ed397e046 | 63 | int main() |
Rvs94 | 9:774fc3c6a39e | 64 | { |
Margreeth95 | 0:284ed397e046 | 65 | pc.baud(115200); |
Rvs94 | 3:687729d7996e | 66 | pc.printf("Tot aan loop werkt\n"); |
Rvs94 | 9:774fc3c6a39e | 67 | |
Margreeth95 | 0:284ed397e046 | 68 | ScopeTime.attach_us(&ScopeSend, 10e4); |
Rvs94 | 11:0793a78109a2 | 69 | myControllerTicker.attach( &motor2_Controller, 0.01f ); // 100 Hz |
Rvs94 | 9:774fc3c6a39e | 70 | while(true) |
Margreeth95 | 0:284ed397e046 | 71 | { |
Rvs94 | 10:80fe931a71e4 | 72 | pc.printf("position = %f aantal degs = %f \n",reference,position); |
Rvs94 | 10:80fe931a71e4 | 73 | wait(0.2f); |
Margreeth95 | 0:284ed397e046 | 74 | } |
Rvs94 | 9:774fc3c6a39e | 75 | |
Margreeth95 | 0:284ed397e046 | 76 | } |