File content as of revision 5:bb77e2a6c1e8:

#include "mbed.h"
#include "QEI.h"

#include "servoController.h"
#include "emg.h"

#define pi 3.14159265359;

PwmOut Motor1_pwm(D5);
DigitalOut SlideMotor_Direction(D4);
PwmOut Motor2_pwm(D6);
DigitalOut LiftMotor_Direction(D7);
AnalogIn Potmeter(A0);
AnalogIn Potmeter2(A1);
QEI Slide_Encoder(D12,D13,NC,64);
QEI Lift_Encoder(D10,D11,NC,64);
//Serial pc(USBTX,USBRX);
Ticker Controller;

//char Key;
bool Controller_Flag=0;
//float Frequency = 30;
float Frequency_PWM = 10000;

float Slide_Radius = 12.5;
float Slide_Multiplier = 1;
float k1 = 1;
float k2 = 0.02f;
float k3 = 0.1f;
float Start_slow = 40;
float Start_lock = 0;
float End_slow = 340;
float End_lock = 380;
float Slide_Counts;
float Slide_Revolutions;
float Slide_Angle;
float Slide_Position;

float Slide_Input_force = 0;
float Slide_Curr_speed = 0;
float Slide_Desired_speed;
float Slide_Delta_speed;
float Slide_Int_delta_speed;
float Slide_Deriv_delta_speed = 0;
float Slide_Prev_delta_speed = 0;
float Slide_PI;

float Lift_Radius = 10;
float Lift_Multiplier = 1;
float Lift_k1 = 0.08;
float Lift_k2 = 0.05;
float Lift_k3 = 0.01;
float Lift_Start = 0;
float Lift_End = 50;
float Lift_Counts;
float Lift_Revolutions;
float Lift_Angle;
float Lift_Position;

float Lift_Input_force = 0;
float Lift_Desired_position;
float Lift_Delta_position;
float Lift_Int_delta_position;
float Lift_Deriv_delta_position = 0;
float Lift_Prev_delta_position = 0;
float Lift_PI;

void Slide_Controller();
void Lift_Controller();
void Ticker_Flag();

int main()
{
    Motor1_pwm.period(1.0/Frequency_PWM);//T=1/f
    Motor2_pwm.period(1.0/Frequency_PWM);//T=1/f
    Controller.attach(&Ticker_Flag,1/Frequency);
    pc.baud(9600);
    
    
    Lift_Input_force =  Potmeter.read();
    Slide_Input_force = Potmeter2.read();
    
    notch_50.add( &bq1 ).add( &bq2 ).add( &bq3 );
    high_pass.add( &bq4 ).add( &bq5 );
    
    change_state.attach( &calibrate,5);
    change_state2.attach( &run,10);
    emgSampleTicker.attach( &emgSample, 0.002);
    
    treshold = (cali_max-cali_min)*treshold_multiplier;
//    servoTick.attach(&control_servo, 1/Frequency);
    ServoPWMpin.period(0.01f);                          // 0.01 second period
    
    while (true) {
        pc.printf("\n\r%f", Norm_EMG_0);
         
        if (go_emgSample == true){
                EMG_filter();
        }
        
        if (Controller_Flag == true){
            Slide_Controller();
            Lift_Controller();
            control_servo(Norm_EMG_0);
            
            Controller_Flag = false;
        }

    }
    return 0;
}

void Ticker_Flag(){
    Controller_Flag = true;
}

void Slide_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)

    Slide_Counts =      Slide_Encoder.getPulses();
    Slide_Revolutions = Slide_Counts /(32*131);
    Slide_Angle =       Slide_Revolutions*2*pi;
    Slide_Position =    Slide_Angle*Slide_Radius + 135;
            
    Slide_Desired_speed= (Norm_EMG_0*2 -1)*Slide_Multiplier;
   
    if (Slide_Position < Start_slow && Slide_Desired_speed > 0){
        Slide_Desired_speed *= (Slide_Position-Start_lock)/(Start_slow-Start_lock);
        
    }
    if (Slide_Position > End_slow && Slide_Desired_speed < 0){
       Slide_Desired_speed *= (End_lock-Slide_Position)/(End_lock-End_slow);
    }
    Slide_Prev_delta_speed = Slide_Delta_speed;
    Slide_Delta_speed = Slide_Desired_speed-Slide_Curr_speed;                   // P
    Slide_Int_delta_speed += Slide_Delta_speed/Frequency;                       // I
    
    Slide_PI = k1*Slide_Delta_speed + k2*Slide_Int_delta_speed;
    if (Slide_PI<0){
        SlideMotor_Direction = 0;
    }else{
        SlideMotor_Direction = 1;
    }   
    
    Motor1_pwm.write(abs(Slide_PI));
    //return k1*Delta_speed + k2*Int_delta_speed;
}

void Lift_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
    
    Lift_Counts =       Lift_Encoder.getPulses();
    Lift_Revolutions =  Lift_Counts /(32*131);
    Lift_Angle =        Lift_Revolutions*2*pi;
    Lift_Position =     Lift_Angle*Lift_Radius;
    
    Lift_Desired_position = (Lift_Input_force*2-1)*30*Lift_Multiplier;
    //pc.printf("\n\r%f - %f", Lift_Desired_position, Lift_Position);
    Lift_Prev_delta_position = Lift_Delta_position;                             
    Lift_Delta_position = Lift_Desired_position-Lift_Position;             // P
    Lift_Int_delta_position += Lift_Delta_position/Frequency;                   // I
    Lift_Deriv_delta_position = (Lift_Delta_position-Lift_Prev_delta_position)*Frequency;
    
    Lift_PI = Lift_k1*Lift_Delta_position + Lift_k2*Lift_Int_delta_position + Lift_k3*Lift_Deriv_delta_position;
    if (Lift_PI<0){
        LiftMotor_Direction = 1;
    }else{
        LiftMotor_Direction = 0;
    }   
    
    Motor2_pwm.write(abs(Lift_PI));
    //return k1*Delta_speed + k2*Int_delta_speed;
}