File content as of revision 9:182b33cabd45:

#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

double checkm1;
double checkm2; 

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


// PID gegevens
double pulses2rad=(2*pi)/4200;
double rad2pulses=4200/(2*pi);
double position;
double ref1;
double ref2;
double PI1;
double PI2;
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 = 0.2;   // proportional coefficient motor 1
double Ki1 = 0.02;   // integrating  coefficient motor 1

double Kp2 = 0.0015;   // proportional coefficient motor 2
double Ki2 = 0.035;   // 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.02;     //referentie snelheid m/s
        vy = 0;
        }
        else if(key=='d')
        {
        vx = -0.02;
        vy = 0;
        }
        else if(key=='w')
        {
        vx = 0;
        vy = 0.02;          
        }
        else if(key=='s')
        {
        vx = 0;
        vy = -0.02;          
        }
        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;   // converteert de radialen weer terug naar pulses voor verwerking in PID
    ref2 = q2*rad2pulses;
    }

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

error2_2   = error1_2; // opslaan variabelen voor integraal onderdeel
error_I2_2 = error_I_2;

    //Motor control
    if (PI1==0 || PI2==0)
    {
        if (PI1<=0)
        {
            motor1DirectionPin = 0;
            motor1MagnitudePin = fabs(PI1);
            motor2MagnitudePin = 0;
        }
        else if (PI1>0)
        {
            motor1DirectionPin = 1;
            motor1MagnitudePin = fabs(PI1);
            motor2MagnitudePin = 0;
        }
        else if (PI2<=0)
        {
            motor2DirectionPin = 0;
            motor2MagnitudePin = fabs(PI2);
            motor1MagnitudePin = 0;
        }
        else if  (PI2>0)
        {
            motor2DirectionPin = 1;
            motor2MagnitudePin = fabs(PI2);
            motor1MagnitudePin = 0;
        }
    }
                      
    else if (PI1<0 && PI2<0)
    {
    motor1DirectionPin = 1;
    motor1MagnitudePin = fabs(PI1);
    motor2DirectionPin = 0;
    motor2MagnitudePin = fabs(PI2);
    }
    else if (PI1>0 && PI2<0)
    {
    motor1DirectionPin = 1;
    motor1MagnitudePin = fabs(PI1);
    motor2DirectionPin = 0;
    motor2MagnitudePin = fabs(PI2);
    }

    else if (PI1<0 && PI2>0)
    {
    motor1DirectionPin = 0;
    motor1MagnitudePin = fabs(PI1);
    motor2DirectionPin = 1;
    motor2MagnitudePin = fabs(PI2);
    }
    
    else if (PI1>0 && PI2>0)
    {
    motor1DirectionPin = 0;
    motor1MagnitudePin = fabs(PI1);
    motor2DirectionPin = 1;
    motor2MagnitudePin = fabs(PI2);
    }
    
    checkm1 = motor1MagnitudePin;
    checkm2 = motor2MagnitudePin;
}

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();  
     
     if(n==500){         
    printf("checkm1 = %f    checkm2 = %f\n\r", ref1, ref2);
    n=0;
    }
    else{
             n=n+1;
             }       
}


int main()
{
    pc.baud(115200);
    tick_wasd.attach_us(&wasd, 100000);
    tick_sample.attach_us(&aansturing, 1000); //sample frequency 1000 Hz;
    led_g = 1; 
     
    while(true)
    {

    }
}