File content as of revision 10:7339dca7d604:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "HIDScope.h"
#include "BiQuad.h"
#include "PID_controller.h"
#include "kinematics.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);

AnalogIn    potmeter1(A2);
AnalogIn    potmeter2(A3);
DigitalIn   button(D0);

Ticker Sample;
Timer state_timer;

enum States {failure, waiting, calib_enc, calib_emg, operational, demo}; //All possible robot states

//Global variables/objects
States current_state;
Ticker loop_ticker; //The Ticker object that will ensure perfect timing of our looping code
float e, u1, u2, emg_signal_raw_0, processed_emg_0, emg_signal_raw_1, processed_emg_1, robot_end_point, reference_end_point, motor_angle, motor_counts, q_ref; //will be set by the motor_controller function
int counts_per_rotation = 32;
bool state_changed = false;
double samplingfreq = 1000;
double x; // Making the position (x,y) of the end effector global
double y;

float processing_chain_emg(int num) {
    return 6.0;
}

void measure_all() 
{
    motor_angle = motor_counts*2.0f*3.1415926535f/counts_per_rotation; //do this here, and not in the encoder interrupt, to reduce computational load
    robot_end_point = forwardkinematics_function(motor_angle);  //motor_angle is global, this function ne
    emg_signal_raw_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
    emg_signal_raw_1 = emg1.read();
    processed_emg_0 = processing_chain_emg(0);  // some function ‘float my_emg_processing_chain()’ that returns a float. The raw emg is global
    processed_emg_1 = processing_chain_emg(1);
}

void output_all() {
    motor1_pwm = fabs(u1);
    motor1_dir = u1 > 0.5f;
    motor2_pwm = fabs(u2);
    motor2_dir = u2 > 0.5f;
    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
            }
            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;
            }
            u1 = 0.6f; //a low PWM value to move the motors slowly (0.0 to 0.45 don’t do much due to friction)
            // fabs(motor1.velocity()) < 0.1f && 
            if (state_timer.read() > 5.0f) {
                current_state = calib_emg; //the NEXT loop we will be in calib_emg state
                state_changed = true;
            }
            break;
            
        case calib_emg:     //calibrate emg-signals
            
            break;
        
        case operational:       //interpreting emg-signals to move the end effector
            if (state_changed==true) { int x = 5; }
            // example
            reference_end_point = robot_end_point + processed_emg_0;
            if (button.read() == true) { current_state = demo; }
            
            break;
            
        case demo: //moving according to a specified trajectory
            
            if (button.read() == true) { current_state = demo; }
            
            break;
        
        case failure: //no way to get out
            u1 = 0.0f;
            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
        q_ref += inversekinematics_function(reference_end_point)/samplingfreq; //many different states can modify your reference position, so just do the inverse kinematics once, here
        e1 = q_ref - motor_angle; //tracking error (q_ref - q_meas)
        e2 = q_ref - motor_angle;
        PID_controller(e1,e2,u1,u2); //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;
    loop_ticker.attach(&loop_function, 1/samplingfreq); //Run the function loop_function 1000 times per second
    
    while (true) { }  //Do nothing here (timing purposes)
}