File content as of revision 8:008a7bf80fa0:

#include "stdio.h"
#include "math.h"
#include "mbed.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"

MODSERIAL pc(USBTX, USBRX);
QEI wheel_M1 (D13, D12, NC, 32);
QEI wheel_M2 (D10, D11, NC, 32);
PwmOut pwm_M1 (D6);
PwmOut pwm_M2 (D5);
DigitalOut dir_M1 (D7);
DigitalOut dir_M2 (D4);

DigitalOut ledg (LED_GREEN);
DigitalOut ledr (LED_RED);
DigitalOut ledb (LED_BLUE);
InterruptIn knop_biceps(SW2);
InterruptIn knop_triceps(SW3);
InterruptIn knop_switch(D9);

volatile double q1 = 0;
volatile double q2 = 0;
volatile double q1_begin;
volatile double q2_begin;
volatile double l1 = 0.3626;
volatile double l2 = 0.420;
volatile double q1_v;
volatile double q2_v;
volatile double q1_ref;
volatile double q2_ref;
volatile double ctrlOutput_M1 = 0;
volatile double ctrlOutput_M2 = 0;
volatile double vx;
volatile double vy;
volatile bool translatie_richting = true;  //true is verticaal, false is horizontaal

const double TS = 0.02;
const double M1_Kp = 0.5, M1_Ki = 0.00, M1_Kd = 0;        // Waardes vinden?
const double M2_Kp = 0.5, M2_Ki = 0.00, M2_Kd = 0;
const double N = 0.1;

Ticker begin_waarde_q;
int n = 0;
void begin_waarde()
{
    n++;
    if(n == 2)
    {
        q1_begin = wheel_M1.getPulses() / (1334.355/2);
        q2_begin = wheel_M2.getPulses() / (1334.355/2);
        begin_waarde_q.detach();
    }
}

Ticker update_encoder_ticker;
volatile bool go_flag_update_encoder = false;
void flag_update_encoder()
{
    go_flag_update_encoder = true;
}

void update_encoder()
{
    q1 = wheel_M1.getPulses()/(1334.355/2);
    q2 = wheel_M2.getPulses()/(1334.355/2);
    pc.printf("q1 = %f \tq1_ref = %f \tPID1 = %f \tq2 = %f \tq2_ref = %f \tPID2 = %f \tq1_v=%f \tq2_v=%f\n\r",q1, q1_ref, ctrlOutput_M1,q2,q2_ref,ctrlOutput_M2,q1_v,q2_v);
}

volatile bool go_flag_initialize = false;

void flag_initialize()
{
    go_flag_initialize = true;
}

void initialize()
{
    q1_ref = 15*2*3.1415/360;
    q2_ref = -30*2*3.1415/360;
}

void biceps()
{
    if (translatie_richting) {      // verticaal / up
        vx = 0;
        vy = 0.02;
    } else {                        // horizontaal / right
        vx = 0.02;
        vy = 0;
    }
}

void triceps()
{
    if (translatie_richting) {      // verticaal / down
        vx = 0;
        vy = -0.02;
    } else {                        // horizontaal / left
        vx = -0.02;
        vy = 0;
    }

}

void switcher()
{
    if ( (vx == 0) && (vy == 0) ) {
        translatie_richting = !translatie_richting;
    } else {
        vx = 0;
        vy = 0;
    }

    if (translatie_richting == 1) {
        ledr = 1;                   // blauw - verticaal
        ledg = 1;
        ledb = 0;
    } else {
        ledr = 0;                   // rood - horizontaal
        ledg = 1;
        ledb = 1;
    }
}

Ticker update_ref_ticker;
volatile double J_1;
volatile double J_2;
volatile double J_3;
volatile double J_4;
volatile bool go_flag_update_ref = false;
void flag_update_ref()
{
    go_flag_update_ref = true;
}

void update_ref()
{
    q1 = wheel_M1.getPulses() / (1334.355/2);     // rad
    q2 = wheel_M2.getPulses() / (1334.355/2);

    J_1 = -(l2*sin(q1 + q2))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));
    J_2 = (l2*cos(q1 + q2))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));
    J_3 = (l2*sin(q1 + q2) + l1*sin(q1))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));
    J_4 = -(l2*cos(q1 + q2) + l1*cos(q1))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));

    q1_v = J_1 * vx + J_2 * vy;
    q2_v = J_3 * vx + J_4 * vy;

    q1_ref = q1 + q1_v*TS;
    q2_ref = q2 + q2_v*TS;
    
    if ( (q1 > (90*2*3.1415/360)) && (q1_v > 0 ) ) {                // WAARDES VINDEN 0.8726 (50 graden)
        q1_v = 0;
        q2_v = 0;
    } else if ( (q1 < -(90*2*3.1415/360)) && (q1_v < 0) ) {
        q1_v = 0;
        q2_v = 0;
    } else if ( (q2 < (-140*2*3.1415/360)) && (q2_v < 0) ) {        // WAARDES VINDEN -2.4434 (-140 graden) --> werkelijke max -2.672452
        q1_v = 0;
        q2_v = 0;
    } else if ( (q2 > 0) && (q2_v > 0) ) {
        q1_v = 0;
        q2_v = 0;
    }
}

BiQuad pidf_M1;
BiQuad pidf_M2;
Ticker PIDcontrol_M1;
Ticker PIDcontrol_M2;
volatile bool go_flag_M1_controller = false;
volatile bool go_flag_M2_controller = false;

void flag_M1_controller()
{
    go_flag_M1_controller = true;
}

void flag_M2_controller()
{
    go_flag_M2_controller = true;
}

void M1_controller()
{
    ctrlOutput_M1 = pidf_M1.step(q1_ref-q1);

    if (ctrlOutput_M1 < 0) {
        dir_M1 = 1;
    } else {
        dir_M1 = 0;
    }
    pwm_M1 = abs(ctrlOutput_M1);
    if (pwm_M1 < 0) {
        pwm_M1 = 0;
    //} else {
    //    pwm_M1 = pwm_M1 + 0.1;
    }
}

void M2_controller()
{
    ctrlOutput_M2 = pidf_M2.step(q2_ref-q2);

    if (ctrlOutput_M2 < 0) {
        dir_M2 = 1;
    } else {
        dir_M2 = 0;
    }
    pwm_M2 = abs(ctrlOutput_M2);
    if (pwm_M2 < 0) {
        pwm_M2 = 0;
    //} else {
    //    pwm_M2 = pwm_M2 + 0.1;
    }
}

int main()
{
    pc.baud(115200);
    wheel_M1.reset();
    wheel_M2.reset();
    pidf_M1.PIDF(M1_Kp,M1_Ki,M1_Kd,N,TS);
    pidf_M2.PIDF(M2_Kp,M2_Ki,M2_Kd,N,TS);
    knop_biceps.rise(&biceps);
    knop_triceps.rise(&triceps);
    knop_switch.rise(&switcher);
    
    // flag functions/tickers
    update_encoder_ticker.attach(&flag_update_encoder, TS);
    update_ref_ticker.attach(&flag_update_ref, TS);
    PIDcontrol_M1.attach(&flag_M1_controller, TS);
    PIDcontrol_M2.attach(&flag_M2_controller, TS);
    begin_waarde_q.attach(&begin_waarde, 3);
    
    pc.printf("pidf_M1 is %s", pidf_M1.stable() ? "stable" : "unstable");
    pc.printf("pidf_M2 is %s", pidf_M2.stable() ? "stable" : "unstable");
    
    while(1) {

        // initialize function
        initialize();
        if (go_flag_initialize == true) {
            go_flag_initialize = false;
            initialize();
        }
        // update encoder
        if (go_flag_update_encoder == true) {
            go_flag_update_encoder = false;
            update_encoder();
        }
        // update joint positions/velocities
        if (go_flag_update_ref == true) {
            go_flag_update_ref = false;
            update_ref();
        }
        // controller M1
        if (go_flag_M1_controller == true) {
            go_flag_M1_controller = false;
            M1_controller();
        }
        // controller M2
        if (go_flag_M2_controller == true) {
            go_flag_M2_controller = false;
            M2_controller();
        }
    }
}