File content as of revision 11:a9a23042fc9e:

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

DigitalOut Direction(D4); //1 = CCW - 0 = CW
PwmOut PowerMotor(D5); //van 0 tot 1
DigitalIn Button(D1);
AnalogIn PotMeter(A1);
QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
Serial pc(USBTX, USBRX);
Ticker Pot;

double z=0; //initiele waarde potmeter
const double twopi = 6.2831853071795;
const double pi = twopi/2;
int Pulses;
double Rotatie; //aantal graden dat de motor is gedraaid
double Rotatieup; //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 Errorv = 0;
double Kp = 2; //Moet berekend worden aan de hand van Control concept slides
double Kd = 0.1;
double v = 0; //snelheid van de motor (0-0.1
double upperlimit; //max aantal rotaties
bool Excecute = false;

double n = 2;

void readpot()
{
    z = PotMeter.read(); //uitlezen Potmeter of voor EMG bijvoorbeeld
}


int main()
{
    upperlimit = n*twopi;
    pc.baud(115200);
    PowerMotor.write(0);
    Pot.attach(readpot,0.1); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde
    while (true) {
        if (Button == 0) {
            Excecute =! Excecute;
            }
        
            while (Excecute ) {
                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 ("Potmeter = %f\n", z);
                pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
                Goal = z*upperlimit; // Het doel waar hij naar toe moet
                Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
                Errorv = 0-v;
                if (Error >= 0) { //Bepalen van de rotatie richting
                    Direction =0;
                } else {
                    Direction =1;
                }
                pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
                v = Kp*fabs(Error)+Kd*fabs(Errorv); // Snelheid van motor
                PowerMotor.write(v); // Snelheid die naar de motor geschreven wordt (wordt alleen in een schaal van 0-1 weergegeven). (uit ervaring van Motor4_Setpoint loopt dit maar tussen 0-0.1).
            }
        }
    }