File content as of revision 5:21e846902e3e:

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

// Connecties
Serial      pc(USBTX, USBRX);        //Serial PC connectie
DigitalOut  led_g(LED_GREEN);         //Groene led op k64f bord
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

// globale gegevens
Ticker tick_sample; // ticker voor aanroepen aansturing
Ticker tick_wasd; //ticker voor toetsenbord aansturing
char key;
double ts=0.001;
int q1_puls;
int q2_puls;
int n = 0;

// 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 = 0;
double vy = 0;


// PID gegevens
double pulses2rad;
double position;
double ref1;
double ref2;
double PD1;
double PD2;
double error1_1=0;
double error1_2=0;
double error2_1=0;
double error2_2=0;
double error_I_1=0;
double error_I2_1=0;
double error_I_2=0;
double error_I2_2=0;

double Kp1;   // proportional coefficient motor 1
double Ki1;   // integrating  coefficient motor 1

double Kp2;   // proportional coefficient motor 2
double Ki2;   // integrating  coefficient motor 2

void wasd()
{
    static char oldkey = 'p';
    static double oldvx = 0;
    static double oldvy = 0;
    
    if(pc.readable()==true)  
    {   key = pc.getc();

        if (key=='a')
        {
        vx = 0.04;     //referentie snelheid m/s
        vy = 0;
        }
        else if(key=='d')
        {
        vx = -0.04;
        vy = 0;
        }
        else if(key=='w')
        {
        vx = 0;
        vy = 0.04;          
        }
        else if(key=='s')
        {
        vx = 0;
        vy = -0.04;          
        }
        else
        {
            key = oldkey;
            vx = oldvx;
            vy = oldvy;
        }

    }                           //einde eerste if statemnet
    
    else if (pc.readable()==false) 
     {
     vx=0;
     vy=0;
     key='p'; 
     }
oldkey = key;
oldvx = vx;
oldvy = vy;    

}

void kinematics()
    {
               
    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) * ts  + 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) * ts + q2_pos;
    
    ref1 = q1*rad2pulses;
    ref2 = q2*rad2pulses;
    }

void controller_oud(double q1ref, double q2ref)
    {
        // error = qset
        // referentie bepalen    
        
        error1 = q1ref - q1_pos;
        error2 = q2ref - q2_pos;

        //PD 
        PD1 = Kp*(error1) + Kd*((error1m-error1)/ts) ; // PD sum; PD = proportianal + differential
        error1m = error1;                              // waarde voor qset wordt opgeslagen (m = memory)
        PD2 = Kp*(error2) + Kd*((error2m-error2)/ts) ;
        error2m = error2;
        
        //Motor control
        if (PD1<0 && PD2<0 )
        {
        motor1MagnitudePin = fabs(PD1);
        motor1DirectionPin = 0;
        motor2MagnitudePin = fabs(PD2);
        motor2DirectionPin = 1;
        }
        else if (PD1>0 && PD2<0)
        {
        motor1MagnitudePin = fabs(PD1);
        motor1DirectionPin = 1;
        motor2MagnitudePin = fabs(PD2);
        motor2DirectionPin = 1;
        }
        
        else if (PD1<0 && PD2>0)
        {
        motor1MagnitudePin = fabs(PD1);
        motor1DirectionPin = 0;
        motor2MagnitudePin = fabs(PD2);
        motor2DirectionPin = 0;
        }

        else if (PD1>0 && PD2>0)
        {
        motor1MagnitudePin = fabs(PD1);
        motor1DirectionPin = 1;
        motor2MagnitudePin = fabs(PD2);
        motor2DirectionPin = 0;
        }
   
        
        
    }

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

error2_2   = error1_2; 
error_I2_2 = error_I_2;

    //Motor control 1
    if (PI1<0 &&)
    {
    motor1DirectionPin = 0;
    motor1MagnitudePin = fabs(PI1);

    }
    else if (PI1>0)
    {
    motor1DirectionPin = 1;
    motor1MagnitudePin = fabs(PI1);
    }

    //Motor control 2
    if (PI2<0 )
    {
    motor2DirectionPin = 0;
    motor2MagnitudePin = fabs(PI2);

    }
    else if (PI2>0)
    {
    motor2DirectionPin = 1;
    motor2MagnitudePin = fabs(PI2);
    }
}

void aansturing()
    {
    // 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    
     
     // kinematica              bepaald gewenste q1 en q2 referenties afhankelijk van ingegeven vx en vy
     kinematics();
                 
     // PD controller       gebruikt PD control en stuurt motor aan
     controller(q1, q2);    
    }


int main()
{
    pc.baud(115200);
    pulses2rad=(2*pi)/4200;
    rad2pulses=4200/(2*pi);
    tick_sample.attach_us(&aansturing, 1000); //sample frequency 1000 Hz;
    tick_wasd.attach(&wasd, 0.1);
    led_g = 0; 
     
    while(true)
    {

    }
    

}