File content as of revision 12:c59b25d07bb9:

#include "mbed.h"
#include "BiQuad.h"
#include "HIDScope.h"
#include <stdio.h>   
#include <math.h>      
#include "QEI.h"


Serial pc(USBTX,USBRX);
Timer timer; 
float Ts = 0.002;
int sensor_sensitivity = 32;
int gear_ratio = 131;
float full_ratio  = gear_ratio*sensor_sensitivity*4;
double pi = 3.141592653589793238462643383279502884;
DigitalIn but1(D2);

QEI Encoder1(D10,D11,NC,sensor_sensitivity); //First one is B, Second one is A
QEI Encoder2(D12,D13,NC,sensor_sensitivity); //

DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_GREEN);
DigitalOut led_blue(LED_BLUE);

int counts_m1 = 0;
int counts_m2 = 0;
int counts_m1_prev = 0;
int counts_m2_prev = 0;
float deg_m1 = 0;
float deg_m2 = 0;



DigitalOut motor1_direction(D4);// draairichting motor 1 (0 is CCW )
PwmOut motor1_speed_control(D5);//aanstuursnelheid motor 1
PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2
DigitalOut motor2_direction(D7);// draairichting motor 2 (0 is CCW )

    
    float int_u1 = 0;
    float int_u2 = 0;
    float u1 = 0;
    float u2 = 0;
    
     float ref_q1 = 0;    
     float ref_q2 = 0;
    float L0 = 0.1;
    float L1 = 0.1;
    float L2 = 0.4;
    
    float ref_v1;
    float ref_v2;
    float teraterm1;
    float teraterm2;
    float error1;
    float error2;
    float filtered_d_error1;
    float filtered_d_error2;
    
    float kp1 = 0.1;
    float kp2 = 0.1;
    float ki1 = 0.05;
    float ki2 = 0.05;
    float kd1 = 0.005;
    float kd2 = 0.005;
    float olderror1;
    float olderror2;
    float d_error1;
    float d_error2;
    
enum States {failure, waiting, calib_motor, homing ,calib_emg, operational, demo};
enum Operations {rest, forward, backward, up, down};

States current_state = calib_motor;
Operations movement = rest;

float max1 = 0; //initial threshold value for emg signals, changes during calibration left arm
float max2 = 0; // right arm
float threshold1;
float threshold2;
float thresholdtime = 1.0; // time waiting before switching modes

Ticker      loop_timer;
Ticker      sample_timer;
Ticker      sample_timer2;
//HIDScope    scope(3);

AnalogIn raw_emg1_input(A0);//input for first emg signal 1, for the modes
AnalogIn raw_emg2_input(A1);//input for first emg signal 2, for the strength

volatile float emg1_input;
volatile float emg2_input;

volatile float raw_filteredsignal1;//the first filtered emg signal 1
volatile float raw_filteredsignal2;//the first filtered emg signal 2

volatile float filteredsignal1;//the first filtered emg signal 1
volatile float filteredsignal2;//the first filtered emg signal 2

bool state_changed = false;
static BiQuad Notch1(0.9714498065192796,-1.5718388053127037,0.9714498065192796,-1.5718388053127037,0.9428996130385592);
static BiQuad Notch2(0.9714498065192796,-1.5718388053127037,0.9714498065192796,-1.5718388053127037,0.9428996130385592);
static BiQuad HighPass1(0.95653708,  -1.91307417, 0.95653708, -1.91118480, 0.91496354);
static BiQuad HighPass2(0.95653708,  -1.91307417, 0.95653708, -1.91118480, 0.91496354);
static BiQuad LowPass1(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
static BiQuad LowPass2(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
static BiQuad LowpassFilter1(0.0640,0.1279,0.0640,-1.1683,0.4241);
static BiQuad LowpassFilter2(0.0640,0.1279,0.0640,-1.1683,0.4241);
void filterall()
{
    //Notch 50 Hz BW 6 Hz
    float notch1 = Notch1.step(emg1_input);
    float notch2 = Notch2.step(emg2_input);
    //Highpass Biquad 5 Hz 
    float high1 = HighPass1.step(notch1);
    float high2 = HighPass2.step(notch2);
    // Rectify the signal(absolute value)
    float abs1 = fabs(high1);
    float abs2 = fabs(high2);
    //Lowpass Biquad 10 Hz
    float low1 = LowPass1.step(abs1);
    float low2 = LowPass2.step(abs2);

raw_filteredsignal1 = low1;
raw_filteredsignal2 = low2;


}

void measureall(){ // changes all variables according in sync with the rest of the code
    
    emg1_input = raw_emg1_input.read();
    emg2_input = raw_emg2_input.read();
    
    filteredsignal1 = raw_filteredsignal1;
    filteredsignal2 = raw_filteredsignal2;
    
    //Reading of motor
            
    counts_m1 = Encoder1.getPulses() - counts_m1_prev;
    counts_m2 = Encoder2.getPulses() - counts_m2_prev;
    deg_m1 = deg_m1 + counts_m1*(360/(full_ratio));
    deg_m2 = deg_m2 + counts_m2*(360/(full_ratio));
    counts_m1_prev = Encoder1.getPulses();
    counts_m2_prev = Encoder2.getPulses();                
            
}

void scopedata()
{
    //scope.set(0,filteredsignal1); // 
    //scope.set(1,filteredsignal2); //    
    //scope.set(2,ref_q1); //
   // scope.set(3,movement);//
   // scope.set(4,ref_q1);
    //scope.send(); // send info to HIDScope server
}


 //////////////////// MOVEMENT STATES
 float twist[2] = {0,0};
 float twistf[2] = {0,0};
 float abs_sig;  
 
    int gain = 3;
    
    int sign1 = 0.5;
    int sign2 = 1;
    
void do_forward(){
    
    sign1 = 0.5;
    sign2 = 1;
    twistf[0] = 1;
    twistf[1] = 0;
    
    if (filteredsignal2 > (0.4*max2)){
        abs_sig = (filteredsignal2 - (0.4*max2))/(0.6*max2)* gain;                    
        }
    else
    {    abs_sig = 0;
        }    
    twist[0] = twistf[0] * abs_sig ;
    twist[1] = twistf[1] * abs_sig ;
             
    motor1_speed_control = fabs(u1);
    
    if(u1 > 0){
        motor1_direction = 1;}
    if(u1 < 0){
        motor1_direction = 0;}

    motor2_speed_control = fabs(u2);
    
    if(u2 > 0){
        motor2_direction = 1;}
    if(u2 < 0){
        motor2_direction = 0;} 
    
    if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
        {
          movement = backward;
           timer.reset();
        
        d_error1 = 0;
        d_error2 = 0;
        u1 = 0;
        u2 = 0;
        int_u1 = 0;
        int_u2 = 0;
        led_green = 0;
           }
    }

void do_backward(){
  
    sign1 = -1;
    sign2 = 1;
    twistf[0] = -1;
    twistf[1] = 0;
    
    if (filteredsignal2 > (0.4*max2)){
        abs_sig = (filteredsignal2 - (0.4*max2))/(0.6*max2)* gain;                    
        }
    else
    {    abs_sig = 0;
        }    
    twist[0] = twistf[0] * abs_sig ;
    twist[1] = twistf[1] * abs_sig ;
             
    motor1_speed_control = fabs(u1);
    
    if(u1 > 0){
        motor1_direction = 1;}
    if(u1 < 0){
        motor1_direction = 0;}

    motor2_speed_control = fabs(u2);
    
    if(u2 > 0){
        motor2_direction = 1;}
    if(u2 < 0){
        motor2_direction = 0;} 
    
     if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
        {
        movement = up;
        timer.reset();
       
        d_error1 = 0;
        d_error2 = 0;
        u1 = 0;
        u2 = 0;
        int_u1 = 0;
        int_u2 = 0;
        led_red = 1;
            }
    }

void do_up(){
    
    sign1 = 1;
    sign2 = 1;
    twistf[0] = 0;
    twistf[1] = 1;
    
    if (filteredsignal2 > (0.4*max2)){
        abs_sig = (filteredsignal2 - (0.4*max2))/(0.6*max2)* gain;                    
        }
    else
    {    abs_sig = 0;
        }    
    twist[0] = twistf[0] * abs_sig ;
    twist[1] = twistf[1] * abs_sig ;
             
    motor1_speed_control = fabs(u1);
    
    if(u1 > 0){
        motor1_direction = 1;}
    if(u1 < 0){
        motor1_direction = 0;}

    motor2_speed_control = fabs(u2);
    
    if(u2 > 0){
        motor2_direction = 1;}
    if(u2 < 0){
        motor2_direction = 0;}
    
     if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
        {
        movement = down;
        timer.reset();
       
        d_error1 = 0;
        d_error2 = 0;
        u1 = 0;
        u2 = 0;
        int_u1 = 0;
        int_u2 = 0;
        led_green = 1;
        led_blue = 0;
            }
    }
void do_down(){
    
    sign1 = 1;
    sign2 = -1;
    twistf[0] = 0;
    twistf[1] = -1;
    
    if (filteredsignal2 > (0.4*max2)){
        abs_sig = (filteredsignal2 - (0.4*max2))/(0.6*max2)* gain;                    
        }
    else
    {    abs_sig = 0;
        }    
    twist[0] = twistf[0] * abs_sig ;
    twist[1] = twistf[1] * abs_sig ;
             
    motor1_speed_control = fabs(u1);
    
    if(u1 > 0){
        motor1_direction = 1;}
    if(u1 < 0){
        motor1_direction = 0;}

    motor2_speed_control = fabs(u2);
    
    if(u2 > 0){
        motor2_direction = 1;}
    if(u2 < 0){
        motor2_direction = 0;}
    
     if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
        {
        movement = rest;
        timer.reset();
       
        d_error1 = 0;
        d_error2 = 0;
        u1 = 0;
        u2 = 0;
        int_u1 = 0;
        int_u2 = 0;
        led_red = 0;
        led_green = 0;
        
            }
    }
void do_wait(){
    
             
        if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
        {
            movement = forward;
            timer.reset();
            led_green = 1;
            led_blue = 1;
            }
    }    
///////////END MOVEMENT STATES/////////////////////////
///////////ROBOT ARM STATES ///////////////////////////
    
void do_state_failure(){    
     
    }
    
   int count1 = 0;
   int count2 = 0;  
   
void do_state_calib_motor(){
    if (state_changed==true) {
        state_changed = false;        
    }
    led_red = 0;
    
    
    
    int deriv1 = deg_m1 - count1;
    int deriv2 = deg_m2 - count2;
    
    count1 = deg_m1;
    count2 = deg_m2;
   
    if (  timer.read() > 3 && deriv1 < 0.5 && deriv2 < 0.5) {
        motor1_speed_control = 0;
        motor2_speed_control = 0;
        current_state = homing;
        
        state_changed = true;
        wait(1);
        deg_m1 = 57.4;
        deg_m2 = 72.93;
        led_red = 1;
        led_green = 0;
    }
    }
    
float wu1;
float wu2;  

void do_state_homing(){
     if (state_changed==true) {
        timer.reset();
        state_changed = false;        
    }
     float kp11 = 0.1;
     float kp12 = 0.1; 
        
    float werror1 = deg_m1 - 0;
    float werror2 = deg_m2 - 0;
    
    if(werror1 > 5){
        wu1 = 1;        }
    if(werror1 < -5){
        wu1 = -1;        }
    else{
        wu1 = kp11*werror1; //+ (u1 + werror1*0.002)*ki1;   
    }
    
    if(werror2 > 5){
        wu2 = 1;}
    if(werror2 < -5){
        wu2 = -1;}
    else{
        wu2 = kp12*werror2; //+ (u2 + werror2*0.002)*ki2);
        }
    
    motor1_speed_control = fabs(wu1)/16;
    
    if(wu1 > 0){
        motor1_direction = 1;}
    if(wu1< 0){
        motor1_direction = 0;}

    motor2_speed_control = fabs(wu2)/16;
    
    if(wu2 > 0){
        motor2_direction = 1;}
    if(wu2 < 0){
        motor2_direction = 0;}
        
    if (  timer.read() > 4) {
         motor1_speed_control = 0;
         motor2_speed_control = 0;  
        
        ref_q1 = 0;
        ref_q2 = 0;
        deg_m1 = 0;
        deg_m2 = 0;
        d_error1 = 0;
        d_error2 = 0;
        u1 = 0;
        u2 = 0;
        int_u1 = 0;
        int_u2 = 0;
         wait(1); 
        current_state = calib_emg;
        timer.reset();
        state_changed = true;
        led_green = 1;
        led_blue = 0;
         
    }   

    
    }
void do_state_calib_emg(){    
    if (state_changed==true) {
        state_changed = false;        
    }
    
    if(filteredsignal1 > max1){//calibrate to a new maximum
       max1 = filteredsignal1;
       threshold1 = 0.5 * max1;
       }
    if(filteredsignal2 > max2){//calibrate to a new maximum
       max2 = filteredsignal2;
       threshold2 = 0.5 * max2;
       }
       
    if (timer.read() > 5 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
        current_state = operational;
        timer.reset();
        state_changed = true;
        led_green = 0;
        led_red = 0;
    }    
}

void do_state_operational(){
     if (state_changed==true) {
        state_changed = false;        
    }
    
    switch(movement) {// a separate function for what happens in each state
    case rest:
        do_wait();
        break;
    case forward:
        do_forward();
         
        break;
    case backward:
        do_backward();
        
        break;    
    case up:
        do_up();        
        break;
    case down:
        do_down();      
        break;    
    }
       
    
}

void do_state_waiting(){
    if (state_changed==true) {
        state_changed = false;        
    }   
       
    if (filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
        current_state = operational;
        state_changed = true;
        movement = rest;
        led_green = 0;
        led_blue = 0;
        led_red = 0;
    } 
}
//////////////// END ROBOT ARM STATES //////////////////////////////
 
    
void motor_controller(){   
     
    float jacobian[4];
    float inv_jacobian[4];
    
    jacobian[0] = L1;
    jacobian[1] = (L2*cos(deg_m2/180*pi))+L1;
    jacobian[2] = -L0;
    jacobian[3] = -L0 - (L2*sin(deg_m2/180*pi));
    
    float det = 1/((jacobian[0]*jacobian[3])-(jacobian[1]*jacobian[2]));
    
    inv_jacobian[0] = det*jacobian[3];
    inv_jacobian[1] = -det*jacobian[1]; 
    inv_jacobian[2] = -det*jacobian[2];
    inv_jacobian[3] = det*jacobian[0];
    
    
    ref_v1 = inv_jacobian[0]*twist[0]+inv_jacobian[1]*twist[1];
    ref_v2 = inv_jacobian[2]*twist[0]+inv_jacobian[3]*twist[1];
       
       
     
    ref_q1 = ref_q1 + 0.002*ref_v1 * sign1;
    ref_q2 = ref_q2 + 0.002*ref_v2 * sign2;
    
    error1 = deg_m1 - ref_q1;
    error2 = deg_m2 - ref_q2;
    
    


    
    
   // if(error1 > 5){
     //   u1 = 1;        }
    //if(error1 < -5){
    //    u1 = -1;        }
    //else{
        d_error1 = (error1 - olderror1)/Ts;
        filtered_d_error1 = LowpassFilter1.step(d_error1);
        int_u1 = int_u1 + error1 * Ts;
        u1 = kp1*error1 + int_u1*ki1 + kd1*filtered_d_error1;
        teraterm1 = u1;   // to see how big u1 and u2 actually get, they should lie between -1 and 1 for the right gains.
        if(u1>1){
            u1 =1;
           }
        if(u1<-1){
            u1 = -1;
            }
        
    //}
    olderror1 = error1;
    
    //if(error2 > 5){
     //   u2 = 1;}
   // if(error2 < -5){
     //   u2 = -1;}
    //else{
         d_error2 = (error2 - olderror2)/Ts;
        filtered_d_error2 = LowpassFilter2.step(d_error2);       
        int_u2 = int_u2 + error2 * Ts;
        u2 = kp2*error2 + int_u2*ki2 + kd2*filtered_d_error2;
        teraterm2 = u2;
         if(u2>1){
            u2 =1;
            }
        if(u2<-1){
            u2 = -1;
            }       
     //   }
    olderror2 = error2;
    
       
    }

void loop_function() { //Main loop function
    measureall();    
    switch(current_state) {
    case failure:
        do_state_failure(); // a separate function for what happens in each state
        break;
    case calib_motor:
        do_state_calib_motor();  
        break ;
    case homing:
        do_state_homing();
        break;    
    case calib_emg:
        do_state_calib_emg();
        break;
    case operational:
        do_state_operational();
    break;
    case waiting:
        do_state_waiting();
        break;  
}
motor_controller();
// Outputs data to the computer

}


int main()
{        
   // motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz
    
    motor1_direction = 0; //set motor 1 to run clockwisedirection for calibration
    motor1_speed_control = 0.15;//to make sure the motor will not run at too high velocity
    motor2_direction = 0; // set motor 2 to run clockwise direction
    motor2_speed_control = 0.15; 
     
    led_red = 1;  
    led_green = 1;
    led_blue = 1;   
    timer.start();
    loop_timer.attach(&loop_function, Ts);    
    sample_timer.attach(&scopedata, Ts);    
    sample_timer2.attach(&filterall, Ts);
    
    pc.baud(115200);
    while (true) { 
            printf("%i %i %f %f %f %f \n\r",movement,current_state,error1,error2,deg_m1, deg_m2);
    }
}