#include "testApp.h"


void testApp::arduinoLoop() {
    Sleep(100);
Input = (arduino.getAnalog(0)/4.0);
if (PIDon){
   //Setpoint = (arduino.getAnalog(5)/4.0);


   /* Ile czasu upłynęło od ostatnich obliczeń */
   gettimeofday(&t1, NULL);
   now  = t1.tv_usec / 100000.0;

   if (now> lastTime)
   timeChange = (now - lastTime);
   if (now < lastTime)
   timeChange = (lastTime- now);
   if(timeChange>= .01)
   {

   /* Przeliczenie wszystkich zmiennych przechowujących odchyłki */


   dErr = (error - lastErr) / 1;

   /* Wyliczenie wyjścia PID */
   if  (pvar == false)
        Ep = 0;
        else{
           error = Setpoint - Input;
            Ep = (kp * error);
   }
     if (ivar == false)
        Ei = 0 ;
     else{
         errSum += error *1;
        Ei = (ki * errSum);
     }
     if (dvar == false)
        Ed = 0 ;
     else{
         dErr = (Input - lastinput) / 1;
        Ed = (kd * dErr);
     }


   Fout= Ep + Ei + Ed;
if (Fout > 255)
  Fout = 255;
if (Fout < 0 )
  Fout = 0;

   /* Zapamiętaj niektóre zmienne do następnych obliczeń */
   lastErr = error;
   lastTime = now;
   lastinput = Input;

      //analogPin0 = arduino.getAnalog(0)/1023.0; // odczyt pinu analogowego A0

           if (zero)
     Fout = 0;
      else
      arduino.sendPwm(3,Fout);
   }
}

   if (OnOffctr){
     if (Input > Setpoint) {
        Fout -= 2;
     }
     if (Input < Setpoint) {
        Fout += 10;


     }


           if (zero)
     Fout = 0;
      else
      arduino.sendPwm(3,Fout);

   }
if (DEAD){
        if (Input > Setpoint1)
            DEADctr = false;

        if (Input < Setpoint)
            DEADctr = true;
        if (DEADctr)
        Fout += 10;
        if (!DEADctr)
            Fout -= 2;

      if (zero)
      Fout = 0;
      else
      arduino.sendPwm(3,Fout);

   }


 // arduino.update();// pobranie ewentualnych aktualizacji Arduino
}// koniec void testApp::arduinoLoop()

