File content as of revision 12:0765ea2c4c85:

#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"
//#include "HIDScope.h"

//This code is for Mbed 2
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
MODSERIAL pc(USBTX, USBRX);         //Establish connection
Ticker MyControllerTicker;          //Declare Ticker for Motors
Ticker MyTickerServo;               //Declare Ticker Servo control
Ticker MyTickerMean;                //Declare Ticker Signalprocessing

InterruptIn But1(PTA4);             //Declare button for min calibration
InterruptIn Mode(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
SignalNumber SignalMode;
Movement MoveLeft;                  //Declare Movement determiner
Movement MoveRight;

AnalogIn    emg0( A0 );             //Set Inputpin for EMG 0 signal Left 
AnalogIn    emg2( A2 );             //Set Inputpin for EMG 2 signal Right
AnalogIn    emg4( A4 );             //Set Inputpin for EMG 4 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  Led_calibration(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 = 500;                  //Window size for the mean value, also adjust in signalnumber.cpp
const int action =100;               //Number of same mean values to change the signalnumber
const int m = 500;                  //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=1; Led_green=1; Led_blue=1; Led_calibration=0;
}

// One calibration
void maxcalibration(){
    pc.printf("start max calibration \r\n");
    emg_offsetmaxLeft = SignalLeft.calibrate(m, FilterLeft(emg0));
    emg_offsetmaxRight = SignalRight.calibrate(m, FilterRight(emg2));
    emg_offsetmaxMode = SignalMode.calibrate(m, FilterMode(emg4));
    kLeft = 1/emg_offsetmaxLeft;
    kRight = 1/emg_offsetmaxRight;
    kMode = 1/emg_offsetmaxMode;
    Led_calibration = 1;   //Set led to Green
}

//------------------------------------------------------------------------------
//---------------------------------Servo----------------------------------------
//------------------------------------------------------------------------------
void up(){
    Up = 1;
    Up = 0;
    Up = 1;
}
void down(){
    Down = 1;
    Down = 0;
    Down = 1;
}

int mode =0;
void servo(){
     if(mode==3||mode==6){   
        Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);//*kLeft
        CaseLeft = SignalLeft.getnumberLeft(n, action, Signal_filteredLeft);
        Signal_filteredRight = fabs(FilterRight(emg2)*kRight);//*kRight
        CaseRight = SignalRight.getnumberRight(n, action, Signal_filteredRight);
        if (CaseLeft == 2){
            up();
        }
        else if (CaseRight == 2){
            down();
        }
    }
}


//------------------------------------------------------------------------------
//---------------------------Mode selection-------------------------------------
//------------------------------------------------------------------------------

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

// 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)*kLeft);//*kLeft
//    mean_value_left = SignalLeft.getmeanLeft(n, Signal_filteredLeft);  
    mean_value_left = SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read())*kLeft));        
    CaseLeft = SignalLeft.getnumberLeft(n, action, Signal_filteredLeft);
    //Right
    Signal_filteredRight = fabs(FilterRight(emg2)*kRight);//*kRight
//    mean_value_right = SignalRight.getmeanRight(n, Signal_filteredRight);
    mean_value_right = SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())*kRight)); 
    CaseRight = SignalRight.getnumberRight(n, action, Signal_filteredRight);
    //Mode
    /*
    Signal_filteredMode = fabs(FilterMode(emg4));//*kMode
    mean_value_mode = SignalMode.getmeanMode(n, Signal_filteredMode);
    CaseMode = SignalMode.getnumberMode(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 = 650.0, max_rangey = 450.0; //Max range on the y axis
const float x_offset = 156.15, y_offset = -76.97;   //Starting positions from the shoulder joint
const float alpha_offset = -0.4114;
const float beta_offset  = 0.0486;
const float L1 = 450.0, L2 = 490.0;                 //Length of the bodies

//------------------------------------------------------------------------------
//--------------------------------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_l =0, position_math_r=0;

void motor1_control(){
    float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, position_math_l, position_math_r);
    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;
            
    //Determine Motor Magnitude
    if (fabs(magnitude1)>1) {
        motor1 = 1;
    }
    else {
        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_l, position_math_r);
    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;
        
    //Determine Motor Magnitude
    if (fabs(magnitude2)>1) {
        motor2 = 1;
    }
    else {
        motor2 = fabs(magnitude2);
    }
    
    
    //Determine Motor Direction
    if (magnitude2 > 0){
        motor2DirectionPin = 1;
    }
    else{
        motor2DirectionPin = 0;
    }
}

//------------------------------------------------------------------------------
//----------------------------Motor controller----------------------------------
//------------------------------------------------------------------------------
int direction_l, direction_r;
float magnitude1, magnitude2;

void motor_control(){
    direction_l = MoveLeft.getdirectionLeft(mode);                              //x-direction
    direction_r = MoveRight.getdirectionRight(mode);                            //y-direction
    position_math_l= MoveLeft.getpositionLeft(CaseLeft, mode, max_rangex);      //x-direction
    position_math_r= MoveRight.getpositionRight(CaseRight, mode, max_rangey);   //y-direction
    motor1_control(); //magnitude1 = 
    motor2_control(); // magnitude2 = 
  
}
//------------------------------------------------------------------------------
//-------------------------------HID Scope--------------------------------------
//------------------------------------------------------------------------------

//HIDScope scope(4);
//Ticker scopeTimer;
Ticker FilterTimer;

void FilterUpdate(){
    FilterLeft(emg0.read());
    FilterRight(emg2.read());
    SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read())));
    SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())));    
}

//Working
//SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read())))
//SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())))

//------------------------------------------------------------------------------
//------------------------------Main--------------------------------------------
//------------------------------------------------------------------------------

int main(){
    pc.baud(115200);
    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);
    But1.rise(&maxcalibration);
    Mode.rise(&mode_selection);
    motor1.period(0.0001f); motor2.period(0.0001f);
    MyControllerTicker.attach(&motor_control, CONTROLLER_TS); //Determining motorposiitons
    MyTickerMean.attach(&signalnumber, MEAN_TS);  //Detemining the signalnumbers
    MyTickerServo.attach(&servo, 0.1f);

    //Scope
    //scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
    FilterTimer.attach_us(&FilterUpdate, 1e3);
    
    while(1) {
        //pc.printf(" direction  %i , %i  Signal numbers  %i  %i Position %f  %f  \r\n", direction_l, direction_r, CaseLeft, CaseRight, position_math_l, position_math_r); 
        pc.printf("direction  %i , %i  Signal numbers  %i  %i Position %f  %f  magnitude1 =%f   magnitude2=%f \r\n",direction_l, direction_r, CaseLeft, CaseRight, position_math_l, position_math_r, magnitude1, magnitude2);
        wait(0.1f);
    }
}