M9 Biorobotics Group 8 / Mbed 2 deprecated Encoder-Motor-Control-EMG

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?

UserRevisionLine numberNew 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 }