File content as of revision 2:de3bb38dae4e:

//================================================================  Script: Ball-E ==================================================================
// Author: Ewoud Velu, Lisa Verhoeven, Robert van der Wal, Thijs van der Wal, Emily Zoetbrood
/* This is the script of a EMG measurment moving robot. The purpose of the robot is to amuse people with the disease of Ducenne.
 The robot is designed to throw a ball in to a certain chosen pocket. In order to achieve this movement we use a ‘shoulder’ that can turn in the vertical plane
*/

//******************************************** Library DECLARATION **************************************
#include "mbed.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "biquadFilter.h"
#include <cmath>
#include <stdio.h>

//******************************************* FUNCTION DECLA *******************************************
//**************** INPUTS *****************
AnalogIn  EMG1(A0);                                            // Input EMG
AnalogIn  EMG2(A1);                                            // Input EMG
//AnalogIn pot(A2);
//AnalogIn pot1(A3);

QEI wheel1 (D10, D11, NC, 64,QEI::X4_ENCODING);          // Function for Encoder1 on the motor1 to the Microcontroller
QEI wheel2 (D12, D13, NC, 64,QEI::X4_ENCODING);          // Function for Encoder2 on the motor2 to the Microcontroller

//**************** OUTPUTS ****************
DigitalOut red(LED_RED);                                        // LED declared for calibration
DigitalOut green(LED_GREEN);                                    // LED declared for sampling

DigitalOut led_rood(D1);
DigitalOut led_geel(D3);
DigitalOut led_groen(D9);

DigitalOut magnet(D2);

DigitalOut motor1direction(D4);                                 // D4 en D5 zijn motor 2 (op het motorshield)
PwmOut motor1speed(D5);
DigitalOut motor2direction(D7);                                 // D6 en D7 zijn motor 1 (op het motorshield)
PwmOut motor2speed(D6);

//**************** FUNCTIONS **************
/* HIDScope    scope(4);          */                              // HIDScope declared with 4 scopes
MODSERIAL pc(USBTX, USBRX);                                     // Function for Serial communication with the Microcontroller to the pc.

//******************************* GLOBAL VARIABLES DECLARATION ************************************
const int       led_on             = 0;                         // Needed for the LEDs to turn on and off
const int       led_off            = 1;
const int       relax              = 0;
const int       graden             = 360;

int             games_played       = -1;                                    // -1 omdat het spel daar eerst loopt voor het spelen om alles na te checken
int             games_played1       = 0;
bool            flag_calibration   = true;

//********* VARIABLES FOR MOTOR CONTROL ********
const float     cw                 = 1;                          // The number if: motor1 moves clock wise motor2 moves counterclockwise
const float     ccw                = 0;                          // The number if: motor1 moves counterclock wise motor2 moves clockwise
bool            flag_s             = false;                      // Var flag_s sensor ticker
bool            flag_m             = false;                      // Var flag_m motor ticker
float           ain                = 0;

//********* VARIABLES FOR CONTROL 1 ************
volatile bool   sample_go;                                       // Ticker EMG function
int             Fs                 = 512;                        // Sampling frequency

const double    low_b1             = 1.480219865318266e-04;                        //Filter coefficients
const double    low_b2             = 2.960439730636533e-04;
const double    low_b3             = 1.480219865318266e-04;
const double    low_a2             = -1.965293372622690e+00;                       // a1 is normalized to 1
const double    low_a3             = 9.658854605688177e-01;
const double    high_b1            = 8.047897937631126e-01;
const double    high_b2            = -1.609579587526225e+00;
const double    high_b3            = 8.047897937631126e-01;
const double    high_a2            = -1.571102440190402e+00;                      // a1 is normalized to 1
const double    high_a3            = 6.480567348620491e-01;

double u1;
double y1;                                            // Declaring the input variables
double u2;
double y2;

int calibratie_metingen = 0;

//********* VARIABLES FOR FASE SWITCH **********
bool            begin               = true;
int             fase                = 3;                                   // To switch between different phases and begins with the reset phase
const float     fasecontract        = 0.7;                                 // The value the EMG signal must be for fase change
float           rightbiceps         = y2;
float           leftbiceps          = ain;
int             reset               = 0;
int             button_pressed      = 0;
int             j                   = 1;
int             N                   = 200;
bool            fase_switch_wait    = true;

//********* VARIABLES FOR CONTROL 2 ************
const float     contract            = 0.5;                                 // The minimum value for which the motor moves
const float     maxleft             = -200;                               // With this angle the motor should stop moving
const float     maxright            = 200;                                // With this angle the motor should stop moving
const float     speed_plate_1       = 0.1;                                 // The speed of the plate in control 2
bool            flag_left           = true;
bool            flag_right          = true;
float           pos_board           = 0;                                   // The position of the board begins at zero
int             pos_board1          = 0;
float           Encoderread2        = 0;


//********* VARIABLES FOR CONTROL 3 ************
const float     minimum_contract    = 0.4;                                 // Certain levels for muscle activity to activate motor
const float     medium_contract     = 0.6;                                 // "Medium contract muscle"
const float     maximum_leftbiceps  = 0.8;                                 // "Maximum contract muscle"
const float     on                  = 1.0;
const float     off                 = 0.0;
const float     minimum_throw_angle = 20;
const float     maximum_throw_angle = 110;
float           pos_arm             = 0;
int             pos_arm1            = 0;
float           Encoderread1        = 0;
int             switch_rondjes      = 2;
int             rondjes             = 0;
float           pos_armrondjes_max  = 3;

bool            problem1            = false;
bool            problem2            = false;
float           problem_velocity    = 0;
//********* VARIABLES FOR CONTROL 4 ************

float           reset_positie       = 0;      // Belangrijk
int             reset_arm           = 0;
int             reset_board         = 0;
float           speedcompensation;
float           speedcompensation2;
int             t                   = 5;                           // aftellen naar nieuw spel
int             switch_reset        = 1;
bool            arm                 = false;
bool            board1              = false;


// **************************************** Tickers *****************************************
Ticker Sensor_control;                                          // Adds ticker function for the variable function : Sensors
Ticker Motor_control;
Ticker EMG_Control;

//=============================================================================================== SUB LOOPS ==================================================================================================================
//**************************** CONTROL 1-EMG CALIBRATION ***********************************

biquadFilter highpass1(high_a2, high_a3, high_b1, high_b2, high_b3);    // Different objects for different inputs, otherwise the v1 and v2 variables get fucked up
biquadFilter highpass2(high_a2, high_a3, high_b1, high_b2, high_b3);
biquadFilter lowpass1(low_a2, low_a3, low_b1, low_b2, low_b3);
biquadFilter lowpass2(low_a2, low_a3, low_b1, low_b2, low_b3);


double cali_fact1 = 0.9;                                          // calibration factor to normalize filter output to a scale of 0 - 1
double cali_fact2 = 0.9;
double cali_array1[2560] = {};                                  // array to store values in
double cali_array2[2560] = {};

double cali_array3[512] = {};

void hoog_laag_filter()
{
    u1 = EMG1;
    u2 = EMG2;
    y1 = highpass1.step(u1);
    y2 = highpass2.step(u2);                                // filter order is: high-pass --> rectify --> low-pass
    y1 = fabs(y1);
    y2 = fabs(y2);
    y1 = lowpass1.step(y1)*cali_fact1;
    y2 = lowpass2.step(y2)*cali_fact2;                      // roughly normalize to a scale of 0 - 1, where 0 is minimum and 1 is roughly the maximum output.
}


//======================================= TICKER LOOPS ==========================================
void SENSOR_LOOP()
{
    flag_s = true;
}

void MOTOR_LOOP()
{
    flag_m = true;
}

void samplego()                                       // ticker function, set flag to true every sample interval
{
    sample_go = 1;
}


//================================================================================================== HEAD LOOP ================================================================================================================
int main()
{

    pc.baud(115200);
    Sensor_control.attach(&SENSOR_LOOP, 0.01);                              // TICKER FUNCTION
    Motor_control.attach(&MOTOR_LOOP, 0.01);
    EMG_Control.attach(&samplego, (float)1/Fs);

    led_groen.write(0);
    led_geel.write(0);
    led_rood.write(0);

    pc.printf("===============================================================\n");
    pc.printf(" \t\t\t Ball-E\n");
    pc.printf("In the module Biorobotics on the University of Twente is this script created\n");
    pc.printf("Autors:\tE. Velu, L. Verhoeven, R. v/d Wal, T. v/d Wal, E. Zoetbrood\n\n");
    wait(3);
    pc.printf("The script will begin with a short calibration\n\n");
    wait(2.5);
    pc.printf("===============================================================\n");
   
    //************************* CONTROL 1-EMG CALIBRATION ****************************
    while(1) {

        if(sample_go) {
            red.write(led_off);                     // Toggles red calibration LED off
            green.write(led_on);                    // Toggles green sampling LED on
            hoog_laag_filter();
            sample_go = 0;
        }

        if (flag_calibration) {
            // function to calibrate the emg signals from the user. It takes 5 seconds of measurements of maximum output, then takes the max and normalizes to that.
            calibratie_metingen ++;                                      
            cali_fact1 = 0.9;                                          // calibration factor to normalize filter output to a scale of 0 - 1
            cali_fact2 = 0.9;
            double cali_max1 = 0;                                   // declare max y1
            double cali_max2 = 0;                                   //declare max y2
            pc.printf(" \n\n EMG Signal starting up Calibration measurement........... \n");
            wait(2);
            led_rood.write(0);
            wait(0.2);
            led_rood.write(1);                                      //Toggles red calibration LED on
            wait(0.2);
            led_rood.write(0);
            wait(0.2);
            led_rood.write(1);
            wait(0.2);
            led_rood.write(0);
            wait(0.2);
            led_rood.write(1);
            wait(1);
            
            pc.printf("\t.....Calibrating Signal of EMG 1 and 2 .......\n");
            led_rood.write(0);
            led_geel.write(1);
            wait(0.5);
            pc.printf("\t......contract muscles..... \n");

            for(int cali_index1 = 0; cali_index1 < 2560; cali_index1++) { // Records 5 seconds of y1
                hoog_laag_filter();
                cali_array1[cali_index1] = y1;
                wait((float)1/Fs);
            }
            for(int cali_index2 = 0; cali_index2 < 2560; cali_index2++) { // Records 5 seconds of y2
                hoog_laag_filter();
                cali_array2[cali_index2] = y2;
                wait((float)1/Fs);
            }
            for(int cali_index3 = 0; cali_index3 < 2560; cali_index3++) { // Scales y1
                if(cali_array1[cali_index3] > cali_max1) {
                    cali_max1 = cali_array1[cali_index3];
                }
            }
            for(int cali_index4 = 0; cali_index4 < 2560; cali_index4++) { // Scales y2
                if(cali_array2[cali_index4] > cali_max2) {
                    cali_max2 = cali_array2[cali_index4];
                }
            }
            cali_fact1 = (double)1/cali_max1;                              // Calibration factor for y1
            cali_fact2 = (double)1/cali_max2;                              // Calibration factor for y2

            // Toggles green sampling LED off
            led_geel.write(0);
            pc.printf(" \t....... Calibration has been completed!\n");
            wait(0.2);
            led_groen.write(led_off);
            wait(0.2);
            led_groen.write(led_on);
            wait(0.2);
            led_groen.write(led_off);
            wait(0.2);
            led_groen.write(led_on);
            wait(4);
            pc.printf("Beginning with Ball-E board settings\n");
            led_groen.write(led_off);
            wait(2);
            y1 = 0;
            y2 = 0;

            j = 1;                                         // Wachten van fase switch initialiseren
            fase_switch_wait = true;
        }


        //************************* CONTROL MOTOR ****************************************



        if (flag_s) {
            Encoderread1 = wheel1.getPulses();
            pos_arm = (Encoderread1/((64.0*131.0)/360.0));   // Omrekenen naar graden van arm
            pos_arm1 = pos_arm;

            Encoderread2 = wheel2.getPulses();
            pos_board = (Encoderread2/((64.0*131.0)/360.0));   // Omrekenen naar graden van board
            pos_board1 = pos_board;
            
            flag_calibration = false;
        }
        //************************* FASE SWITCH ******************************************
        //******************** Fase determination *************
        if (fase_switch_wait == true) {
            j++;
            wait(0.01);                                              // Problemen met EMG metingen die te hoog zijn op het begin van script na calibratie. vandaar ff wachten en de sample loop een paar keer doorlopen.
            pc.printf("waarde j = %i \n",j);
            led_rood.write(0);
            led_groen.write(1);
            led_geel.write(0);
        }

        if( j > N) {
            fase_switch_wait = false;
            switch(fase) {

                    //******************* Fase 1 **************************
                case(1):
                        led_rood.write(1);
                        led_groen.write(0);
                        led_geel.write(0);
                    rondjes = 0;
                    if (y1> contract) {
                        flag_right = false;
                        flag_left = true;

                    }

                    if (y2 > contract) {
                        flag_right = true;
                        flag_left = false;
                      
                    }

                    if (pos_board < maxleft) {
                        flag_left = false;
                        motor2speed.write(relax);
                    
                    }

                    if (pos_board > maxright) {
                        flag_right = false;
                        motor2speed.write(relax);
                    }

                    if (flag_left == true) {
                        if (y1> contract) {
                            motor2direction.write(ccw);
                            motor2speed.write(speed_plate_1);
                        } else {
                            motor2speed.write(relax);
                        }
                    }

                    if (flag_right == true) {
                        if (y2 > contract) {
                            motor2direction.write(cw);
                            motor2speed.write(speed_plate_1);
                        } else {
                            motor2speed.write(relax);
                        }
                    }
                    pc.printf("Boardposition \t %f  EMG1 en EMG2 signaal = %f \t %f\n", pos_board, y1, y2);
                    if (y1> fasecontract && y2 > fasecontract) {
                        motor2speed.write(relax);
                        fase = 2;
                        fase_switch_wait = true;
                        led_rood.write(0);
                        led_groen.write(0);
                        led_geel.write(1);
                        j = 0;
                    }
                    break;
                    //******************* Fase 2 **************************
                case(2):
                led_rood.write(0);
                led_groen.write(0);
                led_geel.write(1);
                    motor1direction.write(cw);
                    pos_arm1 = (pos_arm - (rondjes * 360));

                    switch(switch_rondjes) {
                        case(1):
                            rondjes ++;
                            switch_rondjes = 2;
                            break;
                        case(2):
                            if(pos_arm1>360 & 368<pos_arm1) {
                                switch_rondjes = 1;
                            }
                            break;
                    }


                    if (y2 > minimum_contract & y2 < medium_contract) {
                        motor1speed.write((y2 - minimum_contract)/(medium_contract - minimum_contract));
                    }
                    if (y2 > medium_contract) {               // Hoger dan drempelwaarde = Actief
                        motor1speed.write(1);
                    }
                    if (y2 < minimum_contract) {              // Lager dan drempel = Rust
                        motor1speed.write(relax);
                    }

                    if( rondjes == pos_armrondjes_max) {                    // max aantal draaing gemaakt!!!!!!
                        problem1 = true;
                        problem2 = true;
                        motor1speed.write(relax);
                        
                          while (problem1) {
                            j++;
                            wait(0.01);                                              // Problemen met EMG metingen die te hoog zijn op het begin van script na calibratie. vandaar ff wachten en de sample loop een paar keer doorlopen.
                            Encoderread1 = wheel1.getPulses();
                            pos_arm = (Encoderread1/((64.0*131.0)/360.0));   // Omrekenen naar graden van arm

                        if( j > N) {
                            problem1 = false;
                            }
                            }
                            
                        wait(0.1);
                        led_rood.write(0);
                        wait(0.1);
                        led_rood.write(1);
                        wait(0.1);
                        led_rood.write(0);
                        wait(0.1);
                        led_rood.write(1);
                        wait(0.1);
                        led_rood.write(0);
                        wait(0.1);
                        led_rood.write(1);
                        wait(1.5);

                    while(problem2) {
                            motor1direction.write(ccw);
                            motor1speed.write(0.02);
                            
                            if (flag_s) {
                                Encoderread1 = wheel1.getPulses();
                                pos_arm = (Encoderread1/((64.0*131.0)/360.0));   // Omrekenen naar graden van arm
                            }
                            if (pos_arm < 10) {
                                
                                problem2 = false;
                                motor1speed.write(0);
                                rondjes = 0;
                                wait(1);
                            }
                        }
                    }
                    if (pos_arm1 > minimum_throw_angle & pos_arm1 < maximum_throw_angle) {
                        if (y1> maximum_leftbiceps) {
                            magnet.write(off);
                            motor1speed.write(relax);
                            fase = 3;
                            pc.printf("Van fase 2 naar fase 3\n");
                            
                            wait(2);
                            j = 0;
                            fase_switch_wait = true;
                            
                        }
                    }
                    pc.printf("Armposition \t %i \t EMG1 en EMG2 = %f \t %f \n", pos_arm1, y1, y2);
                    break;
                    //********************************************* Fase 3 **********************************************
                case(3):
                        led_rood.write(0);
                        led_groen.write(1);
                        led_geel.write(0);
                    switch(switch_reset) {
                        case(1):
                            if(pos_arm < reset_positie) {                   //ene kant op draaien
                                motor1direction.write(cw);
                                speedcompensation2 = 0.05;//((reset_arm - pos_arm1)/900.0 + (0.02));
                                motor1speed.write(speedcompensation2);
                            }
                            if(pos_arm > reset_positie)                  //andere kant op
                        {
                            motor1direction.write(ccw);
                            speedcompensation2 = 0.05;//((pos_arm1 - reset_arm)/500.0 + (0.02));
                            motor1speed.write(speedcompensation2);
                        }
                        if(pos_arm < reset_positie+5 && pos_arm > reset_positie-5)                    // Dit gaat niet goed komen, omdat het precies die waarde moet zijn
                        {
                            motor1speed.write(0);
                            arm = true;
                            switch_reset = 2;
                        }
                            pc.printf("Positie_arm = %f \t \t snelheid = %f \n",pos_arm, speedcompensation);
                            wait(0.0001);
                            break;

                        case(2):
                            pc.printf("\t switch magneet automatisch \n");
                            wait(0.2);
                            magnet.write(on);
                            wait(2);
                            switch_reset = 3;
                            break;
                                    
                        case(3):
                            if(pos_board < reset_board) {
                                motor2direction.write(cw);
                                speedcompensation = 0.05; //((reset_board - pos_board1)/500.0 + (0.1));
                                motor2speed.write(speedcompensation);
                            }

                            if(pos_board > reset_board) {
                                motor2direction.write(ccw);
                                speedcompensation = 0.05;//((pos_board1 - reset_board)/500.0 + (0.05));
                                motor2speed.write(speedcompensation);
                            }
                            if(pos_board < reset_board+5 && pos_board > reset_board-5) {
                                motor2speed.write(0);
                                board1 = true;
                            }
                            if(board1 == true) {
                                red=0;
                                pc.printf("fase switch  naar 1\n\n");
                                board1 = false;
                                arm = false;
                                //  flag_calibration = true;           !!!!!                Moet naar gekeken worden of we elke spel willen calibreren
                                wait(2);
                                games_played ++;
                                games_played1 = games_played - (3*calibratie_metingen - 3);
                                pc.printf("Games played total count = %i and loop %i\n",games_played,games_played1);

                                if(games_played1 == 3) {

                                    flag_calibration = true;
                                    while(t) {
                                        pc.printf("\tCalibratie begint over %i \n",t);
                                        t--;
                                        led_geel.write(1);
                                        wait(0.5);
                                        led_geel.write(0);
                                        wait(0.5);
                                    }
                                }
                                while(t) {
                                    pc.printf("\tNieuw spel begint over %i \n",t);
                                    t--;
                                    led_geel.write(1);
                                    wait(0.5);
                                    led_geel.write(0);
                                    wait(0.5);
                                }

                                fase = 1;                                                           // Terug naar fase 1
                                switch_reset = 1;                                                   // De switch op orginele locatie zetten
                                t = 5;

                            }

                            pc.printf("Positie_board = %f \t \t snelheid = %f \n",pos_board, speedcompensation);
                            wait(0.0001);
                            break;
                    }
                    break;
                    //=================================================== STOP SCRIPT ============================================================
            }
        }
    }
}