File content as of revision 1:811bb0e318a8:

#include "PID.h"
#include "QEI.h"
#define RATE  0.01
#define Kc    0.473
#define Ti    0.025
#define Td    0.0

PwmOut Motor(p21);
QEI Qei(p29, p30, NC, 504);
PID Controller(Kc, Ti, Td, RATE);
//-------
// Datoteke
//-------
LocalFileSystem local("local");
FILE* fp;

Timer endTimer;

//Radne varijable

volatile int Pulses     = 0;
volatile int PrevPulses = 0;
volatile float PwmDuty  = 0.0;
volatile float Velocity = 0.0;
//Zadana brzina
int goal = 21000;
 

void initializeMotors(void);
void initializePidControllers(void);
void initializeMotors(void){
 
    Motor.period_us(50);
    Motor = 1.0;
}
 
void initializePidControllers(void){
 
    Controller.setInputLimits(0.0, 54500);
    Controller.setOutputLimits(0.0, 1.0);
    Controller.setBias(1.0);
    Controller.setMode(AUTO_MODE);
}
 
int main() {
 wait(2);
    //Inicijalizacija
    initializeMotors();
    initializePidControllers();
 
    //Kreiranje datoteke za spremanje rezultata.
    fp = fopen("/local/pidtest.csv", "w");
    
    endTimer.start();
 
    Controller.setSetPoint(goal);
 
    //Vrijeme mjerenja je 3 sekunde
    while (endTimer.read() < 3.0){
        Pulses = Qei.getPulses();
        Velocity = (Pulses - PrevPulses) / RATE;
        PrevPulses = Pulses;
        Controller.setProcessValue(Velocity);
        PwmDuty = Controller.compute();
        Motor = PwmDuty;
        fprintf(fp, "%f,%f,\n", Velocity, PwmDuty);
        wait(RATE);
    }
    //Spremanje izmjerenih podataka
    fclose(fp);
}