File content as of revision 42:ae78ff03d9d6:

#include <vector>
#include <numeric>
#include <algorithm>
#include "mbed.h"
#include "Matrix.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"
#include "FastPWM.h"
#include "refGen.h"
#include "controller.h"
#include "motorConfig.h"
#include "errorFetch.h"
#include "BiQuad.h"

// ADJUSTABLE PARAMETERS
// controller ticker time interval
const float Ts = 0.01;

// EMG filter parameters
// calibration time
const int calSamples = 1000;

// Initialize average and max EMG value for calibration to 0 and 1 respectively
volatile float avgEMGvalue = 0;
volatile double maxEMGvalue = 1;

// high pass
BiQuadChain HPbqc;
BiQuad HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770);
BiQuad HPbq2(1.0000, -2.0000, 1.0000, 1.0000, -1.9903, 0.9904);


// low pass
BiQuadChain LPbqc;
BiQuad LPbq1(1.0e-7*1.2023, 1.0e-7*2.4046, 1.0e-7*1.2023, 1.0000, -1.9313, 0.9327);
BiQuad LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.9702, 0.9716);

// Controller parameters
const float k_p = 1;
const float k_i = 0; // Still needs a reasonable value
const float k_d = 0; // Again, still need to pick a reasonable value

// Defining motor gear ratio - for BOTH motors as this is the same in the current configuration
const float gearRatio = 131;

// LOGISTICS
// Declaring finite-state-machine states
enum robotStates {KILLED, ACTIVE, CALIBRATING};
volatile robotStates currentState = KILLED;
volatile bool stateChanged = true;

// Declaring a controller ticker and volatile variables to store encoder counts and revs
Ticker controllerTicker;
volatile int m1counts = 0;
volatile int m2counts = 0;
volatile float m1revs = 0.00;
volatile float m2revs = 0.00;

// PWM settings
float pwmPeriod = 1.0/5000.0;
int frequency_pwm = 10000; //10kHz PWM

// Initializing encoder
QEI Encoder1(D12,D13,NC,64, QEI::X4_ENCODING);
QEI Encoder2(D11,D10,NC,64, QEI::X4_ENCODING);
MODSERIAL pc(USBTX, USBRX);
HIDScope scope(5);

// Defining inputs
InterruptIn sw2(SW2);
InterruptIn sw3(SW3);
InterruptIn button1(D2);
InterruptIn button2(D3);
//AnalogIn pot2(A2);
//AnalogIn emg0( A0 );
//AnalogIn emg1( A1 );

// Defining LED outputs to indicate robot state-us
DigitalOut ledG(LED_GREEN);
DigitalOut ledR(LED_RED);
DigitalOut ledB(LED_BLUE);

// Setting up HIDscope
volatile float x;
volatile float y;  
volatile float z; 
volatile float q;
volatile float k;
volatile float w;


void sendDataToPc(float data1, float data2, float data3, float data4, float data5, float data6){
        // Capture data
        x = data1;
        y = data2;
        z = data3;
        q = data4;
        k = data5;
        w = data6;
        scope.set(0, x);   
        scope.set(1, y); 
        scope.set(2, z);
        scope.set(3, q);
        scope.set(4, z);
        scope.set(5, w);
        scope.send(); // send what's in scope memory to PC
}


// REFERENCE PARAMETERS
int posRevRange = 1; // describes the ends of the position range in complete motor output shaft revolutions in both directions
const float maxAngle = 1*3.14*posRevRange; // max angle in radians


// Function getReferencePosition returns reference angle based on potmeter 1
refGen ref1(A2, maxAngle);
refGen ref2(A3, maxAngle);

// readEncoder reads counts and revs and logs results to serial window
errorFetch e1(gearRatio, Ts);
errorFetch e2(gearRatio, Ts);

// Generate a PID controller with the specified values of k_p, k_d and k_i
controller motorController1(k_p, k_d, k_i);
controller motorController2(k_p, k_d, k_i);

motorConfig motor1(D4,D5);
motorConfig motor2(D7,D6);

// PROBLEM: if I'm processing the state machine in the endless while loop, how can I adjust robot behavior in the ticker (as it'll keep running)? Do I need to also implement it there? If so, why bother with the while(1) in the main function in the first place?
void measureAndControl(){
    // Read encoders and EMG signal (unnfiltered reference)
    m1counts = Encoder1.getPulses();
    m2counts = Encoder2.getPulses();
    
    double m1position = e1.fetchMotorPosition(m1counts);
    double m2position = e2.fetchMotorPosition(m2counts);
    
    // measuring and normalizing EMG signals to use as basis for reference

    double emg1 = ref1.getReference();
    double emg2 = ref2.getReference();
    
    
    // Filtering the EMG signals   

    double emg1HP = HPbqc.step(emg1);
    double emg1HP_abs = fabs(emg1HP);
    double emg1HPLP_abs = LPbqc.step(emg1HP_abs);
//    thet1 = fabs(thet1);
//    thet1 = LPbqc.step(thet1);
    

    double emg2HP = HPbqc.step(emg2);
    double emg2HP_abs = fabs(emg2);
    double emg2HPLP_abs = LPbqc.step(emg2HP_abs);
//    thet2 = LPbqc.step(thet2);

    // Something worth trying: set a position setpoint that constantly changes but will never be reached until EMG value is 0 as it is computed from the robot's current position    
//    thet1 = m1position + thet1;
//    thet2 = m1position + thet2;
    
    // Other possibility: use thet1 and thet2 directly as reference angles. That'll require the user to hold muscle tension to stay in a certain position though
    
    
    // Finite state machine
    switch(currentState){
            case KILLED:
                {
                // Initialization of KILLED state: cut power to both motors
                if(stateChanged){
                    motor1.kill();
                    motor2.kill();
                    pc.printf("Killed state \r\n");
                    stateChanged = false;               
                    }

                // Send reference data to pc
                
                // Set LED to red
                ledR = 0;
                ledG = 1;
                ledB = 1;
                // need something here to check if "killswitch" has been pressed (?)
                // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn
//                e1.fetchError(m1position, thet1);
//                e2.fetchError(m2position, thet1);

                sendDataToPc(emg1, emg1HP_abs, emg1HPLP_abs, emg2, emg2HP, emg2HPLP_abs); // just send the EMG signal value to HIDscope
                
                break;
                }
            case ACTIVE:
                {
                if(stateChanged){
                    pc.printf("Active state \r\n");
                    }
                
                // Compute error
                e1.fetchError(m1position, emg1);
                e2.fetchError(m2position, emg2);
                
                // Compute motor value using controller and set motor
                float motorValue1 = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
                float motorValue2 = motorController2.control(e2.e_pos, e2.e_int, e2.e_der);
                motor1.setMotor(motorValue1);
                motor2.setMotor(motorValue2);
                
                // Send data to HIDscope
                sendDataToPc(emg1, emg2, e1.e_pos, e2.e_pos, motorValue1, motorValue2);
                
                // Set LED to blue
                ledR = 1;
                ledG = 1;
                ledB = 0;
                // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn
                break;
                }
            case CALIBRATING:
                {
                // NOTE: maybe wrap this whole calibrating thing in a library?
                
               // Do I need to kill motors here?

                
                // Initialization of CALIBRATE state
                static int Ncal = 0;
                std::vector<float> EMGsamples;
                
                if(stateChanged){                                        
                    // Kill motors
                    pc.printf("Calibrate state \r\n");
                    motor1.kill();
                    motor2.kill();
                    
                    // Clear sample value vector and reset counter variable
                    EMGsamples.clear();
                    Ncal = 0;
                    stateChanged = false;
                    }

                // fill the array with sample data if it is not yet filled
                if(Ncal < calSamples){
                    EMGsamples.push_back(emg1);
                    Ncal++;
                    }
                // if array is filled compute the mean value and switch to active state
                else {
                    // Set new avgEMGvalue
                    avgEMGvalue = std::accumulate(EMGsamples.begin(), EMGsamples.end(), 0)/calSamples; // still needs to return this value 
                    double maxEMGvalue = *std::max_element(EMGsamples.begin(), EMGsamples.end());
                    
                    
                    pc.printf("Avg emg value is %.2f changed state to active", avgEMGvalue);
                    // State transition logic
                    currentState = ACTIVE;
                    stateChanged = true;
                    Ncal = 0;
                    }
                
                // Set LED to green
                ledR = 1;
                ledG = 0;
                ledB = 1;
//                sendDataToPc(thet1, thet2, 0.0, 0.0, 0.0, 0.0);
                break;
                }
        }
    
    
    
    }

void r1SwitchDirection(){
    ref1.r_direction = !ref1.r_direction;
    pc.printf("Switched reference direction! \r\n");
    }
    
void r2SwitchDirection(){
    ref2.r_direction = !ref2.r_direction;
    pc.printf("Switched reference direction! \r\n");
    }

void killSwitch(){
    currentState = KILLED;
    stateChanged = true;
    }
    
void activateRobot(){
    currentState = ACTIVE;
    stateChanged = true;
    }
    
void calibrateRobot(){
    currentState = CALIBRATING;
    stateChanged = true;
    }

int main()
    {
    pc.baud(115200);
    pc.printf("Main function");
    HPbqc.add(&HPbq1).add(&HPbq2);
    LPbqc.add(&LPbq1).add(&LPbq2);
    
    // Attaching state change functions to buttons;
    sw2.fall(&killSwitch);
    sw3.fall(&activateRobot);
    button1.rise(&r1SwitchDirection);
    button2.rise(&r2SwitchDirection);
    
    controllerTicker.attach(measureAndControl, Ts);
    pc.printf("Encoder ticker attached and baudrate set");    
    }