Using HIDScope for P(I)D controller
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Fork of PES_tutorial_5 by
main.cpp@11:4e3ef6150a2e, 2018-10-15 (annotated)
- Committer:
- nicollevanrijswijk
- Date:
- Mon Oct 15 14:36:33 2018 +0000
- Revision:
- 11:4e3ef6150a2e
- Parent:
- 9:b002572e37fd
- Child:
- 12:1ecd11dc2c00
Adding Proportional controller (assignment 1.3)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
1856413 | 0:2e33035d4e86 | 1 | #include "mbed.h" |
1856413 | 0:2e33035d4e86 | 2 | #include "FastPWM.h" // FastPWM library |
1856413 | 2:34c14fb36b5d | 3 | #include "MODSERIAL.h" |
lweersink | 4:49c5fd62a192 | 4 | #include "QEI.h" |
1856413 | 2:34c14fb36b5d | 5 | MODSERIAL pc(USBTX, USBRX); |
1856413 | 0:2e33035d4e86 | 6 | DigitalOut motor1_direction(D7); |
1856413 | 8:ceb9abb5a4a8 | 7 | AnalogIn potMeter1(A4); |
1856413 | 8:ceb9abb5a4a8 | 8 | InterruptIn button2(D3); |
1856413 | 1:c19fc63d555f | 9 | FastPWM motor1_pwm(D6); //FastPWM input, PES lecture 2 |
1856413 | 9:b002572e37fd | 10 | QEI Encoder (D12, D13, NC, 64, QEI::X4_ENCODING); |
1856413 | 8:ceb9abb5a4a8 | 11 | Ticker MotorSpeedCounts; |
1856413 | 9:b002572e37fd | 12 | Ticker measurevelocity; |
1856413 | 9:b002572e37fd | 13 | |
1856413 | 9:b002572e37fd | 14 | //Global variables |
1856413 | 9:b002572e37fd | 15 | volatile double RotationalPosition = 0.0; |
1856413 | 9:b002572e37fd | 16 | volatile double measuredVelocity = 0.0; |
1856413 | 9:b002572e37fd | 17 | const double tickertime = 0.001; |
nicollevanrijswijk | 5:a1fb2d2fb2d0 | 18 | |
nicollevanrijswijk | 11:4e3ef6150a2e | 19 | /*void Motor() |
1856413 | 0:2e33035d4e86 | 20 | { |
1856413 | 1:c19fc63d555f | 21 | // Aflezen Potentiometers voor PWM |
1856413 | 9:b002572e37fd | 22 | motor1_pwm = potMeter1.read(); // Aflezen PotMeter 1 |
nicollevanrijswijk | 11:4e3ef6150a2e | 23 | |
1856413 | 8:ceb9abb5a4a8 | 24 | // Encoder counts printen |
1856413 | 9:b002572e37fd | 25 | //pc.printf("%i\r\n", Encoder.getPulses()); |
nicollevanrijswijk | 11:4e3ef6150a2e | 26 | }*/ |
nicollevanrijswijk | 11:4e3ef6150a2e | 27 | |
nicollevanrijswijk | 11:4e3ef6150a2e | 28 | double ReferencePosition() |
nicollevanrijswijk | 11:4e3ef6150a2e | 29 | { |
nicollevanrijswijk | 11:4e3ef6150a2e | 30 | double y_ref = 314.0*potMeter1.read(); // Reference value y, scaled from 0 to 100 pi |
nicollevanrijswijk | 11:4e3ef6150a2e | 31 | pc.printf('Value potmeter is %f\r\n', y_ref); |
nicollevanrijswijk | 11:4e3ef6150a2e | 32 | return y_ref; |
1856413 | 0:2e33035d4e86 | 33 | } |
nicollevanrijswijk | 11:4e3ef6150a2e | 34 | |
nicollevanrijswijk | 11:4e3ef6150a2e | 35 | double EncoderPosition() |
nicollevanrijswijk | 11:4e3ef6150a2e | 36 | { |
nicollevanrijswijk | 11:4e3ef6150a2e | 37 | double OldRotationalPosition = RotationalPosition; |
nicollevanrijswijk | 11:4e3ef6150a2e | 38 | double counts = Encoder.getPulses(); |
nicollevanrijswijk | 11:4e3ef6150a2e | 39 | RotationalPosition = ( counts / 4.0 * 64.0) * 6.28; // Rotational position in radians |
nicollevanrijswijk | 11:4e3ef6150a2e | 40 | return RotationalPosition; |
nicollevanrijswijk | 11:4e3ef6150a2e | 41 | } |
nicollevanrijswijk | 11:4e3ef6150a2e | 42 | |
nicollevanrijswijk | 11:4e3ef6150a2e | 43 | void PositionGain() |
nicollevanrijswijk | 11:4e3ef6150a2e | 44 | { |
nicollevanrijswijk | 11:4e3ef6150a2e | 45 | double Kp = 10.0*potMeter2.read(); // Scale 0 to 10 |
nicollevanrijswijk | 11:4e3ef6150a2e | 46 | } |
nicollevanrijswijk | 11:4e3ef6150a2e | 47 | |
nicollevanrijswijk | 11:4e3ef6150a2e | 48 | |
1856413 | 8:ceb9abb5a4a8 | 49 | void changeDirectionButton() |
1856413 | 1:c19fc63d555f | 50 | { |
1856413 | 9:b002572e37fd | 51 | motor1_direction = 1 - motor1_direction; // |
1856413 | 9:b002572e37fd | 52 | pc.printf("Motor direction value %i \r\n", motor1_direction); |
1856413 | 1:c19fc63d555f | 53 | } |
1856413 | 0:2e33035d4e86 | 54 | |
nicollevanrijswijk | 11:4e3ef6150a2e | 55 | /* double GetReferenceVelocity() |
1856413 | 8:ceb9abb5a4a8 | 56 | { |
1856413 | 8:ceb9abb5a4a8 | 57 | // Returns reference velocity in rad/s. |
1856413 | 8:ceb9abb5a4a8 | 58 | // Positive value means clockwise rotation. |
1856413 | 8:ceb9abb5a4a8 | 59 | // 60 rpm = 60*2*pi/60 = 6.28 ~ 6.2 rad/s |
1856413 | 9:b002572e37fd | 60 | const double maxVelocity=6.2; // in rad/s of course! |
1856413 | 8:ceb9abb5a4a8 | 61 | double referenceVelocity; // in rad/s |
1856413 | 9:b002572e37fd | 62 | switch (motor1_direction) |
1856413 | 9:b002572e37fd | 63 | { |
1856413 | 9:b002572e37fd | 64 | case 0: // check if motor1_direction is 0 |
1856413 | 8:ceb9abb5a4a8 | 65 | // Clockwise rotation |
1856413 | 9:b002572e37fd | 66 | referenceVelocity = potMeter1 * maxVelocity; |
1856413 | 9:b002572e37fd | 67 | pc.printf("Reference velocity is %f. \r\n", referenceVelocity); |
1856413 | 9:b002572e37fd | 68 | break; |
1856413 | 9:b002572e37fd | 69 | case 1: // check if motor1_direction is 1 |
1856413 | 8:ceb9abb5a4a8 | 70 | // Counterclockwise rotation |
1856413 | 9:b002572e37fd | 71 | referenceVelocity = -1*potMeter1 * maxVelocity; |
1856413 | 9:b002572e37fd | 72 | pc.printf("Reference velocity is %f. \r\n", referenceVelocity); |
1856413 | 9:b002572e37fd | 73 | break; |
1856413 | 9:b002572e37fd | 74 | default: |
1856413 | 9:b002572e37fd | 75 | pc.printf("Something has gone wrong. \r \n"); |
1856413 | 8:ceb9abb5a4a8 | 76 | } |
1856413 | 8:ceb9abb5a4a8 | 77 | return referenceVelocity; |
nicollevanrijswijk | 11:4e3ef6150a2e | 78 | } */ |
1856413 | 8:ceb9abb5a4a8 | 79 | |
nicollevanrijswijk | 11:4e3ef6150a2e | 80 | /*void GetMeasuredVelocity() |
1856413 | 8:ceb9abb5a4a8 | 81 | { |
1856413 | 8:ceb9abb5a4a8 | 82 | // Get actual velocity from the motor plant |
1856413 | 8:ceb9abb5a4a8 | 83 | // Use encoder (counts) |
1856413 | 9:b002572e37fd | 84 | double OldRotationalPosition = RotationalPosition; |
1856413 | 9:b002572e37fd | 85 | double counts = Encoder.getPulses(); |
1856413 | 9:b002572e37fd | 86 | RotationalPosition = ( counts / 4 * 64) * 6.28; // Rotational position in radians |
1856413 | 8:ceb9abb5a4a8 | 87 | // hier komt de berekening van measured velocity |
1856413 | 9:b002572e37fd | 88 | double measuredVelocity = (RotationalPosition - OldRotationalPosition) / tickertime; |
1856413 | 9:b002572e37fd | 89 | //measuredVelocity defined as global variable, doesn't have to be returned anymore, is stored in memory all the time; |
nicollevanrijswijk | 11:4e3ef6150a2e | 90 | }*/ |
nicollevanrijswijk | 11:4e3ef6150a2e | 91 | |
nicollevanrijswijk | 11:4e3ef6150a2e | 92 | double Error() |
nicollevanrijswijk | 11:4e3ef6150a2e | 93 | { |
nicollevanrijswijk | 11:4e3ef6150a2e | 94 | double Error = ReferencePosition()-EncoderPosition(); |
nicollevanrijswijk | 11:4e3ef6150a2e | 95 | return Error; |
1856413 | 8:ceb9abb5a4a8 | 96 | } |
1856413 | 8:ceb9abb5a4a8 | 97 | |
1856413 | 0:2e33035d4e86 | 98 | int main() |
1856413 | 0:2e33035d4e86 | 99 | { |
1856413 | 6:bd73804c8cec | 100 | pc.baud(115200); |
nicollevanrijswijk | 11:4e3ef6150a2e | 101 | bool motor1_direction = 0; |
1856413 | 0:2e33035d4e86 | 102 | motor1_pwm.period_us(60.0); // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to done once), FastPWM variabele |
nicollevanrijswijk | 11:4e3ef6150a2e | 103 | //MotorSpeedCounts.attach(ReferencePosition, 0.5); // Ticker die de functie Motor aanroept elke halve seconde, meer tijd is tragere respons op potmeter |
1856413 | 8:ceb9abb5a4a8 | 104 | button2.rise(changeDirectionButton); |
nicollevanrijswijk | 11:4e3ef6150a2e | 105 | //measurevelocity.attach(GetMeasuredVelocity, tickertime); |
1856413 | 9:b002572e37fd | 106 | |
nicollevanrijswijk | 11:4e3ef6150a2e | 107 | //double referenceSnelheid = GetReferenceVelocity(); |
nicollevanrijswijk | 11:4e3ef6150a2e | 108 | //pc.printf("Reference velocity is %f \r\n Measured velocity is %f. \r\n", referenceSnelheid, measuredVelocity); |
1856413 | 9:b002572e37fd | 109 | |
nicollevanrijswijk | 11:4e3ef6150a2e | 110 | while(Error ==! 0) // when reference postion is reached, stop with the while loop |
nicollevanrijswijk | 11:4e3ef6150a2e | 111 | { |
nicollevanrijswijk | 11:4e3ef6150a2e | 112 | MotorSpeedCounts.attach(PositionGain,0.5); |
nicollevanrijswijk | 11:4e3ef6150a2e | 113 | } |
nicollevanrijswijk | 11:4e3ef6150a2e | 114 | } |