File content as of revision 1:83fccd7d8483:

#include "mbed.h"
#include "QEI.h"
#define pi 3.14159265359;

PwmOut Motor1_pwm(D5);
DigitalOut SlideMotor_Direction(D4);
PwmOut Motor2_pwm(D6);
DigitalOut LiftMotor_Direction(D7);
AnalogIn Potmeter(A0);
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);
    while (true) {
        
        Slide_Counts =      Slide_Encoder.getPulses();
        Slide_Revolutions = Slide_Counts /(32*131);
        Slide_Angle =       Slide_Revolutions*2*pi;
        Slide_Position =    Slide_Angle*Slide_Radius + 135;
        
        Lift_Counts =       Lift_Encoder.getPulses();
        Lift_Revolutions =  Lift_Counts /(32*131);
        Lift_Angle =        Lift_Revolutions*2*pi;
        Lift_Position =     Lift_Angle*Lift_Radius;
        //pc.printf("\n\r%f", Lift_Counts);
        
        if (Controller_Flag == true){
            Slide_Controller();
            Lift_Controller();
        }
        
        if (pc.readable()) {
            Key = pc.getc();
            switch(Key) {                         //Check to see which Key key...
                case 0x41:                        //It was the UP Key key...
                    pc.printf("\n\r UP!");
                    Lift_Input_force = -1;
                    break;
                case 0x42:                        //It was the DOWN Key key...
                    pc.printf("\n\r DOWN!");
                    Lift_Input_force = 1;
                    break;
                case 0x43:                        //It was the RIGHT Key key...
                    //pc.printf("\n\r RIGHT!");
                     Slide_Input_force = 1;   
                    break;
                case 0x44:                        //It was the LEFT Key key...
                    //pc.printf("\n\r LEFT!");
                    Slide_Input_force = -1; 
                    break;
            }
        } else { 
            Slide_Input_force = 0;
            wait(1/Frequency);
        }   
        //Motor1_pwm.write(Slide_Controller(Input_force));
        
    }
    return 0;
}
void Ticker_Flag(){
    Controller_Flag = true;
}

void Slide_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
    
    Slide_Desired_speed= Slide_Input_force*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_Desired_position = (Potmeter.read()*2-1)*30*Lift_Multiplier;
     pc.printf("\n\r%f - %f", Lift_Desired_position, Lift_Position);
    if (Lift_Desired_position < Lift_Start){
      // Lift_Desired_position = Lift_Start;
    }
    if (Lift_Desired_position > Lift_End){
     //  Lift_Desired_position = Lift_End;
    }
    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;
}