File content as of revision 43:2f946b617d62:

#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "FastPWM.h"
#include <math.h>
#include "Servo.h"

// Definieer objecten
Serial pc(USBTX, USBRX);

PwmOut motor1(D6);  // Misschien moeten we hiervoor DigitalOut gebruiken, moet 
PwmOut motor2(D5);  // samen kunnen gaan met de servo motor

DigitalOut motor1_dir(D7);
DigitalOut motor2_dir(D4);
  
DigitalIn Power_button_pressed(D1); // Geen InterruptIn gebruiken!
DigitalIn Emergency_button_pressed(D2);  

AnalogIn EMG_biceps_right_raw (A0);
AnalogIn EMG_biceps_left_raw (A1);
AnalogIn EMG_calf_raw (A2);

Ticker loop_ticker;
Ticker HIDScope_ticker;
Ticker emgSampleTicker;

HIDScope scope(3);

BiQuadChain bqc; // Let op !!! Deze coëfficiënten moeten nog worden veranderd
BiQuad bq1(0.0030, 0.0059, 0.0030, -1.8404,0.8522); //voor low-pass
BiQuad bq2(0.9737, -1.9474, 0.9737, -1.9467, 0.9481); //voor high-pass
BiQuad bq3(0.9912, -1.9823,0.9912, -1.9822, 0.9824); //lage piek eruit-> voor coëfficienten, zie matlab

bool calib = false; // MOGELIJK GAAT HET HIER FOUT
int i_calib = 0;

void emgSampleFilter() // Deze functie wordt aangeroepen dmv een ticker. Het sampled 
                       // hierdoor het EMG signaal en het haalt er een filter overheen
{   
    float filtered_EMG_biceps_right=bqc.step(EMG_biceps_right_raw.read());
    float filtered_EMG_biceps_left=bqc.step(EMG_biceps_left_raw.read());
    float filtered_EMG_calf=bqc.step(EMG_calf_raw.read());
    
    float filtered_EMG_biceps_right_total=filtered_EMG_biceps_right;
    float filtered_EMG_biceps_left_total=filtered_EMG_biceps_left;
    float filtered_EMG_calf_total=filtered_EMG_calf;
    
    if (calib)
        {   
            if (i_calib < 500)
                {
                    filtered_EMG_biceps_right_total=filtered_EMG_biceps_right+filtered_EMG_biceps_right_total;
                    filtered_EMG_biceps_left_total=filtered_EMG_biceps_left+filtered_EMG_biceps_left_total;
                    filtered_EMG_calf_total=filtered_EMG_calf+filtered_EMG_calf_total;
                    i_calib++;
                }
            if (i_calib >= 500)
                {   
                    mean_EMG_biceps_right=filtered_EMG_biceps_right_total/500;
                    mean_EMG_biceps_left=filtered_EMG_biceps_left_total/500;
                    mean_EMG_calf=filtered_EMG_calf_total/500;
                    calib = false; 
                }
         }
}

void sendHIDScope() // Deze functie geeft de gefilterde EMG-signalen weer in de HIDScope
                    // Wordt eveneens gerund dmv een ticker
{
    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
    scope.set(0, filtered_EMG_biceps_right() ); // Werkt dit zo? Of nog met .read?
    scope.set(1, filtered_EMG_biceps_left() );
    scope.set(2, filtered_EMG_calf() );
    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
    *  Ensure that enough channels are available (HIDScope scope( 2 ))
    *  Finally, send all channels to the PC at once */
    scope.send();
    // Eventueel nog een ledje laten branden
}

// Emergency    
void emergency()
    {
        loop_ticker.detach(); 
        motor1.write(0);
        motor2.write(0);
        pc.printf("Ik ga exploderen!!!\r\n");
        // Alles moet uitgaan (evt. een rood LEDje laten branden), moet 
        // opnieuw worden opgestart. Mogelijk kan dit door de ticker te 
        // detachen
    }    

// Motoren uitzetten    
void motors_off()
    {
        motor1.write(0);
        motor2.write(0);
        pc.printf("Motoren uit functie\r\n");
    }  
    
// Motoren aanzetten
void motors_on()
    {
        motor1.write(0.9);
        motor1_dir.write(1);
        motor2.write(0.1);
        motor1_dir.write(1);
        pc.printf("Motoren aan functie\r\n");
    }
        
// EMG kalibreren        
void  emg_calibration()   
     {
        // Gedurende bijv. 5 seconden EMG meten, wanneer de spieren maximaal
        // worden aangespannen -> maximaal potentiaal verkrijgen. Een fractie 
        // hiervan kan als drempel worden gebruikt voor beweging
        
        // *Tijd instellen* 
        // Iets met DOUBLE_MAX? https://docs.microsoft.com/en-us/cpp/c-language/cpp-integer-limits?view=vs-2019
        
        // Ledje van kleur laten veranderen
        
        // MOGELIJK NIET MEER NODIG???
       
     }

// Finite state machine programming (calibration servo motor?)
enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};

states currentState = Motors_off; 
bool stateChanged = true; // Make sure the initialization of first state is executed

void ProcessStateMachine(void) 
{   
    switch (currentState) 
    { 
        case Motors_off:
        
            if (stateChanged) 
            {
                motors_off(); // functie waarbij motoren uitgaan
                stateChanged = false;
                pc.printf("Motors off state\r\n");
            }          
            if  (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
            {      
                motors_on();
                currentState = Calib_motor;
                stateChanged = true;
                pc.printf("Moving to Calib_motor state\r\n");
            }
            if  (Emergency_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
            { 
                emergency();
            } 
            break;
            
        case Calib_motor:
            
            if (stateChanged)
            {       
                // Hier wordt een kalibratie uitgevoerd, waarbij de motorhoeken worden bepaald
                currentState = Calib_EMG;
                stateChanged = true;
                pc.printf("Moving to Calib_EMG state\r\n");
            }    
            if  (Emergency_button_pressed.read() == false) 
            { 
                emergency();
            } 
            break;
            
         case Calib_EMG:
            
            motors_off();
            if (stateChanged)
            {
                // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald
                calib = true;
                emgSampleFilter() // Gaat dit nu goed? -> moet sws worden toegevoegd bij relevante onderdelen?
                if (i_calib >= 500) // of wait(10);?
                {
                    currentState = Homing;
                    stateChanged = true;
                    pc.printf("Moving to Homing state\r\n");
                }
            if  (Emergency_button_pressed.read() == false) 
            { 
                emergency();
            } 
            break; 
            
         case Homing:
            
            motors_on();
            if (stateChanged)
            {   
                // Ervoor zorgen dat de motoren zo bewegen dat de robotarm 
                // (inclusief de end-effector) in de juiste home positie wordt gezet
                currentState = Operation_mode;
                stateChanged = true;
                pc.printf("Moving to operation mode \r\n");
            }
            if  (Emergency_button_pressed.read() == false) 
            { 
                emergency();
            } 
            break; 
            
         case Operation_mode: // Overgaan tot emergency wanneer referentie niet 
                              // overeenkomt met werkelijkheid
        
            if (stateChanged)
             
                // Hier moet een functie worden aangeroepen die ervoor zorgt dat 
                // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd, 
                // zodat de robotarm kan bewegen   
                
                if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
                {
                    motors_off();
                    currentState = Motors_off;
                    stateChanged = true;
                    pc.printf("Terug naar de state Motors_off\r\n");
                }
                if  (Emergency_button_pressed.read() == false) 
                { 
                    emergency();
                } 
                // wait(5);
                else 
                { 
                    currentState = Homing; 
                    stateChanged = true; 
                    pc.printf("Terug naar de state Homing\r\n");
                }
                break;
                     
        default:
            // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
            motors_off();
            pc.printf("Unknown or uninplemented state reached!\r\n");
            
    }
}

int main(void)
    {
        pc.printf("Opstarten\r\n");
        bqc.add(&bq1).add(&bq2).add(&bq3);
        emgSampleTicker.attach(&emgSampleFilter, 0.01f);
        HIDScope_ticker.attach(&sendHIDScope, 0.01f);
        loop_ticker.attach(&ProcessStateMachine, 5.0f);  
        while(true) 
        { /* do nothing */}
    }