#include <Servo.

h>
#include <FreqMeasure.h>

Servo myservo;

//inputs

double angle = 0;//set the angles

String readString;
double setpoint = 80;//I am setting it to move through 100 degrees
double Kp = 100;// you can set these constants however you like depending on trial
& error
double Ki = 7.07;
double Kd = 20;
double val = 0;
float last_error = 0;
float error = 0;
float changeError = 0;
float totalError = 0;
float pidTerm = 0;
float pidTerm_scaled = 0;

//int Ref_in = 3;

int CHPWA = 24;

int CHPWSW = 19;
#define PoT A16
#define ELEV A12
#define CHPWB A13
//outputs
int Dn = 23;
int Up = 22;
int En = 21;
int M2n = 20;
//variables
uint16_t valEle = 0;
uint16_t filteredEle;
int valPot = 0;
int valCHPWB = 0;
double sum=0;
int count=0;
float Time = 0;
int CHPWAState = 0;
int CHPWBState = 0;
int Tdesface = 120;

void setup() {
myservo.attach(9);
Serial2.begin(57600);
Serial.begin(9600);
FreqMeasure.begin();
pinMode(CHPWSW, INPUT);
pinMode(CHPWA, INPUT);
pinMode(Up,OUTPUT);
pinMode(Dn,OUTPUT);
pinMode(En,OUTPUT);
pinMode(M2n,OUTPUT);
pinMode(6,OUTPUT);
 analogReadResolution(13);
attachInterrupt(digitalPinToInterrupt(3), Ref_26, CHANGE);
}

void Ref_26()
{
// delayMicroseconds(Time-Tdesface);
delayMicroseconds(Time-Tdesface);
valEle = analogRead(ELEV);
// valCHPWB = analogRead(CHPWB);

void loop() {

if (FreqMeasure.available()) {
// average several reading together
sum = sum + FreqMeasure.read();
count = count + 1;
if (count > 30) {
float frequency = FreqMeasure.countToFrequency(sum / count);
// Serial.println(frequency);
sum = 0;
count = 0;
Time = ((1/frequency)*1000000)/4; //tiempo en microsegundos de cada media
onda
// Serial.print(" ");
// Serial.print(frequency);
// Serial.print(" ");
// Serial.println(Time);
}
digitalWrite(En,LOW);
valPot = analogRead(PoT);
valPot = map(valPot, 0, 8192, 0, 512);
setpoint = valPot;
filteredEle = firstOrderFilter (valEle, filteredEle, 75);
// Serial.println(filteredEle);
val = 0;
val = map(filteredEle, 0, 8192, 0, 512);

PIDcalculation();// find PID value

if (angle < setpoint-2) {

digitalWrite(Up,HIGH);
delayMicroseconds(100);
analogWrite(En,pidTerm_scaled);
digitalWrite(Dn,LOW);
}
else if (angle > setpoint+2){
digitalWrite(Dn,HIGH);
delayMicroseconds(100);
digitalWrite(Up,LOW);
analogWrite(En, pidTerm_scaled);
}
else{
digitalWrite(En,LOW);
digitalWrite(Up,LOW);
digitalWrite(Dn,LOW);
delayMicroseconds(100);
 }

Serial.print(angle);
Serial.print(" ");
Serial.print(error);
Serial.print(" ");
Serial.print(valPot);
Serial.print(" ");
Serial.println(pidTerm_scaled);
delayMicroseconds(100);

// Serial.println(valEle);

void PIDcalculation(){
angle = val ;//count to angle conversion
error = abs(setpoint - angle);
pidTerm_scaled = map(error, 0,512 , 50, 255);
// changeError = error - last_error; // derivative term
// totalError += error; //accumalate errors to find integral term
// pidTerm = (Kp * error) + (Ki * totalError) + (Kd * changeError);//total gain
// pidTerm = constrain(pidTerm, -255, 255);//constraining to appropriate value
// pidTerm_scaled = abs(pidTerm);//make sure it's a positive value

last_error = error;
// delay(5);
}