File content as of revision 15:2772f8cbf382:

#include "mbed.h" // Use revision 119!!
#include "HIDScope.h" // For displaying data, select MBED - HID device, restart for every new code 
#include "QEI.h" // For reading the encoder of the motors 
#include <ctime> // for the timer during the process (if needed)
#include "Servo.h" // For controlling the servo
#include "BiQuad.h"

#define SERIAL_BAUD 115200

// In- en outputs 
// -----------------------------------------------------------------------------

// EMG related
// Constants EMG filter
const double m1 = 0.5000;
const double m2 = -0.8090;
const double n0 = 0.5000;
const double n1 = -0.8090;
const double n2 = 0.0;
const double a1 = 0.9565;
const double a2 = -1.9131;
const double b0 = 0.9565;
const double b1 = -1.9112;
const double b2 = 0.9150;
const double c1 = 0.0675;
const double c2 = 0.1349;
const double d0 = 0.0675;
const double d1 = -1.1430;
const double d2 = 0.4128;
  
// Variables EMG
double emg0;
double emg1;
double emg2;
double notch0;
double notch1;
double notch2;
double high0;
double high1;
double high2;
double absolute0;
double absolute1;
double absolute2;
double low0;
double low1;
double low2;

// BiQuad values
BiQuadChain notch;
BiQuad N1( m1, m2, n0, n1, n2);
BiQuad N2( m1, m2, n0, n1, n2);
BiQuad N3( m1, m2, n0, n1, n2);
BiQuadChain highpass;
BiQuad H1( a1, a2, b0, b1, b2);
BiQuad H2( a1, a2, b0, b1, b2);
BiQuad H3( a1, a2, b0, b1, b2);
BiQuadChain lowpass;
BiQuad L1( c1, c2, d0, d1, d2);
BiQuad L2( c1, c2, d0, d1, d2);
BiQuad L3( c1, c2, d0, d1, d2);

const float T=0.001f;

// EMG
const int sizeMovAg = 100; //Size of array over which the moving average (MovAg) is calculated
double sum, sum1, sum2, sum3; //Variables used in calibration and MovAg to sum the elements in the array
double StoreArray0[sizeMovAg] = {}, StoreArray1[sizeMovAg] = {}, StoreArray2[sizeMovAg] = {};

//Empty arrays to calculate MovAgs
double Average0, Average1, Average2; //Outcome of MovAg
const int sizeCali = 500; //Size of array over which the Threshold will be calculated
double StoreCali0[sizeCali] = {}, StoreCali1[sizeCali] = {}, StoreCali2[sizeCali] = {};

//Empty arrays to calculate means in calibration
double Mean0, Mean1, Mean2; //Mean of maximum contraction, calculated in the calibration
double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1; //Thresholds for muscles 0 to 2
int g = 0; //Part of the switch void, where the current state can be changed
int emg_calib=0; //After calibration this value will be 1, enabling the

// EMG
Ticker Filter_tick;
Ticker MovAg_tick;
AnalogIn emg0_in( A0 );
AnalogIn emg1_in( A1 );
AnalogIn emg2_in( A2 );

// Motor related
DigitalOut dirpin_1(D4); // direction of motor 1 (translation)
PwmOut pwmpin_1(D5); // PWM pin of motor 1
DigitalOut dirpin_2(D7); // direction of motor 2 (rotation)
PwmOut pwmpin_2(D6); // PWM pin of motor 2

// Extra stuff
DigitalIn button_motorcal(SW2); // button for motor calibration, on mbed
DigitalIn button_emergency(D7); // button for emergency mode, on bioshield
DigitalIn button_wait(SW3); // button for wait mode, on mbed 
DigitalIn button_demo(D6); // button for demo mode, on bioshield  

DigitalIn led_red(LED_RED); // red led 
DigitalIn led_green(LED_GREEN); // green led
DigitalIn led_blue(LED_BLUE); // blue led

AnalogIn pot_1(A1); // potmeter for simulating EMG input
Servo myservo(D8); // Define the servo to control (in penholder), up to start with

// Other stuff
// -----------------------------------------------------------------------------
// Define stuff like tickers etc

Ticker ticker; // Name a ticker, use each ticker only for 1 function! 
HIDScope scope(6); // Number of channels in HIDScope
QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); // Define the type of encoding: X4 encoding(default is X2)
QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
Serial pc(USBTX,USBRX);
Timer t; // For timing the time in each state (https://os.mbed.com/handbook/Timer)

// Variables 
// ----------------------------------------------------------------------------- 
// Define here all variables needed throughout the whole code 
volatile double time_overall;
volatile double time_in_state;
volatile double motor_velocity = 0;
volatile double EMG = 0;
volatile double errors = 0;
volatile int counts1_prev = 0;
volatile int counts2_prev = 0;
volatile int counts1;
volatile int counts2;

// MOTOR_CAL
volatile double tower_1_position = 0.1; // the tower which he reaches first
volatile double tower_end_position = 0.1; // the tower which he reaches second
volatile double rotation_start_position = 0.1; // the position where the rotation will remain
volatile double position;
float speed = 0.70;
int dir = 0;

// states
enum states {WAIT, MOTOR_CAL, EMG_CAL, START, OPERATING, FAILURE, DEMO}; // states the robot can be in
states CurrentState = WAIT; // the CurrentState to start with is the WAIT state
bool StateChanged = true; // the state must be changed to go into the next state

// Functions
// -----------------------------------------------------------------------------
 
// Encoder 
// Getting encoder information from motors
int Counts1(volatile int& a) // a = counts1
{
    counts1_prev = a;
    a =  Encoder2.getPulses();
    return a;
    }

int Counts2(volatile int& a) // a = counts2
{
    counts2_prev = a;
    a = Encoder1.getPulses(); 
    return a;
    }
    
// Potmeter for controlling motor, must be removed when EMG is used to control the motor
float potmeter()
{
    float out_2 = (pot_1 * 2.0f) - 1.0f;
                
    dirpin_1.write(out_2 < 0);
    dirpin_2.write(out_2 < 0);
    
    pwmpin_1 = fabs (out_2); // Has to be positive value
    pwmpin_2 = fabs (out_2);
        
    return out_2;
    }
    
// Servo control
// To lift the pen up, with a push of button
void servocontrol()
{
    while (button_motorcal == false) // If button is pushed, pen should go up
    {
        myservo = 0.1;
        }
    myservo = 0.0; 
    }
    
// EMG filter
// To process the EMG signal before information can be caught from it 

// Filter of the first EMG signal
void filtering()
{
    // Reading the EMG signal
    emg0 = emg0_in.read(); 
    emg1 = emg1_in.read();
    emg2 = emg2_in.read(); 
    
    // Applying a notch filter over the EMG data
    notch0 = N1.step(emg0); 
    notch1 = N2.step(emg1);
    notch2 = N3.step(emg2); 
    
    // Applying a high pass filter
    high0 = H1.step(notch0); 
    high1 = H2.step(notch1); 
    high2 = H3.step(notch2);
    
    // Rectifying the signal
    absolute0 = fabs(high0); 
    absolute1 = fabs(high1);
    absolute2 = fabs(high2); 
    
    // Applying low pass filter
    low0 = L1.step(absolute0); 
    low1 = L2.step(absolute1);
    low2 = L3.step(absolute2); 
    }

// Moving average filter
// To determine the moving average, apply after filtering
void MovAg()
{    
    // For statement to make an array of the last datapoints of the filtered signal
    for (int i = sizeMovAg - 1; i >= 0; i--) 
    {
        // Shifts the i'th element one place to the right
        StoreArray0[i] = StoreArray0[i-1]; 
        StoreArray1[i] = StoreArray1[i-1];
        StoreArray2[i] = StoreArray2[i-1];
        }
    
    // Stores the latest datapoint in the first element of the array
    StoreArray0[0] = low0; 
    StoreArray1[0] = low1;
    StoreArray2[0] = low2;
    sum1 = 0.0;
    sum2 = 0.0;
    sum3 = 0.0;

    // For statement to sum the elements in the array
    for (int a = 0; a<=sizeMovAg-1; a++) 
    { 
        sum1+=StoreArray0[a];
        sum2+=StoreArray1[a];
        sum3+=StoreArray2[a];
        }
    
    // Calculates an average over the datapoints in the array    
    Average0 = sum1/sizeMovAg; 
    Average1 = sum2/sizeMovAg;
    Average2 = sum3/sizeMovAg;
    
    // Sending the signal to the HIDScope
    // Change the number of channels in the beginning of the script when necessary
    scope.set( 0, emg0);  
    scope.set( 1, low0); 
    scope.set( 2, Average0);
    scope.set( 3, low1);
    scope.set( 4, emg2);
    scope.set( 5, low2);
    scope.send(); 
    }

// WAIT
// To do nothing
void wait_mode()
{
    // go back to the initial values
    // Copy here the variables list with initial values
    // all pwm's to zero
    // all counts to zero
    }

// MOTOR_CAL
// To calibrate the motor angle to some mechanical boundaries
// Kenneth mee bezig 
void pos_store(int a){ //store position in counts to know count location of the ends of bridge

    if (tower_1_position == 0.1)
    {
        tower_1_position = a;
        }
    else if (tower_end_position == 0.1)
    {
        tower_end_position = a;
        }
    else if (rotation_start_position == 0.1)
    {
        rotation_start_position = a;
        }    
    }

// Start translation   
void translation_start(int a, float b) // a = dir , b = speed 
{ 
    dirpin_1.write(a);
    pwmpin_1 = b;
    }
    
// Stop translation                            
void translation_stop()
{ 
    pwmpin_1 = 0.0;
    }
    
// Start rotation         
void rotation_start(int a, float b)
{ 
    dirpin_2.write(a);
    pwmpin_2 = b;
    }
 
// Stop rotation    
void rotation_stop()
{ 
    pwmpin_2 = 0.0;
    } 

// Calibration of translation
void calibration_translation()
{
    for(int m = 1; m <= 2; m++) // to do each direction one time
    {
        // dir = 0, means that the pen moves to the translation motor, dir = 1 means it moves to the rotation motor
        pc.printf("\r\nTranslatie loop\r\n");
        translation_start(dir,speed); 
        pc.printf("Direction = %i\r\n", dir);
        
        bool g = true; // to make a condition for the while loop
        while (g == true)
        {
            if (button_demo == false) // if button_demo is pushed, the translation should stop and change direction
            {
                translation_stop();    
                pos_store(Counts1(counts1)); 
                pc.printf("position of first tower = %.1f, position of second tower = %.1f, position of rotation motor = %.1f \r\n",tower_1_position,tower_end_position,rotation_start_position);
                dir = dir + 1;
                                
                g = false; // to end the while loop
                }
                
            wait(0.01); 
            }
        
        wait(1.0); // wait 3 seconds before next round of translation/rotation
        }
    }
    
void calibration_rotation()
{
    rotation_start(dir, speed);
    pc.printf("\r\nRotatie start\r\n");

    bool f = true; // condition for while loop
    while(f == true)
    {
        if (button_motorcal == false) // If button_motorcal is pushed, then the motor should stop and remain in that position until homing
        {
            rotation_stop();
            
            f = false; // to end the while loop
            }
        
        wait(0.01);
        }
    int start_counts = 0;    
    pos_store(Counts2(start_counts));
    pc.printf("position of first tower = %.1f, position of second tower = %.1f, position of rotation motor = %.1f \r\n",tower_1_position,tower_end_position,rotation_start_position);
    }
    
void motor_calibration()
{
    // translation
    calibration_translation();
    
    pc.printf("before wait\r\n");
    wait(1.5);
    
    // rotation
    calibration_rotation();
    
    pc.printf("Motor calibration done");
    }

// EMG_CAL
// To calibrate the EMG signal to some boundary values
// Void to switch between signals to calibrate
void switch_to_calibrate() 
{
    g++;
    //If g = 0, led is blue
    if (g == 0) 
    { 
        led_blue==0;
        led_red==1;
        led_green==1;
        } 
    //If g = 1, led is red
    else if(g == 1) 
    { 
        led_blue==1;
        led_red==0;
        led_green==1;
        } 
    //If g = 2, led is green
    else if(g == 2) 
    { 
        led_blue==1;
        led_red==1;
        led_green==0;
        } 
    //If g > 3, led is white
    else 
    { 
        led_blue==0;
        led_red==0;
        led_green==0;
        emg_calib = 0;
        g = 0;
        }
    }

// Void to calibrate the signals, depends on value g. While calibrating, maximal contraction is required
void calibrate() 
{
    switch (g) 
    {
        case 0: 
        { // Case zero, calibrate EMG signal of right biceps
            sum = 0.0;
            
            //For statement to make an array of the latest datapoints of the filtered signal
            for (int j = 0; j<=sizeCali-1; j++) 
            {
                
                StoreCali0[j] = low0; // Stores the latest datapoint in the first element of the array
                sum+=StoreCali0[j]; // Sums the elements in the array
                wait(0.001f);
                }
            Mean0 = sum/sizeCali; // Calculates the mean of the signal
            Threshold0 = Mean0; // Factor *2 is for resting and *1 is for max contraction
            break;
            }
        case 1: 
        { // Case one, calibrate EMG signal of left biceps
            sum = 0.0;
            for(int j=0; j<=sizeCali-1; j++) 
            {
                StoreCali1[j] = low1;
                sum+=StoreCali1[j];
                wait(0.001f);
                }
            Mean1 = sum/sizeCali;
            Threshold1 = Mean1; // Factor *2 is for resting and *1 is for max contraction
            break;
            }
        case 2: 
        { // Case two, calibrate EMG signal of calf
            sum = 0.0;
            for(int j=0; j<=sizeCali-1; j++) 
            {
                StoreCali2[j] = low2;
                sum+=StoreCali2[j];
                wait(0.001f);
                }   
            Mean2 = sum/sizeCali;
            Threshold2 = Mean2; //Factor *2 is for resting and *1 is for max contraction
            break;
            }
        case 3: 
        { // Sets calibration value to 1; robot can be set to Home position
            emg_calib=1;
            wait(0.001f);
            break;
            }
        default: 
        { // Ensures nothing happens if g is not equal to 0, 1 or 2.
            break;
            }
        }
    }

// Void to calibrate the EMG signal
void emg_calibration()
{
    for(int m = 1; m <= 4; m++)
    {
        led_blue == 0;
        led_red == 1;
        led_green == 1; 
        
        MovAg_tick.attach(&MovAg,T); // Moving average calculation every T sec.
        
        
        pc.printf("g is %i\n\r",g);
        pc.printf("Average0 = %f , Average1 = %f, Average2 = %f \n\r",Average0, Average1, Average2);
        pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
        
        bool k = true;
        while(k == true)
        {
            if(button_motorcal == false)
            {
                calibrate(); // Calibrate threshold for 3 muscles
                k = false;
                }
            wait(0.2f); // Wait to avoid bouncing of button
            }
        
        bool h = true;
        while(h == true)
        {
            if (button_demo == false)
            {
                switch_to_calibrate(); // Switch state of calibration (which muscle)
                h = false;
                }
            
            wait(0.2f); // Wait to avoid bouncing of button
            }   
        }
        
        // Turning all leds off 
        led_red == 1;
        led_blue == 1;
        led_green == 1;
    }
    
// PID controller
// To control the input signal before it goes into the motor control
// Simon mee bezig
void pid_controller()
{
    // Simons code here
    }

// START
// To move the robot to the starting position: middle
void start_mode() 
{
    // move to middle, only motor1 has to do something, the other one you can move yourself during the calibration

    //translation home
    if (counts2 > ((tower_end_position - tower_1_position)/2))
    {
        translation_start(0,1.0);
        }
    else {    
        translation_start(1,1.0);
        }
    if (counts2 > ((tower_end_position - tower_1_position)/2 - 100))
    {
        if (counts2 < ((tower_end_position - tower_1_position)/2 + 100))
        {
            translation_stop();
            }
        }
    }

// OPERATING
// To control the robot with EMG signals
// Kenneth bezig met motor aansturen
void operating()
{
    servocontrol(); // make sure the servo is used in this mode, maybe attach to a ticker? 
    // RKI fuction
    }    

// DEMO
// To control the robot with a written code and write 'HELLO'
// Voor het laatst bewaren
void demo_mode()
{
    // code here
    }

// FAILURE
// To shut down the robot after an error etc 
void failure_mode()
{
    led_red == 0; // turning red led on to show emergency mode 

    // all pwmpins zero 
    pwmpin_1 = 0.0;
    pwmpin_2 = 0.0;
    
    // Servo up?
    // myservo = 0.1;
    
    pc.printf("Emergency mode, reset system to continue\r\n");
    }

// Main function
// -----------------------------------------------------------------------------
int main()
{
    pc.baud(115200); // For TeraTerm, the baudrate, set also in TeraTerm itself! 
    pc.printf("Start code\r\n"); // To check if the program starts 
    pwmpin_1.period_us(60); // Setting period for PWM
    Filter_tick.attach(&filtering,T); // EMG signals must be filtered all the time! EMG signals filtered every T sec.
    
    while(true){
        // timer
        clock_t start; // start the timer
        start = clock(); 
        time_overall = (clock() - start) / (double) CLOCKS_PER_SEC;
        myservo = 0.1; // Keep the pen lifted until servo function is called (operation mode)
        
        //pc.printf("potmeter value = %f ", potmeter_value);
        //pc.printf("counts = %i\r\n", counts); 
        
        // With the help of a switch loop and states we can switch between states and the robot knows what to do
        switch(CurrentState)
        {
            case WAIT:
            
            if(StateChanged) // so if StateChanged is true
            {
                pc.printf("State is WAIT\r\n");
                
                // Execute WAIT mode
                wait_mode();
                
                StateChanged = false; // the state is still WAIT
                }
                
            if(button_motorcal == false) // condition for WAIT --> MOTOR_CAl; button_motorcal press
            {
                CurrentState = MOTOR_CAL;
                pc.printf("State is MOTOR_CAL\r\n");
                StateChanged = true;
                }
                
            if (button_emergency == false) // condition for WAIT --> FAILURE; button_emergency press 
            {
                CurrentState = FAILURE;
                pc.printf("State is FAILURE\r\n");
                StateChanged = true;
                }
            
            break;
            
            case MOTOR_CAL:
            
            if(StateChanged) // so if StateChanged is true
            {
                t.reset();
                t.start();
                
                // Execute MOTOR_CAL mode
                motor_calibration();
                
                t.stop();
                time_in_state = t.read();
                pc.printf("Time here = %f\r\n", time_in_state);
                
                StateChanged = false; // the state is still MOTOR_CAL
                }
            
            if((time_in_state > 10.0) && (pwmpin_1 < 0.01) && (pwmpin_2 < 0.01)) // condition for MOTOR_CAL --> EMG_CAL; 3s and motors stopped moving
            {
                CurrentState = EMG_CAL;
                pc.printf("State is EMG_CAL\r\n");
                StateChanged = true;
                }
                
            if (button_emergency == false) // condition for MOTOR_CAL --> FAILURE; button_emergency press 
            {
                CurrentState = FAILURE;
                pc.printf("State is FAILURE\r\n");
                StateChanged = true;
                }
                
            break;
            
            case EMG_CAL:
            
            if(StateChanged) // so if StateChanged is true
            {
                t.reset();
                t.start();
                
                // Execute EMG_CAL mode
                emg_calibration();
                
                t.stop();
                time_in_state = t.read();
                pc.printf("Time here = %f\r\n", time_in_state);
                
                StateChanged = false; // state is still EMG_CAL
                }
            
            if((time_in_state > 10.0) && (Average0 < 0.04) && (Average1 < 0.04) && (Average2 < 0.04)) // condition for EMG_CAL --> START; 5s and EMG is low
            {
                CurrentState = START;
                pc.printf("State is START\r\n");
                StateChanged = true;
                }
                
            if (button_emergency == false) // condition for EMG_CAL --> FAILURE; button_emergency press 
            {
                CurrentState = FAILURE;
                pc.printf("State is FAILURE\r\n");
                StateChanged = true;
                }
                
            break;
            
            case START:
            
            if(StateChanged) // so if StateChanged is true
            {
                t.reset();
                t.start();
                
                // Execute START mode
                start_mode();
                
                t.stop();
                time_in_state = t.read();
                pc.printf("Time here = %f\r\n", time_in_state);
                
                StateChanged = false; // state is still START
                }
            
            if((time_in_state > 5.0) && (pwmpin_1 < 0.01) && (pwmpin_2 < 0.01)) // condition for START --> OPERATING; 5s and motors stopped moving
            {
                CurrentState = OPERATING;
                pc.printf("State is OPERATING\r\n");
                StateChanged = true;
                }
                
            if (button_emergency == false) // condition for START --> FAILURE; button_emergency press 
            {
                CurrentState = FAILURE;
                pc.printf("State is FAILURE\r\n");
                StateChanged = true;
                }
                
            break;
            
            case OPERATING:
            
            if(StateChanged) // so if StateChanged is true
            {
                // Execute OPERATING mode
                
                StateChanged = false; // state is still OPERATING
                }
            
            if(button_emergency == false) // condition for OPERATING --> FAILURE; button_emergency press
            {
                CurrentState = FAILURE;
                pc.printf("State is FAILURE\r\n");
                StateChanged = true;
                }
                
            if(button_demo == false) // condition for OPERATING --> DEMO; button_demo press
            {
                CurrentState = DEMO;
                pc.printf("State is DEMO\r\n");
                StateChanged = true;
                }
                
            if(button_wait == false) // condition OPERATING --> WAIT; button_wait press  
            {
                CurrentState = WAIT;
                pc.printf("State is WAIT\r\n");
                StateChanged = true;
                }
                
            break;
            
            case FAILURE:
            
            if(StateChanged) // so if StateChanged is true
            {
                // Execute FAILURE mode
                failure_mode();
                                
                StateChanged = false; // state is still FAILURE
                }
            // no other states possible, a full reset (push on reset button) is necessary    
            break;
            
            case DEMO:
            
            if(StateChanged) // so if StateChanged is true
            {
                // Execute DEMO mode
                demo_mode();
                
                StateChanged = false; // state is still DEMO
                }
            
            if(button_wait == false) // condition for DEMO --> WAIT; button_wait press 
            {
                CurrentState = WAIT;
                pc.printf("State is WAIT\r\n");
                StateChanged = true;
                }
                
            if (button_emergency == false) // condition for DEMO --> FAILURE; button_emergency press 
            {
                CurrentState = FAILURE;
                pc.printf("State is FAILURE\r\n");
                StateChanged = true;
                }
                
            break;
            
            // no default  
            }
            
        // while loop does not have to loop every time
        }
        
}