File content as of revision 7:2bbd0147b07b:

#include "mbed.h"
#include "HIDScope.h"
#include "biquadFilter.h"
#include "FastPWM.h"

// Define the HIDScope and Ticker object
HIDScope    scope(5);
Ticker      scopeTimer;
volatile bool LoopTimerFlag;
Ticker      ForceTimer;
volatile bool LoopTimerFlag2;

// Read the analog input
AnalogIn    a_in2(A3); //OUT-
AnalogIn    a_in3(A4); //OUT+

Ticker Loopticker;                  // Deze ticker wordt gebruikt om de looptimerflag aan te roepen.
AnalogIn Referentie(A0);            // De schuifpotmeter naast de geleiding van de draad is de referentie.
AnalogIn Boardpotmeter2(A2);
DigitalOut motor1direction(D7);     //D6 en D7 voor motor 1 (op het motorshield)
FastPWM motor1speed(D6);
DigitalOut Led(LED_RED);
DigitalOut Roodisplus(D5);
DigitalOut Versterker(D4);

float Error;
float Referentie2;//=1.0;
float Output;
float Input;
double Error_prev = 0;
double Error_int = 0;
double Error_der;
double Ref_der = 0;
double Ref_prev = 0;
double Ref_prev2 = 0;
double Ref2;
double Input3;
double Plaats_der;
double Force_nieuw=0;
double Ref_der_prev;
double Plaats_der1;

//LowPass filter 2 Hz
const double a1_LP =  -1.940778135263835, a2_LP = 0.942482022027066; // Biquad 300 Hz. fk = 2
const double b0_LP = 0.000425971690807714, b1_LP = 0.000851943381615428, b2_LP = 0.000425971690807714;
const double a1_HP = -1.982228929792529, a2_HP = 0.982385450614125;  // fk = 0.5;
const double b0_HP =  0.991153595101663, b1_HP = -1.982307190203327, b2_HP = 0.991153595101663;
const double a1_LP5 = -1.822694925196308, a2_LP5 = 0.837181651256023 ;    
const double b0_LP5 =  0.003621681514929 , b1_LP5 =  0.007243363029857, b2_LP5 = 0.003621681514929; //Laagdoorlaatfilter, 100 Hz fK = 2
const double a1_LP2 =  -1.561018075800718, a2_LP2 = 0.641351538057563;                               // Laagdoorlaatfilter 300 Hz, fK = 15
const double b0_LP2 = 0.020083365564211, b1_LP2 = 0.040166731128423, b2_LP2 = 0.020083365564211;


biquadFilter Filter1(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP);        // Lowpassfilter voor signaal van krachtsensor, 300 Hz, fk = 2
biquadFilter Filter2(a1_HP, a2_HP, b0_HP, b1_HP, b2_HP);        // Highpassfilter voor signaal van krachtsensor, 300 Hz, fk = 10 
biquadFilter Filter3(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2);   // Lowpassfilter voor signaal positie-potmeter, 300 Hz fK = 15
biquadFilter Filter4(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2);   // Lowpassfilter voor geschaalde signaal positie-potmeter, 300 Hz fK = 15
biquadFilter Filter6(a1_LP5, a2_LP5, b0_LP5, b1_LP5, b2_LP5); // Laagdoorlaatfilter 100 Hz, fk is 2 Hz


double Y_filt;
double Y_filt2;
double Y_filtprev;
double Y_filtder;
int A;
float time_;
const float time_increment = 0.005;

// The data read and send function
void scopeSend()
{
    scope.set(0,Ref2); // Force_nieuw);
    scope.set(1,Y_filtder);
    scope.set(2,Output); //Plaats_der);
    scope.set(3,Input);
    scope.set(4,A);
    scope.send();

}

void tickerfunctie()                //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden.
{
    LoopTimerFlag = 1;
}

void tickerfunctie2()                //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden.
{
    LoopTimerFlag2 = 1;
}


double Referentieschaling(double A, double B)
{
    double Y= 787.3008916207206*pow(A,4) -565.1143141517078*pow(A,3) + 122.8516837382677*pow(A,2) + 0.0556616744031*A + 0.0912411880277;
    Referentie2 = Filter4.step(Y);
    if (Referentie2<=2.5 or Referentie2>=9.2) {
        Led = 0;
        Referentie2 = B;
    }
    return Referentie2;
}

double Inputberekening(double B)
{
    double Inp = 10*B;            // De potmeter geeft ook waardes tussen 0 en 1, dit wordt met een factor 10 geschaald zodat deze als een positie in cm opgelegd kunnen worden.
    Input = Filter3.step(Inp);
    if (Input>=9) {                             // De Input moet binnnen een aantal grenzen blijven. Groter dan 7 is mechanisch niet mogelijk.
        Input=9;                                // Bij een waarde kleiner dan 1,5 zijn de strings niet meer gewikkeld en werkt de controller averechts en is deze uiterst instabiel.
    } else if (Input<=3.0) {
        Input = 3.0;
    }
    return Input;
}


double Force_prev;
double Input_prev3;

double Inputberekening3(double B)
{
    if (Force_nieuw>=2.5) {
        if (A == 1) {
            Input3 = Input_prev3-0.01;
        }
        if (A == 0 && Force_prev>2.5) {
            A = 0;
        }
        if (A <= 0 && Force_prev<=2.5) {
            A++;
            Input3 = Input_prev3+0.01;
        }
    } else if (Force_nieuw<=-2.5) {
        if (A == -1) {
            Input3 = Input_prev3+0.01;
        }
        if (A == 0 && Force_prev<-2.5) {
            A = 0;
        }
        if (A >= 0 && Force_prev>=-2.5) {
            A--;
            Input3 = Input_prev3-0.01;
        }
    } else {
        Input3 = Input_prev3 - A*0.01;
    }
    Force_prev= Force_nieuw;
    if (Input3 >= 9) {
        Input3 =9;
    }
    if (Input3 <= 3.5) {
        Input3 =3.5;
    }
    Input_prev3 = Input3;
    return Input3;

}

double Errorberekening(double Ref,double Input)
{
    Error = Input-Ref;                  // Het Error-signaal wordt ook gebruikt voor de PWMOut, dus mag deze niet hoger worden dan 1, 1 is immers al full speed voor de motor.

    return Error;
}
double PID_controller(double Error, double KP, double KI, double KD, double Ts, double &Error_int, double &e_prev)
{
    Error_der = (Error-Error_prev)/Ts;
    Error_prev = Error;
    Error_int = Error_int + Ts*Error;

    return KP*Error+KI*Ts*Error_int+KD*Ts*Error_der;
}

void Afgeleide_Force()
{
    Y_filtder = (Y_filt-Y_filtprev)/0.01;
    Y_filtprev = Y_filt;
}

void Afgeleide_Plaats()
{
    LoopTimerFlag2 = 0;
    Plaats_der = Ref_der/0.01;
    Plaats_der1 = Filter6.step(Plaats_der);
    Plaats_der = (Plaats_der1-Ref_der_prev)/0.1;
    if (Plaats_der>=0) {
        Plaats_der = Plaats_der;
    } else if (Plaats_der<0) {
        Plaats_der = 0.5*Plaats_der;
    }
    Ref_der_prev= Plaats_der1;
}

void Motor_controller()
{
    if(Output>=0) {
        motor1direction.write(1);
        motor1speed = 0.2+fabs(Output);
    } else if (Output<0) {
        motor1direction.write(0);
        motor1speed = fabs(Output);
    }
}

int main()
{
    Roodisplus.write(1);
    Versterker.write(1);
    // Attach the data read and send function at 100 Hz
    scopeTimer.attach_us(&tickerfunctie, 1/0.0003);// 1e4);
    ForceTimer.attach_us(&tickerfunctie2, 1e4);
    while(1)    {
        while(LoopTimerFlag !=1);                  // Als LTF 0 is, blijft hij 0 en stopt de loop.
        LoopTimerFlag = 0;    // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
        Led = 1;
        Y_filt = 3*Filter1.step(a_in3.read());
        Y_filt2 = Filter2.step(Y_filt);
        Input = Inputberekening3(Force_nieuw); 
        time_ += time_increment;
        Ref2 =  Referentieschaling(Referentie.read()/2,Input);       // De referentiewaarde is via deze functie (gevonden met metingen en polyfit) verbonden aan de afstand in centimeters voor waarden tussen 0 en 0.5.
        Error = Errorberekening(Ref2, Input);
        Ref_der = Referentie2-Ref_prev;
        Ref_prev = Referentie2;
        Output = PID_controller(Error,0.5,0.1,0.05,0.01, Error_prev, Error_int);
        Motor_controller();
        if (LoopTimerFlag2==1) {
            Afgeleide_Force();
            Afgeleide_Plaats();
            Force_nieuw = Y_filtder-Boardpotmeter2.read()*Plaats_der;
        }
        scopeSend();
    }


}