File content as of revision 31:372ff8d49430:

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

#define P_Gain 0.995
#define tres_bi 0.05    //Biceps emg treshold
#define Mass 1          // Mass value
#define dt 0.002        //Sample frequency

//Motor control
DigitalOut Dir(p21);
PwmOut Step(p22);

//Signal to and from computer
Serial pc(USBTX, USBRX);

DigitalOut Enable(p25);

//Microstepping
DigitalOut MS1(p27);
DigitalOut MS2(p28);
DigitalOut MS3(p29);

//Potmeter and EMG
AnalogIn Pot1(p19);
AnalogIn Pot2(p20);
AnalogIn emg0(p17);
HIDScope scope(2);
Ticker   scopeTimer;

//lcd
C12832_LCD lcd;

//Variables for motor control
float setpoint = 6500; //Frequentie
float step_freq = 1;


// Filters
arm_biquad_casd_df1_inst_f32 lowpass_pot;
arm_biquad_casd_df1_inst_f32 lowpass_step;

//lowpass filter settings: Fc = 1 Hz, Fs = 100 Hz, Gain = -3 dB onepole-lp
float lowpass_const[] = {0.0201, 0.0402 , 0.0201, 1.5610, -0.6414};
//Lowpass filter potmeter: Fc = 0.5 Hz, Fs = 500 Hz,
//float lowpass_const[] = {0.000009825916403675327, 0.000019651832807350654, 0.000009825916403675327, 1.991114207740345, -0.9911535114059596};
//lowpass for step_freq: Fc = 2 Hz, Fs = 100, Gain = 6 dB
//float lowpass1_const[] = {0.007820199259120319, 0.015640398518240638, 0.007820199259120319, 1.7347238224240125, -0.7660046194604936};

//EMG filter
arm_biquad_casd_df1_inst_f32 lowpass_biceps;
//lowpass filter settings biceps: Fc = 2 Hz, Fs = 500 Hz, Gain = -3 dB
float lowpass2_const[] = {0.00015514839749793376, 0.00031029679499586753, 0.00015514839749793376, 1.9644602512795832, -0.9650808448695751};
arm_biquad_casd_df1_inst_f32 highnotch_biceps;
//highpass filter settings: Fc = 20 Hz, Fs = 500 Hz, Gain = -3 dB, notch Fc = 50, Fs =500Hz, Gain = -3 dB
float highnotch_const[] = {0.8370879899975344, -1.6741759799950688, 0.8370879899975344, 1.6474576182593796, -0.7008943417307579, 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};


//state values
float lowpass_biceps_states[4];
float highnotch_biceps_states[8];
float lowpass_pot_states[4];
float lowpass1_step_states[4];

//global variabels
float filtered_biceps;
float filtered_average_bi;
float filtered_pot;
float filtered_average_pot;
float filtered_step;
float pot_value1_f32;
float filt_avg_bi_old;
float speed_old;
float acc;
float force;
float spd;
float spd_old;
float D = 0;
float Damp;
float K_Gain;

void average_biceps(float filtered_biceps,float *average)
{
    static float total=0;
    static float number=0;
    total = total + filtered_biceps;
    number = number + 1;
    if ( number == 50) {
        *average = total/50;
        total = 0;
        number = 0;
    }
}

void looper_emg()
{
    /*variable to store value in*/
    volatile uint16_t emg_value1;

    float emg_value1_f32;


    /*put raw emg value both in red and in emg_value*/
    emg_value1 = emg0.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
    emg_value1_f32 = emg0.read();

    //process emg biceps
    arm_biquad_cascade_df1_f32(&highnotch_biceps, &emg_value1_f32, &filtered_biceps, 1 );
    filtered_biceps = fabs(filtered_biceps);
    arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_biceps, &filtered_biceps, 1 );
    average_biceps(filtered_biceps,&filtered_average_bi);

    /*send value to PC. */
    scope.set(0,filtered_average_bi);  //Raw EMG signal biceps
    scope.set(1,filtered_biceps); //Filtered signal

}
/*void looper_pot()
{

    pot_value1_f32 = Pot1.read() - 0.500;

    //process input
    arm_biquad_cascade_df1_f32(&lowpass_pot, &pot_value1_f32, &filtered_pot, 1 );

}*/


/*void looper_motor()
{
    float new_step_freq;
    float speed;

    speed = 0.02*filtered_average_bi + 0.02*filt_avg_bi_old + 0.96*speed_old; //Value between 0 and 1
    new_step_freq = (setpoint*speed);
    step_freq = abs(new_step_freq); //Gives the PWM frequenty to the motor.
    speed_old = speed;
    filt_avg_bi_old = filtered_average_bi;

    if (step_freq < 500 || step_freq > 8000) {
        Enable = 1;
    } else {
        Enable = 0;
    }
    Step.period(1.0/(100 + step_freq)); //Step_freq is het aantal Hz.

}*/
//Motor accelereren met EMG treshold
/*void looper_motor()
{
    float new_step_freq;
    Dir = 0;

    if (filtered_average_bi > tres_bi) {
        Enable = 0;
        new_step_freq = ((1-P)*setpoint) + (P*step_freq);
        step_freq = new_step_freq;
        Step.period(1.0/step_freq);
    } else {
        Enable = 1;
        step_freq = 1;
    }

}*/

void looper_motor()
{
    Dir = 0;
    K_Gain = 20*Pot2.read();
    force = K_Gain*filtered_biceps;
    force = force - D;
    acc = force/Mass;
    spd = spd_old + (acc * dt);
    Damp = 2*Pot1.read();
    D = spd * Damp;
    step_freq = (setpoint*spd);
    Step.period(1.0/step_freq);
    spd_old = spd;

    if (step_freq < 800) {
        Enable = 1;
    } else {
        Enable = 0;
    }
}


int main()
{
    // Attach the HIDScope::send method from the scope object to the timer at 50Hz. Hier wordt de sample freq aangegeven.
    scopeTimer.attach_us(&scope, &HIDScope::send, 2e3);

    /* Ticker log_timer;
     //set up filters. Use external array for constants
     arm_biquad_cascade_df1_init_f32(&lowpass_pot, 1 , lowpass_const, lowpass_pot_states);
     log_timer.attach(looper_pot, 0.01);*/

    Ticker emgtimer;
    arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1 , lowpass_const, lowpass_biceps_states);
    arm_biquad_cascade_df1_init_f32(&highnotch_biceps, 2 , highnotch_const, highnotch_biceps_states);
    emgtimer.attach(looper_emg, 0.002);

    Ticker looptimer;
    looptimer.attach(looper_motor, 0.01);

    MS1 = 1;
    MS2 = 0;
    MS3 = 0;
    //Step.period(1/step_freq);
    Step.write(0.5); // Duty cycle van 50%

    while (1) {

        lcd.printf("Freq %.0f Hz \n", step_freq); //snelheid meting op lcd
        //pc.printf(" %.4f \n", Pot1.read());
        //lcd.printf("filt %.3f raw %.3f \n", filtered_biceps, emg0.read());
        //pc.printf("Spd %.0f Hz p1 %.4f \n", step_freq, pot_value1_f32); //snelheid meting op lcd
        wait(0.01);


    }
}