File content as of revision 5:f97bf30bec14:

#include "mbed.h"
#include "FastPWM.h"
#include <iostream>
#include "HIDScope.h"
#include "math.h"
#include "biquadFilter.h"


DigitalOut Rood(LED_RED);
DigitalOut Groen(LED_GREEN);
DigitalOut Blauw(LED_BLUE);
DigitalIn Button1pressed(SW2);
DigitalIn Button2pressed(SW3);
volatile bool LoopTimerFlag = 0;
Serial pc(USBTX, USBRX);

int N;
bool Start(0);
double Setting(0);
bool Dotask(0);
int i;

Ticker Finitestatemachine;

// Verklaren van de in en outputs
Ticker Loopticker;                  // Deze ticker wordt gebruikt om de looptimerflag aan te roepen.
volatile bool LoopTimerFlag2 = 0;    // Volatile, omdat deze heel vaak verandert van waar naar onwaar.
//Ticker Loopticker2;                  // Deze ticker wordt gebruikt om de looptimerflag aan te roepen.
//volatile bool LoopTimerFlag3 = 0;
AnalogIn Referentie(A0);            // De schuifpotmeter naast de geleiding van de draad is de referentie.
AnalogIn Boardpotmeter(A2);         // POT1 op het board wordt gebruikt als input van de controlloop.
AnalogIn Boardpotmeter2(A1);
DigitalOut motor1direction(D7);     //D6 en D7 voor motor 1 (op het motorshield)
FastPWM motor1speed(D6);


float potmeter;
float Error;
float Referentie2;//=0.0;
float Input;
float Output;
float POT;
float Foutje;
float Potmetertje;
double Error_prev = 0;
double Error_int = 0;
double Error_der;
double Ref_der = 0;
double Ref_prev = 0;
float Amplitude;
float Baseline;
// Define the HIDScope and Ticker object
HIDScope    scope(4);
Ticker Hidscope;
// Lowpassfilter
const double a1_LP =  -1.561018075800718, a2_LP = 0.641351538057563;
const double b0_LP = 0.020083365564211, b1_LP = 0.040166731128423, b2_LP = 0.020083365564211;
const double a1_LP2 =  -1.561018075800718, a2_LP2 = 0.641351538057563;
const double b0_LP2 = 0.020083365564211, b1_LP2 = 0.040166731128423, b2_LP2 = 0.020083365564211;
//const double a1_LP2 =  -1.866892279711715, a2_LP2 = 0.875214548253684;
//const double b0_LP2 = 0.002080567135492, b1_LP2 = 0.004161134270985, b2_LP2 = 0.002080567135492;
//const double a1_HP = -1.982228929792529, a2_HP = 0.982385450614125;
//const double b0_HP =  0.991153595101663, b1_HP = -1.982307190203327, b2_HP = 0.991153595101663;

float time_;
const float time_increment = 0.01;

biquadFilter Filter1(a1_LP, a2_LP, b0_LP, b1_LP, b2_LP);
biquadFilter Filter2(a1_LP2, a2_LP2, b0_LP2, b1_LP2, b2_LP2);

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 = Filter1.step(Y);
    if (Referentie2<=2.5 or Referentie2>=9.2) {
        Rood = 0;
        Referentie2 = B;
    }
    return Referentie2;
}

double Inputberekening(double B)
{
    Input = Baseline+(Amplitude*-cos(2*time_));
    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 Errorberekening(double Input,double Ref)
{
    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.
    /*if (Error>=1) {
        Error=1;
    } else if (Error<=-1) {
        Error = -1;
    } else if (fabs(Error)<0.01) {
        Error = 0;
    }
    */
    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*Error_int+KD*Error_der;
}

// The data read and send function
void scopeSend()
{
    scope.set(0,Referentie2);       // Kanaal 1 van HIDscope geeft: De positie van de schuifpotmeter in cm.
    scope.set(1,Input);             // Kanaal 2 van HIDscope geeft: De gewenste positie in cm door de potmeter op het board ingesteld.
    scope.set(2,Error);             // Kanaal 3 van HIDscope geeft: De waarde van de Error die de P_controller in gaat.
    scope.set(3,motor1speed.read());// Kanaal 1 van HIDscope geeft: De snelheid van de motor, is een absolute waarde, richting wordt elders gegeven.
    scope.send();

}

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

void Motor_controller(float Output)                 // De P_controller, door de ticker elke honderdste seconde uitgevoerd.
{
    if(Ref_der < 0) {
        if(Output>=0) {
            motor1direction.write(1);
            motor1speed = 0.01*fabs(Output);
        } else if (Output<0) {
            motor1direction.write(0);
            motor1speed = 0.05*fabs(Output);
        }
    }
    if(Ref_der > 0) {
        if(Output>=0) {
            motor1direction.write(1);
            motor1speed = (20*fabs(Output));
        } else if (Output<0) {
            motor1direction.write(0);
            motor1speed = 0.01*fabs(Output);
        }
    }
}

void Motor_controller2(float Foutje)
    {
        if(Foutje>=0.01){
            motor1direction.write(1);
            motor1speed.write(0.05*Foutje);
            }
        else if(Foutje<=-0.01){
            motor1direction.write(0);
            motor1speed.write(-Foutje);
            }
        else {
            motor1speed.write(0);
            }
    }

void Extendfinger()
{
    Error = 0;
    int J = 0;
    time_ = 0;                                      // Voordat het hele programma begint, staat de Error op 0, zodat de motor niet spastisch gaat draaien om dit te compenseren.
          while(1 && J<=320) {
            while(LoopTimerFlag2 !=1);                   // Als LTF 0 is, blijft hij 0 en stopt de loop.
            LoopTimerFlag2 = 0;                         // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
            J= J+1;
            //pc.printf(" J is %i \n",J);
            Rood = 1;
            double Input = Inputberekening(Boardpotmeter.read());
            time_ += time_increment;
            double 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.
            Ref_der = Referentie2-Ref_prev;
            Ref_prev = Referentie2;
            Error = Errorberekening(Input, Referentie2);
            Output = PID_controller(Error,5,1,0.1,0.01, Error_prev, Error_int);
            Motor_controller(Output);
        }
    //}
    motor1speed.write(0);
}

void tickerfunctie()
{
    LoopTimerFlag = 1;
}

void P_controller(double Max, bool Keuze)
{
    while (Button1pressed.read()==1) { 
        while(LoopTimerFlag2 !=1);                  // Als LTF 0 is, blijft hij 0 en stopt de loop.
        LoopTimerFlag2 = 0;                           // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
        if (bool (0)) {
            POT = Boardpotmeter.read();
        } else if (bool (1)) {
            POT = Boardpotmeter2.read();
        }
        Input = 10*POT; //Inputberekening2(POT,0);  Inputberekening(Boardpotmeter.read());          // 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.
        // time_ += time_increment;
        double 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.
        Rood = 1;
        //Ref_prev = Ref2;
        Error = Referentie2-Input;
      // pc.printf("Input = %f Referentie= %f Error= %f \n", Input, Referentie2, Error);
        Motor_controller2(Error);
       // pc.printf("output = %f \n", motor1speed.read());
        
    }
    
    Groen = 0;
    wait (0.5);
}

void Determinetask()
{
    if (Button1pressed == 0) {

        Groen = 0;
        N++;
        wait (0.5);
        Groen = 1;
    }
}

int main()
{
    Rood = 1;
    Blauw = 1;
    Groen = 1;
    int i = 0;
    pc.printf("Hello World! %i \n", i);
    Finitestatemachine.attach(tickerfunctie,0.1);
    Loopticker.attach(tickerfunctie2,0.01);
    Hidscope.attach(scopeSend,0.01);                // Verzenden naar HIDscope

    while (true) {
        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
        if (Start==0 && Setting==0 && Dotask==0) { // In eerste instantie gaan de leds even uit

            Start = 1;
            Groen = 0;
            pc.printf("State 1\n");
        }

        if (Start==1 && Setting==0 && Dotask==0) {      // State 2 heeft Geel als kleur
            Groen = 0;
            Rood = 0;
            pc.printf("State 2\n");
            if (Button1pressed.read()==1 && Button2pressed.read()==0) {
                Setting = true;
                Start = false;
                Groen = 1;
                Rood = 1;
                pc.printf("State 2 A\n");
                wait(0.5);


            }
            if (Button1pressed.read()==0 && Button2pressed.read()==1 && N>0) {
                Groen = 1;
                Rood = 1;
                Dotask = 1;
                Start=0;
                pc.printf("State 2 B\n");
                wait(0.5);
            }
        }
        if (Start==0 && Setting==1 && Dotask==0) {          // Leds zijn uit, elke bepaling gaat het groene lampje even branden.
            pc.printf("State 2 C\n");
            P_controller(0,0);
            double Maximum = 10*POT;
            Groen = 1;
            pc.printf("Max = %f \n", Maximum);
            P_controller(0,1);
            double Minimum = 10*POT;
            Groen = 1;
            pc.printf("Min = %f \n", Minimum);
            Amplitude = (Minimum+Maximum)/2; 
            Baseline = (Maximum-Minimum)/2;
            Rood = 0;
            Blauw = 0;
            wait (2);
            Rood = 1;
            Blauw = 1;
            Setting = 2;
        }

        if (Start==0 && Setting==2 && Dotask==0) {          // Leds zijn uit, elke count gaat het groene lampje even branden.
            pc.printf("State 3\n");
            Determinetask();                            // Aantal gewenste herhalingen van Dotask instellen
            pc.printf("N = %i \n", N);
            if (Button1pressed.read()== 1 && Button2pressed.read()== 0) {
                Setting = 0;
                Dotask = 1;
                pc.printf("State 3 B\n");
            }
        }

        if (Start==0 && Setting==0 && Dotask==1) {
            pc.printf("State 4\n");
            while(i < N) {

                pc.printf("i = %i  N = %i \n", i, N);
                Extendfinger();
                //int J = 0;
                pc.printf("Extendfinger()afgerond\n", i, N);
                i++;
            }
            if (i==N) {
                Dotask = 0;
                Start = 1;
                i=0;
                pc.printf("Final state \n");
            }

        }
    }
}