![](/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@7:9125918ca8c5, 2019-10-07 (annotated)
- Committer:
- Jellehierck
- Date:
- Mon Oct 07 11:47:45 2019 +0000
- Revision:
- 7:9125918ca8c5
- Parent:
- 6:c352578a95b3
Added first version of EMG reading to PUTTY (hihi), does not work properly yet
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Jellehierck | 7:9125918ca8c5 | 1 | #include "mbed.h" // Base library |
freek100 | 0:1843eec2b552 | 2 | //#include "HIDScope.h" |
Jellehierck | 7:9125918ca8c5 | 3 | #include "QEI.h" // For encoder |
Jellehierck | 7:9125918ca8c5 | 4 | #include "MODSERIAL.h" // To connect to pc |
freek100 | 0:1843eec2b552 | 5 | //#include "BiQuad.h" |
Jellehierck | 7:9125918ca8c5 | 6 | #include "FastPWM.h" // PWM manager |
Jellehierck | 7:9125918ca8c5 | 7 | #include <vector> // For easy array management |
Jellehierck | 7:9125918ca8c5 | 8 | using namespace std; |
freek100 | 0:1843eec2b552 | 9 | |
Jellehierck | 6:c352578a95b3 | 10 | // Button and potmeter control |
Jellehierck | 5:54ce02ad7a50 | 11 | InterruptIn button1(D11); |
Jellehierck | 5:54ce02ad7a50 | 12 | InterruptIn button2(D10); |
Jellehierck | 2:6f5f300f0569 | 13 | AnalogIn potmeter(A0); |
Jellehierck | 6:c352578a95b3 | 14 | |
Jellehierck | 6:c352578a95b3 | 15 | // Encoder |
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 | 6:c352578a95b3 | 20 | // Motor |
Jellehierck | 3:2d45e3d0b0f0 | 21 | DigitalOut motor2Direction(D4); |
Jellehierck | 3:2d45e3d0b0f0 | 22 | FastPWM motor2Power(D5); |
Jellehierck | 3:2d45e3d0b0f0 | 23 | DigitalOut motor1Direction(D7); |
Jellehierck | 3:2d45e3d0b0f0 | 24 | FastPWM motor1Power(D6); |
Jellehierck | 3:2d45e3d0b0f0 | 25 | |
Jellehierck | 7:9125918ca8c5 | 26 | // EMG |
Jellehierck | 7:9125918ca8c5 | 27 | AnalogIn emg1(A5); |
Jellehierck | 7:9125918ca8c5 | 28 | |
Jellehierck | 6:c352578a95b3 | 29 | // PC connection |
Jellehierck | 6:c352578a95b3 | 30 | MODSERIAL pc(USBTX, USBRX); |
Jellehierck | 6:c352578a95b3 | 31 | |
Jellehierck | 6:c352578a95b3 | 32 | // Intializing tickers |
Jellehierck | 3:2d45e3d0b0f0 | 33 | Ticker motorTicker; |
Jellehierck | 4:28d71b0a29aa | 34 | Ticker encoderTicker; |
Jellehierck | 3:2d45e3d0b0f0 | 35 | |
Jellehierck | 5:54ce02ad7a50 | 36 | const float maxVelocity = 7.958701 * 0.75; // 76 RPM to rad/s at 75% efficiency due to 9V instead of 12V |
Jellehierck | 7:9125918ca8c5 | 37 | const float PWM_period = 1e-6; // PWM period in seconds |
Jellehierck | 2:6f5f300f0569 | 38 | |
Jellehierck | 4:28d71b0a29aa | 39 | volatile int counts; // Encoder counts |
Jellehierck | 7:9125918ca8c5 | 40 | volatile int countsPrev = 0; // Set previous encoder reading to zero |
Jellehierck | 7:9125918ca8c5 | 41 | volatile int deltaCounts; // Difference in encoder counts |
Jellehierck | 4:28d71b0a29aa | 42 | |
Jellehierck | 5:54ce02ad7a50 | 43 | // motor1Direction = 1; |
Jellehierck | 5:54ce02ad7a50 | 44 | volatile int motor1Toggle = 1; |
Jellehierck | 5:54ce02ad7a50 | 45 | |
Jellehierck | 4:28d71b0a29aa | 46 | float factorin = 6.23185/64; // Convert encoder counts to angle in rad |
Jellehierck | 4:28d71b0a29aa | 47 | float gearratio = 131.25; // Gear ratio of gearbox |
Jellehierck | 2:6f5f300f0569 | 48 | |
Jellehierck | 7:9125918ca8c5 | 49 | vector<float> emg_samples; // Create empty scalable array for storing EMG samples |
Jellehierck | 7:9125918ca8c5 | 50 | volatile float currentEMG; |
Jellehierck | 7:9125918ca8c5 | 51 | |
Jellehierck | 7:9125918ca8c5 | 52 | // Expected velocity of motor |
Jellehierck | 3:2d45e3d0b0f0 | 53 | float getRefVelocity() |
Jellehierck | 3:2d45e3d0b0f0 | 54 | { |
Jellehierck | 3:2d45e3d0b0f0 | 55 | float refVelocity; |
Jellehierck | 3:2d45e3d0b0f0 | 56 | |
Jellehierck | 3:2d45e3d0b0f0 | 57 | if (button1) { |
Jellehierck | 3:2d45e3d0b0f0 | 58 | refVelocity = potmeter.read() * maxVelocity; |
Jellehierck | 3:2d45e3d0b0f0 | 59 | } else { |
Jellehierck | 3:2d45e3d0b0f0 | 60 | refVelocity = potmeter.read() * maxVelocity * -1; |
Jellehierck | 3:2d45e3d0b0f0 | 61 | } |
Jellehierck | 3:2d45e3d0b0f0 | 62 | return refVelocity; |
Jellehierck | 3:2d45e3d0b0f0 | 63 | } |
Jellehierck | 3:2d45e3d0b0f0 | 64 | |
Jellehierck | 7:9125918ca8c5 | 65 | // Set the motor power |
Jellehierck | 3:2d45e3d0b0f0 | 66 | void motorControl() |
Jellehierck | 3:2d45e3d0b0f0 | 67 | { |
Jellehierck | 3:2d45e3d0b0f0 | 68 | float potValue = potmeter.read(); |
Jellehierck | 5:54ce02ad7a50 | 69 | motor1Power.pulsewidth(potValue * PWM_period * motor1Toggle); |
Jellehierck | 4:28d71b0a29aa | 70 | } |
Jellehierck | 4:28d71b0a29aa | 71 | |
Jellehierck | 7:9125918ca8c5 | 72 | // Read encoder data |
Jellehierck | 4:28d71b0a29aa | 73 | void readEncoder() |
Jellehierck | 4:28d71b0a29aa | 74 | { |
Jellehierck | 4:28d71b0a29aa | 75 | counts = encoder.getPulses(); |
Jellehierck | 4:28d71b0a29aa | 76 | deltaCounts = counts - countsPrev; |
Jellehierck | 4:28d71b0a29aa | 77 | |
Jellehierck | 4:28d71b0a29aa | 78 | countsPrev = counts; |
Jellehierck | 3:2d45e3d0b0f0 | 79 | } |
Jellehierck | 3:2d45e3d0b0f0 | 80 | |
Jellehierck | 7:9125918ca8c5 | 81 | // Reverse motor direction |
Jellehierck | 5:54ce02ad7a50 | 82 | void flipDirection() |
Jellehierck | 5:54ce02ad7a50 | 83 | { |
Jellehierck | 5:54ce02ad7a50 | 84 | motor1Direction = !motor1Direction; |
Jellehierck | 5:54ce02ad7a50 | 85 | } |
Jellehierck | 5:54ce02ad7a50 | 86 | |
Jellehierck | 7:9125918ca8c5 | 87 | // Toggle power to the motor |
Jellehierck | 5:54ce02ad7a50 | 88 | void toggleMotor() |
Jellehierck | 5:54ce02ad7a50 | 89 | { |
Jellehierck | 5:54ce02ad7a50 | 90 | motor1Toggle = !motor1Toggle; |
Jellehierck | 5:54ce02ad7a50 | 91 | } |
Jellehierck | 5:54ce02ad7a50 | 92 | |
Jellehierck | 7:9125918ca8c5 | 93 | // Read EMG |
Jellehierck | 7:9125918ca8c5 | 94 | void readEMG() |
Jellehierck | 7:9125918ca8c5 | 95 | { |
Jellehierck | 7:9125918ca8c5 | 96 | currentEMG = emg1.read(); |
Jellehierck | 7:9125918ca8c5 | 97 | } |
Jellehierck | 7:9125918ca8c5 | 98 | |
freek100 | 0:1843eec2b552 | 99 | int main() |
freek100 | 0:1843eec2b552 | 100 | { |
freek100 | 0:1843eec2b552 | 101 | pc.baud(115200); |
freek100 | 0:1843eec2b552 | 102 | pc.printf("\r\nStarting...\r\n\r\n"); |
Jellehierck | 3:2d45e3d0b0f0 | 103 | |
Jellehierck | 4:28d71b0a29aa | 104 | motor1Power.period(PWM_period); |
Jellehierck | 3:2d45e3d0b0f0 | 105 | |
Jellehierck | 3:2d45e3d0b0f0 | 106 | motorTicker.attach(motorControl, 0.01); |
Jellehierck | 5:54ce02ad7a50 | 107 | |
Jellehierck | 4:28d71b0a29aa | 108 | float T_encoder = 0.1; |
Jellehierck | 4:28d71b0a29aa | 109 | encoderTicker.attach(readEncoder, T_encoder); |
Jellehierck | 3:2d45e3d0b0f0 | 110 | |
Jellehierck | 5:54ce02ad7a50 | 111 | button1.fall(&flipDirection); |
Jellehierck | 5:54ce02ad7a50 | 112 | button2.fall(&toggleMotor); |
Jellehierck | 3:2d45e3d0b0f0 | 113 | |
freek100 | 0:1843eec2b552 | 114 | while (true) { |
Jellehierck | 2:6f5f300f0569 | 115 | float potValue = potmeter.read(); // Read potmeter |
Jellehierck | 4:28d71b0a29aa | 116 | float angle = counts * factorin / gearratio; // Angle of motor shaft in rad |
Jellehierck | 4:28d71b0a29aa | 117 | float omega = deltaCounts / T_encoder * factorin / gearratio; // Angular velocity of motor shaft in rad/s |
Jellehierck | 7:9125918ca8c5 | 118 | |
Jellehierck | 7:9125918ca8c5 | 119 | readEMG(); |
Jellehierck | 5:54ce02ad7a50 | 120 | |
Jellehierck | 7:9125918ca8c5 | 121 | // pc.printf("Potmeter: %f \r\n", potValue); |
Jellehierck | 7:9125918ca8c5 | 122 | // pc.printf("Direction: %i On/off: %i\r\n", motor1Direction, motor1Toggle); |
Jellehierck | 7:9125918ca8c5 | 123 | pc.printf("Angle: %f Omega: %f\r\n", angle, omega); |
Jellehierck | 7:9125918ca8c5 | 124 | pc.printf("currentEMG: %f\r\n", currentEMG); |
Jellehierck | 5:54ce02ad7a50 | 125 | |
Jellehierck | 2:6f5f300f0569 | 126 | wait(0.5); |
freek100 | 0:1843eec2b552 | 127 | } |
freek100 | 0:1843eec2b552 | 128 | } |