File content as of revision 3:329acce2487c:

#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=0;
double error1m=0;
double error2=0;
double error2m=0;

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


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 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1)))*vx + ((L2*sin(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1)))*vy) * ts  + q1_pos;
    q2 = (((L3 - L1*sin(q1) + L2*cos(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1))*vx) +  ((L1*cos(q1) - L2*sin(q1 - q2))/(L1*L2*cos(2*q1 - q2) + L1*L3*cos(q1)))*vy) * ts + q2_pos;
    }

void controller(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 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);
    
    if(n==500){     
    printf("motor1 = %f     motor2 = %f\n\r", PD1, PD2);
    n=0;
    }
    else{
             n=n+1;
             }     
    }


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

    }
    

}