File content as of revision 27:22bfc75f8d1a:

//Libraries
#include "mbed.h"
#include <math.h>
#include <cmath>
#include "MODSERIAL.h"
#include "BiQuad.h"
#include <algorithm>
//#include    "FastPWM.h"
#define PI 3.14159265

//Inputs and outputs
MODSERIAL pc(USBTX, USBRX);                                     //Connecting to PC
AnalogIn EMG0In(A0);                                            //EMG input 0
AnalogIn EMG1In(A1);                                            //EMG input 1
InterruptIn button(SW3);                                        //Define button
DigitalOut greenled(LED_GREEN);                                 //Green led
DigitalOut blueled(LED_BLUE);                                   //Blue led
DigitalOut redled(LED_RED);                                     //Red led

//Constants
const float ll = 230;                                          //Length of the lower arm
const float lu = 198;                                          //Length of the upper arm
const float lb = 50;                                           //Length of the part between the upper arms
const float le = 79;                                           //Length of the end-effector beam
const float xbase = 450-lb;                                    //Length between the motors
const int Length = 10000;                                       //Length of the array for the calibration
const int Parts = 50;                                           //Mean average filter over 50 values

//parameters for kinematics
float theta1 = PI*0.49;                                        //Angle of the left motor
float theta4 = PI*0.49;                                        //Angle of the right motor
float thetaflip = 0;                                           //Angle of the flipping motor
float prefx;                                                   //Desired x velocity
float prefy;                                                   //Desired y velocity                                                        "
float dt = 0.002;                                              //Time step of the system 
float iJ[2][2];                                                //inverse Jacobian matrix
//Time step (dependent on sample frequency)

//Kinematics parameters for x
float xendsum;
float xendsqrt1;
float xendsqrt2;
float xend;
float jacobiana;
float jacobianc;

//Kinematics parameters for y
float yendsum;
float yendsqrt1;
float yendsqrt2;
float yend;
float jacobianb;
float jacobiand;

//Parameters for the first EMG signal
float EMG0;                                                     //float for EMG input
float EMG0filt;                                                 //float for filtered EMG
float EMG0filtArray[Parts];                                     //Array for the filtered array
float EMG0Average;                                              //float for the value after Moving Average Filter
float Sum0 = 0;                                                 //Sum0 for the moving average filter
float EMG0Calibrate[Length];                                    //Array for the calibration
int ReadCal0 = 0;                                               //Integer to read over the calibration array
float MaxValue0 = 0;                                            //float to save the max muscle
float Threshold0 = 0;                                           //Threshold for the first EMG signal

//Parameters for the second EMG signal
float EMG1;                                                     //float for EMG input
float EMG1filt;                                                 //float for filtered EMG
float EMG1filtArray[Parts];                                     //Array for the filtered array
float EMG1Average;                                              //float for the value after Moving Average Filter
float Sum1 = 0;                                                 //Sum0 for the moving average filter
float EMG1Calibrate[Length];                                    //Array for the calibration
int ReadCal1 = 0;                                               //Integer to read over the calibration array
float MaxValue1 = 0;                                            //float to save the max muscle
float Threshold1 = 0;                                           //Threshold for the second EMG signal

//Filter variables
BiQuad Notch50_0(0.9049,-1.4641,0.9049,-1.4641,0.8098);         //Make Notch filter around 50 Hz
BiQuad Notch50_1(0.9049,-1.4641,0.9049,-1.4641,0.8098);         //Make Notch filter around 50 Hz
BiQuad Notch100_0(0.8427,-0.5097,0.8247,-0.5097,0.6494);        //Make Notch filter around 100 Hz
BiQuad Notch100_1(0.8427,-0.5097,0.8247,-0.5097,0.6494);        //Make Notch filter around 100 Hz
BiQuad Notch150_0(0.7548,0.4665,0.7544,0.4665,0.5095);          //Make Notch filter around 150 Hz
BiQuad Notch150_1(0.7548,0.4665,0.7544,0.4665,0.5095);          //Make Notch filter around 150 Hz
BiQuad Notch200_0(0.6919,1.1196,0.6919,1.1196,0.3839);          //Make Notch filter around 200 Hz
BiQuad Notch200_1(0.6919,1.1196,0.6919,1.1196,0.3839);          //Make Notch filter around 200 Hz
BiQuad High_0(0.9150,-1.8299,0.9150,-1.8227,0.8372);            //Make high-pass filter
BiQuad High_1(0.9150,-1.8299,0.9150,-1.8227,0.8372);            //Make high-pass filter
BiQuad Low_0(0.6389,1.2779,0.6389,1.143,0.4128);                //Make low-pass filter
BiQuad Low_1(0.6389,1.2779,0.6389,1.143,0.4128);                //Make low-pass filter
BiQuadChain filter0;                                            //Make chain of filters for the first EMG signal
BiQuadChain filter1;                                            //Make chain of filters for the second EMG signal

//Timers and Tickers
Ticker FindMax0_timer;                                          //Timer for finding the max muscle
Ticker FindMax1_timer;                                          //Timer for finding the max muscle
Timer local_timer;
Ticker hoofdticker;

//Bool for movement
bool xMove = false;                                             //Bool for the x-movement
bool yMove = false;                                             //Bool for the y-movement

//Parameters for the state machine
enum States {Calibration, WorkingMode};                         //Initialize state machine
States CurrentState = Calibration;                              //Start in the calibration mode
bool StateBool = true;                                          //Bool to first go in a state
bool SwitchStateBool = false;                                   //Bool to switch from calibration to working mode

//Function to read and filter the EMG
void ReadUseEMG0()
{
    for(int i = Parts ; i > 0 ; i--) {                          //Make a first in, first out array
        EMG0filtArray[i] = EMG0filtArray[i-1];                  //Every value moves one up
    }

    Sum0 = 0;
    EMG0 = EMG0In;                                              //Save EMG input in float
    EMG0filt = filter0.step(EMG0);                              //Filter the signal
    EMG0filt = abs(EMG0filt);                                   //Take the absolute value
    EMG0filtArray[0] = EMG0filt;                                //Save the filtered signal on the first place in the array

    for(int i = 0 ; i < Parts ; i++) {                          //Moving Average filter
        Sum0 += EMG0filtArray[i];                               //Sum the new value and the previous 49
    }
    EMG0Average = (float)Sum0/Parts;                            //Divide the sum by 50

    if (EMG0Average > Threshold0) {                             //If the value is higher than the threshold value
        redled = 0;                                             //Turn the LED on
        xMove = true;                                           //Set movement to true
    } else {
        redled = 1;                                             //Otherwise turn the LED off
        xMove = false;                                          //Otherwise set movement to false
    }
}
//Function to read and filter the EMG
void ReadUseEMG1()
{
    for(int i = Parts ; i > 0 ; i--) {                          //Make a first in, first out array
        EMG1filtArray[i] = EMG1filtArray[i-1];                  //Every value moves one up
    }

    Sum1 = 0;
    EMG1 = EMG1In;                                              //Save EMG input in float
    EMG1filt = filter1.step(EMG1);                              //Filter the signal
    EMG1filt = abs(EMG1filt);                                   //Take the absolute value
    EMG1filtArray[0] = EMG1filt;                                //Save the filtered signal on the first place in the array

    for(int i = 0 ; i < Parts ; i++) {                          //Moving Average filter
        Sum1 += EMG1filtArray[i];                               //Sum the new value and the previous 49
    }
    EMG1Average = (float)Sum1/Parts;                            //Divide the sum by 50

    if (EMG1Average > Threshold1) {                             //If the value is higher than the threshold value
        greenled = 0;                                           //Turn the LED on
        yMove = true;                                           //Set y movement to true
    } else {
        greenled = 1;                                           //Otherwise turn the LED off
        yMove = false;                                           //Otherwise set y movement to false
    }
}


//Function to make an array during the calibration
void CalibrateEMG0()
{
    for(int i = Parts ; i > 0 ; i--) {                          //Make a first in, first out array
        EMG0filtArray[i] = EMG0filtArray[i-1];                  //Every value moves one up
    }

    Sum0 = 0;
    EMG0 = EMG0In;                                              //Save EMG input in float
    EMG0filt = filter0.step(EMG0);                              //Filter the signal
    EMG0filt = abs(EMG0filt);                                   //Take the absolute value
    EMG0filtArray[0] = EMG0filt;                                //Save the filtered signal on the first place in the array

    for(int i = 0 ; i < Parts ; i++) {                          //Moving Average filter
        Sum0 += EMG0filtArray[i];                               //Sum the new value and the previous 49
    }
    EMG0Calibrate[ReadCal0] = (float)Sum0/Parts;                //Divide the sum by 50

    ReadCal0++;
}

//Function to make an array during the calibration
void CalibrateEMG1()
{
    for(int i = Parts ; i > 0 ; i--) {                          //Make a first in, first out array
        EMG1filtArray[i] = EMG1filtArray[i-1];                  //Every value moves one up
    }

    Sum1 = 0;
    EMG1 = EMG1In;                                              //Save EMG input in float
    EMG1filt = filter1.step(EMG1);                              //Filter the signal
    EMG1filt = abs(EMG1filt);                                   //Take the absolute value
    EMG1filtArray[0] = EMG1filt;                                //Save the filtered signal on the first place in the array

    for(int i = 0 ; i < Parts ; i++) {                          //Moving Average filter
        Sum1 += EMG1filtArray[i];                               //Sum the new value and the previous 49
    }
    EMG1Calibrate[ReadCal1] = (float)Sum1/Parts;                //Divide the sum by 50

    ReadCal1++;
}

//Function to find the max value during the calibration
void FindMax0()
{
    MaxValue0 = *max_element(EMG0Calibrate+500,EMG0Calibrate+Length);    //Find max value, but discard the first 100 values
    Threshold0 = 0.30f*MaxValue0;                               //The threshold is a percentage of the max value
    pc.printf("The calibration value of the first EMG is %f.\n\r The threshold is %f. \n\r",MaxValue0,Threshold0);              //Print the max value and the threshold
    FindMax0_timer.detach();                                    //Detach the timer, so you only use this once
}

//Function to find the max value during the calibration
void FindMax1()
{
    MaxValue1 = *max_element(EMG1Calibrate+500,EMG1Calibrate+Length);    //Find max value, but discard the first 100 values
    Threshold1 = 0.30f*MaxValue1;                               //The threshold is a percentage of the max value
    pc.printf("The calibration value of the second EMG is %f.\n\r The threshold is %f. \n\r",MaxValue1,Threshold1); //Print the Max value and the threshold
    FindMax1_timer.detach();                                    //Detach the timer, so you only use this once
}

//Function to stop the reading of the EMG
void StopProgram()
{
    greenled = 1;                                               //Turn the LEDs off
    blueled = 1;
    redled = 1;
    exit (0);                                                   //Abort mission!!
}

//Function to switch a state
/*void SwitchState()
{
    SwitchStateBool = true;                                     //Set the bool for the start of a state to true
    SwitchState_timer.detach();                                 //Use this function once
}*/

//forward kinematics function , &xend and&yend are output.
void FK(float &xend_, float &yend_, float theta1_, float theta4_)
{
    
    //Below we have the forward kinematics formula. Input should be the measured angles theta1 &theta4. Output 
    float xendsum_ = lb + xbase +ll*(cos(theta1_) - cos(theta4_));
    float xendsqrt1_ = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1_)+cos(theta4_))/2) -ll*(1+ cos(theta1_+theta4_)))*(-sin(theta1_)+sin(theta4_));
    float xendsqrt2_ = sqrt(pow((-xbase/ll+cos(theta1_)+cos(theta4_)),2)+ pow(sin(theta1_) - sin(theta4_),2));
    xend_ = (xendsum_ + xendsqrt1_/xendsqrt2_)/2;

    float yendsum_ = -le + ll/2*(sin(theta1_)+sin(theta4_));
    float yendsqrt1_ = (-xbase/ll + cos(theta1_)+cos(theta4_))*sqrt(-xbase*xbase/4 + lu*lu + ll/2*(xbase*(cos(theta1_)+cos(theta4_))- ll*(1+cos(theta1_+theta4_))));
    float yendsqrt2_ = sqrt(pow((-xbase/ll + cos(theta1_)+ cos(theta4_)),2)+ pow((sin(theta1_)-sin(theta4_)),2));
    yend_ = (yendsum_ + yendsqrt1_/yendsqrt2_);
}

//Below we have the inverse kinematics function.
void inverse(float prex, float prey)
{
    theta1 += (prefx*(iJ[0][0])+iJ[0][1]*prey)*dt;                          //theta 1 is itself + the desired speeds in x and y direction, both
    theta4 += (prefx*iJ[1][0]+iJ[1][1]*prey)*dt;                            //multiplied with a prefactor which comes out of the motor   
                                                                            //the iJ values are defined in the "kinematics" function
    
    //Calling the forward kinematics, to calculate xend and yend
    FK(xend,yend,theta1,theta4);

}

void kinematics()
{

    float xend1,xend2,xend3,yend1,yend2,yend3;
    const float dq = 0.0001;
    FK(xend1,yend1,theta1,theta4);
    FK(xend2,yend2,theta1+dq,theta4);
    FK(xend3,yend3,theta1,theta4+dq);

    float a,b,c,d;
    a = xend2-xend1;
    b = xend3-xend1;
    c = yend2-yend1;
    d = yend3-yend1;

    float Q = 1/(a*d-b*c)/dq;


    iJ[0][0] = d*Q;
    iJ[0][1]= -c*Q;
    iJ[1][0] = -b*Q;
    iJ[1][1] = a*Q;
    
    
    prefx = 1*xMove;                    //sw3, Prefx has multiplier one, but that has to become a value dependant on the motor
    prefy = 1*yMove;                    //sw2,
    inverse(prefx, prefy);
}

// these values are printed for controlling purposes (can later be removed)
void printvalue()
{
    pc.printf("X-value: %f \t Y-value: %f \n\r \t theta 1 = %f \t theta4 =  %f",xend, yend,theta1,theta4);
    }

//State Machine
void StateMachine()
{
    switch(CurrentState) {                                      //Determine in which state you are

        case Calibration:
            //Calibration mode
            if(StateBool) {                                     //If you go into this state
                pc.printf("You can start calibrating. \n\r");   //Print that you are in this state
                StateBool = false;                              //Set the bool for the start of a state to false
                FindMax0_timer.attach(&FindMax0,15);            //Find the maximum value after 15 seconds
                FindMax1_timer.attach(&FindMax1,15);            //Find the maximum value after 15 seconds
                //local_timer.reset();
                local_timer.start();
                blueled = 0;
            }

            CalibrateEMG0();   //start emg calibration every 0.005 seconds
            CalibrateEMG1();   //Start EMG calibration every 0.005 seconds

            if (local_timer.read() > 20) {                              //If the bool is changed
                CurrentState = WorkingMode;                     //Change the state to the working mode
                StateBool = true;                               //Set the start of a state bool to true
            }
            break;

        case WorkingMode:                                       //Mode to get the robot working
            if (StateBool) {                                    //If you start to go in this state
                pc.printf("You are know in the working mode. \r\n");    //Print in which mode you are
                StateBool = false;                              //Set the start of state bool to true
            }
            blueled = 1;

            ReadUseEMG0();//Start the use of EMG
            ReadUseEMG1();//Start the use of EMG
            kinematics(); //Starts calculating the x and y value of the endeffector, as well as the desired values and their BIJBEHORENDE angles

            //motorcontroller
            //Set the blue led off
            //pc.printf("First EMG: %f, Second EMG: %f \n\r",EMG0Average,EMG1Average);
            break;
    }
}

int main()
{
    FK(xend,yend,theta1,theta4);
    pc.baud(115200);
    greenled = 1;                                               //First turn the LEDs off
    blueled = 1;
    redled = 1;
    filter0.add(&Notch50_0).add(&Notch100_0).add(&Notch150_0).add(&Notch200_0).add(&Low_0).add(&High_0);                        //Make filter chain for the first EMG
    filter1.add(&Notch50_1).add(&Notch100_1).add(&Notch150_1).add(&Notch200_1).add(&Low_1).add(&High_1);                        //Make filter chain for the second EMG
    button.rise(StopProgram);                                   //If the button is pressed, stop program
    hoofdticker.attach(&StateMachine,0.002);
    while(true) {
        printvalue();
        wait(0.75);
    }
}