Afgesplitste versie van motor control waarbij we ook iets met EMG gaan doen
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Revision 7:9125918ca8c5, committed 2019-10-07
- Comitter:
- Jellehierck
- Date:
- Mon Oct 07 11:47:45 2019 +0000
- Parent:
- 6:c352578a95b3
- Commit message:
- Added first version of EMG reading to PUTTY (hihi), does not work properly yet
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r c352578a95b3 -r 9125918ca8c5 main.cpp --- a/main.cpp Mon Oct 07 07:28:56 2019 +0000 +++ b/main.cpp Mon Oct 07 11:47:45 2019 +0000 @@ -1,9 +1,11 @@ -#include "mbed.h" +#include "mbed.h" // Base library //#include "HIDScope.h" -#include "QEI.h" -#include "MODSERIAL.h" +#include "QEI.h" // For encoder +#include "MODSERIAL.h" // To connect to pc //#include "BiQuad.h" -#include "FastPWM.h" +#include "FastPWM.h" // PWM manager +#include <vector> // For easy array management +using namespace std; // Button and potmeter control InterruptIn button1(D11); @@ -21,6 +23,9 @@ DigitalOut motor1Direction(D7); FastPWM motor1Power(D6); +// EMG +AnalogIn emg1(A5); + // PC connection MODSERIAL pc(USBTX, USBRX); @@ -29,11 +34,11 @@ Ticker encoderTicker; const float maxVelocity = 7.958701 * 0.75; // 76 RPM to rad/s at 75% efficiency due to 9V instead of 12V -const float PWM_period = 1e-6; +const float PWM_period = 1e-6; // PWM period in seconds volatile int counts; // Encoder counts -volatile int countsPrev = 0; -volatile int deltaCounts; +volatile int countsPrev = 0; // Set previous encoder reading to zero +volatile int deltaCounts; // Difference in encoder counts // motor1Direction = 1; volatile int motor1Toggle = 1; @@ -41,6 +46,10 @@ float factorin = 6.23185/64; // Convert encoder counts to angle in rad float gearratio = 131.25; // Gear ratio of gearbox +vector<float> emg_samples; // Create empty scalable array for storing EMG samples +volatile float currentEMG; + +// Expected velocity of motor float getRefVelocity() { float refVelocity; @@ -53,12 +62,14 @@ return refVelocity; } +// Set the motor power void motorControl() { float potValue = potmeter.read(); motor1Power.pulsewidth(potValue * PWM_period * motor1Toggle); } +// Read encoder data void readEncoder() { counts = encoder.getPulses(); @@ -67,16 +78,24 @@ countsPrev = counts; } +// Reverse motor direction void flipDirection() { motor1Direction = !motor1Direction; } +// Toggle power to the motor void toggleMotor() { motor1Toggle = !motor1Toggle; } +// Read EMG +void readEMG() +{ + currentEMG = emg1.read(); +} + int main() { pc.baud(115200); @@ -96,10 +115,13 @@ float potValue = potmeter.read(); // Read potmeter float angle = counts * factorin / gearratio; // Angle of motor shaft in rad float omega = deltaCounts / T_encoder * factorin / gearratio; // Angular velocity of motor shaft in rad/s + + readEMG(); - pc.printf("Potmeter: %f \r\n", potValue); - pc.printf("Counts: %i DeltaCounts: %i\r\n", counts, deltaCounts); - pc.printf("Angle: %f Omega: %f\r\n", angle, omega); + // pc.printf("Potmeter: %f \r\n", potValue); + // pc.printf("Direction: %i On/off: %i\r\n", motor1Direction, motor1Toggle); + pc.printf("Angle: %f Omega: %f\r\n", angle, omega); + pc.printf("currentEMG: %f\r\n", currentEMG); wait(0.5); }