File content as of revision 32:2596cc9156ec:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "HIDScope.h"
#include "BiQuad.h"
#include "PID_controller.h"
#include "kinematics.h"
#include "processing_chain_emg.h"

//Define objects
MODSERIAL   pc(USBTX, USBRX);
HIDScope    scope(2);

// emg inputs
AnalogIn    emg0( A0 );
AnalogIn    emg1( A1 );

// motor ouptuts
PwmOut      motor1_pwm(D5);
DigitalOut  motor1_dir(D4);
PwmOut      motor2_pwm(D7);
DigitalOut  motor2_dir(D6);

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

// other objects
AnalogIn    potmeter1(A2);
AnalogIn    potmeter2(A3);
DigitalIn   button(D0);
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;

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

//Global variables/objects
States current_state;
Emg_measures_states current_emg_calibration_state = not_in_calib_emg;

double des_vx, des_vy, x, y, q1, q2, qref1, qref2, e1, e2, u1, u2; //will be set by the motor_controller function
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;

// 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 defined for the homing state
double q1_homing = 0, q2_homing = 0;
double q1_dot_ref_homing, q2_dot_ref_homing;
double beta = 5;
double k_hom = 2;

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

// 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;


// 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
void measure_all() 
{

    q1 = motor_1_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation; //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;
    forwardkinematics_function(q1,q2,x,y);  //motor_angle is global, this function ne
    raw_emg_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
    raw_emg_1 = emg1.read();
    processing_chain_emg(raw_emg_0, raw_emg_1, process_emg_0, process_emg_1);  // processes the emg signals
}

void output_all() {
    motor1_pwm = fabs(u1);
    motor1_dir = u1 > 0.0f;
    motor2_pwm = fabs(u2);
    motor2_dir = u2 > 0.0f;
    static int output_counter = 0;
    output_counter++;
    if (output_counter == 100) {pc.printf("Something something... %f",u1); output_counter = 0;}  //Print to screen at 10 Hz with MODSERIAL
}

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()==true) 
            {
                current_state = calib_enc; //the NEXT loop we will be in calib_enc state
                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;
                n = 0;
                led_G = 0;
                led_B = 1;
                led_R = 1;
                u1 = 0.55f; //a low PWM value to move the motors slowly (0.0 to 0.45 don’t do much due to friction)
                u2 = 0.55f;
                q1old = 0; 
                q2old = 0;
            }
            
            if (q1-q1old == 0.0 && q2 - q2old < 0.0 && state_timer.read() > 5.0f)
            {
                current_state = calib_emg; //the NEXT loop we will be in calib_emg state
                current_emg_calibration_state = calib_right_bicep;
                state_changed = true;
                
            }    
            q1old = q1;
            q2old = q2;
            
            n++;
            if (n%1000 == 0)
            {
                led_G = !led_G;
            }
            
            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;
                }
            
            switch(current_emg_calibration_state){
                case calib_right_bicep:
                    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){
                            current_emg_calibration_state = calib_right_tricep;
                            emg_timer.reset();
                            emg_timer.start();
                        }
                    break;
                case calib_right_tricep:
                    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){
                            current_emg_calibration_state = calib_left_bicep;
                            emg_timer.reset();
                            emg_timer.start();
                        }
                    break;
                                case calib_left_bicep:
                    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){
                            current_emg_calibration_state = calib_left_tricep;
                            emg_timer.reset();
                            emg_timer.start();
                        }
                    break;
                case calib_left_tricep:
                        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){
                            current_emg_calibration_state = not_in_calib_emg;
                            current_state = homing;
                            state_changed = true;
                            emg_timer.reset();
                        }
                    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;
                }
            q1_dot_ref_homing = min(beta, k_hom*(q1 - q1_homing));
            q2_dot_ref_homing = min(beta, k_hom*(q2 - q2_homing));
            qref1 = q1_dot_ref_homing*T + q1;
            qref2 = q2_dot_ref_homing*T + q2;
                
            // 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;
                    }
                }
            else{
                state_timer.reset();    
            }
            break;
        
        case operational:       //interpreting emg-signals to move the end effector
            if (state_changed == true){
                    state_changed = false;
                }
            
                       
            // here we have to determine the desired velocity based on the processed emg signals and calibration
            if (process_emg_0 >= 0.16) { des_vx = vxmax; }
            else if(process_emg_0 >= 0.09) { des_vx = vxmax * 0.66; }
            else if(process_emg_0 >= 0.02) { des_vx = vxmax * 0.33; }
            else { des_vx = 0; }
            
            if (process_emg_1 >= 0.16) { des_vy = vymax; }
            else if(process_emg_1 >= 0.09) { des_vy = vymax * 0.66; }
            else if(process_emg_1 >= 0.02) { des_vy = vymax * 0.33; }
            else { des_vy = 0; }
            
            if (button.read() == true) {
                current_state = demo; 
                state_changed = true;
                } // Isn't this going to cause problems as it will switch on and of very quickly?
            
            
            break;
            
        case demo: //moving according to a specified trajectory
            if (state_changed == true){
                    state_changed = false;
                }
                
            if (button.read() == true) {
                current_state = operational;
                state_changed = true;
            }
            
            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
        inversekinematics_function(x,y,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
        e1 = qref1 - q1; //tracking error (q_ref - q_meas)
        e2 = qref2 - q2;
        PID_controller(e1,e2,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);        // filter cascade for emg
    bqc1.add(&bq1high).add(&bq1notch);        // filter cascade for emg
    loop_ticker.attach(&loop_function, T); //Run the function loop_function 1000 times per second
    led_R = 1;
    led_B = 1;
    led_G = 1;

    while (true) { }  //Do nothing here (timing purposes)
}