File content as of revision 7:1d57463393c6:

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

//Servo myservo(D8);

Ticker myControllerTicker1;
Ticker myControllerTicker2;

AnalogIn potMeter1(A1);
AnalogIn potMeter2(A0);

PwmOut motor1(D6);  
PwmOut motor2(D5);  

DigitalOut motor1_dir(D7);
DigitalOut motor2_dir(D4);


InterruptIn button1(D1);
InterruptIn button2(D2);

//richting wisselen van motor 1
void direction1(void)
    {
        motor1_dir=!motor1_dir;
    }

//richting wisselen van motor 2
void direction2(void)
    {
        motor2_dir=!motor2_dir;
    }


//snelheid motor 1 aansturen
void motor1Controller( )
    {
    // Determine reference (desired) fan speed
        double y1_des = potMeter1.read();
    // Controller (calculate motor torque/pwm)
        if( y1_des > 1 ) y1_des = 1; // y1_des must be <= 1
        if( y1_des < 0 ) y1_des = 0; // y1_des must be >= 0
        double power1 = pow(y1_des, 2.0); // Inverse relation between input and output
    // Send to motor
        motor1.write( power1 );
        pc.printf("power1: %.2f\t",power1);
    }

//snelheid motor 2 aansturen
void motor2Controller( )
    {
    // Determine reference (desired) fan speed
        double y2_des = potMeter2.read();
    // Controller (calculate motor torque/pwm)
        if( y2_des > 1 ) y2_des = 1; // y2_des must be <= 1
        if( y2_des < 0 ) y2_des = 0; // y2_des must be >= 0
        double power2 = pow(y2_des, 2.0); // Inverse relation between input and output
    // Send to motor
        motor2.write( power2 );
        pc.printf("power2: %.2f\n\r",power2);
    }

// Finite state machine programming (calibration servo motor?)
enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode, 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)
            {
                // functie toevoegen waarbij motoren uitgaan
                stateChanged = false;
                pc.printf("Motors off\n");
            }            
            if // (Power_button_pressed()) nog een button toevoegen voordat dit kan
            {      
                currentState = Calib_motor;
                stateChanged = true;
                pc.printf("Moving to Calib_motor state\n");
            }
            if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
            {
                currentState = Failure_mode;
                stateChanged = true;
                pc.printf("Moving to failure_mode\n");
            }
                
            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\n");
            }   
            if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
            {
                currentState = Failure_mode;
                stateChanged = true;
                pc.printf("Moving to failure_mode\n");
            }
                
            break;
            
        case Calib_EMG:
            
            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\n");
            }
            if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
            {
                currentState = Failure_mode;
                stateChanged = true;
                pc.printf("Moving to failure_mode\n");
            }
            
            break;
            
        case Homing:
            
            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\n");
            }
            if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
            {
                currentState = Failure_mode;
                stateChanged = true;
                pc.printf("Moving to failure_mode\n");
            }   
            break;
                  
        case Operation_mode:
        
            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())
                {
                    currentState = Motors_off;
                    stateChanged = true;
                }
                if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
                {
                    currentState = Failure_mode;
                    stateChanged = true;
                    pc.printf("Moving to failure_mode\n");     
                }
                else 
                { 
                    currentState = Homing; 
                    stateChanged = true; 
                }
                break;
                
        case Failure_mode:
        
            if (stateChanged)
            { 
                pc.printf("Ik ga exploderen!!!");
                // 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()
    {
      //  for(float p=0; p<1.0; p += 0.1)
        //    {
        //        myservo == p;
         //       wait(0.2);
         //   }
        myControllerTicker1.attach(motor1Controller, 0.1 ); // Every 1/10 second
        myControllerTicker2.attach(motor2Controller, 0.1 ); // Every 1/10 second

        button1.mode(PullUp);   
        button1.rise(direction1);
        button2.mode(PullUp);   
        button2.rise(direction2);


        while( true ) { /* do nothing */ }
    return 0;
    }