File content as of revision 8:c7d3b67346db:

#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);  
PwmOut motor2(D5);  

InterruptIn Power_button_pressed(D1);
InterruptIn Emergency_button_pressed(D2);

Ticker loop_ticker;
    
// Motoren uitzetten
void motors_on_off()
    {
          motor1=!motor1;
          motor2=!motor2;
          pc.printf("Motoren staan aan/uit");
    }    
    
void emergency()
    {
        motor1.write(0);
        motor2.write(0);
        pc.printf("Noodgeval");
    }    
    
void motors_off()
    {
        motor1.write(0);
        motor2.write(0);
        pc.printf("Motoren staan uit");
    }  
    
    

// Finite state machine programming (calibration servo motor?)
enum states {Motors_off, Failure_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\n");
            }            
            if  (Power_button_pressed)
            {      
                Power_button_pressed.mode(PullUp); 
                Power_button_pressed.rise(motors_on_off); // functie waarbij de motoren aan-/ uitgaan 
                stateChanged = true;
                pc.printf("Moving to Calib_motor state\n");
            }
            if  (Emergency_button_pressed) 
            { 
                Emergency_button_pressed.mode(PullUp); 
                Emergency_button_pressed.rise(emergency); // functie die de motoren uitzet
                currentState = Failure_mode;
                stateChanged = true;
                pc.printf("Moving to failure_mode\n");
            }
                
            break;
                
        case Failure_mode:
        
            if (stateChanged)
            { 
                pc.printf("Ik ga exploderen!!!");
                loop_ticker.detach(); 
                // Alles moet uitgaan (evt. een rood LEDje laten branden), moet 
                // opnieuw worden opgestart. Mogelijk kan dit door de ticker de 
                // detachen
            }
            break;
            
        default:
            // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
            pc.printf("Unknown or uninplemented state reached!\n");
            
        }
    }

int main(void)
    {
        loop_ticker.attach(&ProcessStateMachine, 2.0f);  
        while(true) 
        { /* do nothing */ ;}
    }