File content as of revision 4:e15fc329b88b:

#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 MyControllerTicker1;         //Declare Ticker Motor 1
Ticker MyControllerTicker2;         //Declare Ticker Motor 2
Ticker MySampleTicker;              //Declare Ticker HIDscope
Ticker MyTickerMean;                //Declare Ticker Signalprocessing

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

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
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     M( D9 );             //Set digital in for mode selection
DigitalOut  Led_red(LED_RED);          
DigitalOut  Led_green(LED_GREEN);
DigitalOut  Led_blue(LED_BLUE);  

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

//Testing methods
/*
AnalogIn potmeter1(A5);
AnalogIn potmeter5(A3);             //Set Inputpin for x axis
AnalogIn potmeter2(A4);             //Set Inputpin for y axis
*/

//------------------------------------------------------------------------------
//---------------------------Mode selection-------------------------------------
//------------------------------------------------------------------------------
// From the other Mbed there will be send a signal to determine in which mode the system is in
int mode =0;

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



//------------------------------------------------------------------------------
//-----------------------------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 = 300;                  //Number of samples for calibration
int CaseLeft;                           //Strength of the muscles Left
int CaseRight;                           //Strength of the muscles Right
float emg_offsetLeft;                   //Calibtarion value to get zero
float emg_offsetmaxLeft;                //Calibration value to scale to 1
float emg_offsetRight;                   //Calibtarion value to get zero
float emg_offsetmaxRight;                //Calibration value to scale to 1
float meanxL;                        //Temporary variable for mean value
float meanxR;
float kLeft;                      //Scaling factor mean value
float kRight;                      //Scaling factor mean value

//BiQuad filter variables
BiQuad LP1( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
BiQuad HP2( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
BiQuad NO3( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923,  0.41279762014290533); //Notch filter Biquad 
BiQuadChain BiQuad_filter;

void setled(){
    Led_red=0;
    Led_green=1;
    Led_blue=1;
}

// Calibration function
void mincalibration(){
    pc.printf("start cali \r\n");
    emg_offsetLeft = SignalLeft.calibrate(m,((emg0+emg1)/2));
    emg_offsetRight = SignalRight.calibrate(m,((emg2+emg3)/2));
//    pc.printf("calibrated, offset = %f \r\n", emg_offset);
    Led_green=0;
    Led_red=0;
}

void maxcalibration(){
    pc.printf("start cali max\r\n");
    emg_offsetmaxLeft = SignalLeft.calibrate(m,((emg0+emg1)/2))-emg_offsetLeft;
    emg_offsetmaxRight = SignalRight.calibrate(m,((emg2+emg3)/2))-emg_offsetRight;
    kLeft = 1/emg_offsetmaxLeft;
    kRight = 1/emg_offsetmaxRight;
//    pc.printf("calibrated, offset = %f scale = %f \r\n",emg_offsetmax, k);
    Led_red=1;
}

//Filter de EMG signals with a BiQuad filter
float Filter(float input0, float input1, float offset){
    float Signal=input0-offset; //((input0+input1)/2)
    float Signal_filtered= BiQuad_filter.step(Signal);
    return Signal_filtered;
}


//Determine the signalnumbers (i.e. speed) based on the EMG signals
void signalnumber(){
    //Left
    float Signal_filteredLeft = fabs(Filter(emg0, emg1, emg_offsetLeft));
    meanxL = SignalLeft.getmean(n, Signal_filteredLeft)*kLeft;          //Testing variable
    CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft, kLeft);
    pc.printf("m %f C %i \r\n",meanxL, CaseLeft);                       //Testing print
    //Right
    float Signal_filteredRight = fabs(Filter(emg2, emg3, emg_offsetRight));
    meanxR = SignalRight.getmean(n, Signal_filteredRight)*kRight;          //Testing variable
    CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight, kRight);
    pc.printf("m %f C %i \r\n",meanxR, CaseRight);                       //Testing print
}

//------------------------------------------------------------------------------
//-------------------------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;
double M1_v2 = 0.0;
const double motor1_gain = 2*PI;
const float M1_N = 0.5;


void motor1_control(){
    float *position_math;
    position_math[0]= MoveLeft.getposition(CaseLeft, mode, 0, max_rangex);
    position_math[1]= MoveRight.getposition(CaseRight, mode, 1, max_rangey);
    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_err_int = 0;
const double motor2_gain = 2*PI;
const float M2_N = 0.5;
double M2_v1 = 0.0;
double M2_v2 = 0.0;


void motor2_control(){
    float *position_math;
    position_math[0]= MoveLeft.getposition(CaseLeft, mode, 0, max_rangex);
    position_math[1]= MoveRight.getposition(CaseRight, mode, 1, max_rangey);
    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;
    }
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------

int main(){
    pc.baud(115200);
    setled(); 
    BiQuad_filter.add( &LP1 ).add( &HP2 ).add( &NO3);
    But2.rise(&mincalibration);
    But1.rise(&maxcalibration);
//    M.rise(&mode_selection);
    motor1.period(0.0001f);
    motor2.period(0.0001f);
    MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); 
    MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
    MyTickerMean.attach(&signalnumber, MEAN_TS);

    while(1) {}   
}