![](/media/cache/group/9e3cc099b3b04bca937a1cca1da81b19.jpg.50x50_q85.jpg)
Poah als je je encoder in je motor wil aflezen en de beweging van je motor wilt aanpassen is dit programma echt voor jou!
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp@4:28d71b0a29aa, 2019-10-04 (annotated)
- Committer:
- Jellehierck
- Date:
- Fri Oct 04 12:58:04 2019 +0000
- Revision:
- 4:28d71b0a29aa
- Parent:
- 3:2d45e3d0b0f0
- Child:
- 5:54ce02ad7a50
Encoder angle and angular velocity can be read
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
freek100 | 0:1843eec2b552 | 1 | #include "mbed.h" |
freek100 | 0:1843eec2b552 | 2 | //#include "HIDScope.h" |
freek100 | 0:1843eec2b552 | 3 | #include "QEI.h" |
freek100 | 0:1843eec2b552 | 4 | #include "MODSERIAL.h" |
freek100 | 0:1843eec2b552 | 5 | //#include "BiQuad.h" |
Jellehierck | 2:6f5f300f0569 | 6 | #include "FastPWM.h" |
freek100 | 0:1843eec2b552 | 7 | |
freek100 | 0:1843eec2b552 | 8 | DigitalOut ledr(LED_RED); |
freek100 | 0:1843eec2b552 | 9 | DigitalOut ledg(LED_GREEN); |
freek100 | 0:1843eec2b552 | 10 | DigitalOut ledb(LED_BLUE); |
freek100 | 0:1843eec2b552 | 11 | PwmOut led1(D10); |
Jellehierck | 3:2d45e3d0b0f0 | 12 | DigitalIn button1(D11); |
Jellehierck | 2:6f5f300f0569 | 13 | AnalogIn potmeter(A0); |
freek100 | 0:1843eec2b552 | 14 | DigitalIn sw(SW2); |
freek100 | 0:1843eec2b552 | 15 | MODSERIAL pc(USBTX, USBRX); |
freek100 | 0:1843eec2b552 | 16 | DigitalIn encA(D13); |
freek100 | 0:1843eec2b552 | 17 | DigitalIn encB(D12); |
freek100 | 0:1843eec2b552 | 18 | QEI encoder(D13,D12,NC,64,QEI::X4_ENCODING); |
freek100 | 0:1843eec2b552 | 19 | |
Jellehierck | 3:2d45e3d0b0f0 | 20 | DigitalOut motor2Direction(D4); |
Jellehierck | 3:2d45e3d0b0f0 | 21 | FastPWM motor2Power(D5); |
Jellehierck | 3:2d45e3d0b0f0 | 22 | DigitalOut motor1Direction(D7); |
Jellehierck | 3:2d45e3d0b0f0 | 23 | FastPWM motor1Power(D6); |
Jellehierck | 3:2d45e3d0b0f0 | 24 | |
Jellehierck | 3:2d45e3d0b0f0 | 25 | Ticker motorTicker; |
Jellehierck | 4:28d71b0a29aa | 26 | Ticker encoderTicker; |
Jellehierck | 3:2d45e3d0b0f0 | 27 | |
Jellehierck | 3:2d45e3d0b0f0 | 28 | const float maxVelocity = 7.958701; // 76 RPM to rad/s |
Jellehierck | 4:28d71b0a29aa | 29 | const float PWM_period = 1e-6; |
Jellehierck | 2:6f5f300f0569 | 30 | |
Jellehierck | 4:28d71b0a29aa | 31 | volatile int counts; // Encoder counts |
Jellehierck | 4:28d71b0a29aa | 32 | volatile int countsPrev = 0; |
Jellehierck | 4:28d71b0a29aa | 33 | volatile int deltaCounts; |
Jellehierck | 4:28d71b0a29aa | 34 | |
Jellehierck | 4:28d71b0a29aa | 35 | float factorin = 6.23185/64; // Convert encoder counts to angle in rad |
Jellehierck | 4:28d71b0a29aa | 36 | float gearratio = 131.25; // Gear ratio of gearbox |
Jellehierck | 2:6f5f300f0569 | 37 | |
Jellehierck | 3:2d45e3d0b0f0 | 38 | float getRefVelocity() |
Jellehierck | 3:2d45e3d0b0f0 | 39 | { |
Jellehierck | 3:2d45e3d0b0f0 | 40 | float refVelocity; |
Jellehierck | 3:2d45e3d0b0f0 | 41 | |
Jellehierck | 3:2d45e3d0b0f0 | 42 | if (button1) { |
Jellehierck | 3:2d45e3d0b0f0 | 43 | refVelocity = potmeter.read() * maxVelocity; |
Jellehierck | 3:2d45e3d0b0f0 | 44 | } else { |
Jellehierck | 3:2d45e3d0b0f0 | 45 | refVelocity = potmeter.read() * maxVelocity * -1; |
Jellehierck | 3:2d45e3d0b0f0 | 46 | } |
Jellehierck | 3:2d45e3d0b0f0 | 47 | return refVelocity; |
Jellehierck | 3:2d45e3d0b0f0 | 48 | } |
Jellehierck | 3:2d45e3d0b0f0 | 49 | |
Jellehierck | 3:2d45e3d0b0f0 | 50 | void motorControl() |
Jellehierck | 3:2d45e3d0b0f0 | 51 | { |
Jellehierck | 3:2d45e3d0b0f0 | 52 | float potValue = potmeter.read(); |
Jellehierck | 4:28d71b0a29aa | 53 | motor1Power.pulsewidth(potValue * PWM_period); |
Jellehierck | 4:28d71b0a29aa | 54 | } |
Jellehierck | 4:28d71b0a29aa | 55 | |
Jellehierck | 4:28d71b0a29aa | 56 | void readEncoder() |
Jellehierck | 4:28d71b0a29aa | 57 | { |
Jellehierck | 4:28d71b0a29aa | 58 | counts = encoder.getPulses(); |
Jellehierck | 4:28d71b0a29aa | 59 | deltaCounts = counts - countsPrev; |
Jellehierck | 4:28d71b0a29aa | 60 | |
Jellehierck | 4:28d71b0a29aa | 61 | countsPrev = counts; |
Jellehierck | 3:2d45e3d0b0f0 | 62 | } |
Jellehierck | 3:2d45e3d0b0f0 | 63 | |
freek100 | 0:1843eec2b552 | 64 | int main() |
freek100 | 0:1843eec2b552 | 65 | { |
freek100 | 0:1843eec2b552 | 66 | pc.baud(115200); |
freek100 | 0:1843eec2b552 | 67 | pc.printf("\r\nStarting...\r\n\r\n"); |
Jellehierck | 3:2d45e3d0b0f0 | 68 | |
Jellehierck | 4:28d71b0a29aa | 69 | motor1Power.period(PWM_period); |
Jellehierck | 3:2d45e3d0b0f0 | 70 | |
Jellehierck | 3:2d45e3d0b0f0 | 71 | motorTicker.attach(motorControl, 0.01); |
Jellehierck | 4:28d71b0a29aa | 72 | |
Jellehierck | 4:28d71b0a29aa | 73 | float T_encoder = 0.1; |
Jellehierck | 4:28d71b0a29aa | 74 | encoderTicker.attach(readEncoder, T_encoder); |
Jellehierck | 3:2d45e3d0b0f0 | 75 | |
Jellehierck | 3:2d45e3d0b0f0 | 76 | motor1Direction = 0; |
Jellehierck | 3:2d45e3d0b0f0 | 77 | |
freek100 | 0:1843eec2b552 | 78 | while (true) { |
Jellehierck | 3:2d45e3d0b0f0 | 79 | |
Jellehierck | 2:6f5f300f0569 | 80 | float potValue = potmeter.read(); // Read potmeter |
Jellehierck | 4:28d71b0a29aa | 81 | |
Jellehierck | 4:28d71b0a29aa | 82 | float angle = counts * factorin / gearratio; // Angle of motor shaft in rad |
Jellehierck | 4:28d71b0a29aa | 83 | float omega = deltaCounts / T_encoder * factorin / gearratio; // Angular velocity of motor shaft in rad/s |
Jellehierck | 2:6f5f300f0569 | 84 | |
Jellehierck | 4:28d71b0a29aa | 85 | pc.printf("Potmeter: %f \r\n", potValue); |
Jellehierck | 4:28d71b0a29aa | 86 | pc.printf("Counts: %i DeltaCounts: %i\r\n", counts, deltaCounts); |
Jellehierck | 4:28d71b0a29aa | 87 | pc.printf("Angle: %f Omega: %f\r\n", angle, omega); |
Jellehierck | 4:28d71b0a29aa | 88 | |
Jellehierck | 2:6f5f300f0569 | 89 | wait(0.5); |
freek100 | 0:1843eec2b552 | 90 | } |
freek100 | 0:1843eec2b552 | 91 | } |