File content as of revision 5:3562c205d001:

//initialisation 
#include "mbed.h"
#include "encoder.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "math.h"
#include "BiQuad.h"
#include "Matrix.h"
#include "MatrixMath.h"
#include <stdlib.h>
#include <iostream>
#include <iomanip>
#include <complex>
#include <vector>

Ticker                  MainTicker;

//Define objects EMG
AnalogIn                emg0( A0 );
AnalogIn                emg1( A1 );
DigitalIn               buttonCal(D11);
DigitalIn               changeX(D2);            
DigitalIn               changeY(D3);
InterruptIn             turning(D10);

Ticker                  sample_timer;
Ticker                  calX_timer;
Ticker                  calY_timer;
//HIDScope                scope( 4 );
DigitalOut              led(LED1);
DigitalOut              led2(LED_GREEN);  

//Define variables EMG            
const int size = 100;
vector<double> SX(size,0);
vector<double> SY(size,0);
double meanSum = 0;
double maxsignal = 0;

//Filters
BiQuadChain Notch;
BiQuadChain Notch50;
BiQuadChain bqcLP;
BiQuadChain bqc;

//Notch filter chain for 100 Hz
BiQuad bqNotch1( 9.75180e-01, -1.85510e+00, 9.75218e-01, -1.87865e+00, 9.75178e-01 );
BiQuad bqNotch2( 1.00000e+00, -1.90228e+00, 1.00004e+00, -1.88308e+00, 9.87097e-01 );
BiQuad bqNotch3( 1.00000e+00, -1.90219e+00, 9.99925e-01, -1.89724e+00, 9.87929e-01 );

//Notch filter chain for 50 Hz
BiQuad bq1( 9.50972e-01, -1.53921e+00, 9.51003e-01, -1.57886e+00, 9.50957e-01 );
BiQuad bq2( 1.00000e+00, -1.61851e+00, 9.99979e-01, -1.57185e+00, 9.74451e-01 );
BiQuad bq3( 1.00000e+00, -1.61855e+00, 9.99988e-01, -1.62358e+00, 9.75921e-01 );

//2 Hz High Pass filter
BiQuad Highpass1(9.82385e-01, -1.96477e+00, 9.82385e-01, -1.96446e+00, 9.65081e-01);

//80 Hz Low Pass filter
BiQuad bqLP1( 5.78904e-02, 5.78898e-02, 0.00000e+00, -2.90527e-01, 0.00000e+00 );
BiQuad bqLP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -7.53537e-01, 4.06308e-01 );


//Define objects MotorControl
DigitalOut              gpo(D0);
DigitalOut              UpperMotorDir(D4);
PwmOut                  UpperMotorVel(D5);
DigitalOut              LowerMotorDir(D6);
PwmOut                  LowerMotorVel(D7);      
AnalogIn                potMeterIn(A1);
InterruptIn             ButMotorDir(D3);
Encoder                 encoder1(D13,D12);
Encoder                 encoder2(D9,D8);
Ticker                  m1_Ticker;
Ticker                  setpoint_Ticker;

//Define variables MotorControl
float           M_PI = 3.14159265359;

float           M1_KP = 1.0f;
float           M1_KI = 0.1f;
const float     M1_TS = 0.002;
const float     q_RAD_PER_PULSE_m2 = 0.36f*M_PI/1300.0f;                        //2*M_PI/4320 voor als het nu geen rechte lijn meer is8 
const float     q_RAD_PER_PULSE_m1 = 0.5f*M_PI/3300.0f;

float           m1_err_int = 0;
float           m2_err_int = 0;
int             motorD1 = 0;
float           motor1 = 0;
int             motorD2 = 0;
float           motor2 = 0;
float           position1 = 0;
float           position2 = 0;

float           emgX = 0;
float           emgY = 0;

float           L0 = 0.123;
float           L1 = 0.37;
float           L2 = 0.41;
float           q1 = 0.0f*M_PI;
float           q2 = 0.0f*M_PI;
float           Periode = 0.001;   //1000 is het aantal Hz

float           ref_pos1;
float           ref_pos2;

float           Qset1 = 0;
float           Qset2 = 0;

int UpperMotorPos;
int UpperMotorPos_prev;
int LowerMotorPos;
int LowerMotorPos_prev;

int             NPC = 0;
float           NoTurnies = 1; 

//Define matrices MotorControl
Matrix          JAPPAPP(2,2);
Matrix          qdot(2,1);
Matrix          Vdes(2,1);

//Define object Servo Motors
DigitalOut              Ledr(LED_RED);
DigitalOut              Ledg(LED_GREEN);
DigitalOut              Ledb(LED_BLUE);
PwmOut                  LeftServoVel(D11);
PwmOut                  RightServoVel(D10); 
DigitalIn               ServoButton(D2);
Ticker                  ServoTick;


//Other variables
float Cal_Strength = 0.6f;


//Connection with TeraTerm
MODSERIAL pc(USBTX, USBRX);

//Creating of the states
enum states{calib1, calib2, calibEMG, UpDown, LeftRight, Vertical, MotionEndEffector, GrabbingPlacing, Off, Idle, No_State};

//Root Mean Square value calculator for useful envelope, using vectors
double findRMS(vector<double> array) {
     int i;
     double sumsquared = 0.000;     //define variable that resets to 0 for every call of the function
     double RMS;
     
     for (i = 0; i < size; i++)     //for loop to calculate the total sum of the squared values
     {
        sumsquared = sumsquared + array.at(i)*array.at(i);
     }
     RMS = sqrt((double(1)/size)*(sumsquared));     //root mean square calculation
     return RMS;
}


//Function to show the EMG signals in HIDScope
void sample() {
    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
    //Standard emg value without filters, offset removed with the - value
    //scope.set(0, emg0.read()-0.4542f);
    
    //Use S vector to save values coming from EMG signal
    SX.erase(SX.begin());                         //remove first value
    double emgXvalue = bqc.step(emg0.read()-0.4542f);
    SX.push_back(emgXvalue);                             //add extra value to begin of vector
    
    SY.erase(SY.begin());                         //remove first value
    double emgYvalue = bqc.step(emg0.read()-0.4542f);
    SY.push_back(emgYvalue);                             //add extra value to begin of vector
    
    
    //Calculate Root Mean Square value using findRMS function as defined above
    double emgX = findRMS(SX);
    double emgY = findRMS(SY);
    
    //Display results
    //scope.set(1, emgX);
    //scope.set(2, emgY);
    
    //Send all scopes to HIDScope
    //scope.send();
    
    /* To indicate that the function is working, the LED is toggled */
    led = !led;
}

//Calibration function for the EMG signals
void calibrationX() {
    //function to determine maximal EMG input in order to allow motorcontrol to be activated
    
    if (buttonCal.read() == false){       //activated when button is pressed
        led2 = !led2;
        for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds
                SX.erase(SX.begin());
                double emgXCalvalue = bqc.step(emg0.read()-0.4542f);
                SX.push_back(emgXCalvalue);
                double signalfinal = findRMS(SX);    //calculate RMS values from EMG signals
            if (signalfinal >= maxsignal){          //Check if current signal is higher than current maximal signal
                maxsignal = signalfinal;            //keep changing the maximal signal
            }
        }
        led2 = 1;                                   //Turn LED off when calibration is finished
    }
}

void calibrationY() {
    //function to determine maximal EMG input in order to allow motorcontrol to be activated
    
    if (buttonCal.read() == false){       //activated when button is pressed
        led2 = !led2;
        for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds
                SY.erase(SY.begin());
                double emgYCalvalue = bqc.step(emg1.read()-0.4542f);
                SY.push_back(emgYCalvalue);
                double signalfinal = findRMS(SY);    //calculate RMS values from EMG signals
            if (signalfinal >= maxsignal){          //Check if current signal is higher than current maximal signal
                maxsignal = signalfinal;            //keep changing the maximal signal
            }
        }
        led2 = 1;                                   //Turn LED off when calibration is finished
    }
}


void NegPosChanger(){
    if(NPC == 0){
        NoTurnies = 0;
        NPC++;
    } else {
        NoTurnies = 1;
        NPC= 0;
    }
}


void qSetpointSet() {
    // Fill Matrix with data.
    q1 = encoder1.getPosition()*q_RAD_PER_PULSE_m1; //Calibreren nog toevoegen
    q2 = 0.5f*M_PI - q1 + encoder2.getPosition()*q_RAD_PER_PULSE_m2; //Calibreren mist nog
    
    JAPPAPP(1,1) = (cos(q1)*cos(q2) - sin(q1)*sin(q2))/(L1*sin(q2)*cos(q1)*cos(q1) + L1*sin(q2)*sin(q1)*sin(q1));
    JAPPAPP(1,2) = (cos(q1)*sin(q2) + cos(q2)*sin(q1))/(L1*sin(q2)*cos(q1)*cos(q1) + L1*sin(q2)*sin(q1)*sin(q1));
    JAPPAPP(2,1) = -(L1*cos(q1) + L2*cos(q1)*cos(q2) - L2*sin(q1)*sin(q2))/(L1*L2*sin(q2)*cos(q1)*cos(q1) + L1*L2*sin(q2)*sin(q1)*sin(q1));
    JAPPAPP(2,2) =  -(L1*sin(q1) + L2*cos(q1)*sin(q2) + L2*cos(q2)*sin(q1))/(L1*L2*sin(q2)*cos(q1)*cos(q1) + L1*L2*sin(q2)*sin(q1)*sin(q1));
       
    // Fill Matrix with data.
    int Xchanger = (changeX == 1) ? -1 : 1;
    int Ychanger = (changeY == 1) ? -1 : 1;
    Vdes(1,1) = emgX*Xchanger*NoTurnies;
    Vdes(2,1) = emgY*Ychanger*NoTurnies;
    int turnChanger = (turning == 1) ? -1 : 1;
    
    qdot = JAPPAPP*Vdes;
    
    Qset1 = Qset1 + qdot(1,1)*Periode;       //Vdes(1,1)
    Qset2 = Qset2 + qdot(2,1)*Periode;      //Vdes(2,1)

    if (Qset1 <= 0.43f*M_PI && Qset1 >= 0.01f){
       Qset1 = Qset1;
    }
    else if (Qset1 < 0.01f){
        Qset1 = 0.01f;
    }
    else if (Qset1 > 0.43f*M_PI){
        Qset1 = 0.43f*M_PI;
    }
    if (Qset1 + Qset2 < 0.5f*M_PI){
        Qset2 = 0.5f*M_PI - Qset1;
    }
    if (Qset2 <= 0.5f*M_PI && Qset2 >= 0.1f){
       Qset2 = Qset2;
    }
    else if (Qset2 < .1f){
        Qset2 = 0.1f;
    }
    else if (Qset1+Qset2 > M_PI){       
        Qset2 = M_PI - Qset1;
    }
    
}

float PI( float e, const float Kp, const float Ki, float Ts, float &e_int ){                 // Reusable PI controller
    return fabs(Kp * e);
}

void PositionControl() {
    //position1 = RAD_PER_PULSE * encoder1.getPosition();
    //position2 = RAD_PER_PULSE * encoder2.getPosition();
    ref_pos1 = Qset1 - q1;
        //if (ref_pos1 < 0.1) { ref_pos1 = 0; }
    ref_pos2 = Qset2 - q2;                                          // Don’t use magic numbers!
        //if (ref_pos2 < 0.1) { ref_pos2 = 0; }
    motor1 = PI(ref_pos1, M1_KP, M1_KI, M1_TS, m1_err_int );
    motor2 = PI(ref_pos2, M1_KP, M1_KI, M1_TS, m2_err_int );
    if (ref_pos1 <= 0) {
            motorD1=1;
        }
        else{
            motorD1 = 0;
        }
    if (ref_pos2 <= 0) {
            motorD2=0;
        }
        else{
            motorD2=1;
        }
        UpperMotorDir.write(motorD1);
        UpperMotorVel.write(motor1);
        LowerMotorDir.write(motorD2);
        LowerMotorVel.write(motor2);
}

/** Servo motor control function **/

//Servo initialisation
enum servostates{Close, Open};

//Set default state to open, as mentioned in paragraph ... of the report
servostates CurrentState = Open;

int i = 0;
int j = 0;

//Simply switch the current state when button is pressed
void Change() {
    if(CurrentState == Close){
    CurrentState = Open;
    } 
    else{
    CurrentState = Close;
    }
}

//State machine for the servo motors, to determine the state
void ProcessStateMachine(void) {
/**
State machine for the servo motors, motor response dependent on the current state and the amount of milliseconds of activation.
The entire function must be sampled at 2000 Hz, since it counts half milliseconds. This is required since the servos respond to half millisecond actuation.
For further information on the functioning of the servo motors; see paragraph ... in report 'Design of a Jenga playing robot' by group 2.
**/
    switch (CurrentState)
        {        
        case Close:
            if(j <= 35){
                RightServoVel.write(0);
                LeftServoVel.write(0);
                j++;
            }
            else if (j == 36){
                RightServoVel.write(1);
                LeftServoVel.write(0);
                j++;
            }
            else if (j == 37){
                RightServoVel.write(1);
                LeftServoVel.write(0);
                j++;
            }
            else if (j == 38){
                RightServoVel.write(1);
                LeftServoVel.write(0);
                j++;
            }
            else if (j == 39){
                RightServoVel.write(1);
                LeftServoVel.write(1);
                j = 0;
            }
        break;
        
        case Open:
            if(j <= 35){
                RightServoVel.write(0);
                LeftServoVel.write(0);
                j++;
            }
            else if (j == 36){
                RightServoVel.write(0);
                LeftServoVel.write(0);
                j++;
            }
            else if (j == 37){
                RightServoVel.write(0);
                LeftServoVel.write(1);
                j++;
            }
            else if (j == 38){
                RightServoVel.write(1);
                LeftServoVel.write(1);
                j++;
            }
            else if (j == 39){
                RightServoVel.write(1);
                LeftServoVel.write(1);
                j = 0;
            }
        break;
    }
}

//StateMachine for the moving of the end effector

void ProcessStateMachineMain(void) {
    states CurrentState1 = calibEMG;
    switch (CurrentState1)
        {        
        case MotionEndEffector:
            m1_Ticker.attach( &PositionControl, M1_TS );    
            setpoint_Ticker.attach(&qSetpointSet, Periode);  
        break;
        
        case calibEMG:
            //Function to calibrate the filtered EMG signals to the user, in order to control the motors
                double maxsignal = 0;   //empty variable to check the maximal EMG output
                for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 5 seconds
                    double signalfinal = fabs(bqc.step(emg0.read()));   //get values from the filtered EMG signal
                    if (signalfinal >= maxsignal){  //check whether the received signal is higher or lower than the current highest signal value
                        maxsignal = signalfinal; //keep changing the maximal signal
                    }
                }
        break;

        case calib1:
            UpperMotorVel = Cal_Strength; UpperMotorDir = true;          //depends on definitions
            LowerMotorVel = 0.0f;
            UpperMotorPos = encoder1.getPosition();
            if (abs(UpperMotorPos - UpperMotorPos_prev) < 0.01f){
                if (Cal_Strength < 1.0f) { 
                    Cal_Strength += 0.05f;
                }
                else{
                    encoder1.setPosition(0);            //Afhankelijk van RKI uitzoeken of deze echt naar nul moet of naar 0.5pi
                    CurrentState1 = calib2;
                    Cal_Strength = 0.6f;
                }
            int UpperMotorPos_prev = UpperMotorPos;
            } 
        break;

        case calib2:
            LowerMotorVel = Cal_Strength; LowerMotorDir = true;          // Check definitions
            UpperMotorVel = 1.0f;
            LowerMotorPos = encoder2.getPosition();
            if (abs(LowerMotorPos - UpperMotorPos_prev) < 0.01f){
                if (Cal_Strength < 1.0f) { 
                    Cal_Strength += 0.05f;
                }
                else{
                    encoder2.setPosition(0);
                    CurrentState1 = Idle;
                    //M1_controller = true; //turn on the position controller here
                    UpperMotorVel = 0.0f; LowerMotorVel = 0.0f;
                }
                int UpperMotorPos_prev = UpperMotorPos;
            }
        break;
       
        case Idle:
            emgX = 0;
            emgY = 0;
            m1_Ticker.attach( &PositionControl, M1_TS );    
            setpoint_Ticker.attach(&qSetpointSet, Periode);  
        break;
        
        case Off:
            //Robot is off, also return to default state so you have a reference position when it starts up again
        break;
        
        default:
            //Robot is in idle mode, not doing anything (motors still powered to hold current position) (Posible when waiting for your turn)

    }
}
        
//Main function for the servo motors, to change open or closed state
int main() {
    //allow for the 2000 Hz sampling of the Servo state machine function
    MainTicker.attach(ProcessStateMachineMain, 0.0005);
    
    ServoTick.attach(ProcessStateMachine, 0.0005);
    
      
            

    
    /** EMG signals **/
    //Constructing the notch filter chain
    bqc.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 ).add( &bqLP1 ).add( &bqLP2 );
    //Sample every 0.002 seconds, or 500 Hz
    sample_timer.attach(&sample, 0.002);
    //Ticker to check if motor is being calibrated, also at 500 Hz
    calX_timer.attach(&calibrationX, 0.002);
    calY_timer.attach(&calibrationY, 0.002);
    //Turn LED off, turned on when calibrating
    led2 = 1;      
    
    turning.rise(&NegPosChanger);                         
    
    //MODSERIAL connection
    pc.baud(115200);
    
    
    while (true)
    {

        if (buttonCal.read() == false){   //when calibrate button is pressed
            states CurrentState1 = calibEMG;
        }  
        
        if (!ServoButton) {
            Change();
            wait(0.3);
        }
        
        
        /*Control end-effector
        wait(0.1);
        float v_ref = GetReferenceVelocity();
        setMotor(v_ref);
        pc.printf("%f \r\n", FeedForwardControl(v_ref));
        UpperMotorDir.write(motorDirection);
        UpperMotorVel.write(RightServoVel);   //PWM Speed Control
        */
    }
}