File content as of revision 10:83f3cec8dd1c:

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

DigitalIn Power_button_pressed(D1);
DigitalIn Emergency_button_pressed(D2);

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

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

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
                pc.printf("Motors off state\r\n");
                stateChanged = false;
            }          
            if  (Power_button_pressed.read() == false)
            {      
                stateChanged = true;
                pc.printf("Moving to Calib_motor state\r\n");
                motors_on_off();
                currentState = Calib_motor;
            }
            if  (Emergency_button_pressed.read() == false) 
            { 
                pc.printf("Ik ga exploderen!!!\r\n");
                loop_ticker.detach(); 
                emergency();
                // Alles moet uitgaan (evt. een rood LEDje laten branden), moet 
                // opnieuw worden opgestart. Mogelijk kan dit door de ticker de 
                // detachen
            } 
            break;
            
        case Calib_motor:
            
            if (stateChanged)
            {       
                // Hier wordt een kalibratie uitgevoerd, waarbij de motorhoeken worden bepaald
                pc.printf("Moving to Calib_EMG state\r\n");
            }    
           
            if  (Emergency_button_pressed.read() == false) 
            { 
                pc.printf("Ik ga exploderen!!!\r\n");
                loop_ticker.detach(); 
                emergency();
                // 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!\r\n");
            
        }
    }

int main(void)
    {
        pc.printf("Opstarten\r\n");
        loop_ticker.attach(&ProcessStateMachine, 3.0f);  
        while(true) 
        { 
            // pc.printf("powerbutton: %d\r\n", Power_button_pressed.read());
            // pc.printf("emergencybutton: %d\r\n", Emergency_button_pressed.read());
            // wait(0.5);
            /* do nothing */}
    }