File content as of revision 0:34751b6a7dc9:

/*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
Triceps =           2
Pectoralis Major =  3
Deltoid posterior = 4
The "x" and "y" at the end of variables stand for the X-Spindle or Y-Spindle respectivly.
The code has been revised to work with the new board and also has a secondary way of controlling it
*/

#include "mbed.h"
#include "MODSERIAL.h"
#include "arm_math.h"
#include "HIDScope.h"

#define P_Gain      0.99
#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
#define EMG_tresh1   0.02
#define EMG_tresh2   0.02
#define EMG_tresh3   0.02
#define EMG_tresh4   0.02
#define H_Gain  5
#define Pt_x    0.80
#define Pt_y    0.50
#define error_tresh 0.02

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);

DigitalOut Ledr(LED1);
DigitalOut Ledg(LED2);
DigitalOut Ledb(LED3);

//EMG inputs
AnalogIn emg1(A2); //biceps bottom emg board
AnalogIn emg2(A3); //triceps
AnalogIn emg3(A4); //Pectoralis major
AnalogIn emg4(A5); //Deltoid 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 filter
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;

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;

//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};


/*
//suffer from high wire motion influence and dont react to continuous tension only rapid activation

//lowpass 1 filter settings: Fc = 20 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.206572083826148,0.413144167652296,0.206572083826148,0.369527377351241,-0.195815712655833};
float lowpass2_const[] = {0.0000876555487540065, 0.000175311097508013, 0.0000876555487540065, 1.97334424978130, -0.973694871976315};
//highpass filter settings: Fc = 3 Hz, Fs = 100 Hz

float highpass_const[] = {0.875183092438135,-1.75036618487627,0.875183092438135,1.73472576880928,-0.766006600943264};
*/

/*
Previous coefficients testing to see if new variants work
lowpass 1 filter settings: Fc = 45 Hz, Fs = 100 Hz, Gain = -3 dB
lowpass 2 filter settings: Fc = 2 Hz, Fs = 100 Hz, Gain = -3 dB
float lowpass1_const[] = {0.800592403464570, 1.60118480692914, 0.800592403464570, -1.56101807580072, -0.641351538057563};
float lowpass2_const[] = {0.0000876555487540065, 0.000175311097508013, 0.0000876555487540065, 1.97334424978130, -0.973694871976315};
//highpass filter settings: Fc = 20 Hz, Fs = 100 Hz

float highpass_const[] = {0.391335772501769, -0.782671545003537,0.391335772501769,0.369527377351241,-0.195815712655833 };
*/
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;

//state values
float lowpass1_biceps_states[4];
float lowpass2_biceps_states[4];
float highpass_biceps_states[4];
float lowpass1_triceps_states[4];
float lowpass2_triceps_states[4];
float highpass_triceps_states[4];
float lowpass1_pect_states[4];
float lowpass2_pect_states[4];
float highpass_pect_states[4];
float lowpass1_deltoid_states[4];
float lowpass2_deltoid_states[4];
float highpass_deltoid_states[4];

//global variabels
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 cx = 0;
float cy = 0;
float errorx = 0.2;
float errory = 0.2;
float Ps_x = 0;
float Ps_y = 0;
float hstep_freqx = 1;
float hstep_freqy = 1;
float emg_y_abs = 0;
float emg_x_abs = 0;

void looper_emg()
{
    float emg_value1_f32, emg_value2_f32, emg_value3_f32, emg_value4_f32;
    emg_value1_f32 = emg1.read();
    emg_value2_f32 = emg2.read();
    emg_value3_f32 = emg3.read();
    emg_value4_f32 = emg4.read();
    
    //process emg biceps
    arm_biquad_cascade_df1_f32(&lowpass1_biceps, &emg_value1_f32, &filtered_biceps, 1 );
    arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_biceps, &filtered_biceps, 1 );
    filtered_biceps = fabs(filtered_biceps);                                                //Rectifier, The Gain is already implemented.
    arm_biquad_cascade_df1_f32(&lowpass2_biceps, &filtered_biceps, &filtered_biceps, 1 );    //low pass filter

    //process emg triceps
   arm_biquad_cascade_df1_f32(&lowpass1_triceps, &emg_value2_f32, &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 );

   /* //process emg pectoralis major
    arm_biquad_cascade_df1_f32(&lowpass1_pect, &emg_value3_f32, &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 );

    //process emg deltoid
    arm_biquad_cascade_df1_f32(&lowpass1_deltoid, &emg_value4_f32, &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.

    scope.set(0,filtered_biceps); //Filtered EMG signal
    scope.set(1,filtered_triceps);
    /*scope.set(2,filtered_pect);
    scope.set(3,filtered_deltoid);*/
    scope.set(2,emg1.read());//testing emg signal for cause of variance
    scope.set(3,emg2.read());
    scope.send();
    
}

 
void looper_motory()
{

    /*emg_y = (filtered_biceps - filtered_triceps);
    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*/0.000625);
    //speed_old1 = speed1;

    if (filtered_triceps > 0.06) {
        Diry.write(1);
        Enabley.write(0); //Enable = 1 turns the motor off.
        Ledr.write(0);
        Ledb.write(1);
    }
    else if (filtered_biceps > 0.06) {
        Diry.write(0);
        Enabley.write(0); //Enable = 1 turns the motor off.
        Ledr.write(1);
        Ledb.write(0);
    }
    else if (filtered_triceps < 0.04 && filtered_biceps < 0.04) {
        Enabley.write(1);
    }
    else{}
    //Speed limit
    /*if (speed1 > 1) {
        speed1 = 1;
        step_freq1 = setpoint;
    }*/

}
/*
void looper_motorx()
{

    emg_x = (filtered_pect - filtered_deltoid);
    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;

    if (emg_x > 0) {
        Dirx = 1;
    }
    if (emg_x < 0) {
        Dirx = 0;
    }
    //Speed limit
    if (speed2 > 1) {
        speed2 = 1;
        step_freq2 = setpoint;
    }
    //EMG treshold
    if (filtered_pect < EMG_tresh3 && filtered_deltoid < EMG_tresh4) {
        Enablex = 1; //Enable = 1 turns the motor off.
    } else {
        Enablex = 0;
    }

}*/

int main()
{
    
    //scopeTimer.attach_us(&scopeSend, 2e3);
    pc.baud(115200);
    Ledr=1;
    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;
/*Diry.write(1);*/
Stepy.write(0.5f);/*
Enabley.write(1);
Dirx.write(1);*/
Stepx.write(0.5f);
Enablex.write(1);
Stepy.period(0.000625);//use period change for speed adjustments
Stepx.period(0.000625);//frequency of 1600 Hz

float Left_value = 0.6;
float Right_value = 0.9;
float Up_value = 0.6;
float Down_value = 0.9;
/*
while(1){//code for controlling the mechanism with a joystick
    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);
}
        pc.printf("X value is %f and Y value is %f\n", X_control.read(), Y_control.read());
}*/

    //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);
    //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);
    /*//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);
    //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);*/
    emgtimer.attach(looper_emg, 0.01);

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

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

    while(1){};
}