File content as of revision 6:64146e16e10c:

#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, No_mov_before_pick_up, Servo_horizontal, Position_1, Slide, Lift, No_mov_before_drop_off, Position_drop_off, Servo_tilt_down, Position_2};

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 // (Motor_calib_button_pressed()) nog een extra button toevoegen voordat dit kan
            {      
                currentState = Calib_motor;
                stateChanged = true;
                pc.printf("Moving to Calib_motor state\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");
            }       
            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");
            }
            break;
            
        case Homing:
            
            if (stateChanged)
            {   
                // Ervoor zorgen dat de motoren zo bewegen dat de robotarm in de juiste home positie wordt gezet
                currentState = No_mov_before_pick_up;
                stateChanged = true;
                pc.printf("Moving to no movement before pick up state \n");
            }
            break;
            
        case No_mov_before_pick_up: // De robot arm beweegt niet, soort ruststand na de homing
        
            if (stateChanged)
            {
                if (// eis om naar positie 1 te gaan)
                {
                currentState = Position_1
                stateChanged = true;
                pc.printf("Moving to position_1 state\n");
                }
                
                if (// eis om naar positie 2 te gaan)
                {
                currentState = Position_2;
                stateChanged = true;
                pc.printf("Moving to position_2 state\n");
                }
                
                if (// eis om naar drop off positie te gaan, als het eraf glijden mislukt is)
                {
                currentState = Position_drop_off;
                stateChanged = true;
                pc.printf("Moving to position_drop_off state\n");
                }
                
                if (// eis voor nieuwe poging om home positie juist te bereiken)
                {
                currentState = Homing; // Mogelijk de state Calib_motor
                stateChanged = true;
                pc.printf("Retry homing\n");
                }
                
                if (// eis om de motoren uit te zetten)
                {
                currentState = Motors_off
                stateChanged = true;
                pc.printf("Shutting down\n");
                }
            }
            break;
            
        case Position_1:
        
            if (stateChanged)
            { 
                // Ervoor zorgen dat de end-effector 
             
        
    
           if (stateChanged)
            {   
                // Ervoor zorgen dat de motoren zo bewegen dat de robotarm in de juiste home positie wordt gezet
                currentState = No_mov_before_pick_up;
                stateChanged = true;
                pc.printf("Moving to no movement before pick up state \n");
            }
            break;
             
                

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