File content as of revision 8:668afaa63c96:

#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;
double q1_puls;
double 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.0002;   // proportional coefficient motor 1
double Ki1 = 0.0003;   // integrating  coefficient motor 1

double Kp2 = 0.00015;   // proportional coefficient motor 2
double Ki2 = 0.0035;   // integrating  coefficient motor 2

void toetsen()
{
    if(pc.readable()==true)  
    {   key = pc.getc();
        if (key=='a')
        {
        vx = -20;        //reference wordt 500 pulses
        }
        else if(key=='d')
        {
        vx = 60;          //reference wordt 0 pulses
        }
        if (key=='w')
        {
        vy= 75;        //reference wordt 500 pulses
        }
        else if(key=='s')
        {
        vy= -85;          //reference wordt 0 pulses
        }
     }
     else
     {
         vx = 0;
         vy = 0;
         }
}

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 = 45;     //referentie snelheid m/s
        vy = 0;
        }
        else if(key=='d')
        {
        vx = -27;
        vy = 0;
        }
        else if(key=='w')
        {
        vx = 0;
        vy = 75;          
        }
        else if(key=='s')
        {
        vx = 0;
        vy = -75;          
        }
        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()
{  
    //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
    if (PI1<0 && PI2<0)
    {
    motor1DirectionPin = 0;
    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 = 1;
    motor1MagnitudePin = fabs(PI1);
    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();  
     if(n==500){         
    printf("PI1 = %f     PI2 = %f\n\r", vx, vy);
    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)
    {

    }
}