File content as of revision 15:ad065ab92d11:

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

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

// 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
                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");
        loop_ticker.attach(&ProcessStateMachine, 5.0f);  
        while(true) 
        { /* do nothing */}
    }