File content as of revision 29:7653adbbb101:

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



// Motor 1 & 2
DigitalOut      Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down
PwmOut          PowerMotor(D5); //van 0 tot 1
QEI             Encoder(D10,D11,NC,32,QEI::X2_ENCODING); //Encoder
DigitalOut      Direction2(D7);
PwmOut          PowerMotor2(D6);
QEI             Encoder2(D12,D13,NC,32,QEI::X2_ENCODING);
PwmOut          PowerServo(D3);

// Buttons & EMG (PotMeter)
DigitalIn       Button(PTC6);
DigitalIn       Button2(PTA4);
AnalogIn        PotMeter(A0);
AnalogIn        PotMeter2(A2);
//AnalogIn        EMG(A0);
//AnalogIn        Emg(A1);

// Tickers & timers
Ticker          MotorWrite;
Ticker          Sender;
Ticker          sampleEMG;
Timer           timer;

// Debugging
Serial          pc(USBTX, USBRX);
HIDScope        scope(6);



// Waardes
const double    twopi = 6.2831853071795;
const double    Fs=100;
int             Fired = 0;

// EMG
double          emg_value;
double          emg_value2;
const double    T1 = 0.33333; // Treshold 1
const double    T2 = 0.66666; // Treshold 2

// Motor 1 (Translatie)
double          n1 = 3.861464193; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog)
int             Pulses;
double          Rotatie = 0; //aantal graden dat de motor is gedraaid
double          Goal; //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 omhoog
const double    downlimit = 0.4;
const double    margin = 0.4;
bool            OutRange = false;

// Motor 2 (Rotatie)
double          n2 = 0.3125; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie)
int             Pulses2;
double          Rotatie2 = 0;
double          Goal2;
double          Error2 = 0;
double          Errord2 = 0;
double          Errori2 = 0;
double          Errorp2 = 0;
const double    Kp2 = 0.2;
const double    Kd2 = 10;
const double    Ki2 = 0.2;
double          v2 = 0;
double          turnlimit = 0.4; // max aantal rotaties voor roteren hele robot
// Margin 2 is in ons geval 0
bool            OutRange2 = false;

// Activatie tussen het schietgedeelte en terugkeergedeelte
bool            Excecute = false;
bool            Home = false;



// Voids voor berekeningen in het hoofdprogramma

void MotorSet()
{
    // Eerst motor 1 (translatie)
    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));

    // Dan motor 2 (rotatie)
    if (OutRange2) {
        Error2 = Goal2-Rotatie2; // De error die het motortje maakt ten opzichte van je Goal
        Errord2 = (Error2-Errorp2)/Fs;
        Errorp2 = Error2;
        if (fabs(Error2) <= 0.5) {
            Errori2 = Errori2 + Error2*1/Fs;
        } else {
            Errori2 = 0;
        }
        if (Error2>=0) {
            Direction2 = 0;
        } else {
            Direction2 = 1;
        }
        v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2;
    }
    PowerMotor2.write(fabs(v2));
}
void Send()
{
    Pulses = Encoder.getPulses();
    Rotatie = (Pulses*twopi)/4200; 
    Pulses2 = Encoder2.getPulses();
    Rotatie2 = (Pulses2*twopi)/4200;
    scope.set(0,Goal);
    scope.set(1,Rotatie);
    scope.set(2,emg_value);
    scope.set(3,Goal2);
    scope.set(4,Rotatie2);
    scope.set(5,emg_value2);
    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();
    emg_value2 = PotMeter2.read();
}

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


// Calibratie moet nog worden uitgevoerd!!!!


int main()
{
    upperlimit = n1*twopi;
    turnlimit = n2*twopi;
    pc.baud(115200);
    PowerMotor.write(0);
    PowerMotor2.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();
        Encoder2.reset();
        if (Button == 0) {
            Excecute =! Excecute;
        }
        while (Excecute ) {
            // Eerst wordt motor 1 aangestuurd
            pc.printf("Rotatie = %f \n", Rotatie); 
            pc.printf("Rotatie2 = %f \n", Rotatie2);
            if (Rotatie >= upperlimit) { //Als hij buiten bereik is
                Goal = upperlimit - margin;
                OutRange = true;
            }
            if (Rotatie <= downlimit) { //Als hij buiten bereik is
                Goal = 0 + margin;
                OutRange = true;
            }
            if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is
                OutRange = false;
            }
            if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing
                if (emg_value >= T2 ) {
                    Direction = 1;
                    v = 0.1;
                }
                if (emg_value > T1 && emg_value < T2) {
                    Direction = 0;
                    v = 0.1;
                }
                if (emg_value <= T1) {
                    Direction = 0;
                    v = 0;
                }
            }

            // Vanaf hier wordt motor 2 aangestuurd
            if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is
                Goal2 = turnlimit;
                OutRange2 = true;
            }
            if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is
                Goal2 = -turnlimit;
                OutRange2 = true;
            }
            if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is
                OutRange2 = false;
            }
            if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing
                if (emg_value2 >= T2 ) {
                    Direction2 = 1;
                    v2 = 0.05;
                }
                if (emg_value2 > T1 && emg_value2 < T2) {
                    Direction2 = 0;
                    v2 = 0.05;
                }
                if (emg_value2 <= T1) {
                    Direction2 = 0;
                    v2 = 0;
                }
            }
            if (Button2 == 0) { //Afvuren van de RBG
                Fire();
            }
        }

        while (Home) { //Terugkeren naar vaste positie
            OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd.
            Goal = 0;
            Goal2 = 0;
            if (fabs(Error)<=0.0015 && fabs(Error2)<=0.0015) {
                timer.start();
            } else {
                timer.stop();
                timer.reset();
            }
            if (timer.read() >= 3) {
                Home = false;
                Errori = 0;
                Errord = 0;
                Errorp = 0;
                Errori2 = 0;
                Errord2 = 0;
                Errorp2 = 0;
            }
        }
    }
}