File content as of revision 0:638eb51d9082:

/*
 * Date: march 2020.
 * Decription: Bilo je potrebno napraviti sistem PID regulatora ciji je osnovni
    zadatak odrzavanja pritiska vrednosti 0 Bara. 
    Sistem se sastoji iz: 
        1.Senzora pritiska
        2.Step motora koji pogoni elektricnu pumpu
    Izlaz PID regulatira je frekvencija obrtanja step motora koji tera pumpu. 
    Sto se motor brze okrece to pumpa vise kolicine izvlaci.
    Na ovaj nacin obezbedjeno je precizno protocno merenje kolicine u ispitivanju
    injekotra.
    Sistem tokom celokupnog rada zeli da stabilise pritisak od 0 Bari i time
    ponisti razliku pritisaka koja se javlja usled odredjene kolicine goriva u 
    sistemu.
 
    - Dugorocnim testiranjem utvrdjeni su parametri PID kontrolera.
    - Senzor pritiska se filtira sa MID7 filterom
    - Izmerena kolicina se filtrira sa MID7 filterom
    - PID procesuira zahteve svakih 100ms
*/
#include "mbed.h"
#include "AsynchSerial.h"
#include "PID.h"
#include "FastPWM.h"

#define RATE 0.1
#define ZERO 2.33
AsynchSerial pc(USBTX, USBRX, 115200);

EventQueue queue;
Ticker tick;

//Kc, Ti, Td, interval
PID controller(2.5, 1, 0.0, RATE);
AnalogIn pv(PA_0);
FastPWM   co(PA_1,2);

//mm u opsegu od 0 do 100

float reff; 
float value;
float inValue;

float analogInValues[7];
float freqValues[7];
volatile int cntFreq = 0;
int n = 7;
int enable = 0;
int width = 0;

void getData() {
/*Citanje podatka iz Buffera Serijske komunikacije -uz resnje GRESKE citanja*/
    
    unsigned char buffer[20];
    int16_t len = pc.read(buffer, sizeof(buffer));

    if (len > 0) {
       
        int kom = sscanf((const char *)buffer,"%f",&reff);
       
        if(kom==1){ 
            if(reff > 100.0){
                controller.setSetPoint(0);
                printf("Greska! Referenca veca od 100 mm");
            }else 
                controller.setSetPoint(reff);
        }
    }
}

void rxCb() {
    
    queue.call(getData);  // can't read data in callback
    
}
void printuj(void){
    freqValues[cntFreq++] = value/2.0;
    if(cntFreq == 7){
        cntFreq = 0;
         for (int i = 0; i < n; i++)
            // Last i elements are already in place
            for (int j = 0; j < n-i-1; j++)
                if (freqValues[j] > freqValues[j+1]){
                    float temp = freqValues[j];
                    freqValues[j]=freqValues[j+1];
                    freqValues[j+1]=temp;
                }
                
        printf("Ref: %.1f \n",reff);
        printf("Input Analog High: %.1f mm \n", inValue);
        printf("F: %.1f Hz\n",freqValues[4]);
        printf("Q: %.1f ml\n",freqValues[4]*0.045);
         
    } 
}
void setData(void){
    
    queue.call(printuj);
    
}
 
int main(){
        
      Thread eventThread;
      eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
      pc.attach(callback(rxCb), AsynchSerial::RX); 
      pc.init(); //Serial buffer interrupt
      tick.attach(&setData, 0.3);
        
      //Set the period to 1ms = 1kHz
      co.pulsewidth_ms(0);     
      co.prescaler(2);
      
      controller.setInputLimits(-100.0, 100.0);
      //Pwm output from 0.0 to 1.0
      controller.setOutputLimits(4, 3200);
      //If there's a bias.
      controller.setBias(0);
      controller.setMode(1); //not null value is AUTO MODE
      //We want the process variable to be 1.7V
      controller.setSetPoint(0.0);
     
      while(1){
        
        for(int i=0;i<n;i++){
            analogInValues[i] = pv.read();
            wait_ms(10);
        }
        // sort . . .
        for (int i = 0; i < n; i++)
            // Last i elements are already in place
            for (int j = 0; j < n-i-1; j++)
                if (analogInValues[j] > analogInValues[j+1]){
                    float temp = analogInValues[j];
                    analogInValues[j]=analogInValues[j+1];
                    analogInValues[j+1]=temp;
                }
        
        //Update the process variable.
        //input u mm
        inValue = (float)(((ZERO - analogInValues[4] * (float)3.29) * 100.0)/1.59)*(-1.0);
        //inValue = (float)(((ZERO - pv.read() * (float)3.29) * 100.0)/1.59);
        controller.setProcessValue(inValue);
        
        //Set the new output.
        value = controller.compute();
        
        if(value == 0){
            enable = 0;
            printf("Greska! Value = 0");
        }else{
            int period = (int)(1.0/value * 1000000);
            co.period_us(period); 
            enable = 1;
        }   
        if(enable == 1){
            if(value > 100)
                co.pulsewidth_us(25);
            else
                co.pulsewidth_ms(2);
        }else{
            co.pulsewidth_us(0);
        }         
        wait_ms(100 - 10*n);
        
      }
}