#include <Servo.h>
//#include <Adafruit_MPU6050.h>
//#include <Adafruit_Sensor.h>
//#include <Wire.h>

//Adafruit_MPU6050 mpu;
Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards

int pos = 0;    // variable to store the servo position
int potpin=A0;
int potpin1=A1;
int set_point = 90;
int val=100;
int val_servo=0;
int setpoint=90;
int temp_val=1;
int error_setpoint=0;
// parametrii inițiali PID
float T=1;
float tau=.1;
float KP=1;
float KD=1;
float KI= 1;


float Kp=0;
float Ki=0;
float Kd=0;
float bias=512;

float prop[] = {1,1};
float integ[]={1,1};
float deriv[]={1,1};
float error[]={1,1};
float xcomand[]={1,1};

void setup() {

 //Serial.begin(115200);

  
 //val = map(val, 0, 1023, 0, 180);
 //setpoint = map(setpoint, 0, 1023, 0, 180);
  Serial.begin(9600);
 Kp=KP;
 Ki=KP/T;
 Kd=KP*T;

}


//sensors_event_t event;


void loop() {
  //val = analogRead(potpin); 
 // val = map(val, 0, 1023, 0, 180);     // scale it to use it with the servo (value between 0 and 180)
  //val = event.acceleration.x;
  set_point = analogRead(potpin1); 
  //set_point = map(set_point, 0, 1023, 0, 180); 
  Serial.print("setpoint: ");
  Serial.print(set_point);
  Serial.println(" step");

  Serial.print("val: ");
  Serial.print(val);
  Serial.println(" step");
//Calcul valori discrete PID
  error[1]=set_point - xcomand[1];
  prop[1]=Kp*error[1];
  integ[1]=((Ki*T)/2)*(error[1]+error[0])+integ[0];
  deriv[1]= ((2*Kd)/(2*tau+T))*(error[1]-error[0])+((2*tau-T)/(2*tau+T))*deriv[0];
  xcomand[1]= prop[1]+integ[1]+deriv[1];

  Serial.print("error1: ");
  Serial.print(error[1]);
  Serial.println(" step");
  Serial.print("error0: ");
  Serial.print(error[0]);
  Serial.println(" step");
  Serial.print("xcomand: ");
  Serial.print(xcomand[1]);
  Serial.println(" step");
  prop[0]=prop[1];
  integ[0]=integ[1];
  deriv[0]=integ[1];
  xcomand[0]=xcomand[1];
  error[0]=error[1];
  //error[1]=set_point-xcomand[1];
  //xcomand[1] = map(xcomand[1], 0, 1023, 0, 180);
  

         //if (error[1] > 0){
            val_servo=xcomand[1];
           myservo.write(val_servo);
            delay(500);
        //}

       // if (error[1] < 0){
         //   val_servo=xcomand[1];
           //myservo.write(val_servo);
            //delay(500);
        //}
       

}