File content as of revision 1:a9c933f1dc71:

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


Serial pc(USBTX,USBRX);
Timer timer; 
float Ts = 0.002;


DigitalIn buttonR(D2);//rigth button on biorobotics shield
DigitalIn buttonL(D3);//left button on biorobotics shield

int sensor_sensitivity = 32;
int gear_ratio = 131;
float full_ratio  = gear_ratio*sensor_sensitivity*4;

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

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;

//Vector2d twist(0,0);
//Matrix2f jacobian(0, 0, 0, 0), inv_jacobian(0, 0, 0, 0); 

DigitalOut motor1_direction(D4);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
PwmOut motor1_speed_control(D5);//aanstuursnelheid motor 1
PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2
DigitalOut motor2_direction(D7);// draairichting motor 2 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)


enum States {failure, waiting, calib_motor, 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( 5 );

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;
float high1;
float abs1;
float low1;
void filterall()
{
    //Highpass Biquad 5 Hz 
static BiQuad HighPass1(0.95653708,  -1.91307417, 0.95653708, -1.91118480, 0.91496354);
high1 = HighPass1.step(emg1_input);
static BiQuad HighPass2(0.95653708,  -1.91307417, 0.95653708, -1.91118480, 0.91496354);
float high2 = HighPass2.step(emg2_input);
    // Rectify the signal(absolute value)
abs1 = fabs(high1);
float abs2 = fabs(high2);
    //Lowpass Biquad 10 Hz
static BiQuad LowPass1(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
low1 = LowPass1.step(abs1);
static BiQuad LowPass2(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
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 = Encoder1.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,emg1_input); // 
    scope.set(1,high1); //    
    scope.set(2,abs1); //
    scope.set(3,low1);//
    scope.set(4,filteredsignal1);
    scope.send(); // send info to HIDScope server
}


 //////////////////// MOVEMENT STATES
void do_forward(){
    
    //twist << 1, 0;
    //Vector2d twistf(0,0);
    //twistf << 1, 0;
    if (filteredsignal2 > threshold2){
        //double abs_sig = (filteredsignal2 - (0.5*max2))/(0.5*max2);
        
        //twist = twistf * abs_sig;
        
        }
    
    
    if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
        {
            movement = backward;
            timer.reset();
            }
    }

void do_backward(){
  
    
    
     if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
        {
            movement = up;
            timer.reset();
            }
    }

void do_up(){
    
    //Code for moving up
    
     if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
        {
            movement = down;
            timer.reset();
            }
    }
void do_down(){
    
    //Code for moving down
    
     if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
        {
            movement = rest;
            timer.reset();
        
            }
    }
void do_wait(){
    
    if ( filteredsignal2 > threshold2) {// 
        current_state = waiting;
        state_changed = true;
    }              
        if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
        {
            movement = forward;
            timer.reset();
            }
    }    
///////////END MOVEMENT STATES/////////////////////////
///////////ROBOT ARM STATES ///////////////////////////

void do_state_failure(){    
     
    }
    bool on = true;
void do_state_calib_motor(){
    if (state_changed==true) {
        state_changed = false;        
    }
    
    
    if(on){
        timer.reset();
    motor1_direction = 1; //set motor 1 to run counterclockwise (positive) direction for calibration
    motor1_speed_control = 0.3;//to make sure the motor will not run at too high velocity
    motor2_direction = 0; // set motor 2 to run clockwise (negative) direction
    motor2_speed_control = 0.3;
    on = false;
    }
    
    int deriv1 = fabs((counts_m1 - counts_m1_prev)/Ts);
    int deriv2 = fabs((counts_m2 - counts_m2_prev)/Ts);
    
    
    if ( timer.read() > 5 && deriv1 < 1 && deriv2 < 1) {
        motor1_speed_control = 0;
        motor2_speed_control = 0;
        current_state = calib_emg;
        timer.reset();
        state_changed = true;
    }
    }
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() > 10 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
        current_state = operational;
        timer.reset();
        state_changed = true;
    }    
}

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;    
    }
    if (movement == rest && filteredsignal2 > threshold2) {
        current_state = waiting;        
        state_changed = true;
    }       
    
}

void do_state_waiting(){
    if (state_changed==true) {
        state_changed = false;        
    }   
       
    if (filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
        current_state = operational;
        state_changed = true;
    } 
}
//////////////// END ROBOT ARM STATES //////////////////////////////

void motor_controller(){
    float q1;
    float q2;
    //q1 defined
    //q2 defined
    
    //float L0 = 0.1;
    //float L1 = 0.1;
    //float L2 = 0.4;
    
    //jacobian << L1, L2*sin(q1)+L1,-L0,-L0 - L2*cos(q1);
        
    //inv_jacobian = jacobian.inverse();
    //Vector2d reference_vector = inv_jacobian*twist;
    //float ref_v1 = reference_vector(0);
    //float ref_v2 = reference_vector(1);
    
    //float ref_q1 = 0;
    //ref_q1 = ref_p1 + 0.002*ref_v1;
    // float ref_q2 = 0;
    //ref_q2 = ref_p2 + 0.002*ref_v2;
    
    
    
    }

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 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
    
    
    
    timer.start();
    loop_timer.attach(&loop_function, Ts);    
    sample_timer.attach(&scopedata, Ts);    
    sample_timer2.attach(&filterall, Ts);

    
    
    
     //led_red = 1;
     //led_green = 1;
    while (true) {              
    }
}