File content as of revision 20:a6a5bdd7d118:

#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;

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

HIDScope scope(3);

float mean_EMG_biceps_right;
float mean_EMG_biceps_left;
float mean_EMG_calf;
float normalized_EMG_biceps_right;
float filtered_EMG_biceps_right;
float normalized_EMG_biceps_left;
float filtered_EMG_biceps_left;
float normalized_EMG_calf;
float filtered_EMG_calf;

// Definities voor eerste BiQuadChain (High-pass en Notch)
BiQuadChain bqc; 
BiQuad bq1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass 
BiQuad bq2(1, -1.6180, 1, -1.6019, 0.9801); // Notch

// Na het nemen van de absolute waarde moet de tweede BiQuadChain worden toegepast.
// Definieer (twee Low-pass -> vierde orde verkrijgen):
BiQuadChain bqc2;
BiQuad bq3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
BiQuad bq4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass

void emgSampleFilter() // Deze functie wordt aangeroepen dmv een ticker. Het sampled 
                       // hierdoor het EMG signaal en het haalt er een filter overheen.
                       // Het signaal kan tevens via de HIDScope worden gevolgd. 
                       // Tenslotte wordt er een stuk script gerund, wanneer de robot
                       // zich in de kalibratie toestand bevindt. 
{   
    float filtered_EMG_biceps_right_1=bqc.step(EMG_biceps_right_raw.read());
    float filtered_EMG_biceps_left_1=bqc.step(EMG_biceps_left_raw.read());
    float filtered_EMG_calf_1=bqc.step(EMG_calf_raw.read());
    
    float filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
    float filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
    float filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
    
    float filtered_EMG_biceps_right=bqc2.step(filtered_EMG_biceps_right_abs);
    float filtered_EMG_biceps_left=bqc2.step(filtered_EMG_biceps_left_abs);
    float filtered_EMG_calf=bqc2.step(filtered_EMG_calf_abs);
    
    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;
    
    //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
//}
    
    if (calib) // In de kalibratie staat treedt deze loop in werking. De spier wordt
               // dan maximaal aangespannen (gedurende 5 seconden). De EMG waarden
               // worden bij elkaar opgeteld, waarna het gemiddelde wordt bepaald.
        {   
            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)
                {   
                    float mean_EMG_biceps_right=filtered_EMG_biceps_right_total/500;
                    float mean_EMG_biceps_left=filtered_EMG_biceps_left_total/500;
                    float mean_EMG_calf=filtered_EMG_calf_total/500;
                    calib = false; 
                }
         }
}

// 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");
                }

                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   
                
                {
                    float normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
                    float normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
                    float normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
                    
                    if (normalized_EMG_biceps_right >= 0.3)
                        {
                            motor1.write(0.5);
                            motor2.write(0);
                        }
                    if (normalized_EMG_biceps_right < 0.3)
                        {       
                            motor1.write(0);
                            motor2.write(0);
                        }
                    if (normalized_EMG_biceps_left >= 0.3)
                        {
                            motor2.write(0.9);
                            motor1.write(0);
                        }
                    if (normalized_EMG_biceps_left < 0.3)
                        {
                            motor2.write(0);
                            motor1.write(0);
                        }              
                    
                    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);
        bqc2.add(&bq3).add(&bq4);
        emgSampleTicker.attach(&emgSampleFilter, 0.01f);
        loop_ticker.attach(&ProcessStateMachine, 5.0f);  
        while(true) 
        { /* do nothing */}
    }