File content as of revision 14:b9c925a8176a:

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

DigitalOut Direction(D4); //1 = CCW - 0 = CW
PwmOut PowerMotor(D5); //van 0 tot 1
DigitalIn Button(D3);
AnalogIn PotMeter(A1);
Encoder MotorEncoder(D12,D13); //Encoder
Serial pc(USBTX, USBRX);
Ticker MotorWrite;
HIDScope scope(3);

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=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 Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
double v = 0; //snelheid van de motor (0-0.1
double upperlimit; //max aantal rotaties
bool Excecute = false;
double Fs=100;

double n = 8;

void MotorSet()
{
    v=Kp*Error;
    if (v>=0) {
        Direction=1;
    } else {
        Direction=0;
    }
    PowerMotor.write(fabs(v));
}

int main()
{
    upperlimit = n*twopi;
    pc.baud(115200);
    PowerMotor.write(0);
    MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde
    while (true) {
        if (Button == 0) {
            Excecute =! Excecute;
        }
        while (Excecute ) {
            Pulses = MotorEncoder.getPosition();
            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 = PotMeter.read()*upperlimit; // Het doel waar hij naar toe moet
            Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
            pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
            if (Error>=-0.1 && Error<=0.1) {
                Excecute=false;
                PowerMotor.write(0);
            }
            scope.set(0,Goal);
            scope.set(1,Rotatieup);
            scope.set(2,Error);
            scope.send();
        }
    }
}