Motor control, feedback, PI controller, BiQuad filter
Dependencies: FastPWM HIDScope MODSERIAL biquadFilter mbed QEI
main.cpp@10:b002572e37fd, 2018-10-15 (annotated)
- Committer:
- 1856413
- Date:
- Mon Oct 15 13:53:15 2018 +0000
- Revision:
- 10:b002572e37fd
- Parent:
- 8:ceb9abb5a4a8
- Child:
- 11:4e3ef6150a2e
Added GetReferenceVelocity and GetMeasuredVelocity functions
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 | 10:b002572e37fd | 10 | QEI Encoder (D12, D13, NC, 64, QEI::X4_ENCODING); |
1856413 | 8:ceb9abb5a4a8 | 11 | Ticker MotorSpeedCounts; |
1856413 | 10:b002572e37fd | 12 | Ticker measurevelocity; |
1856413 | 10:b002572e37fd | 13 | |
1856413 | 10:b002572e37fd | 14 | //Global variables |
1856413 | 10:b002572e37fd | 15 | volatile double RotationalPosition = 0.0; |
1856413 | 10:b002572e37fd | 16 | volatile double measuredVelocity = 0.0; |
1856413 | 10:b002572e37fd | 17 | const double tickertime = 0.001; |
nicollevanrijswijk | 5:a1fb2d2fb2d0 | 18 | |
1856413 | 0:2e33035d4e86 | 19 | void Motor() |
1856413 | 0:2e33035d4e86 | 20 | { |
1856413 | 1:c19fc63d555f | 21 | // Aflezen Potentiometers voor PWM |
1856413 | 10:b002572e37fd | 22 | motor1_pwm = potMeter1.read(); // Aflezen PotMeter 1 |
1856413 | 8:ceb9abb5a4a8 | 23 | // Encoder counts printen |
1856413 | 10:b002572e37fd | 24 | //pc.printf("%i\r\n", Encoder.getPulses()); |
1856413 | 0:2e33035d4e86 | 25 | } |
1856413 | 1:c19fc63d555f | 26 | |
1856413 | 8:ceb9abb5a4a8 | 27 | void changeDirectionButton() |
1856413 | 1:c19fc63d555f | 28 | { |
1856413 | 10:b002572e37fd | 29 | motor1_direction = 1 - motor1_direction; // |
1856413 | 10:b002572e37fd | 30 | pc.printf("Motor direction value %i \r\n", motor1_direction); |
1856413 | 1:c19fc63d555f | 31 | } |
1856413 | 0:2e33035d4e86 | 32 | |
1856413 | 8:ceb9abb5a4a8 | 33 | double GetReferenceVelocity() |
1856413 | 8:ceb9abb5a4a8 | 34 | { |
1856413 | 8:ceb9abb5a4a8 | 35 | // Returns reference velocity in rad/s. |
1856413 | 8:ceb9abb5a4a8 | 36 | // Positive value means clockwise rotation. |
1856413 | 8:ceb9abb5a4a8 | 37 | // 60 rpm = 60*2*pi/60 = 6.28 ~ 6.2 rad/s |
1856413 | 10:b002572e37fd | 38 | const double maxVelocity=6.2; // in rad/s of course! |
1856413 | 8:ceb9abb5a4a8 | 39 | double referenceVelocity; // in rad/s |
1856413 | 10:b002572e37fd | 40 | switch (motor1_direction) |
1856413 | 10:b002572e37fd | 41 | { |
1856413 | 10:b002572e37fd | 42 | case 0: // check if motor1_direction is 0 |
1856413 | 8:ceb9abb5a4a8 | 43 | // Clockwise rotation |
1856413 | 10:b002572e37fd | 44 | referenceVelocity = potMeter1 * maxVelocity; |
1856413 | 10:b002572e37fd | 45 | pc.printf("Reference velocity is %f. \r\n", referenceVelocity); |
1856413 | 10:b002572e37fd | 46 | break; |
1856413 | 10:b002572e37fd | 47 | case 1: // check if motor1_direction is 1 |
1856413 | 8:ceb9abb5a4a8 | 48 | // Counterclockwise rotation |
1856413 | 10:b002572e37fd | 49 | referenceVelocity = -1*potMeter1 * maxVelocity; |
1856413 | 10:b002572e37fd | 50 | pc.printf("Reference velocity is %f. \r\n", referenceVelocity); |
1856413 | 10:b002572e37fd | 51 | break; |
1856413 | 10:b002572e37fd | 52 | default: |
1856413 | 10:b002572e37fd | 53 | pc.printf("Something has gone wrong. \r \n"); |
1856413 | 8:ceb9abb5a4a8 | 54 | } |
1856413 | 8:ceb9abb5a4a8 | 55 | return referenceVelocity; |
1856413 | 8:ceb9abb5a4a8 | 56 | } |
1856413 | 8:ceb9abb5a4a8 | 57 | |
1856413 | 10:b002572e37fd | 58 | void GetMeasuredVelocity() |
1856413 | 8:ceb9abb5a4a8 | 59 | { |
1856413 | 8:ceb9abb5a4a8 | 60 | // Get actual velocity from the motor plant |
1856413 | 8:ceb9abb5a4a8 | 61 | // Use encoder (counts) |
1856413 | 10:b002572e37fd | 62 | double OldRotationalPosition = RotationalPosition; |
1856413 | 10:b002572e37fd | 63 | double counts = Encoder.getPulses(); |
1856413 | 10:b002572e37fd | 64 | RotationalPosition = ( counts / 4 * 64) * 6.28; // Rotational position in radians |
1856413 | 8:ceb9abb5a4a8 | 65 | // hier komt de berekening van measured velocity |
1856413 | 10:b002572e37fd | 66 | double measuredVelocity = (RotationalPosition - OldRotationalPosition) / tickertime; |
1856413 | 10:b002572e37fd | 67 | //measuredVelocity defined as global variable, doesn't have to be returned anymore, is stored in memory all the time; |
1856413 | 8:ceb9abb5a4a8 | 68 | } |
1856413 | 8:ceb9abb5a4a8 | 69 | |
1856413 | 8:ceb9abb5a4a8 | 70 | |
1856413 | 0:2e33035d4e86 | 71 | int main() |
1856413 | 0:2e33035d4e86 | 72 | { |
1856413 | 6:bd73804c8cec | 73 | pc.baud(115200); |
1856413 | 10:b002572e37fd | 74 | motor1_direction = 0; |
1856413 | 0:2e33035d4e86 | 75 | motor1_pwm.period_us(60.0); // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to done once), FastPWM variabele |
1856413 | 8:ceb9abb5a4a8 | 76 | MotorSpeedCounts.attach(Motor, 0.5); // Ticker die de functie Motor aanroept elke halve seconde, meer tijd is tragere respons op potmeter |
1856413 | 8:ceb9abb5a4a8 | 77 | button2.rise(changeDirectionButton); |
1856413 | 10:b002572e37fd | 78 | measurevelocity.attach(GetMeasuredVelocity, tickertime); |
1856413 | 10:b002572e37fd | 79 | |
1856413 | 10:b002572e37fd | 80 | double referenceSnelheid = GetReferenceVelocity(); |
1856413 | 10:b002572e37fd | 81 | pc.printf("Reference velocity is %f \r\n Measured velocity is %f. \r\n", referenceSnelheid, measuredVelocity); |
1856413 | 10:b002572e37fd | 82 | |
1856413 | 0:2e33035d4e86 | 83 | while(true){} // Endless loop |
1856413 | 0:2e33035d4e86 | 84 | } |