File content as of revision 20:473735947e52:

#include "mbed.h"
#include "QEI.h"
#include "math.h"
#include "HIDScope.h"

DigitalOut Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down
PwmOut PowerMotor(D5); //van 0 tot 1
PwmOut PowerServo(D7);
DigitalIn Button(PTC6);
DigitalIn Button2(PTC4);
AnalogIn PotMeter(A1);
QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
Serial pc(USBTX, USBRX);
Ticker MotorWrite;
Ticker Sender;
Ticker EMG;
Timer timer;
HIDScope scope(3);


double z=0; //initiele waarde potmeter
const double twopi = 6.2831853071795;
int Pulses;
const double downlimit = 0.3
                         const double margin = 0.3
                                 double Rotatie; //aantal graden dat de motor is gedraaid
double Rotatieup=0; //aantal graden dat de motor is gedraaid in een bereik van n*pi
double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
double Error = 0;
double Errord = 0;
double Errori = 0;
double Errorp = 0;
const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
const double Kd = 10;
const double Ki = 0.2;
double v = 0; //snelheid van de motor (0-0.1
double upperlimit; //max aantal rotaties
bool Excecute = false;
bool Excecute2 = false;
const double Fs=100;

double n = 4.43546; // Aantal rondjes dat ons apparaat maximaal mag draaien

void MotorSet()
{
    v=Kp*Error + Kd*Errord + Ki*Errori;
    if (Error>=0) {
        Direction=1;
    } else {
        Direction=0;
    }
    PowerMotor.write(fabs(v));
}
void Send()
{
    scope.set(0,Goal);
    scope.set(1,Rotatieup);
    scope.set(2,Error);
    scope.send();
}
void EMGsignal()
{
    // Lees het EMG signaal uit
    //if (Treshold => x) {
//        v = 1
//    } else {
//        v = 0
//    }
//        PowerMotor.write(v)
}




int main()
{
    upperlimit = n*twopi;
    pc.baud(115200);
    PowerMotor.write(0);
    Sender.attach(Send,0.5/Fs);
    MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
    EMG.attach(EMGsignal,1/Fs);
    while (true) {
        Encoder.reset();
        if (Button == 0) {
            Excecute =! Excecute;
        }
        while (Excecute ) { //Dit wordt gebruikt voor motor 1
            Pulses = Encoder.getPulses();
            Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien
            Rotatieup = fmod(Rotatie,upperlimit); //Aantal radialen draaien binnen het bereik van upperlimit
            pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
            if (Rotatie >= upperlimit) {
                Goal = upperlimit - margin;
                Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
                Errord = (Error-Errorp)/Fs;
                Errorp = Error;
                if (fabs(Error) <= 0.5) {
                    Errori = Errori + Error*1/Fs;
                } else {
                    Errori = 0;
                }
                pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
            }
            if (Rotatie <= downlimit) {
                Goal = 0 + margin;
                Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
                Errord = (Error-Errorp)/Fs;
                Errorp = Error;
                if (fabs(Error) <= 0.5) {
                    Errori = Errori + Error*1/Fs;
                } else {
                    Errori = 0;
                }
                pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
            }

            else {
                // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
                Goal = z*upperlimit;
                Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
                Errord = (Error-Errorp)/Fs;
                Errorp = Error;
                if (fabs(Error) <= 0.5) {
                    Errori = Errori + Error*1/Fs;
                } else {
                    Errori = 0;
                }
                pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
            }
        }
        while (Excecute) { // Dit is voor motor 2, dus naar boven en naar beneden
            Pulses = Encoder.getPulses();
            Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien
            Rotatieup = fmod(Rotatie,upperlimit); //Aantal radialen draaien binnen het bereik van upperlimit
            pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
            if (Rotatie >= upperlimit) {
                Goal = upperlimit - margin;
                Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
                Errord = (Error-Errorp)/Fs;
                Errorp = Error;
                if (fabs(Error) <= 0.5) {
                    Errori = Errori + Error*1/Fs;
                } else {
                    Errori = 0;
                }
                pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
            }
            if (Rotatie <= downlimit) {
                Goal = 0 + margin;
                Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
                Errord = (Error-Errorp)/Fs;
                Errorp = Error;
                if (fabs(Error) <= 0.5) {
                    Errori = Errori + Error*1/Fs;
                } else {
                    Errori = 0;
                }
                pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
            }

            else {
                // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
                Goal = z*upperlimit;
                Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
                Errord = (Error-Errorp)/Fs;
                Errorp = Error;
                if (fabs(Error) <= 0.5) {
                    Errori = Errori + Error*1/Fs;
                } else {
                    Errori = 0;
                }
                pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
            }
        }



// Dit gedeelte moet vervangen worden if fired then Excecut2 = true after waiting 1 second
        //      if (fabs(Error)<=0.0015) {
//                timer.start();
//            } else {
//                timer.stop();
//                timer.reset();
//            }
//            if (timer.read() >= 5) {
//                Excecute=false;
//                Excecute2 = true;
//                Errori = 0;
//                Errord = 0;
//            }

//    }
        while (Excecute2) {
            Pulses = Encoder.getPulses();
            Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
            Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit
            pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
            Goal = 0.3; // Het doel waar hij naar toe moet
            Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
            Errord = (Error-Errorp)/Fs;
            Errorp = Error;
            if (fabs(Error) <= 0.5) {
                Errori = Errori + Error*1/Fs;
            } else {
                Errori = 0;
            }
            pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
            if (fabs(Error)<=0.0015) {
                timer.start();
            } else {
                timer.stop();
                timer.reset();
            }
            if (timer.read() >= 5) {
                Excecute=false;
                Excecute2 = false;
                Errori = 0;
                Errord = 0;
            }
        }
    }
}