File content as of revision 15:a73b5abd0696:

#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 the main Mbed
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
Ticker MyTickerController;          //Declare Ticker for Motors
Ticker MyTickerServo;               //Declare Ticker Servo control
Ticker MyTickerMean;                //Declare Ticker Signalprocessing
Ticker MyTickerFilter;              //Declare Ticker Filtering

InterruptIn But1(PTA4);             //Declare button for emg calibration
InterruptIn Mode(PTC6);             //Declare button for mode selection

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 Left Movement determiner
Movement MoveRight;                 //Declare Right movement determiner

AnalogIn    emg0( A0 );             //Set Inputpin for EMG 0 signal Left 
AnalogIn    emg2( A2 );             //Set Inputpin for EMG 2 signal Right
DigitalOut     Up( D9 );            //Set digital out for raising the pen off the board
DigitalOut   Down( D8 );            //Set digital out for lowering the pen off the board

//Setting LEDs
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;        //Moving average frequency
const float FILTER_TS = 1e3;        //Filter frequency
const float SERVO_TS = 0.01;        //Servo frequency

void setled(){
    Led_red=1; Led_green=1; Led_blue=1; Led_calibration=0;
}
//------------------------------------------------------------------------------
//-----------------------------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 SpeedLeft, SpeedRight;                            //Strength of the muscles 
float emg_offsetLeft, emg_offsetRight;              //Calibration offsets from zero to one for the right arm
float kLeft, kRight;                                //Scaling factors
float Signal_filteredLeft, Signal_filteredRight;    //Variables to store the filterd signals

//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 
BiQuadChain BiQuad_filter_Left;
BiQuadChain BiQuad_filter_Right;

//Filters-----------------------------------------------------------------------

// Filter for the left signal
float FilterLeft(float input){
    float Signal_filtered= BiQuad_filter_Left.step(input);
    return Signal_filtered;
}
// Filter for the right signal
float FilterRight(float input){
    float Signal_filtered= BiQuad_filter_Right.step(input);
    return Signal_filtered;
}

//Calibration-------------------------------------------------------------------

// Calibrating the EMG signals. This function needs to be ativated when the 
// muscles are flexed for over a second
void calibration(){
    emg_offsetLeft = SignalLeft.calibrate(m, FilterLeft(emg0));
    emg_offsetRight = SignalRight.calibrate(m, FilterRight(emg2));
    //Setting the emg scaling constants to reed between zero and one.
    kLeft = 1/emg_offsetLeft;
    kRight = 1/emg_offsetRight;
    //Set led to Green to indicate if calibration is done
    Led_calibration = 1;
}

//Speedcontrol------------------------------------------------------------------
//Determine the signalnumbers (i.e. speed) based on the strenght of the EMG signals
void Speedcontrol(){
    //Left Signals
    Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);//*kLeft
    SpeedLeft = SignalLeft.getspeedLeft(n, action, Signal_filteredLeft);
    //Right Signals
    Signal_filteredRight = fabs(FilterRight(emg2)*kRight);//*kRight
    SpeedRight = SignalRight.getspeedRight(n, action, Signal_filteredRight);
}

//------------------------------------------------------------------------------
//---------------------------------Servo----------------------------------------
//------------------------------------------------------------------------------
//Sending a pulse to the other Mbed to raise the pen
void up(){
    Up = 1; Up = 0; Up = 1;
}
//Sending a pulse to the other Mbed to lower the pen
void down(){
    Down = 1; Down = 0; Down = 1;
}

//Setting the begin mode to zero
int mode =0;
// Enable the servo function when mode 3 or mode 6 is activated.
// If the mean value for the Left arm stays long enough high the servo will raise
// If the mean value for the Right arm stays long enough high the servo will lower 
void servo(){
     if(mode==3||mode==6){   
        Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);
        Signal_filteredRight = fabs(FilterRight(emg2)*kRight);
        SpeedLeft = SignalLeft.getspeedLeft(n, action, Signal_filteredLeft);
        SpeedRight = SignalRight.getspeedRight(n, action, Signal_filteredRight);
        if (SpeedLeft == 2){
            up();
        }
        if (SpeedRight == 2){
            down();
        }
    }
}
//------------------------------------------------------------------------------
//---------------------------Mode selection-------------------------------------
//------------------------------------------------------------------------------

//Function is called when the mode button is pressed and switches modes and led indication 
void mode_selection(){
    //Switching to the next mode. (Total of 6 modes)
    if(mode == 6){
        mode=1;
    }
    else{
        mode++;
    }
    //Setting the LED colour to indicate the operating mode    
    if((mode==3) || (mode==6)) {
        Led_red = 0, Led_green = 0, Led_blue = 0;   //Set Led Mode to White
    }
    else if (mode==1) {
        Led_red = 0, Led_green = 1, Led_blue = 1;   //Set Led Mode to Red
    }
    else if (mode==2) {
        Led_red = 1, Led_green = 0, Led_blue = 1;   //Set Led Mode to Green
    }
    else if (mode==4) {
        Led_red = 1, Led_green = 1, Led_blue = 0;   //Set Led Mode to Blue
        }
    else if (mode==5) {
        Led_red = 0, Led_green = 0, Led_blue = 1;   //Set Led Mode to Yellow
    }
}
//------------------------------------------------------------------------------
//-------------------------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_beta = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, position_math_l, position_math_r);
    float position_beta = RAD_PER_PULSE * encoder1.getPosition();
    float error_beta = reference_beta-position_beta;
    float magnitude1 = PID.get(error_beta, 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_alpha = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, position_math_l, position_math_r);
    float position_alpha = RAD_PER_PULSE * -encoder2.getPosition();
    float error_alpha = reference_alpha-position_alpha;
    float magnitude2 = PID.get(error_alpha, 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;           //Variables to store the directions
float magnitude1, magnitude2;           //Variables to store the magnitude signal

void motor_control(){
    direction_l = MoveLeft.getdirectionLeft(mode);                              //x-direction
    direction_r = MoveRight.getdirectionRight(mode);                            //y-direction
    position_math_l= MoveLeft.getpositionLeft(SpeedLeft, mode, max_rangex);      //x-direction
    position_math_r= MoveRight.getpositionRight(SpeedRight, mode, max_rangey);   //y-direction
    motor1_control(); 
    motor2_control();
}
//------------------------------------------------------------------------------
//-----------------------------Filter Update------------------------------------
//------------------------------------------------------------------------------

//Updating Filter values
void FilterUpdate(){
    FilterLeft(emg0.read());
    FilterRight(emg2.read());
    SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read())));
    SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())));    
}
//------------------------------------------------------------------------------
//------------------------------Main--------------------------------------------
//------------------------------------------------------------------------------

int main(){
    setled(); 
    
    //Creating Filters
    BiQuad_filter_Left.add( &LP1L ).add( &HP2L ).add( &NO3L);
    BiQuad_filter_Right.add( &LP1R ).add( &HP2R ).add( &NO3R);
    
    //Button detection
    But1.rise(&calibration);
    Mode.rise(&mode_selection);
    
    //Setting motor periods
    motor1.period(0.0001f); motor2.period(0.0001f);
    
    //Tickers
    MyTickerController.attach(&motor_control, CONTROLLER_TS);
    MyTickerMean.attach(&Speedcontrol, MEAN_TS);  
    MyTickerServo.attach(&servo, SERVO_TS);
    MyTickerFilter.attach_us(&FilterUpdate, FILTER_TS);
    
    while(1) {
    }
}