Control up to two motors using filtered EMG signals and a PID controller

Dependencies:   FastPWM HIDScope MODSERIAL QEI Matrix biquadFilter controller errorFetch mbed motorConfig refGen MatrixMath inverseKinematics

Fork of Minor_test_serial by First Last

main.cpp

Committer:
tvlogman
Date:
2017-10-22
Revision:
35:99bf23b34ee3
Parent:
34:1a70aa045c8f
Child:
36:f3f3327d1d5a

File content as of revision 35:99bf23b34ee3:

#include <vector>
#include "mbed.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"
#include "FastPWM.h"
#include "refGen.h"
#include "controller.h"
#include "motorConfig.h"
#include "errorFetch.h"
#include "biquadFilter.h"
#include "biquadChain.h"

// ADJUSTABLE PARAMETERS

// EMG filter parameters

// high pass
biquadFilter HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770);
biquadFilter HPbq2(1.0000, -2.0000, 1.0000, 1.0000, -1.9903, 0.9904);
biquadChain HPbqc(HPbq1, HPbq2);
// low pass
biquadFilter LPbq1(1.0e-6*0.1202, 1.0e-6*0.2405, 1.0e-6*0.1202, 1.0000, -1.9313, 0.9327);
biquadFilter LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.9702, 0.9716);
biquadChain LPbqc(LPbq1, LPbq2);

// Controller parameters
const float k_p = 1;
const float k_i = 0; // Still needs a reasonable value
const float k_d = 0; // Again, still need to pick a reasonable value

// Defining motor gear ratio - for BOTH motors as this is the same in the current configuration
const float gearRatio = 131;



// Declaring a controller ticker and volatile variables to store encoder counts and revs
Ticker controllerTicker;
volatile int m1counts = 0;
volatile int m2counts = 0;
volatile float m1revs = 0.00;
volatile float m2revs = 0.00;

// PWM settings
float pwmPeriod = 1.0/5000.0;
int frequency_pwm = 10000; //10kHz PWM

// Initializing encoder
QEI Encoder1(D12,D13,NC,64, QEI::X4_ENCODING);
QEI Encoder2(D11,D10,NC,64, QEI::X4_ENCODING);
MODSERIAL pc(USBTX, USBRX);
HIDScope scope(5);

// Defining inputs
InterruptIn sw2(SW2);
InterruptIn sw3(SW3);
InterruptIn button1(D2);
InterruptIn button2(D3);
AnalogIn pot2(A3);
//AnalogIn emg0( A0 );
//AnalogIn emg1( A1 );

// Setting up HIDscope
volatile float x;
volatile float y;  
volatile float z; 
volatile float q;
volatile float k;

void sendDataToPc(float data1, float data2, float data3, float data4, float data5){
        // Capture data
        x = data1;
        y = data2;
        z = data3;
        q = data4;
        k = data5;
        scope.set(0, x);   
        scope.set(1, y); 
        scope.set(2, z);
        scope.set(3, q);
        scope.set(4, z);
        scope.send(); // send what's in scope memory to PC
}


// REFERENCE PARAMETERS
int posRevRange = 2; // describes the ends of the position range in complete motor output shaft revolutions in both directions
const float maxAngle = 2*3.14*posRevRange; // max angle in radians
const float Ts = 0.1;

// Function getReferencePosition returns reference angle based on potmeter 1
refGen ref1(A1, maxAngle);
//refGen ref2(A1, maxAngle);

// readEncoder reads counts and revs and logs results to serial window
errorFetch e1(gearRatio, Ts);
//errorFetch e2(gearRatio, Ts);

// Generate a PID controller with the specified values of k_p, k_d and k_i
controller motorController1(k_p, k_d, k_i);
    
// setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
motorConfig motor1(LED_GREEN,LED_RED,LED_BLUE,D5,D4);
    
void measureAndControl(){
    m1counts = Encoder1.getPulses();
    m2counts = Encoder2.getPulses();
    float r1 = ref1.getReference();
    r1 = HPbqc.applyFilter(r1);
    r1 = LPbqc.applyFilter(r1);
    e1.fetchError(m1counts, r1);
    float motorValue = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
    float r1_unfiltered = ref1.getReference();
    sendDataToPc(r1_unfiltered, r1, e1.e_pos, e1.e_der, motorValue);
    motor1.setMotor(motorValue);
    }

void rSwitchDirection(){
    ref1.r_direction = !ref1.r_direction;
    pc.printf("Switched reference direction! \r\n");
    }


int main()
    {
    pc.printf("Main function");
    sw2.fall(&motor1,&motorConfig::killSwitch);
    sw3.fall(&motor1, &motorConfig::turnMotorOn);
    button2.rise(&rSwitchDirection);
    pc.baud(115200);
    controllerTicker.attach(measureAndControl, Ts);
    pc.printf("Encoder ticker attached and baudrate set");    
    }