File content as of revision 7:105e3ae0fb19:

#include "MODSERIAL.h"
#include "AnglePosition.h"
#include "PIDControl.h"
#include "BiQuad.h"
#include "signalnumber.h"
#include "Movement.h"
#include "mbed.h"
#include "encoder.h"
#include "Servo.h"
#include "FastPWM.h"

//This code is for Mbed 2
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
MODSERIAL pc(USBTX, USBRX);         //Establish connection
Ticker MyControllerTicker;          //Declare Ticker Motor 1
Ticker MyTickerMode;                //Declare Ticker Motor 2
Ticker MyTickerMean;                //Declare Ticker Signalprocessing

InterruptIn But2(PTA4);             //Declare button for min calibration
InterruptIn But1(PTC6);             //Declare button for max calibration
InterruptIn Mode(D7);

AnglePosition Angle;                //Declare Angle calculater
PIDControl PID;                     //Declare PID Controller
SignalNumber SignalLeft;            //Declare Signal determiner for Left arm
SignalNumber SignalRight;           //Declare Signal determiner for Right arm
SignalNumber SignalMode;
Movement MoveLeft;                  //Declare Movement determiner
Movement MoveRight;

AnalogIn    emg0( A0 );             //Set Inputpin for EMG 0 signal Left 
//AnalogIn    emg1( A1 );             //Set Inputpin for EMG 1 signal Left
AnalogIn    emg2( A2 );             //Set Inputpin for EMG 2 signal Right
//AnalogIn    emg3( A3 );             //Set Inputpin for EMG 3 signal Right        
AnalogIn    emg4( A4 );             //Set Inputpin for EMG 4 signal Mode
//AnalogIn    emg5( A5 );             //Set Inputpin for EMG 5 signal Mode
DigitalOut     Up( D9 );             //Set digital in for mode selection
DigitalOut   Down( D8 );
DigitalOut  Led_red(LED_RED);          
DigitalOut  Led_green(LED_GREEN);
DigitalOut  Led_blue(LED_BLUE);  
DigitalOut  test(D2);

const float CONTROLLER_TS = 0.02;   //Motor frequency
const float MEAN_TS = 0.002;        //Filter frequency


//------------------------------------------------------------------------------
//-----------------------------EMG Signals--------------------------------------
//------------------------------------------------------------------------------
// Filtering the signal and getting the usefull information out of it.
const int n = 400;                  //Window size for the mean value, also adjust in signalnumber.cpp
const int action =50;               //Number of same mean values to change the signalnumber
const int m = 400;                  //Number of samples for calibration
int CaseLeft, CaseRight, CaseMode;           //Strength of the muscles 
float emg_offsetLeft, emg_offsetmaxLeft;     //Calibration offsets from zero to one for the left arm
float emg_offsetRight, emg_offsetmaxRight;   //Calibration offsets from zero to one for the right arm
float emg_offsetMode, emg_offsetmaxMode;
float mean_value_left, mean_value_right, mean_value_mode;   //Mean value of the filtered system
float kLeft, kRight, kMode;            //Scaling factors
float Signal_filteredLeft, Signal_filteredRight, Signal_filteredMode;

//BiQuad filter variables
BiQuad LP1L( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
BiQuad HP2L( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
BiQuad NO3L( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923,  0.41279762014290533); //Notch filter Biquad 
BiQuad LP1R( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
BiQuad HP2R( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
BiQuad NO3R( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923,  0.41279762014290533); //Notch filter Biquad 
BiQuad LP1M( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
BiQuad HP2M( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
BiQuad NO3M( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923,  0.41279762014290533); //Notch filter Biquad 
BiQuadChain BiQuad_filter_Left;
BiQuadChain BiQuad_filter_Right;
BiQuadChain BiQuad_filter_Mode;

float FilterLeft(float input){
    float Signal_filtered= BiQuad_filter_Left.step(input);
    return Signal_filtered;
}
float FilterRight(float input){
    float Signal_filtered= BiQuad_filter_Right.step(input);
    return Signal_filtered;
}
float FilterMode(float input){
    float Signal_filtered= BiQuad_filter_Mode.step(input);
    return Signal_filtered;
}

//Calibration-------------------------------------------------------------------
void setled(){
    Led_red=0; Led_green=1; Led_blue=1;
}
// Zero calibration
void mincalibration(){
    pc.printf("start min calibration \r\n");
    emg_offsetLeft = SignalLeft.calibrate(m,FilterLeft(emg0));
    emg_offsetRight = SignalRight.calibrate(m,FilterRight(emg2));
    emg_offsetMode = SignalMode.calibrate(m, FilterMode(emg4));
    pc.printf("offsets: %f %f \r\n", emg_offsetLeft, emg_offsetRight);
    Led_green=0; Led_red=0; //Set led to Yellow
}
// One calibration
void maxcalibration(){
    pc.printf("start max calibration \r\n");
    emg_offsetmaxLeft = SignalLeft.calibrate(m, FilterLeft(emg0))-emg_offsetLeft;
    emg_offsetmaxRight = SignalRight.calibrate(m, FilterRight(emg2))-emg_offsetRight;
    emg_offsetmaxMode = SignalMode.calibrate(m, FilterMode(emg4))-emg_offsetMode;
    kLeft = 1/emg_offsetmaxLeft;
    kRight = 1/emg_offsetmaxRight;
    kMode = 1/emg_offsetmaxMode;
    pc.printf("offsets: %f %f, scale %f %f \r\n", emg_offsetmaxLeft, emg_offsetmaxRight, kLeft, kRight);
    Led_red=1;   //Set led to Green
}

//Filtering the signals---------------------------------------------------------
//Filter de EMG signals with a BiQuad filter
/*
float FilterLeft(float input, float offset){
    float Signal=(input-offset);                    //((input0+input1)/2)
    float Signal_filtered= BiQuad_filter_Left.step(Signal);
    return Signal_filtered;
}
float FilterRight(float input, float offset){
    float Signal=input-offset;                    //((input0+input1)/2)
    float Signal_filtered= BiQuad_filter_Right.step(Signal);
    return Signal_filtered;
}
float FilterMode(float input, float offset){
    float Signal=input-offset;                    //((input0+input1)/2)
    float Signal_filtered= BiQuad_filter_Mode.step(Signal);
    return Signal_filtered;
}
*/

//------------------------------------------------------------------------------
//---------------------------------Servo----------------------------------------
//------------------------------------------------------------------------------
void servo(){
    Signal_filteredLeft = fabs(FilterLeft(emg0)-emg_offsetLeft);//*kLeft
    Signal_filteredRight = fabs(FilterRight(emg2)); //*kRight
    CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft);
    CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight);
    if (CaseLeft>=3){
        Up = 0; 
        Up = 1;
    }
    else if (CaseRight >=3){
        Down = 0;
        Down = 1;
    }
}
int milli =0;

//------------------------------------------------------------------------------
//---------------------------Mode selection-------------------------------------
//------------------------------------------------------------------------------
int mode =0;

//Recieving mode selection from EMG mode signal
void mode_selection(){
    if(mode ==6){
        mode=1;
    }
    else{
        mode++;
    }
    if (mode==3||mode==6){
        servo();
    }
    pc.printf("\r\n mode = %i \r\n", mode); 
}

// Control mode selection-------------------------------------------------------







//Determine the signalnumbers (i.e. speed) based on the strenght of the EMG signals
void signalnumber(){
    //Left
    Signal_filteredLeft = fabs(FilterLeft(emg0)-emg_offsetLeft);//*kLeft
    mean_value_left = SignalLeft.getmean(n, Signal_filteredLeft);          
    CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft);
    //Right
    Signal_filteredRight = fabs(FilterRight(emg2)*kRight);
    mean_value_right = SignalRight.getmean(n, Signal_filteredRight); 
    CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight);
    //Mode
    Signal_filteredMode = fabs(FilterMode(emg4)*kMode);
    mean_value_mode = SignalMode.getmean(n, Signal_filteredMode);
    CaseMode = SignalMode.getnumber(n, action, Signal_filteredMode);
    /*if(CaseMode >= 3){
        milli ++;
        if(milli>=150){
            mode_selection();
            milli=0;
        }
    }
    else{
        milli=0;
    }*/
}








//------------------------------------------------------------------------------
//-------------------------Kinematic Constants----------------------------------
//------------------------------------------------------------------------------
const double RAD_PER_PULSE = 0.00074799825*2;       //Number of radials per pulse from the encoder
const double PI = 3.14159265358979323846;           //Pi
const float max_rangex = 500.0;                     //Max range on the x axis
const float max_rangey = 300.0;                     //Max range on the y axis
const float x_offset = 156.15;                      //Start x position from the shoulder joint
const float y_offset = -76.97;                      //Start y position from the shoulder joint
const float alpha_offset = -(21.52/180)*PI;         //Begin angle Alpha at zero zero
const float beta_offset  = 0.0;                     //Begin angle Beta at zero zero
const float L1 = 450.0;                             //Length of the first body
const float L2 = 490.0;                             //Length of the second body



//------------------------------------------------------------------------------
//--------------------------------Motor1----------------------------------------
//------------------------------------------------------------------------------
FastPWM motor1(D5);
DigitalOut motor1DirectionPin(D4);
DigitalIn ENC2A(D12);
DigitalIn ENC2B(D13);
Encoder encoder1(D13,D12);
const float MOTOR1_KP = 40.0;
const float MOTOR1_KI = 0.0;
const float MOTOR1_KD = 15.0;
double M1_v1 = 0.0, M1_v2 = 0.0; //Calculation values
const double motor1_gain = 2*PI;
const float M1_N = 0.5;
float position_math[2]={};

void motor1_control(){
    float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, position_math[0], position_math[1]);
    float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
    float error_alpha = reference_alpha-position_alpha;
    float magnitude1 = PID.get(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain;
    motor1 = fabs(magnitude1);
    // Determine Motor Direction 
    if (magnitude1  < 0){
        motor1DirectionPin = 1;
    }
    else{
        motor1DirectionPin = 0;
    }
}

//------------------------------------------------------------------------------
//--------------------------------Motor2----------------------------------------
//------------------------------------------------------------------------------
FastPWM motor2(D6);
DigitalOut motor2DirectionPin(D7);
DigitalIn ENC1A(D10);
DigitalIn ENC1B(D11);
Encoder encoder2(D10,D11);
const float MOTOR2_KP = 60.0;
const float MOTOR2_KI = 0.0;
const float MOTOR2_KD = 15.0;
double M2_v1 = 0.0, M2_v2 = 0.0; //Calculation values
const double motor2_gain = 2*PI;
const float M2_N = 0.5;

void motor2_control(){
    float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, position_math[0], position_math[1]);
    float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
    float error_beta = reference_beta-position_beta;
    float magnitude2 = PID.get(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain;
    motor2 = fabs(magnitude2);
    //Determine Motor Direction
    if (magnitude2 > 0){
        motor2DirectionPin = 1;
    }
    else{
        motor2DirectionPin = 0;
    }
}

void motor_control(){
    position_math[0]= MoveLeft.getposition(CaseLeft, mode, 0, max_rangex);
    position_math[1]= MoveRight.getposition(CaseRight, mode, 1, max_rangey);
    motor1_control();
    motor2_control();
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------

int main(){
    pc.baud(115200);
    test=0;
    setled(); 
    BiQuad_filter_Left.add( &LP1L ).add( &HP2L ).add( &NO3L);
    BiQuad_filter_Right.add( &LP1R ).add( &HP2R ).add( &NO3R);
    BiQuad_filter_Mode.add( &LP1M ).add( &HP2M ).add( &NO3M);
    But2.rise(&mincalibration);
    But1.rise(&maxcalibration);
    Mode.rise(&mode_selection);
    motor1.period(0.0001f); motor2.period(0.0001f);
    MyControllerTicker.attach(&motor_control, CONTROLLER_TS); 
    MyTickerMean.attach(&signalnumber, MEAN_TS);
//    MyTickerMode.attach(&signalmode, MEAN_TS);
//    MyTickerMean.attach(&signalnumberright, MEAN_TS);
//    MyTickerMean.attach(&signalmode,MEAN_TS);

    while(1) {
//        pc.printf("Mean %f %f %f \r\n", mean_value_left, mean_value_right, mean_value_mode);
//        pc.printf("Case %i %i %i, mode = %i \r\n", CaseLeft, CaseRight, CaseMode, mode);
        pc.printf("mean %f , case = %i \r\n", mean_value_left, CaseLeft);
        wait(0.1f);
    }
}