Afgesplitste versie van motor control waarbij we ook iets met EMG gaan doen
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp@2:6f5f300f0569, 2019-10-03 (annotated)
- Committer:
- Jellehierck
- Date:
- Thu Oct 03 14:56:22 2019 +0000
- Revision:
- 2:6f5f300f0569
- Parent:
- 1:68f74b2ceb7d
- Child:
- 3:2d45e3d0b0f0
Trying out moving the motor. Issue: PWM out does not work yet.
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); |
freek100 | 0:1843eec2b552 | 12 | DigitalIn button1(D2); |
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 | 2:6f5f300f0569 | 20 | DigitalOut motor1Direction(D4); |
Jellehierck | 2:6f5f300f0569 | 21 | FastPWM motor1Power(D5); |
Jellehierck | 2:6f5f300f0569 | 22 | DigitalOut motor2Direction(D7); |
Jellehierck | 2:6f5f300f0569 | 23 | FastPWM motor2Power(D6); |
Jellehierck | 2:6f5f300f0569 | 24 | |
Jellehierck | 2:6f5f300f0569 | 25 | int counts; // Encoder counts |
Jellehierck | 2:6f5f300f0569 | 26 | float degreein; // Angle of DC motor shaft input (before gearbox) |
Jellehierck | 2:6f5f300f0569 | 27 | float factorin= 360/64; // Convert encoder counts to angle in degrees |
Jellehierck | 2:6f5f300f0569 | 28 | float degreeout; // Angle of motor shaft output (after gearbox) |
Jellehierck | 2:6f5f300f0569 | 29 | float gearratio= 131.25; // Gear ratio of gearbox |
Jellehierck | 2:6f5f300f0569 | 30 | |
freek100 | 0:1843eec2b552 | 31 | int main() |
freek100 | 0:1843eec2b552 | 32 | { |
freek100 | 0:1843eec2b552 | 33 | pc.baud(115200); |
freek100 | 0:1843eec2b552 | 34 | pc.printf("\r\nStarting...\r\n\r\n"); |
Jellehierck | 2:6f5f300f0569 | 35 | |
Jellehierck | 2:6f5f300f0569 | 36 | motor1Power.period(1e-3); |
Jellehierck | 2:6f5f300f0569 | 37 | |
freek100 | 0:1843eec2b552 | 38 | while (true) { |
Jellehierck | 2:6f5f300f0569 | 39 | counts = encoder.getPulses(); // Get encoder pulses |
Jellehierck | 2:6f5f300f0569 | 40 | degreein = counts*factorin; // Convert encoder data to angle |
Jellehierck | 2:6f5f300f0569 | 41 | degreeout = degreein/gearratio; // Derived output angle |
freek100 | 1:68f74b2ceb7d | 42 | //pc.printf("%f \r\n", degreein); //draaigraden van inputgear en zo ook encoder. |
Jellehierck | 2:6f5f300f0569 | 43 | pc.printf("%f \r\n", degreeout); // Angle of output |
Jellehierck | 2:6f5f300f0569 | 44 | |
Jellehierck | 2:6f5f300f0569 | 45 | float potValue = potmeter.read(); // Read potmeter |
Jellehierck | 2:6f5f300f0569 | 46 | pc.printf("Potmeter: %f \r\n", potValue); |
Jellehierck | 2:6f5f300f0569 | 47 | |
Jellehierck | 2:6f5f300f0569 | 48 | motor1Power.pulsewidth(potValue * 1e-3); |
Jellehierck | 2:6f5f300f0569 | 49 | |
Jellehierck | 2:6f5f300f0569 | 50 | wait(0.5); |
freek100 | 0:1843eec2b552 | 51 | } |
freek100 | 0:1843eec2b552 | 52 | } |