File content as of revision 4:babe09a69296:

//Voor het toevoegen van een button:
#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "BiQuad.h"
#include "FastPWM.h"

// Algemeen
DigitalIn button3(SW3);  
DigitalIn button2(SW2); 
AnalogIn But2(A5);
AnalogIn But1(A3);

DigitalOut led1(LED_GREEN);
DigitalOut led2(LED_RED);
DigitalOut led3(LED_BLUE);

MODSERIAL pc(USBTX, USBRX);
Timer t;
Timer t2;
//Motoren
DigitalOut direction1(D4);
FastPWM pwmpin1(D5);              
FastPWM pwmpin2(D6);
//PwmOut pwmpin1(D5);
//PwmOut pwmpin2(D6);
DigitalOut direction2(D7);
volatile float pwm1;
volatile float pwm2;

//Encoder
QEI encoder1 (D10, D9, NC, 1200, QEI::X4_ENCODING);
QEI encoder2 (D13, D12, NC, 4800, QEI::X4_ENCODING);
double Pulses1;
double motor_position1;
double Pulses2;
double motor_position2;
double error1;
double u1;

//Pot meter
AnalogIn pot(A1);
AnalogIn pot0(A0);
float Pot2;
float Pot1;

//Ticker
Ticker Pwm;
Ticker PotRead;
Ticker Kdc;

//Servo
FastPWM myservo1(D3);
Ticker ServoTick;
float servo_position;
float Periodlength = 0.00006;
int counts = 0;

// EMG
float EMG1;       // Rotatie
float EMG2;       // Elleboog
float EMG3;       // Hand
float EMG4;       // Reverse
float Input1;       // Voor zonder EMG
float Input2;
int count = 0;

//Kinematica
double stap1;
double stap2;
double KPot;

float ElbowReference;
float Ellebooghoek1;
float Ellebooghoek2;
float Ellebooghoek3;
float Ellebooghoek4;

float PolsReference;
float Polshoek1;
float Polshoek2;
float Polshoek3;
float Polshoek4;

float Hoeknieuw1;
float Hoeknieuw2;

//Limiet in graden
float lowerlim1 = -900;
float upperlim1 = 900;
float lowerlim2 = -50;
float upperlim2 = 1500;   

// VARIABLES PID CONTROLLER
double Kp1 = 12.5;          
double Ki1 = 0;          
double Kd1 = 1;
double Kp2 = 12;          // Zonder arm: 6,0,1, met rotatie: 10, 0, 1
double Ki2 = 0;          
double Kd2 = 1;          
double Ts = 0.0005;      // Sample time in seconds

// Functies Kinematica
float Kinematics1(float EMG1)
{

    if (EMG1 > 0.45f) {
        stap1 = EMG1*450*Ts;
        Hoeknieuw1 = PolsReference + stap1;
        return Hoeknieuw1;
    }

    else if (EMG1 < -0.45f) {
        stap1 = EMG1*450*Ts;
        Hoeknieuw1 = PolsReference + stap1;
        return Hoeknieuw1;
    }

    else {
        return PolsReference;
    }
}
float Kinematics2(float EMG2)
{

    if (EMG2 > 0.45f) {
        stap2 = EMG2*300*Ts;         
        Hoeknieuw2 = ElbowReference + stap2;
        return Hoeknieuw2;
    }

    else if (EMG2 < -0.45f) {
        stap2 = EMG2*300*Ts;
        Hoeknieuw2 = ElbowReference + stap2;
        return Hoeknieuw2;
    }

    else {
        return ElbowReference;
    }
}

float Limits1(float Polshoek2)
{

    if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) {       //Binnen de limieten
        Polshoek3 = Polshoek2;
    }

    else {
        if (Polshoek2 >= upperlim1) {                            //Boven de limiet
            Polshoek3 = upperlim1;
        } else {                                                    //Onder de limiet
            Polshoek3 = lowerlim1;
        }
    }

    return Polshoek3;
}


float Limits2(float Ellebooghoek2)
{

    if (Ellebooghoek2 <= upperlim2 && Ellebooghoek2 >= lowerlim2) {       //Binnen de limieten
        Ellebooghoek3 = Ellebooghoek2;
    }

    else {
        if (Ellebooghoek2 >= upperlim2) {                            //Boven de limiet
            Ellebooghoek3 = upperlim2;
        } else {                                                    //Onder de limiet
            Ellebooghoek3 = lowerlim2;
        }
    }

    return Ellebooghoek3;
}


// PID Controller
double PID_controller1(double error1)
{
    static double error1_integral = 0;
    static double error1_prev = error1; // initialization with this value only done once!
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
      
    // Proportional part:
    double u_k1 = Kp1 * error1;

    // Integral part
    error1_integral = error1_integral + error1 * Ts;
    double u_i1 = Ki1* error1_integral;

    // Derivative part
    double error1_derivative = (error1 - error1_prev)/Ts;
    double filtered_error1_derivative = LowPassFilter.step(error1_derivative);
    double u_d1 = Kd1 * filtered_error1_derivative;
    error1_prev = error1;

    // Sum all parts and return it
    return u_k1 + u_i1 + u_d1;
}
double PID_controller2(double error2)
{
    static double error2_integral = 0;
    static double error2_prev = error2; // initialization with this value only done once!
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
       
    // Proportional part:
    double u_k2 = Kp2 * error2;

    // Integral part
    error2_integral = error2_integral + error2 * Ts;
    double u_i2 = Ki2 * error2_integral;

    // Derivative part
    double error2_derivative = (error2 - error2_prev)/Ts;
    double filtered_error2_derivative = LowPassFilter.step(error2_derivative);
    double u_d2 = Kd2 * filtered_error2_derivative;
    error2_prev = error2;

    // Sum all parts and return it
    return u_k2 + u_i2 + u_d2;
}

// Functies Motor
void moter1_control(double u1)
{
    direction1= u1 > 0.0f; //positief = CW
    if (fabs(u1)> 0.6f) {  
        u1 = 0.6f;
    } else {
        u1= u1;
    }
    pwmpin1.write(fabs(u1)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value
}

void moter2_control(double u2)
{
    direction2= u2 < 0.0f; //positief = CW
    if (fabs(u2)> 0.99f) {
        u2 = 0.99f;
    } else {
        u2= u2;
    }
    pwmpin2.write(fabs(u2)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value
}

void PwmMotor(void)
{
    float Polshoek1 = Kinematics1(Input1);
    float Polshoek4 = Limits1(Polshoek1);
    PolsReference = Polshoek4;
    
    // Reference hoek berekenen, in graden
    float Ellebooghoek1 = Kinematics2(Input2);
    float Ellebooghoek4 = Limits2(Ellebooghoek1);
    ElbowReference = Ellebooghoek4;
    
    // Positie motor berekenen, in graden
    Pulses1 = encoder1.getPulses();
    motor_position1 = -(Pulses1/1200)*360;
    Pulses2 = encoder2.getPulses();
    motor_position2 = -(Pulses2/4800)*360;

    double error1 = PolsReference - motor_position1;
    double u1 = PID_controller1(error1);
    moter1_control(u1);
    
    double error2 = ElbowReference - motor_position2;
    double u2 = PID_controller2(error2);
    moter2_control(u2);

}

void MotorOn(void)
{
    pwmpin1 = 0;
    pwmpin2 = 0;
    Pwm.attach (PwmMotor, Ts);

}

//Servo functie
void ServoPeriod()
{
  led1 = 1;
  led2 = 1;
  double Pulslength = 0.0005 + (servo_position * 0.002); //in seconden
  if (counts <= (Pulslength/Periodlength)) {
        myservo1.pulsewidth(0.00006);
        counts++;
    }
  else if (counts <= (0.02/Periodlength)){
        myservo1.pulsewidth(0);
        counts++;
    }
    else {
        counts = 0;
    }
}

void ContinuousReader(void)
{
    Pot2 = pot.read();
    Pot1 = pot0.read();
    pwm2 =(Pot2*2)-1;            //scaling naar -1 tot 1
    pwm1 =(Pot1*2)-1;
}

// StateMachine

enum states {MOTORS_OFF,CALIBRATION,HOMING1,HOMING2,DEMO,MOVEMENT,FREEZE}; 
int f = 1;
states currentState = MOTORS_OFF; 
bool stateChanged = true; // Make sure the initialization of first state is executed

void ProcessStateMachine(void)
{
  switch (currentState)
  {
    case MOTORS_OFF:
      // Actions
      if (stateChanged)
      {
        // state initialization: rood

        led1 = 1;
        led2 = 0; 
        led3 = 1;
        wait (1);
        stateChanged = false;
      }
    
      // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
        if (!button3)
        {        
        currentState = CALIBRATION  ;
        stateChanged = true;
        }
        else
        {
        currentState = MOTORS_OFF;
        stateChanged = true;
        }   
           
      break;
      
    case CALIBRATION:
    // Actions
      if (stateChanged)
      {
        // state initialization: oranje
                led1 = 0;
                led2 = 0;
                led3 = 1;
                
                servo_position = 0.5;
                ServoTick.attach(ServoPeriod, Periodlength);
                wait(1);
                servo_position = 0.9;
                led2 = 1;
                wait(1);
                servo_position = 0.5;
                wait(1);
                servo_position = 0.1;
                wait (1);
                stateChanged = false;
      }
      
      // State transition logic: automatisch terug naar motors off.

        currentState = HOMING1;
        stateChanged = true;
        break; 

case HOMING1:
    // Actions
      if (stateChanged)
      {
        // state initialization: green
        t.start();
        led1 = 0;
        led2 = 1;
        led3 = 1;
        
        if (!But1)
        {
        led1 = 1;
        float H1 = 0.7f;
        moter1_control(H1);
        wait(0.001f);
        }
        else if (!But2)
        {
        led1 = 1;
        float H1 = -0.7f;
        moter1_control(H1);
        wait(0.001f);
        }
        encoder1.reset();
        motor_position1 = 0;
        pwmpin1 = 0;
        pwmpin2 = 0;
        ;
        
        stateChanged = false;
      }
          
      // State transition logic: naar HOMING (button2), na 5 min naar MOTORS_OFF

        if (!button3)
        {        
        currentState = HOMING2  ;
        stateChanged = true;
        wait(1);
        }
        else if (t>300) 
        {        
        t.stop();
        t.reset();
        currentState = MOTORS_OFF  ;
        stateChanged = true;
        }
        else
        {        
        currentState = HOMING1  ;
        stateChanged = true;
        }
        break;      
    
    case HOMING2:
    // Actions
      if (stateChanged)
      {
        // state initialization: white
        t.start();
        led1 = 0;
        led2 = 0;
        led3 = 0;
        
        if (!But1)
        {
        led1 = 1;
        float H2 = 0.98f;
        moter2_control(H2);
        wait(0.001f);
        }
        else if (!But2)
        {
        led1 = 1;
        float H2 = -0.98f;
        moter2_control(H2);
        wait(0.001f);
        }
        encoder2.reset();
        motor_position2 = 0;
        pwmpin1 = 0;
        pwmpin2 = 0;
        ;
        
        stateChanged = false;
      }
          
      // State transition logic: naar DEMO (button2), naar MOVEMENT(button3)
        if (!button2)
        {        
        currentState = DEMO;
        stateChanged = true;
        }
        else if (!button3)
        {        
        currentState = MOVEMENT  ;
        stateChanged = true;
        led1 = 1;
        led2 = 0;
        led3 = 0;
        wait(1);
        }
        else if (t>300) 
        {        
        t.stop();
        t.reset();
        currentState = MOTORS_OFF  ;
        stateChanged = true;
        }
        else
        {        
        currentState = HOMING2  ;
        stateChanged = true;
        }
        break;
        
        case DEMO:
    // Actions
      if (stateChanged)
      {
        // state initialization: light blue
        led1 = 0;
        led2 = 1;
        led3 = 0;
        wait (1);
        
        stateChanged = false;
      }
          
      // State transition logic: automatisch terug naar HOMING
        currentState = HOMING1;
        stateChanged = true;
        break;
              
    case MOVEMENT:
    // Actions
      if (stateChanged)
      {
        // state initialization: purple
        t.start();

        pwmpin1 = 0;
        pwmpin2 = 0;
        Input1 = pwm1;
        Input2 = pwm2;
                
         // printen
        if(count==500) 
        {
        float tmp = t2.read();
        pc.printf(" Elleboog positie: %f reference: %f, rotatie positie: %f reference: \r\n", motor_position2, ElbowReference, motor_position1, PolsReference);
        count = 0;
        }
        count++;
        
        Pwm.attach (PwmMotor, Ts);
        led1 = 1;
        led2 = 0;
        led3 = 0;
              
        stateChanged = false;
      }
          
      // State transition logic: naar FREEZE (button2), naar MOTORS_OFF(button3) anders naar MOVEMENT
        if (!button2)
        {        
        currentState = FREEZE;
        stateChanged = true; 
        }
        else if (!button3)
        {        
        Pwm.detach ();
        pwmpin2 = 0;
        pwmpin1 = 0;
        currentState = MOTORS_OFF  ;
        stateChanged = true;
        }
        else if (t>300)
        {        
        t.stop();
        t.reset();
        currentState = HOMING1  ;
        stateChanged = true;
        }
        else
        {        
        currentState = MOVEMENT  ;
        stateChanged = true;
        }
        break;
        
        case FREEZE:
    // Actions
      if (stateChanged)
      {
        // state initialization: blue
        led1 = 1;
        led2 = 1;
        led3 = 0;
        wait (1);
        
        stateChanged = false;
      }
      
      // State transition logic: automatisch terug naar MOVEMENT.

        currentState = MOVEMENT;
        stateChanged = true; 
        break; 
         
}
}
 
int main()
{
    
    t2.start();
    int counter = 0;
    myservo1.period_us(60);
    //PotRead.attach(ContinuousReader,Ts);
    pc.baud(115200);
    
    
    while(true)
    {
    led1 = 1;
    led2 =1;
    led3 =1;
    /*
    if(counter==10) 
        {
        float tmp = t2.read();
        printf("%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Pulses2);
        counter = 0;
        }
    counter++;
    */
    ProcessStateMachine();

    
    wait(0.001);
    }
    }