File content as of revision 7:9125918ca8c5:

#include "mbed.h" // Base library
//#include "HIDScope.h"
#include "QEI.h" // For encoder
#include "MODSERIAL.h" // To connect to pc
//#include "BiQuad.h"
#include "FastPWM.h" // PWM manager
#include <vector> // For easy array management
using namespace  std;

// Button and potmeter control
InterruptIn button1(D11);
InterruptIn button2(D10);
AnalogIn potmeter(A0);

// Encoder
DigitalIn encA(D13);
DigitalIn encB(D12);
QEI encoder(D13,D12,NC,64,QEI::X4_ENCODING);

// Motor
DigitalOut motor2Direction(D4);
FastPWM motor2Power(D5);
DigitalOut motor1Direction(D7);
FastPWM motor1Power(D6);

// EMG
AnalogIn emg1(A5);

// PC connection
MODSERIAL pc(USBTX, USBRX);

// Intializing tickers
Ticker motorTicker;
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; // PWM period in seconds

volatile int counts; // Encoder counts
volatile int countsPrev = 0; // Set previous encoder reading to zero
volatile int deltaCounts; // Difference in encoder counts

// motor1Direction = 1;
volatile int motor1Toggle = 1;

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;

    if (button1) {
        refVelocity = potmeter.read() * maxVelocity;
    } else {
        refVelocity = potmeter.read() * maxVelocity * -1;
    }
    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();
    deltaCounts = counts - countsPrev;

    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);
    pc.printf("\r\nStarting...\r\n\r\n");

    motor1Power.period(PWM_period);

    motorTicker.attach(motorControl, 0.01);

    float T_encoder = 0.1;
    encoderTicker.attach(readEncoder, T_encoder);

    button1.fall(&flipDirection);
    button2.fall(&toggleMotor);

    while (true) {
        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("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);
    }
}