File content as of revision 3:57b98989b0b1:

#include "mbed.h"
#include "PID.h"
#include "encoder.h"
#include "math.h"
#include "HIDScope.h"
#include "Filter.h"
#include "FilterDesign.h"
#include "Filter2.h"
#include "FilterDesign2.h"

//D8 doet het niet

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

AnalogIn    emg1(A0);
AnalogIn    emg2(A1);

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
InterruptIn beginopnieuw(SW2); //stoppen

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

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

HIDScope scope(4);

Ticker PID_ticker;
Ticker FILTER_ticker;

Timer tijd;

//constante waardes
const double x_start = 0.255;
const double dt = 0.001;
const double dt_f = 0.001;
const double Vx = 0.05; //m/s
const double Vy = 0.05; //m/s
const double L2 = 0.35; //m
const double y_base = 0.045;
//gegevens
const double x_plek_T = 0.43, y_streep_TY_hoogte = 0.15;
const double x_streep_half = 0.06, x_plek_Y = x_plek_T+0.13, y_midden_Y = 0.09;
//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 bool opgepakt = false;
volatile bool zakpunt = false;
volatile bool uitdrukken = false;
volatile double y_oppakken = 0.135, y_stijgen = 0.15, x_zakken = 0.09;//Voorwaarde voor terugbewegen
volatile bool run_programma = true;
//PID
const double m1_Kp = 2, m1_Ki = 5, m1_Kd = 0.1, m1_N = 50;
double m1_v1 = 0, m1_v2 = 0; // Memory variables
const double m1_Ts = 0.001; // Controller sample time
const double m2_Kp = 2, m2_Ki = 5, m2_Kd = 0.1, m2_N = 50;
double m2_v1 = 0, m2_v2 = 0; // Memory variables
const double m2_Ts = 0.001; // Controller sample time
volatile double compensatie = 0;


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

    reference_1 = -1.5*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 = -1.5*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 )-compensatie;
    if (pwm_2<0){motor_2 = 0;}
    else {motor_2 = 1;}
    pwm_motor_2 = fabs(pwm_2);
    scope.set(0,u_1);
    scope.set(1,y_1);
    scope.set(2,u_2);
    scope.set(3,y_2);
    scope.send();
}
//Ticker filterwaardes
void Filter_Samples()
{
        u_1 = emg1.read();
        y_1 = FilterDesign(u_1);
        u_2 = emg2.read();
        y_2 = FilterDesign2(u_2);
}

//Failsave
void Stop() //Zet allebei de snelheden op nul
{
    PID_ticker.detach();
    pwm_motor_1 = 0;
    pwm_motor_2 = 0;
    run_programma = false;
}

void Beginopnieuw()
{
    run_programma = true;
    x = x_start; y = y_base; m1_v1 = 0; m1_v2 = 0; m2_v1 = 0; m2_v2 = 0;
    theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
    theta_2 = acos((x/L2)-cos(theta_1));
    double positie_motor_1 = -1.5*theta_1;
    double positie_motor_2 = -1.5*theta_2;
    encoder_1.setPosition(positie_motor_1/0.0014959); //positie encodercounts naar hoek
    encoder_2.setPosition(positie_motor_2/0.0014959); //positie encodercounts naar hoek
    PID_ticker.attach(&Controller,dt);
    tijd.reset();
    tijd.start();
}

int main()
{
    pc.baud(115200);
    stop.fall(&Stop); //stop de snelheden van de motoren bij indrukken
    beginopnieuw.fall(&Beginopnieuw);
    pwm_motor_1.period_us(1);
    pwm_motor_2.period_us(1);
    theta_1 = acos((sqrt(pow(x,2)+pow(y,2)))/(2*L2))+asin(y/(sqrt(pow(x,2)+pow(y,2))));
    theta_2 = acos((x/L2)-cos(theta_1));
    double positie_motor_1 = -1.5*theta_1;
    double positie_motor_2 = -1.5*theta_2;
    encoder_1.setPosition(positie_motor_1/0.0014959); //positie encodercounts naar hoek
    encoder_2.setPosition(positie_motor_2/0.0014959); //positie encodercounts naar hoek
    FILTER_ticker.attach(&Filter_Samples,dt_f);
    PID_ticker.attach(&Controller,dt);
    wait(1);
    tijd.reset();
    tijd.start();
    while(true)
    {
    while(run_programma)
    {
    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 && x>(x_start+x_zakken)){mode = 3;}
    else if (knop_1 == 0 && knop_2 == 0 && x<x_plek_T) {mode = 5;}
    else                             {mode = 4;} //default
    /*
    if      (y_1 >= 0.4 && y_2 <  0.4)                        {mode = 1;} 
    else if (y_1 <  0.4 && y_2 >= 0.4)                        {mode = 2;} 
    else if (y_1 >= 0.4 && y_2 >= 0.4 && x>(x_start+x_zakken)){mode = 3;}
    else                                                      {mode = 4;} //default
    */
    switch (mode)
    {
        case 1: x = x + tijd_verschil*Vx;
                if (x>0.6){x = 0.6;}
                y = y_base;
                break;
        case 2: x = x - tijd_verschil*Vx;
                if (x<x_start){x = x_start;}
                y = y_base;
                break;
        case 3: while(y > (y_base-y_oppakken))    
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    y = y - tijd_verschil*Vy*5;
                    x = x;
                    tijd_oud = tijd_nieuw;
                }
                wait(0.05);
                tijd_nieuw = tijd;
                tijd_oud = tijd_nieuw;
                while(y < (y_base+y_stijgen))
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    y = y + tijd_verschil*Vy*5;
                    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*2;
                    tijd_oud = tijd_nieuw;
                }
                while(y>y_base)
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;    
                    y = y - tijd_verschil*Vy*2;
                    x = x;
                    tijd_oud = tijd_nieuw;
                }
                while(x>x_start-0.03)
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;   
                    y = y_base;
                    x = x - tijd_verschil*Vx*2;
                    if (x<x_start)
                    {
                    compensatie = 0.4;
                    }
                    tijd_oud = tijd_nieuw;
                }
                compensatie = 0;
                wait(0.5);
                    x = x_start;
                    y = y_base;
                break;
    case 5: //Naar onderkant T
        while(x < x_plek_T)    
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    x = x + tijd_verschil*Vx;
                    y = y;
                    tijd_oud = tijd_nieuw;
                }
        //Naar bovenkant T
        while(y < (y_base+y_streep_TY_hoogte))
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    y = y + tijd_verschil*Vy;
                    x = x;
                    tijd_oud = tijd_nieuw;
                }
        //Naar linkerkant bovenkant T
        while(x > (x_plek_T - x_streep_half))    
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    x = x - tijd_verschil*Vx;
                    y = y;
                    tijd_oud = tijd_nieuw;
                }
        //Naar rechterkant bovenkant T 
        while(x < (x_plek_T + x_streep_half))    
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    x = x + tijd_verschil*Vx;
                    y = y;
                    tijd_oud = tijd_nieuw;
                }
        //Naar midden T
        while(x > x_plek_T)    
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    x = x - tijd_verschil*Vx;
                    y = y;
                    tijd_oud = tijd_nieuw;
                }
        //Naar onderkant T
        while(y > y_base)
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    y = y - tijd_verschil*Vy;
                    x = x;
                    tijd_oud = tijd_nieuw;
                }
        //Naar onderkant Y
        while(x < x_plek_Y)    
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    x = x + tijd_verschil*Vx;
                    y = y;
                    tijd_oud = tijd_nieuw;
                }
        //Naar midden Y
        while(y < (y_base+y_midden_Y))
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    y = y + tijd_verschil*Vy;
                    x = x;
                    tijd_oud = tijd_nieuw;
                }
        //Naar linksboven Y
        while(y < (y_base+y_streep_TY_hoogte))
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    y = y + tijd_verschil*Vy;
                    x = x - tijd_verschil*Vx;
                    tijd_oud = tijd_nieuw;
                }
        //Naar midden Y
        while(y > (y_base+y_midden_Y))
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    y = y - tijd_verschil*Vy;
                    x = x + tijd_verschil*Vx;
                    tijd_oud = tijd_nieuw;
                }
        //Naar rechtsboven Y
        while(y < (y_base+y_streep_TY_hoogte))
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    y = y + tijd_verschil*Vy;
                    x = x + tijd_verschil*Vx;
                    tijd_oud = tijd_nieuw;
                }
        //Naar midden Y
        while(y > (y_base+y_midden_Y))
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    y = y - tijd_verschil*Vy;
                    x = x - tijd_verschil*Vx;
                    tijd_oud = tijd_nieuw;
                }
        //Naar onderkant Y
        while(y > y_base)
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    y = y - tijd_verschil*Vy;
                    x = x;
                    tijd_oud = tijd_nieuw;
                }
        //Naar basis
        while(x > x_start)    
                {
                    tijd_nieuw = tijd;
                    tijd_verschil = tijd_nieuw - tijd_oud;        
                    x = x - tijd_verschil*Vx;
                    y = y;
                    tijd_oud = tijd_nieuw;
                }
        break;
        default: x = x;
                 y = y;
                 break;
    }    
    }
    }
}