File content as of revision 8:69b7085f5343:

/*Code originally by Jesse Kaiser, s1355783 for control of the 2DOF Planar Table
Some variables are also numbered at the end. The numbers stands for the muscle that controls it.
Biceps =            1 = lower right arm(wrist flexors)
Triceps =           2 = upper right arm(wrist extensors)
Pectoralis Major =  3 = upper left arm(wrist extensors)
Deltoid posterior = 4 = lower left arm(wrist flexors)
The "x" and "y" at the end of variables stand for the X-movement or Y-movement respectivly.
The code has been revised to work with the new board and also has a secondary way of controlling it using a joystick
*/

#include "mbed.h"
#include "MODSERIAL.h"
#include "arm_math.h"
#include "HIDScope.h"
//factors proportional control
#define K_Gain      150   //Gain of the filtered EMG signal
#define Damp        5    //Deceleration of the motor
#define Mass        1    // Mass value
#define dt          0.01 //Sample frequency
//threshold values to determine when the motor needs to stop
#define EMG_tresh1   0.015
#define EMG_tresh2   0.015
#define EMG_tresh3   0.015
#define EMG_tresh4   0.015

//button for switching between proportional and precision speed control
InterruptIn button1(PTC6);

MODSERIAL pc(USBTX,USBRX);
//joystick control
AnalogIn X_control(A1);
AnalogIn Y_control(A0);

//Motor control 1
DigitalOut Diry(D12);
PwmOut Stepy(PTA1);
DigitalOut Enabley(PTC3);

//motor control 2
DigitalOut Dirx(PTC17);
PwmOut Stepx(PTD1);
DigitalOut Enablex(D0);

//Microstepping 1
DigitalOut MS11(D11);
DigitalOut MS21(D10);
DigitalOut MS31(D9);
//Microstepping 2
DigitalOut MS12(PTC2);
DigitalOut MS22(PTA2);
DigitalOut MS32(PTB23);
//initializing lights for testing and notification purposes
DigitalOut Ledr(LED1);
DigitalOut Ledg(LED2);
DigitalOut Ledb(LED3);

//EMG inputs
AnalogIn emg1(A2); //biceps or wrist flexors bottom emg board
AnalogIn emg2(A3); //triceps or wirst extensors
AnalogIn emg3(A4); //Pectoralis major or wrist extensors
AnalogIn emg4(A5); //Deltoid or wrist flexors top emg board

HIDScope scope(4);
Ticker   scopeTimer;
Ticker emgtimer;
Ticker looptimer1;
Ticker looptimer2;

//Variables for motor control
float setpoint = 3200; //Frequentie setpoint
float step_freq1 = 1;
float step_freq2 = 1;

//EMG filters
//Lowpass1
arm_biquad_casd_df1_inst_f32 lowpass1_biceps;
arm_biquad_casd_df1_inst_f32 lowpass1_triceps;
arm_biquad_casd_df1_inst_f32 lowpass1_pect;
arm_biquad_casd_df1_inst_f32 lowpass1_deltoid;
//lowpass2
arm_biquad_casd_df1_inst_f32 lowpass2_biceps;
arm_biquad_casd_df1_inst_f32 lowpass2_triceps;
arm_biquad_casd_df1_inst_f32 lowpass2_pect;
arm_biquad_casd_df1_inst_f32 lowpass2_deltoid;
//highpass
arm_biquad_casd_df1_inst_f32 highpass_biceps;
arm_biquad_casd_df1_inst_f32 highpass_triceps;
arm_biquad_casd_df1_inst_f32 highpass_pect;
arm_biquad_casd_df1_inst_f32 highpass_deltoid;
//used as extra filter for wrist motion
//bandstop
arm_biquad_casd_df1_inst_f32 bandstop_biceps;
arm_biquad_casd_df1_inst_f32 bandstop_triceps;
arm_biquad_casd_df1_inst_f32 bandstop_pect;
arm_biquad_casd_df1_inst_f32 bandstop_deltoid;

//lowpass 1 filter settings: Fc = 49 Hz, Fs = 100 Hz, Gain = -3 dB
//lowpass 2 filter settings: Fc = 0.8 Hz, Fs = 100 Hz, Gain = -3 dB
float lowpass1_const[] = {0.956543225556877,1.91308645111375,0.956543225556877,-1.91119706742607,-0.914975834801434};
float lowpass2_const[] = {0.000609854718717299,0.00121970943743460,0.000609854718717299,1.92894226325203,-0.931381682126903};
//highpass filter settings: Fc = 20 Hz, Fs = 100 Hz
float highpass_const[] = {0.391335772501769,-0.782671545003538,0.391335772501769,0.369527377351241,-0.195815712655833};
//bandstop filter settings Fc=[29Hz 36Hz], Fs= 100Hz
float bandstop_const[] = {0.732022476556623, 0.681064744276583,0.732022476556623,-0.535944148680739,-0.713976335521464,1,0.930387749130681,1,-1.04147956553087,-0.752398361226849};

/*
//values are usable for triceps and biceps continuous motion, not for wrist motion.
//lowpass 1 filter settings: Fc = 45 Hz, Fs = 100 Hz, Gain = -3 dB
//lowpass 2 filter settings: Fc = 0.3 Hz, Fs = 100 Hz, Gain = -3 dB
float lowpass1_const[] = {0.800592403464570,1.60118480692914,0.800592403464570,-1.56101807580072,-0.641351538057563};
float lowpass2_const[] = {8.76555487540147e-05,  0.000175311097508029,  8.76555487540147e-05 , 1.97334424978130, -0.973694871976315};
//highpass filter settings: Fc = 20 Hz, Fs = 100 Hz

float highpass_const[] = {0.391335772501769 ,-0.782671545003538,  0.391335772501769,0.369527377351241,  -0.195815712655833};

*/


//state values for filter initialization
float lowpass1_biceps_states[4];
float lowpass2_biceps_states[4];
float highpass_biceps_states[4];
float bandstop_biceps_states[8];

float lowpass1_triceps_states[4];
float lowpass2_triceps_states[4];
float highpass_triceps_states[4];
float bandstop_triceps_states[8];

float lowpass1_pect_states[4];
float lowpass2_pect_states[4];
float highpass_pect_states[4];
float bandstop_pect_states[8];

float lowpass1_deltoid_states[4];
float lowpass2_deltoid_states[4];
float highpass_deltoid_states[4];
float bandstop_deltoid_states[8];


//global variables
float filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid;
float speed_old1, speed_old2, speed_old3, speed_old4;
float acc1, acc2, acc3, acc4;
float force1, force2, force3, force4;
float speed1, speed2, speed3, speed4;
float damping1, damping2, damping3, damping4;
float emg_x, emg_y;
float emg_y_abs = 0;
float emg_x_abs = 0;
bool proportional = 1;
int count = 0;

void looper_emg()//EMG filtering function
{
    float emg_value1_f32, emg_value2_f32, emg_value3_f32, emg_value4_f32;
    emg_value1_f32 = emg1.read();//read out analog inputs for EMG value
    emg_value2_f32 = emg2.read();
    emg_value3_f32 = emg1.read();
    emg_value4_f32 = emg2.read();
    
    //Biquad process emg biceps
    arm_biquad_cascade_df1_f32(&bandstop_biceps, &emg_value1_f32, &filtered_biceps, 1 );//bandstop filter used for wrist motion
    arm_biquad_cascade_df1_f32(&lowpass1_biceps, &filtered_biceps, &filtered_biceps, 1 );//first lowpass filter
    arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_biceps, &filtered_biceps, 1 );//highpass filter
    filtered_biceps = fabs(filtered_biceps);                                                //Rectifier, The Gain is already implemented.
    arm_biquad_cascade_df1_f32(&lowpass2_biceps, &filtered_biceps, &filtered_biceps, 1 );    //second low pass filter

    //Biquad process emg triceps
   arm_biquad_cascade_df1_f32(&bandstop_triceps, &emg_value2_f32, &filtered_triceps, 1 );    //used for wrist motion
   arm_biquad_cascade_df1_f32(&lowpass1_triceps, &filtered_triceps, &filtered_triceps, 1 );
   arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_triceps, &filtered_triceps, 1 );
   filtered_triceps = fabs(filtered_triceps);
   arm_biquad_cascade_df1_f32(&lowpass2_triceps, &filtered_triceps, &filtered_triceps, 1 );

    //Biquad process emg pectoralis major
    arm_biquad_cascade_df1_f32(&bandstop_pect, &emg_value3_f32, &filtered_pect, 1 );//used for wrist motion
    arm_biquad_cascade_df1_f32(&lowpass1_pect, &filtered_pect, &filtered_pect, 1 );
    arm_biquad_cascade_df1_f32(&highpass_pect, &filtered_pect, &filtered_pect, 1 );
    filtered_pect = fabs(filtered_pect);
    arm_biquad_cascade_df1_f32(&lowpass2_pect, &filtered_pect, &filtered_pect, 1 );

    //Biquad process emg deltoid
    arm_biquad_cascade_df1_f32(&bandstop_deltoid, &emg_value4_f32, &filtered_deltoid, 1 );//used for wrist motion
    arm_biquad_cascade_df1_f32(&lowpass1_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
    arm_biquad_cascade_df1_f32(&highpass_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
    filtered_deltoid = fabs(filtered_deltoid);
    arm_biquad_cascade_df1_f32(&lowpass2_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
 
// send value to PC for control

    scope.set(0,filtered_biceps); //Filtered EMG signals
    scope.set(1,filtered_triceps);
    scope.set(2,filtered_pect);
    scope.set(3,filtered_deltoid);
    scope.send();
    
}

//y motor control
void looper_motory()
{
//determine which direction the motor will rotate
    emg_y = (filtered_biceps - filtered_triceps);
    //switch between proportional EMG control and pre-set speed control
    if(proportional==0)
    {
    //proportional control explained in the report
    Stepy.write(0.5);
    Ledr=0;//red light to make it clear which control method is used
    emg_y_abs = fabs(emg_y);
    force1 = emg_y_abs*K_Gain;
    force1 = force1 - damping1;
    acc1 = force1/Mass;
    speed1 = speed_old1 + (acc1 * dt);
    damping1 = speed1 * Damp;
    step_freq1 = setpoint * speed1;
    Stepy.period(1.0/step_freq1);
    speed_old1 = speed1;
    
        //Speed limit
    if (speed1 > 1) {
        speed1 = 1;
    }
}
    else{
    //precision control
    Ledr=1;
    Stepy.period(0.000625);//frequency of 1600 Hz
    }
    
    if (emg_y > 0) {//downward movement
        Diry = 0;
    }
    if (emg_y < 0) {//upward movement
        Diry = 1;
    }

    //EMG treshold, determine if signal is strong enough to start the motors
    if (filtered_biceps < EMG_tresh1 && filtered_triceps < EMG_tresh2) {
        Enabley = 1; //Enable = 1 turns the motor off.
    }
    else {
        Enabley = 0;//Enable = 0 turns the motor on.
    }
}
//same setup as with the y motor
void looper_motorx()
{
    emg_x = (filtered_pect - filtered_deltoid);
    if(proportional==0)
    {
        //proportional control
        Stepx.write(0.5);
        Ledr=0;
        emg_x_abs = fabs(emg_x);
        force2 = emg_x_abs*K_Gain;
        force2 = force2 - damping2;
        acc2 = force2/Mass;
        speed2 = speed_old2 + (acc2 * dt);
        damping2 = speed2 * Damp;
        step_freq2 = setpoint * speed2;
        Stepx.period(1.0/step_freq2);
        speed_old2 = speed2;
    
        //speed limit
        if (speed2 > 1) {
            speed2 = 1;
        }   

    }
        else{
        //precision control 
        Ledr=1;
        Stepx.period(0.000625);//frequency of 1600 Hz
        }
        
    if (emg_x > 0) {//left movement
        Dirx = 0;
    }
    if (emg_x < 0) {//rig1ht movement
        Dirx = 1;
    }

    //EMG treshold
    if (filtered_pect < EMG_tresh3 && filtered_deltoid < EMG_tresh4) {
        Enablex = 1; //Enable = 1 turns the motor off.
    } else {
        Enablex = 0;
    }
}

void changecontrol(){
        proportional = !proportional;//code for changing speed control
        }


int main()
{
    pc.baud(115200);//sent data to pc for testing purposes
    Ledr=1;//turn of all the lights
    Ledg=1;
    Ledb=1;
   
    MS11=1;//higher microstepping in combination with a higher step frequency reduces the vibration significantly
    MS21=1;//it is now in 16th step mode, which seems to work really well and smooth without causing major vibrations
    MS31=1;
    MS12=1;
    MS22=1;
    MS32=1;
Stepy.write(0.5);//set all motors to half load and disable them.
Enabley.write(1);
Stepx.write(0.5);
Enablex.write(1);
//Stepy.period(0.000625);//use period change for speed adjustments
//Stepx.period(0.000625);//frequency of 1600 Hz


/*//code for controlling the mechanism with a joystick
float Left_value = 0.6;
float Right_value = 0.9;
float Up_value = 0.6;
float Down_value = 0.9;

while(1){
    if (X_control.read() < Left_value){
     Dirx.write(0);
     Enablex.write(0);
     }
else if (X_control.read() > Right_value){
     Dirx.write(1);
     Enablex.write(0);
     }
else{
    Enablex.write(1);
}         
if (Y_control.read() < Up_value){
     Diry.write(0);
     Enabley.write(0);
     }
else if (Y_control.read() > Down_value){
     Diry.write(1);
     Enabley.write(0);
     }
else{
    Enabley.write(1);
}
}*/
//initialization of the biquad filters and appointing of names to filters.
    //biceps
    arm_biquad_cascade_df1_init_f32(&lowpass1_biceps, 1 , lowpass1_const, lowpass1_biceps_states);
    arm_biquad_cascade_df1_init_f32(&lowpass2_biceps, 1 , lowpass2_const, lowpass2_biceps_states);
    arm_biquad_cascade_df1_init_f32(&highpass_biceps, 1 , highpass_const, highpass_biceps_states);
    arm_biquad_cascade_df1_init_f32(&bandstop_biceps, 2 , bandstop_const, bandstop_biceps_states);
    //triceps
    arm_biquad_cascade_df1_init_f32(&lowpass1_triceps, 1 , lowpass1_const, lowpass1_triceps_states);
    arm_biquad_cascade_df1_init_f32(&lowpass2_triceps, 1 , lowpass2_const, lowpass2_triceps_states);
    arm_biquad_cascade_df1_init_f32(&highpass_triceps, 1 , highpass_const, highpass_triceps_states);
    arm_biquad_cascade_df1_init_f32(&bandstop_triceps, 2 , bandstop_const, bandstop_triceps_states);
    //pectoralis major
    arm_biquad_cascade_df1_init_f32(&lowpass1_pect, 1 , lowpass1_const, lowpass1_pect_states);
    arm_biquad_cascade_df1_init_f32(&lowpass2_pect, 1 , lowpass2_const, lowpass2_pect_states);
    arm_biquad_cascade_df1_init_f32(&highpass_pect, 1 , highpass_const, highpass_pect_states);
    arm_biquad_cascade_df1_init_f32(&bandstop_pect, 2 , bandstop_const, bandstop_pect_states);

    //deltoid
    arm_biquad_cascade_df1_init_f32(&lowpass1_deltoid, 1 , lowpass1_const, lowpass1_deltoid_states);
    arm_biquad_cascade_df1_init_f32(&lowpass2_deltoid, 1 , lowpass2_const, lowpass2_deltoid_states);
    arm_biquad_cascade_df1_init_f32(&highpass_deltoid, 1 , highpass_const, highpass_deltoid_states);
    arm_biquad_cascade_df1_init_f32(&bandstop_deltoid, 2 , bandstop_const, bandstop_deltoid_states);

    emgtimer.attach(looper_emg, 0.01);//emg signal filtering

    looptimer1.attach(looper_motorx, 0.01); //X motor control

    looptimer2.attach(looper_motory, 0.01); //Y motor control

    while(1){//if button is pressed the control mode changes with it.
        button1.fall(changecontrol);
        };
}