File content as of revision 20:0f6a88f29a71:

#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "FastPWM.h"
#include <iostream>
MODSERIAL pc(USBTX, USBRX);
AnalogIn ain2(A2);
AnalogIn ain1(A3);
DigitalOut dir2(D4);
DigitalOut dir1(D7);
//D4,D7 direction of motors 2,1 on board, D5,D6- PWM of motors 2,1 on board
PwmOut motor2_pwm(D5);
PwmOut motor1_pwm(D6);
AnalogIn    emg0( A0 );
AnalogIn    emg1( A1 );
QEI motor1 (D8, D9, NC, 32);
QEI motor2 (D12, D13, NC, 32);

Ticker ticktick;
Timer state_time;
Timeout EMG_peak;
Timeout turn;
Ticker      sample_timer;
HIDScope    scope( 2);
DigitalOut  ledred(LED_RED);
DigitalOut  ledblue(LED_BLUE);
DigitalOut  ledgreen(LED_GREEN);
InterruptIn err(SW2);
InterruptIn button(SW3);

volatile float P0;
volatile float P1;
volatile float EMG_min0=1;
volatile float EMG_max0=0;
volatile float EMG_min1=1;
volatile float EMG_max1=0;
volatile bool ignore_peaks=false;
volatile bool ignore_turn=true;
enum states{Waiting,Position_calibration,EMG_calibration,Homing,Operating,Demo,Failure};
states currentState=Waiting;

static float v_refx; //reference speeds 
static float v_refy;
static float T = 0.002; //measuring period
static float L = 0.35;  //distance between the motor axles, has to be checked
volatile float theta1_old = 0.0; //feedback variables
volatile float theta2_old = 0.0;

int m1_count; 
float m1_angle;
int m2_count;
float m2_angle;
float pi = 3.14;

float error_x;
float error_y;

//PID controller variables
bool q = true;
float error_prev;
static float Kp = 8;
static float Ki = 1.02;
static float Kd = 0.1;
float u;
float u_p;
float u_i;
float u_d;
float error_integral = 0.0;

float u_1;
float u_2;

void read_emg()
{
    //EMG signal 0
    static int count0=0;
    static float RMS_value0=0;
    static float HighPass_value0=0;
    count0+=1;
    static float RMS0[150];
    static float HighPass0[30];
    static BiQuad Notch0(0.9695f,-1.5695f,0.9695f,-1.5695f,0.9391f);
    static BiQuad Notch1(0.9695f,-1.5695f,0.9695f,-1.5695f,0.9391f);
    float I0;
    float If0;
    //signal 1
    static int count1=0;
    static float RMS_value1=0;
    static float HighPass_value1=0;
    count1+=1;
    static float RMS1[150];
    static float HighPass1[30];
    float I1;
    float If1;
    I0=emg0.read(); //read signal
    double notched0=Notch0.step(I0);
    HighPass_value0+=(notched0-HighPass0[count0%30])/30.0;
    HighPass0[count0%30]=notched0;
    If0=pow(I0-HighPass_value0,2.0f); // Highpass-filtered value squared
    RMS_value0+=(If0-RMS0[count0%150])/150.0;
    RMS0[count0%150]=If0;
    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
    P0=sqrt(RMS_value0);
    I1=emg1.read(); //read signal
    double notched1=Notch1.step(I1);
    HighPass_value1+=(notched1-HighPass1[count1%30])/30.0;
    HighPass1[count1%30]=notched1;
    If1=pow(I1-HighPass_value1,2.0f); // Highpass-filtered value squared
    RMS_value1+=(If1-RMS1[count1%150])/150.0;
    RMS1[count1%150]=If1;
    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
    P1=sqrt(RMS_value1);
    scope.set(0, P0 ); // send root mean squared
    scope.set(1, notched0);
    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
    *  Ensure that enough channels are available (HIDScope scope( 2 ))
    *  Finally, send all channels to the PC at once */
    scope.send();
    /* To indicate that the function is working, the LED is toggled */
    ledred=1;
    ledgreen=0;
    ledblue=1;
}

void record_min_max(void)
{
    float t=state_time.read();
    if(t>0.4)
    {
        if(P0<EMG_min0)
        {
            EMG_min0=P0;
        }
        else if(P0>EMG_max0)
        {
            EMG_max0=P0;
        }
        if(P1<EMG_min1)
        {
            EMG_min1=P1;
        }
        else if(P1>EMG_max1)
        {
            EMG_max1=P1;
        }
    }
}

void unignore_peaks(void)
{
    ignore_peaks=false;
}
void start_ignore_turn(void)
{
    ignore_turn=true;
}

void set_PWM(void)
{
   static bool motor_on=false;
   float Q0;
   Q0=2.0f*(P0-(EMG_min0+EMG_max0)/2.0f)/(EMG_max0-EMG_min0);
   if (Q0>-0.2f && !ignore_peaks)
   {
        if (motor_on)
        {
            motor1_pwm.write(0.0f);
            EMG_peak.attach(unignore_peaks,0.8);
            turn.attach(start_ignore_turn,1);
            ignore_turn=false;
            ignore_peaks=true;
            motor_on=false;
        }
        else if(ignore_turn)
        {
            motor1_pwm.write(1.0f);
            EMG_peak.attach(unignore_peaks,0.8);
            turn.attach(start_ignore_turn,1);
            ignore_turn=false;
            ignore_peaks=true;
            motor_on=true;
        }
        else
        {
            motor1_pwm.write(1.0f);
            dir1=!dir1;
            EMG_peak.attach(unignore_peaks,1.5);
            ignore_peaks=true;
            motor_on=true;
        } 
    }
   motor2_pwm.write(ain1.read());
}

void error(void){
    m1_count = motor1.getPulses();                                                                                  // Read out both motor counts and convert them to rads.
    m1_angle = (float)m1_count / 4200.0f * (2.0f * pi);
    m2_count = motor2.getPulses();
    m2_angle = (float)m1_count / 4200.0f * (2.0f * pi);
    
    float dvd=L * (tan(m2_angle) * pow(tan(m1_angle),2) - tan(m1_angle) * pow(tan(m2_angle),2));

    float a =  (  -pow(cos(m1_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * pow(tan(m1_angle),2)  )  / dvd;             // inverse matrix components of differential x and differential y
    float b =  (   pow(cos(m1_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * tan(m1_angle)         )  / dvd; 
    float c =  (   pow(cos(m2_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * pow(tan(m2_angle),2)  )  / dvd;
    float d =  (   pow(cos(m2_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * pow(tan(m2_angle),2)  )  / dvd;
    
    float theta1_dot = a*v_refx + b*v_refy;
    float theta2_dot = c*v_refx + d*v_refy;
    
    float theta1_ref = theta1_old+theta1_dot*T;
    float theta2_ref = theta2_old+theta2_dot*T;
    
    error_x = m1_angle - theta1_ref;
    error_y = m2_angle - theta2_ref;
    
    theta1_old = theta1_ref;
    theta2_old = theta2_ref;
    
    }
    
float PID(float err){
    //P action
    u_p = Kp * fabs(err);
    
    //I action 
    error_integral = error_integral + err * T;
    u_i = Ki * error_integral;
    
/*    
    //D action
    if(q){
        error_prev = err;
        q=false;
        }
        
    float error_derivative = (err - error_prev)/T;
    float filtered_error_derivative = LowPassFilter.step(error_derivative);
    u_d = Kd * filtered_error_derivative;
    error_prev = err;
    
*/
    
    u = u_p + u_i;
    
    return u;
    }
    
void setPWM_controller(void){
    u_1 = PID(error_x);
    u_2 = PID(error_y);
    
    if(u_1 < 0.0f){
        dir1 = 1;
    }else{
        dir1 = 0;
        }
    motor1_pwm.write(fabs(u_1));
    
    if(u_2 < 0.0f){
        dir2 = 1;
    }else{
        dir2 = 0;
        }
    motor2_pwm.write(fabs(u_1));
    
    }

void sample()
{
    switch(currentState)
    {
        case Waiting:
            ledred=0;
            ledgreen=0;
            ledblue=1;
            break;
        case Position_calibration:
            ledred=1;
            ledgreen=1;
            ledblue=0;
            break;
        case EMG_calibration:
            ledred=1;
            ledgreen=0;
            ledblue=0;
            read_emg();
            record_min_max();
            break;
        case Homing:
            ledred=0;
            ledgreen=1;
            ledblue=0;
            break;
        case Operating:
            read_emg();
            set_PWM();
            ledred=1;
            ledgreen=0;
            ledblue=1;
            break;
        case Demo:
            ledred=0;
            ledgreen=0;
            ledblue=0;
            break;
        case Failure:
            ledred=0;
            ledgreen=1;
            ledblue=1;
            motor1_pwm.write(0.0);
            motor2_pwm.write(0.0);
            break;
    }
}

void error_occur()
{
    currentState=Failure;
}

void button_press() 
//Press button to change state
{
    state_time.start();
    switch(currentState)
    {
        case Waiting:
            currentState=Position_calibration;
            wait(1);
            break;
        case Position_calibration:
            currentState=EMG_calibration;
            wait(1);
            break;
        case EMG_calibration:
            currentState=Homing;
            wait(1);
            break;
        case Homing:
            currentState=Operating;
            wait(1);
            break;
        case Operating:
            currentState=Demo;
            wait(1);
            break;
        case Demo:
            currentState=Operating;
            wait(1);
            break;
    }
}

int main()
{
    pc.baud(115200);
    pc.printf("Starting...");
    ledred=0;
    sample_timer.attach(&sample, 0.002);
    err.fall(error_occur);
    button.fall(button_press);
    int frequency_pwm=10000;
    motor1_pwm.period(1.0/frequency_pwm);
    motor2_pwm.period(1.0/frequency_pwm);
    
    while (true) {
        wait(10);
    }

}