File content as of revision 23:c97e206cf2a7:

#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(D3);
DigitalIn       Button(PTC6);
DigitalIn       Button2(PTA4);
AnalogIn        PotMeter(A1);
AnalogIn        EMG(A0);
QEI             Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
Serial          pc(USBTX, USBRX);
Ticker          MotorWrite;
Ticker          Sender;
Ticker          sampleEMG;
Timer           timer;
HIDScope        scope(3);


double          emg_value;
const double    twopi = 6.2831853071795;
int             Pulses;
const double    margin = 0.3;
double          Rotatie = 0; //aantal graden dat de motor is gedraaid
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
const double    downlimit = 0.3;
bool            Excecute = false;
bool            Home = false;
bool            OutRange = false;
const double    Fs=100;
const double    T1 = 0.33333; // Treshold 1
const double    T2 = 0.66666; // Treshold 2
int             Fire = 0;

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

void MotorSet()
{
    if (OutRange) {
        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;
        }
        if (Error>=0) {
            Direction=1;
        } else {
            Direction=0;
        }
        v=Kp*Error + Kd*Errord + Ki*Errori;
    }
    PowerMotor.write(fabs(v));
}
void Send()
{
    Pulses = Encoder.getPulses();
    Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien
    scope.set(0,Goal);
    scope.set(2,emg_value);
    scope.send();
}
void EMGsample()
{
    // Lees het EMG signaal uit
    //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben
    emg_value = PotMeter.read();
}



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
    sampleEMG.attach(EMGsample,0.5/Fs);
    PowerServo.period_ms(20);
    while (true) {
        Encoder.reset();
        if (Button == 0) {
            Excecute =! Excecute;
        }
        while (Excecute ) { //Dit wordt gebruikt voor motor 1
            //pc.printf("Rotatie = %f \n", Rotatie);
            //pc.printf("Out of Range = %i \n", OutRange);
            if (Rotatie >= upperlimit-margin) {
                Goal = upperlimit - margin;
                OutRange = true;
            }
            if (Rotatie <= downlimit+margin) {
                Goal = 0 + margin;
                OutRange = true;
            }
            if (Rotatie >= margin && Rotatie <= (upperlimit - margin)) { // Zodra ik hem uit de Range stuur dan
                OutRange = false;
                // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
                if (emg_value >= T2 ) {
                    Direction = 1;
                    v = 1;
                }
                if (emg_value >= T1 && emg_value <= T2) {
                    Direction = 0;
                    v = 1;
                } 
                if (emg_value <= T1){
                    Direction = 0;
                    v = 0;
                }
            }
        }
        while (false ) { //Dit wordt gebruikt voor motor 2 MOET NOG OMGEZET WORDEN NAAR MOTOR 2!!!!
            if (Rotatie >= upperlimit-margin) {
                Goal = upperlimit - margin;
                OutRange = true;
            }
            if (Rotatie <= downlimit+margin) {
                Goal = 0 + margin;
                OutRange = true;
            }
            if (Rotatie >= margin && Rotatie <= (upperlimit - margin)) { // Zodra ik hem uit de Range stuur dan
                OutRange = false;
                // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
                if (emg_value >= T2 ) {
                    Direction = 0;
                    v = 1;
                }
                if (emg_value >= T1 && emg_value <= T2) {
                    Direction = 1;
                    v = 1;
                } else {
                    Direction = 0;
                    v = 0;
                }
            }
        }

        if (Button2 == 0) {
            PowerServo.write(0.27);
            wait (1);
            PowerServo.write(0.04);
            wait (1);
            Fire=Fire+1;
        }
        if (Fire == 3) {
            wait (1);
            Excecute = false;
            Home = true;
        }


        while (Home) {
            OutRange = false;
            Goal = margin; // Het doel waar hij naar toe moet
            if (fabs(Error)<=0.0015) {
                timer.start();
            } else {
                timer.stop();
                timer.reset();
            }
            if (timer.read() >= 3) {
                Excecute=false;
                Home = false;
                Errori = 0;
                Errord = 0;
            }
        }
    }
}