![](/media/cache/group/9e3cc099b3b04bca937a1cca1da81b19.jpg.50x50_q85.jpg)
Afgesplitste versie van motor control waarbij we ook iets met EMG gaan doen
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp@3:2d45e3d0b0f0, 2019-10-04 (annotated)
- Committer:
- Jellehierck
- Date:
- Fri Oct 04 12:21:53 2019 +0000
- Revision:
- 3:2d45e3d0b0f0
- Parent:
- 2:6f5f300f0569
- Child:
- 4:28d71b0a29aa
Motor works, potmeter can change motor velocity
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 | 3:2d45e3d0b0f0 | 26 | |
Jellehierck | 3:2d45e3d0b0f0 | 27 | const float maxVelocity = 7.958701; // 76 RPM to rad/s |
Jellehierck | 3:2d45e3d0b0f0 | 28 | const double PWM_freq = 1e-6; |
Jellehierck | 2:6f5f300f0569 | 29 | |
Jellehierck | 2:6f5f300f0569 | 30 | int counts; // Encoder counts |
Jellehierck | 2:6f5f300f0569 | 31 | float degreein; // Angle of DC motor shaft input (before gearbox) |
Jellehierck | 2:6f5f300f0569 | 32 | float factorin= 360/64; // Convert encoder counts to angle in degrees |
Jellehierck | 2:6f5f300f0569 | 33 | float degreeout; // Angle of motor shaft output (after gearbox) |
Jellehierck | 2:6f5f300f0569 | 34 | float gearratio= 131.25; // Gear ratio of gearbox |
Jellehierck | 2:6f5f300f0569 | 35 | |
Jellehierck | 3:2d45e3d0b0f0 | 36 | float getRefVelocity() |
Jellehierck | 3:2d45e3d0b0f0 | 37 | { |
Jellehierck | 3:2d45e3d0b0f0 | 38 | float refVelocity; |
Jellehierck | 3:2d45e3d0b0f0 | 39 | |
Jellehierck | 3:2d45e3d0b0f0 | 40 | if (button1) { |
Jellehierck | 3:2d45e3d0b0f0 | 41 | refVelocity = potmeter.read() * maxVelocity; |
Jellehierck | 3:2d45e3d0b0f0 | 42 | } else { |
Jellehierck | 3:2d45e3d0b0f0 | 43 | refVelocity = potmeter.read() * maxVelocity * -1; |
Jellehierck | 3:2d45e3d0b0f0 | 44 | } |
Jellehierck | 3:2d45e3d0b0f0 | 45 | return refVelocity; |
Jellehierck | 3:2d45e3d0b0f0 | 46 | } |
Jellehierck | 3:2d45e3d0b0f0 | 47 | |
Jellehierck | 3:2d45e3d0b0f0 | 48 | void motorControl() |
Jellehierck | 3:2d45e3d0b0f0 | 49 | { |
Jellehierck | 3:2d45e3d0b0f0 | 50 | float potValue = potmeter.read(); |
Jellehierck | 3:2d45e3d0b0f0 | 51 | motor1Power.pulsewidth(potValue * PWM_freq); |
Jellehierck | 3:2d45e3d0b0f0 | 52 | } |
Jellehierck | 3:2d45e3d0b0f0 | 53 | |
freek100 | 0:1843eec2b552 | 54 | int main() |
freek100 | 0:1843eec2b552 | 55 | { |
freek100 | 0:1843eec2b552 | 56 | pc.baud(115200); |
freek100 | 0:1843eec2b552 | 57 | pc.printf("\r\nStarting...\r\n\r\n"); |
Jellehierck | 3:2d45e3d0b0f0 | 58 | |
Jellehierck | 3:2d45e3d0b0f0 | 59 | motor1Power.period(PWM_freq); |
Jellehierck | 3:2d45e3d0b0f0 | 60 | |
Jellehierck | 3:2d45e3d0b0f0 | 61 | motorTicker.attach(motorControl, 0.01); |
Jellehierck | 3:2d45e3d0b0f0 | 62 | |
Jellehierck | 3:2d45e3d0b0f0 | 63 | motor1Direction = 0; |
Jellehierck | 3:2d45e3d0b0f0 | 64 | |
freek100 | 0:1843eec2b552 | 65 | while (true) { |
Jellehierck | 2:6f5f300f0569 | 66 | counts = encoder.getPulses(); // Get encoder pulses |
Jellehierck | 2:6f5f300f0569 | 67 | degreein = counts*factorin; // Convert encoder data to angle |
Jellehierck | 2:6f5f300f0569 | 68 | degreeout = degreein/gearratio; // Derived output angle |
freek100 | 1:68f74b2ceb7d | 69 | //pc.printf("%f \r\n", degreein); //draaigraden van inputgear en zo ook encoder. |
Jellehierck | 2:6f5f300f0569 | 70 | pc.printf("%f \r\n", degreeout); // Angle of output |
Jellehierck | 3:2d45e3d0b0f0 | 71 | |
Jellehierck | 2:6f5f300f0569 | 72 | float potValue = potmeter.read(); // Read potmeter |
Jellehierck | 2:6f5f300f0569 | 73 | pc.printf("Potmeter: %f \r\n", potValue); |
Jellehierck | 2:6f5f300f0569 | 74 | |
Jellehierck | 3:2d45e3d0b0f0 | 75 | // motor1Power.pulsewidth(potValue * PWM_freq); |
Jellehierck | 3:2d45e3d0b0f0 | 76 | |
Jellehierck | 2:6f5f300f0569 | 77 | wait(0.5); |
freek100 | 0:1843eec2b552 | 78 | } |
freek100 | 0:1843eec2b552 | 79 | } |