File content as of revision 0:55f9447aa02b:

#include "mbed.h"
#include "PID.h"
#include "encoder.h"
#include "math.h"

//D8 doet het niet

DigitalIn    knop_1(D2); //Motor 1
DigitalIn    knop_2(D3); //Motor 2

DigitalOut motor_1(D7); //richting (1 of 0)
PwmOut pwm_motor_1(D6); //snelheid (tussen 0 en 1)
DigitalOut motor_2(D4); //richting (1 of 0)
PwmOut pwm_motor_2(D5); //snelheid (tussen 0 en 1)
InterruptIn stop(SW3); //stoppen

Serial pc(USBTX, USBRX); //USB ports aanroepen, D0 en D1

Encoder encoder_1(D13,D12);
Encoder encoder_2(D10,D9);

Ticker PID_ticker;

Timer tijd;

//constante waardes
const double x_start = -0.25;
const double dt = 0.001;
const double Vx = 0.05; //m/s
const double Vy = 0.01; //m/s
const double L1=0.25; //m
const double L2=0.35; //m
const double L3=0.05; //m
const double y_base = 0.3;
//filter
volatile double tijd_oud, tijd_nieuw, tijd_verschil;
volatile double u_1, y_1, u_2, y_2; //ongefilerd signaal emg 1, gefilterd signaal emg 1, ongefilterd signaal emg 2, gefilterd signaal emg 2
volatile int mode;   //mode waarin de motors moeten bewegen
//controller
volatile double theta_1, theta_2, reference_1, plaats_1, error_1, pwm_1, reference_2, plaats_2, error_2, pwm_2;
//beginpositie
volatile double x = x_start; //m
volatile double y = y_base; //m
volatile double yc= y + L3 - L1; //m
volatile bool opgepakt = false;
volatile bool zakpunt = false;
volatile bool uitdrukken = false;
volatile double y_oppakken, y_stijgen, x_zakken;
//PID
const double m1_Kp = 2.0, m1_Ki = 0.0, m1_Kd = 0.0, m1_N = 50;
double m1_v1 = 0, m1_v2 = 0; // Memory variables
const double m1_Ts = 0.01; // Controller sample time
const double m2_Kp = 2.0, m2_Ki = 0.0, m2_Kd = 0.0, m2_N = 50;
double m2_v1 = 0, m2_v2 = 0; // Memory variables
const double m2_Ts = 0.01; // Controller sample time


//Controller PID motors
void Controller()
{
    theta_1 = acos((sqrt(pow(x,2)+pow(yc,2)))/(2*L2))+asin(yc/(sqrt(pow(x,2)+pow(yc,2))));
    theta_2 = acos((x/L2)-cos(theta_1));

    reference_1 = theta_1;     //reference
    plaats_1 = 0.0014959*encoder_1.getPosition(); //positie encodercounts naar hoek
    error_1 = reference_1 - plaats_1 ;
    pwm_1 = PID(error_1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_N, m1_v1, m1_v2 );
    if (pwm_1<0){motor_1 = 1;}
    else {motor_1 = 0;}
    pwm_motor_1 = fabs(pwm_1);

    reference_2 = theta_2;     //reference
    plaats_2 = 0.0014959*encoder_2.getPosition(); //positie encodercounts naar hoek
    error_2 = reference_2 - plaats_2 ;
    pwm_2 = PID(error_2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_N, m2_v1, m2_v2 );
    if (pwm_2<0){motor_2 = 0;}
    else {motor_2 = 1;}
    pwm_motor_2 = fabs(pwm_2);
}

//Failsave
void Stop() //Zet allebei de snelheden op nul
{
    pwm_motor_1 = 0;
    pwm_motor_2 = 0;
    while(true){}
}

int main()
{
    pc.baud(115200);
    stop.fall(&Stop); //stop de snelheden van de motoren bij indrukken
    PID_ticker.attach(&Controller,dt);
    tijd.reset();
    tijd.start();
    theta_1 = acos((sqrt(pow(x,2)+pow(yc,2)))/(2*L2))+asin(yc/(sqrt(pow(x,2)+pow(yc,2))));
    theta_2 = acos((x/L2)-cos(theta_1));
    encoder_1.setPosition(theta_1/0.0014959); //positie encodercounts naar hoek
    encoder_2.setPosition(theta_2/0.0014959); //positie encodercounts naar hoek
    
    while(true)
    {
    tijd_nieuw = tijd;
    tijd_verschil = tijd_nieuw - tijd_oud;
    tijd_oud = tijd_nieuw;
    
    if      (knop_1 == 0 && knop_2 == 1){mode = 1;} 
    else if (knop_1 == 1 && knop_2 == 0){mode = 2;} 
    else if (knop_1 == 0 && knop_2 == 0){mode = 3;}
    else                              {mode = 4;} //default
    
    switch (mode)
    {
        case 1: x = x - tijd_verschil*Vx;
                if (x<-0.5){x = -0.5;}
                y = y_base;
                yc= y + L3 - L1;
                break;
        case 2: x = x + tijd_verschil*Vx;
                if (x>-0.25){x = -0.25;}
                y = y_base;
                yc= y + L3 - L1;
                break;
        case 3: while(y > (y_base-y_oppakken))    
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    y = y - tijd_verschil*Vy;
                    yc= y + L3 - L1;
                    x = x;
                    tijd_oud = tijd_nieuw;
                }
                while(y < (y_base+y_stijgen))
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    y = y + tijd_verschil*Vy;
                    yc= y + L3 - L1;
                    x = x;
                    tijd_oud = tijd_nieuw;
                }
                while(x > (x_start+x_zakken))
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    x = x-tijd_verschil*Vx;
                    yc = yc;
                    tijd_oud = tijd_nieuw;
                }
                while(y>y_base)
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;    
                    y = y - tijd_verschil*Vy;
                    yc= y + L3 - L1;
                    x = x;
                    tijd_oud = tijd_nieuw;
                }
                while((x>x_start) && uitdrukken)
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;   
                    y = y_base;
                    yc= y + L3 - L1;
                    x = x - tijd_verschil*Vx;
                    tijd_oud = tijd_nieuw;
                }
                    x = x_start;
                    y = y_base;
                    yc= y + L3 - L1;
                break;
        default: x = x;
                 yc = yc;
                 break;
    }    
    }
}