File content as of revision 0:4c07bb5a9f9f:

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

// Connecties
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(D6);  //Motorrichting op D4 (connected op het bord)
PwmOut      motor2MagnitudePin(D7);  //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
char key;
double ts=0.001;
int q1_puls;
int q2_puls;

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

// reference position
double q1=0;    // positie q2 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 qset1=0;
double qset1m=0;
double qset2=0;
double qset2m=0;

double Kp;   // proportional coefficient       (
double Kd;   // differential coefficient


void wasd(double& vx, double& vy)
{
    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 
        {
            vx=0;
            vy=0;
        }
     }
     else
     {
     vx=0;
     vy=0;    
     }
}

double kinematics(double& q1, double& q2, double q1_pos, double q2_pos, double vx, double vy )
    {
               
        q1 = ((-(L3 + L2*cos(q1_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos))) * vx  +  (-(L2*sin(q2_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos))) * vy) * ts + q1_pos;
        q2 = (((L3 + L2*cos(q2_pos) - L1*sin(q1_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos)) * vx) + ((L1*cos(q1_pos) + L2*sin(q2_pos))/(L1*L3*cos(q1_pos) + L1*L2*cos(q1_pos)*cos(q2_pos) + L1*L2*sin(q1_pos)*sin(q2_pos))) * vy) * ts + q2_pos;
    return 0; 
    }

void controller(double qset1, double qset2)
    {
        // error = qset
        // referentie bepalen    
        ref1 = qset1 + q1_pos;                // genereert het referentiesignaal
        ref2 = qset2 + q2_pos;
        

        //PD 
        PD1 = Kp*(qset1) + Kd*((qset1m-qset1)/ts) ; // PD sum; PD = proportianal + differential
        qset1m = qset1;                              // waarde voor qset wordt opgeslagen (m = memory)
        PD2 = Kp*(qset2) + Kd*((qset2m-qset2)/ts) ;
        qset2m = qset2;
        
        //Motor control
        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 = 1;
        }
        
        else if (PD1<0 && PD2>0)
        {
        motor1MagnitudePin = fabs(PD1);
        motor1DirectionPin = 1;
        motor2MagnitudePin = fabs(PD2);
        motor2DirectionPin = 0;
        }

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

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


int main()
{
    pc.baud(115200);
    pulses2rad=(2*pi)/4200;
    Kp = 0.5/pi;
    Kd = 0.1*Kp;
    tick_sample.attach_us(&aansturing, 1000); //sample frequency 1 mHz;
    
    while(true)
    {
    }
    

}