File content as of revision 23:4572750a5c59:

#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "FastPWM.h"
#define M_PI 3.14159265358979323846  /* pi */
#include <math.h>
#include "Servo.h"
#include <cmath>

Serial pc(USBTX, USBRX);

// TICKERS
Ticker loop_ticker;

// BENODIGD VOOR PROCESS STATE MACHINE
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

// INPUTS
DigitalIn Power_button_pressed(D1); // Geen InterruptIn gebruiken!
DigitalIn Emergency_button_pressed(D2);
DigitalIn Motor_calib_button_pressed(SW2);

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

QEI Encoder1(D12, D13, NC, 8400, QEI::X4_ENCODING); //Checken of die D12, D9 etc wel kloppen, 8400= gear ratio x 64
QEI Encoder2(D9, D10, NC, 8400, QEI::X4_ENCODING);  

// OUTPUTS
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);

// VARIABELEN VOOR ENCODER, MOTORHOEK ETC.
int counts1;
int counts2;
const int CPR = 64; // Counts per revolution
const int full_degrees = 360;
const int half_degrees = 180;
double theta_h_1_deg;
double theta_h_2_deg;
double theta_h_1_rad;
double theta_h_2_rad;

// DEFINITIES VOOR FILTERS

// BICEPS-RECHTS
// Definities voor eerste BiQuadChain (High-pass en Notch) 
BiQuadChain bqcbr; 
BiQuad bqbr1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass 
BiQuad bqbr2(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 bqcbr2;
BiQuad bqbr3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
BiQuad bqbr4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass

// BICEPS-LINKS
// Definities voor eerste BiQuadChain (High-pass en Notch) 
BiQuadChain bqcbl; 
BiQuad bqbl1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass 
BiQuad bqbl2(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 bqcbl2;
BiQuad bqbl3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
BiQuad bqbl4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass

// KUIT
// Definities voor eerste BiQuadChain (High-pass en Notch) 
BiQuadChain bqck; 
BiQuad bqk1(0.8006, -1.6012, 0.8006, -1.5610, 0.6414); // High-pass 
BiQuad bqk2(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 bqck2;
BiQuad bqk3(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass
BiQuad bqk4(1.5515e-4, 3.1030e-4, 1.5515e-4, -1.9645, 0.9651); // Low-pass

// VARIABELEN VOOR EMG + FILTEREN 
double filtered_EMG_biceps_right;
double filtered_EMG_biceps_left;
double filtered_EMG_calf;

double filtered_EMG_biceps_right_1;
double filtered_EMG_biceps_left_1;
double filtered_EMG_calf_1;

double filtered_EMG_biceps_right_abs;
double filtered_EMG_biceps_left_abs;
double filtered_EMG_calf_abs;

double filtered_EMG_biceps_right_total; 
double filtered_EMG_biceps_left_total;
double filtered_EMG_calf_total;

// Variabelen voor HIDScope
HIDScope scope(3);

// VARIABELEN VOOR (INITIATIE VAN) EMG KALIBRATIE LOOP
bool calib = false; 
static int i_calib = 0;

double mean_EMG_biceps_right;
double mean_EMG_biceps_left;
double mean_EMG_calf;

// VARIABELEN VOOR OPERATION MODE
double normalized_EMG_biceps_right;
double normalized_EMG_biceps_left;
double normalized_EMG_calf;

// VOIDS

// Noodfunctie waarbij alles uitgaat (evt. nog een rood LEDje laten branden).
// Enige optie is resetten, dan wordt het script opnieuw opgestart.    
void emergency()
    {
        loop_ticker.detach(); 
        motor1.write(0);
        motor2.write(0);
        pc.printf("Ik ga exploderen!!!\r\n");
    }    

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

// Finite state machine programming (calibration servo motor?)
void ProcessStateMachine(void) 
{   
    // Berekenen van de motorhoeken (in radialen)
    counts1 = Encoder1.getPulses(); 
    counts2 = Encoder2.getPulses(); 
    theta_h_1_deg=(counts1/(double)CPR)*(double)full_degrees;
    theta_h_2_deg=(counts2/(double)CPR)*(double)full_degrees;
    theta_h_1_rad=(theta_h_1_deg/half_degrees)*M_PI;
    theta_h_2_rad=(theta_h_2_deg/half_degrees)*M_PI;
    
    // Eerste deel van de filters (High-pass + Notch) over het ruwe EMG signaal
    // doen. Het ruwe signaal wordt gelezen binnen een ticker en wordt daardoor 'gesampled' 
    filtered_EMG_biceps_right_1=bqbr1.step(EMG_biceps_right_raw.read());
    filtered_EMG_biceps_left_1=bqcbl.step(EMG_biceps_left_raw.read());
    filtered_EMG_calf_1=bqck.step(EMG_calf_raw.read());
    
    // Vervolgens wordt de absolute waarde hiervan genomen
    filtered_EMG_biceps_right_abs=abs(filtered_EMG_biceps_right_1);
    filtered_EMG_biceps_left_abs=abs(filtered_EMG_biceps_left_1);
    filtered_EMG_calf_abs=abs(filtered_EMG_calf_1);
    
    // Tenslotte wordt het tweede deel van de filters (twee low-pass, voor 4e orde filter)
    // over het signaal gedaan
    filtered_EMG_biceps_right=bqcbr2.step(filtered_EMG_biceps_right_abs);
    filtered_EMG_biceps_left=bqcbl2.step(filtered_EMG_biceps_left_abs);
    filtered_EMG_calf=bqck2.step(filtered_EMG_calf_abs);
    
    // De gefilterde EMG-signalen kunnen tevens visueel worden weergegeven in de HIDScope  
    scope.set(0, filtered_EMG_biceps_right); 
    scope.set(1, normalized_EMG_biceps_right);
    scope.set(2, filtered_EMG_calf);
    scope.send();
    
    // Tijdens de kalibratie moet vervolgens een maximale spierspanning worden bepaald, die 
    // later kan worden gebruikt voor een normalisatie. De spieren worden hiertoe gedurende 
    // 5 seconden maximaal aangespannen. De EMG waarden worden bij elkaar opgeteld,
    // waarna het gemiddelde wordt bepaald. 
    if (calib) 
        {   
            if (i_calib == 0)
                {
                    filtered_EMG_biceps_right_total=0;
                    filtered_EMG_biceps_left_total=0;
                    filtered_EMG_calf_total=0;
                }
            if (i_calib <= 2500)
                {
                    filtered_EMG_biceps_right_total+=filtered_EMG_biceps_right;
                    filtered_EMG_biceps_left_total+=filtered_EMG_biceps_left;
                    filtered_EMG_calf_total+=filtered_EMG_calf;
                    i_calib++;
                }
            if (i_calib > 2500)
                {   
                    mean_EMG_biceps_right=filtered_EMG_biceps_right_total/2500.0;
                    mean_EMG_biceps_left=filtered_EMG_biceps_left_total/2500.0;
                    mean_EMG_calf=filtered_EMG_calf_total/2500.0;
                    pc.printf("Ontspan spieren\r\n");
                    pc.printf("Rechterbiceps_max = %f, Linkerbiceps_max = %f, Kuit_max = %f\r\n", mean_EMG_biceps_right, mean_EMG_biceps_left, mean_EMG_calf);
                    calib = false; 
                }
         }
    
    // Genormaliseerde EMG's berekenen
    normalized_EMG_biceps_right=filtered_EMG_biceps_right/mean_EMG_biceps_right;
    normalized_EMG_biceps_left=filtered_EMG_biceps_left/mean_EMG_biceps_left;
    normalized_EMG_calf=filtered_EMG_calf/mean_EMG_calf;
         
    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 && Motor_calib_button_pressed.read() == false)
            {       
                theta_h_1_rad = 0;
                theta_h_2_rad = 0;
                pc.printf("Huidige hoek in radialen motor 1:%f en motor 2: %f (moet 0 zijn) \r\n", theta_h_1_rad, theta_h_2_rad);
                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:
            
            if (stateChanged)
                {
                    motors_off();
                    i_calib = 0;
                    calib = true;
                    pc.printf("Span spieren aan\r\n");
                    stateChanged = false; 
                }    
                
            if (i_calib > 2500) 
                {
                    calib = false;
                    currentState = Homing;
                    stateChanged = true;
                    pc.printf("Moving to Homing state\r\n");
                }
            
            if  (Emergency_button_pressed.read() == false) 
            { 
                emergency();
            } 
            break; 
            
         case Homing: // NOG NAAR KIJKEN
            
            if (stateChanged)
            {   
                // Ervoor zorgen dat de motoren zo bewegen dat de robotarm 
                // (inclusief de end-effector) in de juiste home positie wordt gezet
                motors_on();
                currentState = Operation_mode;
                stateChanged = true;
                motors_off(); 
                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
            
            // pc.printf("normalized_EMG_biceps_right= %f, mean_EMG_biceps_right = %f, filtered_EMG_biceps_right = %f\r\n", normalized_EMG_biceps_right, mean_EMG_biceps_right, filtered_EMG_biceps_right);
            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 (normalized_EMG_biceps_right >= 0.3)
                        {
                            motor1.write(0.5);
                            motor1_dir.write(1);
                            motor2.write(0);
                            motor2_dir.write(1);
                            if (normalized_EMG_calf >= 0.3)
                                {
                                    motor1.write(0.1);
                                    motor1_dir = !motor1_dir;
                                }  
                            if (normalized_EMG_biceps_left >= 0.3)
                                {
                                    motor2.write(0.9);
                                    motor2_dir.write(1);
                                    motor1.write(0);
                                    motor1_dir.write(1);
                                    if (normalized_EMG_calf >= 0.3)
                                        {
                                            motor2.write(0.1);
                                            motor2_dir = !motor2_dir;
                                        }  
                                }
                        }
                    if (normalized_EMG_biceps_right < 0.3)
                        {       
                            motor1.write(0);
                            motor2.write(0);
                            if (normalized_EMG_calf >= 0.3)
                                {
                                    // motor1_dir = !motor1_dir;
                                    // pc.printf("Richting zou om moeten draaien");
                                    // motor2_dir = !motor2_dir;
                                }  
                            if (normalized_EMG_biceps_left >= 0.3)
                                {
                                    motor2.write(0.9);
                                    motor2_dir.write(1);
                                    motor1.write(0);
                                    motor1_dir.write(1);
                                    if (normalized_EMG_calf >= 0.3)
                                        {
                                            // motor1_dir = !motor1_dir;
                                            // pc.printf("Richting zou om moeten draaien");
                                            // motor2_dir = !motor2_dir;
                                        }  
                                }
                        }
                    if (normalized_EMG_biceps_left >= 0.3)
                        {
                            motor2.write(0.9);
                            motor2_dir.write(1);
                            motor1.write(0);
                            motor1_dir.write(1);
                            if (normalized_EMG_calf >= 0.3)
                                {
                                    // motor1_dir = !motor1_dir;
                                    // pc.printf("Richting zou om moeten draaien");
                                    // motor2_dir = !motor2_dir;
                                }  
                            if (normalized_EMG_biceps_right >= 0.3)
                                {
                                    motor1.write(0.5);
                                    motor1_dir.write(1);
                                    motor2.write(0);
                                    motor2_dir.write(1);
                                    if (normalized_EMG_calf >= 0.3)
                                        {
                                            // motor1_dir = !motor1_dir;
                                            // pc.printf("Richting zou om moeten draaien");
                                            // motor2_dir = !motor2_dir;
                                        }  
                                }
                        }
                    if (normalized_EMG_biceps_left < 0.3)
                        {
                            motor2.write(0);
                            motor1.write(0);
                            if (normalized_EMG_biceps_right >= 0.3)
                                {
                                    motor1.write(0.5);
                                    motor1_dir.write(1);
                                    motor2.write(0);
                                    motor2_dir.write(1);
                                    if (normalized_EMG_calf >= 0.3)
                                        {
                                            // motor1_dir = !motor1_dir;
                                            // pc.printf("Richting zou om moeten draaien");
                                            // motor2_dir = !motor2_dir;
                                        }  
                                }
                        }
                }
 
            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(25);
            // 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");
        
        // Chain voor rechter biceps
        bqcbr.add(&bqbr1).add(&bqbr2);
        bqcbr2.add(&bqbr3).add(&bqbr4);
        // Chain voor linker biceps
        bqcbl.add(&bqbl1).add(&bqbl2);
        bqcbl2.add(&bqbl3).add(&bqbl4);  
        // Chain voor kuit
        bqck.add(&bqk1).add(&bqk2);
        bqck2.add(&bqk3).add(&bqk4); 

        loop_ticker.attach(&ProcessStateMachine, 0.002f);  
        
        while(true) 
            { 
                // wait(0.2);
                /* do nothing */
            }
    }