Werkende PID regelaar met PC input, nieuwe constanten (Kp etc) nog niet toegevoegd.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of frdm_Motor_V2_4 by
main.cpp@9:774fc3c6a39e, 2015-09-29 (annotated)
- Committer:
- Rvs94
- Date:
- Tue Sep 29 13:25:09 2015 +0000
- Revision:
- 9:774fc3c6a39e
- Parent:
- 8:69bde5e32dbf
- Child:
- 10:80fe931a71e4
Werkend, functies toegevoegd net als in college, aangestuurd door potmeter. Kp waarde is redelijk accuraat.
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 | 2:099da0fc31b6 | 20 | |
Margreeth95 | 0:284ed397e046 | 21 | void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt |
Margreeth95 | 0:284ed397e046 | 22 | { |
Margreeth95 | 0:284ed397e046 | 23 | scope.set(0, motor2direction.read()); |
Margreeth95 | 0:284ed397e046 | 24 | scope.set(1, motor2speed.read()); |
Rvs94 | 1:48aba8d5610a | 25 | scope.set(2, Aantal_Degs); |
Rvs94 | 5:455773cf460b | 26 | Aantal_Degs = Encoder.getPulses()*360/128/131; |
Rvs94 | 4:0d4aff8b57b3 | 27 | |
Margreeth95 | 0:284ed397e046 | 28 | scope.send(); |
Rvs94 | 1:48aba8d5610a | 29 | |
Margreeth95 | 0:284ed397e046 | 30 | } |
Rvs94 | 9:774fc3c6a39e | 31 | // Controller gain |
Rvs94 | 9:774fc3c6a39e | 32 | const double motor1_Kp = 0.05; |
Rvs94 | 9:774fc3c6a39e | 33 | // Reusable P controller |
Rvs94 | 9:774fc3c6a39e | 34 | double P( double error, const double Kp ) |
Rvs94 | 9:774fc3c6a39e | 35 | { |
Rvs94 | 9:774fc3c6a39e | 36 | return Kp * error; |
Rvs94 | 9:774fc3c6a39e | 37 | } |
Margreeth95 | 0:284ed397e046 | 38 | |
Rvs94 | 9:774fc3c6a39e | 39 | // Next task, measure the error and apply the output to the plant |
Rvs94 | 9:774fc3c6a39e | 40 | void motor1_Controller() |
Rvs94 | 9:774fc3c6a39e | 41 | { |
Rvs94 | 9:774fc3c6a39e | 42 | double reference = potmeter2.read()*360; |
Rvs94 | 9:774fc3c6a39e | 43 | double position = Encoder.getPulses()*360/128/131; // Aantal Degs |
Rvs94 | 9:774fc3c6a39e | 44 | double P2 = P( reference - position, motor1_Kp ); |
Rvs94 | 9:774fc3c6a39e | 45 | motor2speed = abs(P2); |
Rvs94 | 9:774fc3c6a39e | 46 | if(P2 > 0) |
Rvs94 | 9:774fc3c6a39e | 47 | { |
Rvs94 | 9:774fc3c6a39e | 48 | motor2direction = 0; |
Rvs94 | 9:774fc3c6a39e | 49 | } |
Rvs94 | 9:774fc3c6a39e | 50 | else |
Rvs94 | 9:774fc3c6a39e | 51 | { |
Rvs94 | 9:774fc3c6a39e | 52 | motor2direction = 1; |
Rvs94 | 9:774fc3c6a39e | 53 | } |
Rvs94 | 9:774fc3c6a39e | 54 | |
Rvs94 | 9:774fc3c6a39e | 55 | } |
Rvs94 | 3:687729d7996e | 56 | |
Margreeth95 | 0:284ed397e046 | 57 | int main() |
Rvs94 | 9:774fc3c6a39e | 58 | { |
Margreeth95 | 0:284ed397e046 | 59 | pc.baud(115200); |
Rvs94 | 3:687729d7996e | 60 | pc.printf("Tot aan loop werkt\n"); |
Rvs94 | 9:774fc3c6a39e | 61 | |
Margreeth95 | 0:284ed397e046 | 62 | ScopeTime.attach_us(&ScopeSend, 10e4); |
Rvs94 | 9:774fc3c6a39e | 63 | myControllerTicker.attach( &motor1_Controller, 0.01f ); // 100 Hz |
Rvs94 | 9:774fc3c6a39e | 64 | while(true) |
Margreeth95 | 0:284ed397e046 | 65 | { |
Rvs94 | 9:774fc3c6a39e | 66 | pc.printf("position = %f aantal degs = %f \n",potmeter2.read()*360,Aantal_Degs); |
Rvs94 | 9:774fc3c6a39e | 67 | wait(0.1f); |
Margreeth95 | 0:284ed397e046 | 68 | } |
Rvs94 | 9:774fc3c6a39e | 69 | |
Margreeth95 | 0:284ed397e046 | 70 | } |