File content as of revision 2:12efc0d812e1:

#include "PID.h"
#include "QEI.h"
#include "TextLCD.h"



#define RATE  0.01
#define Kc    0.473
#define Ti    0.025
#define Td    0.0

TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD16x2); // rs, e, d4-d7
// Motor
PwmOut Motor(p21);
QEI Qei(p29, p30, NC, 504);
PID Controller(Kc, Ti, Td, RATE);

// Definiranje konstanti


Timer t;                        
int startTimeUs, endTimeUs;     
int deltaUs = 0;                
int startPulse, endPulse;       
int deltaPulse = 0;             
int i = 0;



int Pulses     = 0;
int PrevPulses = 0;
float PwmDuty  = 1.0;
float Velocity = 0.0;
//Velocity to reach.
int goal = 21000;



// Deklaracija funkcija

//Set motors to go "forward".
void initializeMotors(void);
//Set up PID controllers with appropriate limits and biases.
void initializePidControllers(void);

float calcRpm(float deltaTimeSec, int deltaPulse);          

float calcRpm(float deltaTimeSec, int deltaPulse)           
{                                                           
     return 60.0 * ((deltaPulse / deltaTimeSec) / 1008 );   
}                                                           


void initializeMotors(void)
{

    Motor.period_us(50);
    Motor = 1.0;

}

void initializePidControllers(void)
{

    Controller.setInputLimits(0.0, 54500.0);
    Controller.setOutputLimits(0.0, 1.0);
    Controller.setBias(1.0);
    Controller.setMode(AUTO_MODE);

}

int main()
{

    //Initialization.
    initializeMotors();
    initializePidControllers();
//Set velocity set point.
    Controller.setSetPoint(goal);

    startTimeUs = t.read_us();              
    startPulse = Qei.getPulses();           
    t.start();                              
    
    
    while (1) {
        
        
        Pulses = Qei.getPulses();
        Velocity = (Pulses - PrevPulses) / RATE;
        PrevPulses = Pulses;
        Controller.setProcessValue(Velocity);
        PwmDuty = Controller.compute();
        Motor = PwmDuty;
        
        //Ispis brzine na TeraTerm
        if(i > 30) {
        endTimeUs = t.read_us();                                    
        // endPulse = Qei.getPulses();                                
        deltaUs = endTimeUs - startTimeUs;                          
        deltaPulse =Pulses - startPulse;
        lcd.locate(0,0);                         
        lcd.printf("N = %.0f\n",calcRpm(deltaUs/1000000.0, deltaPulse)); 
        lcd.printf(" okr/min"); 
        startPulse = Qei.getPulses();                               
        startTimeUs = t.read_us();
        i = 0;                                  
        }
        i = i+1;
        wait(RATE);
        
    }
}