File content as of revision 22:91ad5984b2f2:

#include "mbed.h"
#include "MODSERIAL.h"
#include <math.h>

AnalogIn emg0_in            (A0);
AnalogIn emg1_in            (A1);
AnalogIn emg2_in            (A2);

InterruptIn button1         (D10);                  //Is this one available? We need to make a map of which pins are used for what.
InterruptIn button2         (D11);

DigitalOut directionpin1    (D4);                   //Motor direction pin
DigitalOut directionpin2    (D7);

DigitalOut ledr             (LED_RED);
DigitalOut ledb             (LED_BLUE);
DigitalOut ledg             (LED_GREEN);

PwmOut pwmpin1              (D5);                   //Pulse width modulation --> speed motor
PwmOut pwmpin2              (D6);

MODSERIAL pc(USBTX, USBRX);                         //Serial communication to test if the code works


//EMG filter variables (also necessary for calibration parts)
double emg0_filt, emg1_filt, emg2_filt;                                                          //Variables for filtered EMG data channel 0, 1 and 2
double emg0_raw, emg1_raw, emg2_raw;
const int windowsize = 150;                                                                      //Size of the array over which the moving average (MovAg) is calculated. (random number)
double sum, sum1, sum2, sum3;                                                                    //variables used to sum elements in array
double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize];                //Empty arrays to calculate MoveAg
double movAg0, movAg1, movAg2;                                                                   //outcome of MovAg (moet dit een array zijn??)

//Calibration variables
int x = -1;                                                                                      //Start switch, colour LED is blue.
int emg_cal = 0;                                                                                 //if emg_cal is set to 1, motors can begin to work in this code (!!)
const int sizeCal = 2000;                                                                        //size of the dataset used for calibration
double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal];                               //arrays to put the dataset of the calibration in
double Mean0,Mean1,Mean2;                                                                        //average of maximum tightening
double Threshold0, Threshold1, Threshold2;                                                        //Threshold variables


//Functions

void switch_to_calibrate()
{
    x++;                        //Every time function gets called, x increases. Every button press --> new calibration state.
                                //Starts with x = -1. So when function gets called 1 time, x = 0.  In the end, x = 4 will reset to -1.

    if(x==0)                    //If x = 0, led is red
    {
        ledr = 0;
        ledb = 1;
        ledg = 1;
    }
    else if (x==1)              //If x = 1, led is blue
    {
        ledr = 1;
        ledb = 0;
        ledg = 1;
    }
    else if (x==2)            //If x = 2, led is green
    {
        ledr = 1;
        ledb = 1;
        ledg = 0;
    }
    else                        //If x = 3 or 4, led is white
    {
        ledr = 0;
        ledb = 0;
        ledg = 0;
    }
   
    if(x==4)                    //Reset back to x = -1
    {
        x = -1;
    }
}
    
        
void calibrate(void)
{
    switch(x)
    {
        case 0:                                         //If calibration state 0:
        {
            sum = 0.0;
            for(int j = 0; j<=sizeCal-1; j++)           //Array filled with datapoints from the EMGfilter signal of muscle 0
            {
                StoreCal0[j] = emg0_filt;
                sum+=StoreCal0[j];
                wait(0.001f);                           //Does there need to be a wait?
            }
            Mean0       = sum/sizeCal;                  //Calculate mean of the datapoints in the calibration set (2000 samples)
            Threshold0  = Mean0/2;                      //Threshold calculation = 0.5*mean
            break;                                      //Stop. Threshold is calculated, we will use this further in the code
        }
        case 1:                                         //If calibration state 1:
        {
            sum = 0.0;                                  
            for(int j = 0; j<=sizeCal-1; j++)           //Array filled with datapoints from the EMGfilter signal of muscle 1
            {
                StoreCal1[j] = emg1_filt;
                sum+=StoreCal1[j];
                wait(0.001f);
            }
            Mean1       = sum/sizeCal;
            Threshold1  = Mean1/2;
            break;
        }
        case 2:                                         //If calibration state 2:
        {
            sum = 0.0;
            for(int j = 0; j<=sizeCal-1; j++)           //Array filled with datapoints from the EMGfilter signal of muscle 2
            {
                StoreCal1[j] = emg2_filt;
                sum+=StoreCal2[j];
                wait(0.001f);
            }
            Mean2       = sum/sizeCal;
            Threshold2  = Mean2/2;
            break;
        }
        case 3:                                         //EMG is calibrated, robot can be set to Home position.
        {
            emg_cal = 1;                                //This is the setting for which the motors can begin turning in this code (!!)
            wait(0.001f);
            break;
        }
        default:                                        //Ensures nothing happens if x is not 0,1 or 2.
        {
            break;
        }
    }
}
 

int main()
{ 
    pc.baud(115200);
    pc.printf("hello\n\r");                     //Check does code work
    
    ledr = 1;                                   //Begin led = blue, press button for first state of calibration --> led will turn red
    ledb = 0;
    ledg = 1;
        
    button1.rise(switch_to_calibrate);          //Switch state of calibration (which muscle)
    wait(0.2f);
    button2.rise(calibrate);                    //Calibrate threshold for 3 muscles
    wait(0.2f);
       
    pwmpin1.period_us(60);                      //60 microseconds PWM period, 16.7 kHz 
    
    if(emg_cal==1)                              //After calibration is finished, emg_cal will be 1. Otherwise 0. 
    { 
         while (true)
            {
               
                if(emg0_filt>Threshold0)        //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
                {
                       pwmpin1 = 1;
                       directionpin1.write(1);
                }
                else                            //If it is not higher than the threshold, the motor will not turn at all. 
                {
                       pwmpin1 = 0;
                }
                
                if(emg1_filt>Threshold1)        //If the filtered EMG signal of muscle 1 is higher than the threshold, motor2 will turn in 1 direction
                {
                       pwmpin2 = 1;
                       directionpin2.write(1);
                }
                else                            //If not higher than the threshold, motor will not turn at all
                {
                       pwmpin2 = 0;
                }
                if(emg2_filt>Threshold2)        //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn
                {
                       pwmpin1 = 1;
                       pwmpin2 = 2;
                       directionpin1.write(1);
                       directionpin2.write(1);
                }
                else                            //If not higher than the threshold, motors will not turn at all
                {
                       pwmpin1 = 0;
                       pwmpin2 = 0;
                }
                       
                    
        
            }
    }   
    
    //Opmerkingen Marijke: ik ben niet zeker van waar ik de while loop moet plaatsen. 
    //Misschien is aan het einde buiten de if beter, schrijf dan while(1){}.
    
    //De manier van aansturen van de motoren is nu puur om te testen of de kalibratie werkt. Op deze manier kunnen
    //We namelijk niet met 2 spieren 2 motoren tegelijk aansturen.
}