File content as of revision 46:9b60a3c1acab:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "HIDScope.h"
#include "BiQuad.h"
#include "PID_controller.h"
//#include "kinematics.h"
#include "Servo.h"
#include "processing_chain_emg.h"
#include <vector>

//pc
MODSERIAL pc(USBTX, USBRX);

// emg inputs
AnalogIn    emg0( A0 );
AnalogIn    emg1( A1 );
AnalogIn    emg2( A2 );
AnalogIn    emg3( A3 );
Servo       gripper(A5);
//InterruptIn button1(SW2);
DigitalIn   buttongripper(D1);
double range;

// motor ouptuts
PwmOut      motor1_pwm(D5);
DigitalOut  motor1_dir(D4);
PwmOut      motor2_pwm(D6);
DigitalOut  motor2_dir(D7);
AnalogIn vx_adjustment(A4);
AnalogIn vy_adjustment(A5);

// defining encoders
QEI motor_1_encoder(D12,D13,NC,32);
QEI motor_2_encoder(D10,D11,NC,32);

// other objects
DigitalIn   button(D0);
DigitalIn   emgstop(SW2);
DigitalOut  led_R(LED_RED);
DigitalOut  led_B(LED_BLUE);
DigitalOut  led_G(LED_GREEN);

// tickers and timers
Ticker loop_ticker;
Timer state_timer;
Timer emg_timer;

// Timeouts and related variables
Timeout make_button_active;
bool button_suppressed = false;
bool gripperopen = true;

// States
enum States {failure, waiting, calib_emg, calib_enc, operational, demo, homing}; //All possible robot states
enum Emg_measures_states {not_in_calib_emg, calib_right_bicep, calib_right_tricep, calib_left_bicep, calib_left_tricep}; // States inside
enum calib_enc_states {not_in_calib_enc, calib_enc_q1, calib_enc_q2};

//Global variables/objects
States current_state;
Emg_measures_states current_emg_calibration_state = not_in_calib_emg;
calib_enc_states calib_enc_state = not_in_calib_enc;

double des_vx, des_vy, x, y, q1, q2, e1, e2, x_norm, y_norm; //will be set by the motor_controller function
double u1, u2;
double vxmax = 1.0, vymax = 1.0;
double right_bicep_max = 0.0, right_tricep_max = 0.0, left_bicep_max= 0.0, left_tricep_max = 0.0;
double scaling_right_bicep = 1.0, scaling_right_tricep = 1.0, scaling_left_bicep = 1.0, scaling_left_tricep = 1.0;

// Variables for emg
double raw_emg_0, process_emg_0;
double raw_emg_1, process_emg_1;
double raw_emg_2, process_emg_2;
double raw_emg_3, process_emg_3;


// Variables for calibration
double calib_q1 = 3.1415926535f;
double calib_q2 = double(7)/double(6) * 3.1415926535f;
double off_set_q1 = 0.5f*3.1415926535f; // This variable is used to determine the off set from our definition from q1 and q2.
double off_set_q2 = 3.1415926535f;

// Variables defined for the homing state
double q1_homing = 0.5f*3.1415926535f, q2_homing = 3.1415926535f;
double qref1 = 0.5f*3.1415926535f, qref2 = 3.1415926535f;
double beta = 0.05;
double k_hom = 0.05;

// For the state calib_enc
double q1old;
double q2old;

vector<double> currentconfig;
vector<double> newconfig;
vector<double> ref;
vector<double> testconfig;

double currentx;
double currenty;

double L1 = 0.4388;
double L2 = 0.4508;

// Meaning of process_emg_0 and such
// - process_emg_0 is right biceps
// - process_emg_1 is right triceps
// - process_emg_2 is left biceps
// - process_emg_3 is left triceps

int counts_per_rotation = 32;
bool state_changed = false;
const double T = 0.001;

bool gripperclosed = false;

// Resolution of the encoder at the output axle
double resolution = (2.0f*3.1415926535f/double(counts_per_rotation))*(1.0/131.0); // In radians

// Functions

vector<double> forwardkinematics_function(double& q1, double& q2) {
    // input are joint angles, output are x and y position of end effector
    
    //previously +x01 and +y01
    currentx = L1*cos(q1)-L2*cos(q2);
    currenty = L1 * sin(q1) - L2 * sin(q2); 
    
    vector<double> xy;
    xy.push_back(currentx);
    xy.push_back(currenty);
    return xy;
   
}
 
vector<double> inversekinematics_function(const double& T, double ref1, double ref2, double& q1, double& q2, double& des_vx, double& des_vy) {
    // x, y: positions of end effector | T: time to hold current velocity | qref1, qref2: reference thetas | q1, q2: current thetas | vx, vy: desired x, y velocities
    
    // pseudo inverse jacobian to get joint speeds
    // input are desired vx and vy of end effector, output joint angle speeds
 
    double q1_star_des; // desired joint velocity of q1_star
    double q2_star_des; // same as above but then for q2_star
    double C;
 
    // The calculation below assumes that the end effector position is calculated before this function is executed
    // In our case the determinant will not equal zero, hence no problems with singularies I think.
    
    C = L1*L2*sin(ref1)*cos(ref2) - L1*L2*sin(ref2)*cos(ref1);
    q1_star_des = double(1/C)*(-L2*cos(ref2)*des_vx - L2*sin(ref2)*des_vy);
    q2_star_des = double(1/C)*((L2*cos(ref2)-L1*cos(ref1))*des_vx + (L2*sin(ref2)-L1*sin(ref1))*des_vy);
    
    qref1 = T*q1_star_des;
    qref2 = T*(q2_star_des+q1_star_des);
    
    double testq1 = ref1+qref1;
    double testq2 = ref2+qref2;
    newconfig = forwardkinematics_function(testq1, testq2);
    
    if (sqrt(pow(newconfig[0],2)+pow(newconfig[1],2)) > 1)
    {
        qref1 = ref1;
        qref2 = ref2;
    }
    else
    {
        qref1 = ref1 + qref1;
        qref2 = ref2 + qref2;
    }

    vector<double> thetas;
    thetas.push_back(qref1);
    thetas.push_back(qref2);
    return thetas;
}

void measure_all() 
{
    q1 = (motor_1_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation)*(1.0/131.0) + off_set_q1; //do this here, and not in the encoder interrupt, to reduce computational load
    q2 = (motor_2_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation)*(1.0/131.0) + off_set_q2;
    currentconfig = forwardkinematics_function(q1,q2); //previously x,y also input 
    raw_emg_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
    raw_emg_1 = emg1.read();
    raw_emg_2 = emg2.read();
    raw_emg_3 = emg3.read();
    processing_chain_emg(raw_emg_0, raw_emg_1, raw_emg_2, raw_emg_3, process_emg_0, process_emg_1, process_emg_2, process_emg_3);  // processes the emg signals
}

void output_all() {
    motor1_pwm = 0.4+0.6*fabs(u1);
    
    if (motor1_pwm > 1.0f){
        motor1_pwm = 1.0f;
    }
    motor1_dir = u1 > 0.0f;
    motor2_pwm = 0.4+0.6*fabs(u2);
    if (motor2_pwm > 1.0f){
        motor2_pwm = 1.0f;
    }
    motor2_dir = u2 > 0.0f;
    static int output_counter = 0;
    output_counter++;
}

void unsuppressing_button(){
    button_suppressed = false;    
}

void state_machine() {
    switch(current_state) { //States can be: failure, wait, calib_enc, calib_emg, operational, demo,
        case waiting:  //Nothing useful here, maybe a blinking LED for fun and communication to the user
            if (button.read()==false) 
            {
                current_state = calib_emg; //the NEXT loop we will be in calib_enc state
                current_emg_calibration_state = calib_right_bicep;
                calib_enc_state = calib_enc_q2;
                state_changed = true;
            }
            
            break; //to avoid falling through to the next state, although this can sometimes be very useful.
        
        case calib_enc:
        
            if (state_changed==true) 
            {
                state_timer.reset();
                state_timer.start();
                state_changed = false;
                led_G = 0;
                led_B = 1;
                led_R = 1;
                //u1 = 0.2;
                //u2 = -0.65f;
                q1old = 0; 
                q2old = 0;
            }
            
            switch(calib_enc_state){
                case calib_enc_q2:
                    if (q2 - q2old == 0.0 && state_timer.read() > 5.0f)
                    {
                        calib_enc_state = calib_enc_q1;
                        off_set_q2 = calib_q2 - q2;
                        //u2 = -0.4;
                        //u1 = -0.75;
                        state_timer.reset();
                        state_timer.start();
                    }    
                    q2old = q2;
                    break;
                case calib_enc_q1:
                    if (q1 - q1old == 0.0 && state_timer.read() > 5.0f)
                    {
                        calib_enc_state = not_in_calib_enc;
                        current_emg_calibration_state = calib_right_bicep;
                        current_state = calib_emg;
                        state_changed = true;
                        off_set_q1 = calib_q1 - q1;
                        //u1 = 0;
                        state_timer.reset();
                        state_timer.start();
                    }    
                    q1old = q1;
                    break;
                default:
                    current_state = failure;
            }
            break;
            
        case calib_emg:     //calibrate emg-signals
            if (state_changed == true){
                state_timer.reset();
                state_timer.start();
                emg_timer.reset();
                emg_timer.start();
                state_changed = false;
                }
            // first the led will be purple, during this time the right biceps should be maximally contracted
            // after five seconds and when the right biceps are relaxed, it switches to a blue led
            // when the blue led is active the right triceps should be maximally contracted
            // then again it switches to purple for left biceps and after that blue for left triceps
            // after this, the emg signals are calibrated
            switch(current_emg_calibration_state){
                case calib_right_bicep:
                    led_R = 0;
                    led_G = 1;
                    led_B = 0;
                    if(emg_timer < 5.0f){
                        if (process_emg_0 > right_bicep_max){
                            right_bicep_max = process_emg_0;
                            }
                        }
                    else if ((process_emg_0 < 0.1*right_bicep_max) || (emgstop.read() == false)){
                            scaling_right_bicep = 1.0/ right_bicep_max;
                            current_emg_calibration_state = calib_right_tricep;
                            emg_timer.reset();
                            emg_timer.start();
                        }
                    break;
                case calib_right_tricep:
                    led_R = 1;
                    led_G = 1;
                    led_B = 0;
                    if(emg_timer < 5.0f){
                            if (process_emg_1 > right_tricep_max){
                                right_tricep_max = process_emg_1;
                                }
                            }
                    else if ((process_emg_1 < 0.1*right_tricep_max) || (emgstop.read() == false)){
                            scaling_right_tricep = 1.0/ right_tricep_max;
                            current_emg_calibration_state = calib_left_bicep;
                            emg_timer.reset();
                            emg_timer.start();
                        }
                    break;
                case calib_left_bicep:
                    led_R = 0;
                    led_G = 1;
                    led_B = 0;
                    if(emg_timer < 5.0f){
                        if (process_emg_2 > left_bicep_max){
                            left_bicep_max = process_emg_2;
                            }
                        }
                    else if ((process_emg_2 < 0.1*left_bicep_max) || (emgstop.read() == false)){
                            scaling_left_bicep = 1.0/ left_bicep_max;
                            current_emg_calibration_state = calib_left_tricep;
                            emg_timer.reset();
                            emg_timer.start();
                        }
                    break;
                case calib_left_tricep:
                    led_R = 1;
                    led_G = 1;
                    led_B = 0;
                    if(emg_timer < 5.0f){
                       if (process_emg_3 > left_tricep_max){
                           left_tricep_max = process_emg_3;
                       }
                    }
                    else if ((process_emg_3 < 0.1*left_tricep_max) || (emgstop.read() == false)){
                      scaling_left_tricep = 1.0/ left_tricep_max;
                      current_emg_calibration_state = not_in_calib_emg;
                      current_state = operational;
                      state_changed = true;
                      emg_timer.reset();
                      led_R = 1;
                      led_G = 1;
                      led_B = 1;
                    }
                    break;
                default:
                    current_state = failure;
                    state_changed = true;
            }
            
            break;
                    
        case homing: 
            if (state_changed == true){
                    state_timer.reset();
                    state_timer.start();
                    qref1 = q1; //NOT SURE IF WE NEED THIS. I do not think so, but just to be sure.
                    qref2 = q2;
                }
            des_vx = min(beta, k_hom*(q1 - q1_homing)); // Little bit different then that Arvid told us, but now it works with the motor controller
            des_vy = min(beta, k_hom*(q2 - q2_homing));
                
            // The value of 3.0 and 2*resolution can be changed
            if (fabs(q1-q1_homing) <= 2*resolution && fabs(q2-q2_homing) <= 2 * resolution ){
                    if (state_timer > 3.0f){
                        current_state = operational;
                        state_changed = true;
                        des_vx = 0; // Not sure if needed but added it anyways.
                        des_vy = 0;
                    }
                }
            else{
                state_timer.reset();    
            }
            break;
        
        case operational:       //interpreting emg-signals to move the end effector
            if (state_changed == true){
                    state_changed = false;
            }
            
            // normalization 
            x_norm = process_emg_0 * scaling_right_bicep - process_emg_1 * scaling_right_tricep;
            y_norm = process_emg_2 * scaling_left_bicep - process_emg_3 * scaling_left_tricep;
               
             //x_norm = -1+2*vx_adjustment.read();
             //y_norm = -1+2*vy_adjustment.read();  
                            
            // here we have to determine the desired velocity based on the processed emg signals and calibration
            // x velocity
            if (x_norm >= 0.8) { des_vx = 0.2; }
            else if(x_norm >= 0.6) { des_vx = 0.15; }
            else if(x_norm >= 0.4) { des_vx = 0.1; }
            else if(x_norm >= 0.2) { des_vx = 0.05; } 
            else if(x_norm <= -0.8) { des_vx = -0.2; }
            else if(x_norm <= -0.6) { des_vx = -0.15; }
            else if(x_norm <= -0.4) { des_vx = -0.1; }
            else if(x_norm <= -0.2) { des_vx = -0.05; }
            else { des_vx = 0; }
            
            // y velocity
            if (y_norm >= 0.8) { des_vy = 0.2; }
            else if(y_norm >= 0.6) { des_vy = 0.15; }
            else if(y_norm >= 0.4) { des_vy = 0.1; }
            else if(y_norm >= 0.2) { des_vy = 0.05; } 
            else if(y_norm <= -0.8) { des_vy = -0.2; }
            else if(y_norm <= -0.6) { des_vy = -0.15; }
            else if(y_norm <= -0.4) { des_vy = -0.1; }
            else if(y_norm <= -0.2) { des_vy = -0.05; }
            else { des_vy = 0; }
    
            if (button.read() == false && button_suppressed == false ) {
                current_state = demo; 
                state_changed = true;
                button_suppressed = true;
                
                make_button_active.attach(&unsuppressing_button,0.5);

            } 
            
            if (buttongripper.read() == false && button_suppressed == false ) {
                button_suppressed = true;
                if (gripperopen){gripper = 0; gripperopen = false;}
                else {gripper = 1; gripperopen = true;}

                make_button_active.attach(&unsuppressing_button,0.5);
            }
            
            break;
            
        case demo: //moving according to a specified trajectory
            // We want to draw a square. Hence, first move to a certain point and then start moving a square.
            if (state_changed == true){
                    state_changed = false;
                    state_timer.reset();
                    state_timer.start();
                    des_vx = 0.1;
                    des_vy = 0;
            }
            double old_vx;
            double old_vy;        
            if (state_timer > 2.0f){
                old_vx = des_vx;
                old_vy = des_vy;
                
                des_vy = -old_vx;
                des_vx = old_vy;
                
                state_timer.reset();  
                state_timer.start(); 
            }
            
            
            //        des_vx = 0.1;           // first we move to the right
            //        des_vy = 0.0;
            
            //static double new_vx, new_vy;
                        
            //if(state_timer > 3.0f){         // after waiting an extra second we start on another side of the square
            //    state_timer.reset();
            //    state_timer.start();
            //    des_vx = new_vx;
            //    des_vy = new_vy;
            //}
            //else if(state_timer > 2.0f){                          // we have a velocity of 0.1 m/s for 2 seconds, so we make a square of 20 by 20 cm
            //    if(des_vx > 0){new_vx = 0.0; new_vy = 0.1;}       // here we check which side of the square we were on
            //    else if(des_vy > 0){new_vx = -0.1; new_vy = 0.0;}
            //    else if(des_vx < 0){new_vx = 0.0; new_vy = -0.1;}
            //    else if(des_vy < 0){new_vx = 0.1; new_vy = 0.0;}
            //    des_vx = 0;
            //    des_vy = 0;
            //}
       
                
            if (button.read() == false && button_suppressed == false ) {
                current_state = operational;
                state_changed = true;
                
                button_suppressed = true;
                
                make_button_active.attach(&unsuppressing_button,0.5);
            }
            
            break;
        
        case failure: //no way to get out
            u1 = 0.0f;
            u2 = 0.0f;
            led_R = 0;
            led_G = 1;
            led_B = 1;
            break;
    }
}

void motor_controller() 
{
    if (current_state >= operational) { // we can (ab)use the fact that an enum is actually an integer, so math/logic rules still apply
        ref = inversekinematics_function(T,qref1,qref2,q1,q2,des_vx,des_vy); //many different states can modify your reference position, so just do the inverse kinematics once, here
        qref1 = ref[0];
        qref2 = ref[1];
        e1 = qref1 - q1; //tracking error (q_ref - q_meas)
        e2 = qref2 - q2;
        PID_controller(e1*(180/3.14),e2*(180/3.14),u1,u2,T); //feedback controller or with possibly fancy controller additions...; pass by reference
        } //otherwise we just don’t mess with the value of control variable ‘u’ that is set somewhere in the state-machine.
}


void loop_function() {
    measure_all();              //measure all signals
    state_machine();            //Do relevant state dependent things
    motor_controller();         //Do not put different motor controllers in the states, because every state can re-use the same motor-controller!
    output_all();               //Output relevant signals, messages, screen outputs, LEDs etc.
}


int main() 
{
    pc.baud(115200);
    motor1_pwm.period_us(60);
    motor2_pwm.period_us(60);
    current_state = waiting; //we start in state ‘waiting’ and current_state can be accessed by all functions
    u1 = 0.0f; //initial output to motors is 0.
    u2 = 0.0f;
    bqc0.add(&bq0high).add(&bq0notch).add(&bq0notch2).add(&bq0notch3).add(&bq0notch4);        // filter cascade for emg 0
    bqc1.add(&bq1high).add(&bq1notch).add(&bq1notch2).add(&bq1notch3).add(&bq1notch4);        // filter cascade for emg 1
    bqc2.add(&bq2high).add(&bq2notch).add(&bq2notch2).add(&bq2notch3).add(&bq2notch4);        // filter cascade for emg 2
    bqc3.add(&bq3high).add(&bq3notch).add(&bq3notch2).add(&bq3notch3).add(&bq3notch4);        // filter cascade for emg 3
    loop_ticker.attach(&loop_function, T); //Run the function loop_function 1000 times per second
    led_R = 1;
    led_B = 1;
    led_G = 1;
    
    gripper = 1;
    int m = 0;
    while (true) {
    m++;
    if (m%1000)
    {   pc.printf("error1: %f, error2: %f, pwm1: %f, pwm2: %f, qref1: %f, qref2: %f, vx: %f, vy: %f, q1: %f, q2: %f, gripper: \r\n",  e1, e2, u1, u2, qref1, qref2, des_vx, des_vy, q1, q2);}
    //testconfig = forwardkinematics_function(qref1,qref2);
    //double x = testconfig[0];
    //double y = testconfig[1];
    //pc.printf("x: %f, y: %f \r\n", x,y);
    }  //Do nothing here (timing purposes)
}