File content as of revision 19:0a3ee31dcdb4:

#include "mbed.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "biquadFilter.h"
#include "QEI.h"

//Define important constants in memory
#define     SAMPLE_RATE     0.002   //500 Hz EMG sample rate
#define     CONTROL_RATE    0.01    //100 Hz Control rate
#define     ENCODER1_CPR    4200    //encoders have 4200 counts per revolution of the output shaft (X2)
#define     ENCODER2_CPR    4200    //64 X4, 32 X4 counts per revolution of motor shaft, gearbox 1:131.25

//Objects
MODSERIAL pc(USBTX,USBRX);      //serial communication
DigitalIn button(PTA1);         //buttons
DigitalIn button1(PTB9);        //

AnalogIn    emg1(A0);           //Analog input - Biceps EMG
AnalogIn    emg2(A1);           //Analog input - Triceps EMG
AnalogIn    emg3(A2);           //Analog input - Flexor EMG
AnalogIn    emg4(A3);           //Analog input - Extensor EMG

Ticker      sample_timer;       //Ticker for EMG sampling
Ticker      control_timer;      //Ticker for control loop
HIDScope    scope(4);           //Scope 4 channels

// AnalogIn potmeter(A4);       //potmeters
// AnalogIn potmeter2(A5);      //

QEI Encoder1(D13,D12,NC,32);    //channel A and B from encoder, counts = Encoder.getPulses();
QEI Encoder2(D10,D9,NC,32);     //channel A and B from encoder, 
PwmOut pwm_motor1(D6);          //D4 and D5 = motor 2, D7 and D6 = motor 1
PwmOut pwm_motor2(D5);          //D4 and D5 = motor 2, D7 and D6 = motor 1
DigitalOut dir_motor1(D7);      //Direction motor 1
DigitalOut dir_motor2(D4);      //Direction motor 2

//Filters
//biquadFilter     filter(a1, a2, b0, b1, b2); coefficients
biquadFilter     filter1( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 );     //eg. notch / lowpass / highpass
biquadFilter     filter2( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 );     //eg. notch / lowpass / highpass
biquadFilter     filter3( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 );     //eg. notch / lowpass / highpass
biquadFilter     filter4( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 );     //eg. notch / lowpass / highpass

void keep_in_range(float * in, float min, float max);
void sample(void);

//Start sampling
void start_sampling(void)
{
    sample_timer.attach(&sample,SAMPLE_RATE);
}

//stop sampling
void stop_sampling(void)
{
    sample_timer.detach();
}

//Sample function 
void sample(void)
{
    double emg_value1 = emg1.read();
    double emg_value2 = emg2.read();
    double emg_value3 = emg3.read();
    double emg_value4 = emg4.read();
    scope.set(0,emg_value1);
    scope.set(1,emg_value2);
    scope.set(2,emg_value3);
    scope.set(3,emg_value4);
    scope.send();
}

//Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
void calibrate_arm(void)
{
}

//EMG MVC measurement
void calibrate_emg(void)
{
}

//use biquadFilter library, output filtered EMG (combine with sample?)
double filter(double u)
{
   double test;
   return test;
}

//Input error and Kp, Kd, Ki, output control signal
void pid()
{
}

//Analyse filtered EMG, calculate reference position from EMG, compare reference position with current position,convert to angles, send error through pid(), send PWM and DIR to motors 
void control()
{
    //analyse emg (= velocity, averages?)
    
    //calculate reference position based on the average emg (integrate)
    
    //calculate error (refpos-currentpos) currentpos = forward kinematics
    
    //inverse kinematics (pos error to angle error)
    
    //PID controller
    pid();
    //send PWM and DIR to motors
}

//Start control
void start_control(void)
{
    control_timer.attach(&control,SAMPLE_RATE);
}

//stop control
void stop_control(void)
{
    control_timer.detach();
}

int main()
{
    // make a menu, user has to initiated next step
    
    calibrate_arm();        //Start Calibration
    calibrate_emg();        
    start_sampling();       //500 Hz EMG 
    start_control();        //100 Hz control
    
    //maybe some stop commands when a button is pressed: detach from timers.
    //stop_control();
    //stop_sampling();
    
    while(1) {
        
        
    }//end of while loop
}//end of main

void keep_in_range(float * in, float min, float max)
{
    *in > min ? *in < max? : *in = max: *in = min;
}