File content as of revision 3:3b5b85a32c9a:

#include "mbed.h"
#include "Serial.h"
#include "math.h"
#include "QEI.h"

Serial      pc(USBTX, USBRX);        //Serial PC connectie
DigitalOut  motor1DirectionPin(D4);  //Motorrichting op D4 (connected op het bord)
PwmOut      motor1MagnitudePin(D5);  //Motorkracht op D5 (connected op het bord)
DigitalOut  motor2DirectionPin(D7);  //Motorrichting op D4 (connected op het bord)
PwmOut      motor2MagnitudePin(D6);  //Motorkracht op D5 (connected op het bord)
QEI         q1_enc(D13, D12, NC, 32);       //encoder motor 1 instellen
QEI         q2_enc(D11, D10, NC, 32);       // encoder motor 2 instellen
const double pi = 3.1415926535897;  // waarde voor pi aanmaken

Timer t;
Ticker aansturing;
Ticker stepres;
Ticker kinmat;

double ref1 = 0;
double refrad1;
double refrad2;
double ref2 = 0;
double Kp1 = 0.002;    //kp motor 2 = 0.0001
double Ki1 = 0;
double Kp2 = 0.0001;
double Ki2 = 0;
int q1_puls;
int q2_puls;
double rad2pulses=(2100/pi);
double pulses2rad = (2*pi)/4200;
double ts = 0.001;
double PI1;
int n;

double error1_1;
double error2_1 = 0;
double error_I_1;
double error_I2_1 = 0;
double error1_2;
double error2_2 = 0;
double error_I_2;
double error_I2_2 = 0;
double PI2;

// kinematica gegevens
// lengte armen
double L1 = 0.250;
double L2 = 0.355;
double L3 = 0.150;

// reference position
double q1=0;    // positie q1 in radialen
double q2=0;    // positie q2 in radialen
double q1_pos;
double q2_pos;

// EMG Input_k
double vx;
double vy;


void kinematica()
{
    // encoders uitlezen    
    q1_puls = -q1_enc.getPulses();
    q1_pos =  q1_puls*pulses2rad;       // berekent positie q1 in radialen
    q2_puls = q2_enc.getPulses();
    q2_pos =  q2_puls*pulses2rad;       // berekent positie q2 in radialen  
    
    vx = 0.04f*sin( t.read()/20 );
    vy = 0;
    
    q1 = ((-(L3 + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vx + ((L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * 5  + q1_pos;
    q2 = (((L3 - L1*sin(q1_pos) + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos))*vx) +  ((L1*cos(q1_pos) - L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * 5 + q2_pos;

    ref1 = q1*rad2pulses;
    ref2 = q2*rad2pulses;   
    
    if (ref1 >=0)
    {
        ref1 = ceil(ref1);
    }
    else if (ref1<0)
    {
        ref1 = floor(ref1);
    }
    if (ref1 >=0)
    {
        ref2 = ceil(ref2);
    }
    else if (ref1<0)
    {
        ref2 = floor(ref2);
    }
}


void controller()
{     

    //PID 1
error1_1 = ref1 - q1_puls;
error_I_1 = error_I2_1 + ts*((error1_1 + error2_1)/2);

PI1 = Kp1*error1_1 + Ki1*error_I_1;

error2_1   = error1_1; // opslaan variabelen voor integraal onderdeel
error_I2_1 = error_I_1;
    
    //PID 2
error1_2 = ref2 - q2_puls;
error_I_2 = error_I2_2 + ts*((error1_2 + error2_2)/2);

PI2 = Kp2*error1_2 + Ki2*(error_I_2);


    if (PI1<=0)
    {
    motor1DirectionPin = 1;
    motor1MagnitudePin = fabs(PI1);
    }
    else if (PI1>0)
    {
    motor1DirectionPin = 0;
    motor1MagnitudePin = fabs(PI1);        
    }
    
    if (PI2>=0)
    {
    motor2DirectionPin = 1;
    motor2MagnitudePin = fabs(PI2);   
    }
    else if(PI2<0)
    {
    motor2DirectionPin = 0;
    motor2MagnitudePin = fabs(PI2);               
    }
    
    if(n==500){         
    printf("PI1 = %f   PI2 = %f     ref1 = %f   ref2 = %f\n\r", PI1, PI2, ref1, ref2);
    n=0;
    }
    else{
             n=n+1;
             }     

}

void stapfunc()
{
 if (ref1==0){
     ref1 = 1000;
     }
     else if (ref1==1000)
     {
         ref1=0; 
     }  
}

int main()
{
    pc.baud(115200);
    t.start();
    aansturing.attach_us(&controller, 1000);
    kinmat.attach(&kinematica, 0.1); 
    //stepres.attach(&stapfunc, 3);
    
    while(true){

        }


}