```
int gearRatio = 298;
int ppr = 3;
//motor 1
const int enablePin1 = 8;
const int pin1 = 10;
const int pin2 = 12;
//encoder
const byte interruptPin = 3;
const int pinEncoder = 9; //interrupt pin
//PID constants
double Kp1 = 18.7;
double Kd1 = 2.11;
double Ki1 = 14.57;
double demandPosition = 0;
volatile float currentPosition = 0;
double errorPosition = 2;
double errorPosition_diff = 0;
double errorPosition_sum = 0;
double errorPosition_prev = 0;
double Output = 0;
double integral;
volatile long int Position = 0; //count ticks from encoder
float degPosition = 0.0;
float radPosition = 0.0;
int flag = 0;
// Initialize timer
long t = 0;
long t0 = micros();
const int freq = 1000;
// Calculate sampling time in us
const int dt = round((1.0/(float)freq)*1000000.0);
void setup () {
Serial.begin(115200);
pinMode(enablePin1,OUTPUT);
digitalWrite(enablePin1, HIGH);
pinMode(pin1, OUTPUT);
pinMode(pin2, OUTPUT);
pinMode(interruptPin, INPUT_PULLUP);
pinMode(pinEncoder, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(interruptPin), encoder, RISING);
}
void loop () {
if (flag==0) {
while(t0<6000000){
if (t>dt) {
if(degPosition>=360.0) {
Position = 0;
}
if (degPosition<=-360.0) {
Position = 0;
}
degPosition = (Position*360.0/(ppr*gearRatio));
radPosition = degPosition*0.0174532925;
demandPosition = 90.0;
currentPosition = degPosition;
errorPosition = demandPosition-currentPosition;
Serial.print(currentPosition, 3);
Serial.print("\t");
errorPosition_diff = (errorPosition-errorPosition_prev)/dt;
errorPosition_sum += errorPosition;
integral = errorPosition_sum*dt*Ki1;
if (integral > 255) {integral = 255;}
if (integral < -255) {integral = -255;}
Output = errorPosition*Kp1 + errorPosition_diff*Kd1 + integral;
errorPosition_prev = errorPosition;
if (Output >= 0) {
if(Output>255) {Output = 255;}
else if(Output<30) {Output = Output+30;}
}
else {
if(Output <-255) {Output = -255;}
else if(Output>-30) {Output = Output-30;}
}
Serial.print(" This is the Output: ");
Serial.println(Output);
if (Output >= 0) {
analogWrite(pin1, round(Output));
analogWrite(pin2, 0);
//Serial.println("clockwise");
} else {
analogWrite(pin2, round(-Output));
analogWrite(pin1, 0);
//Serial.println("Counterclockwise");
}
t0 = micros();
}
else {
t = micros()-t0;
}
}
digitalWrite(enablePin1, LOW);
flag = 1;
}
}
void encoder() {
if(digitalRead(pinEncoder)==LOW){
Position++;
}else {
Position--;
}
}
```

I am trying to implement PID control for position of a DC motor with gearbox and encoder. PD works pretty fine. When I tried to add the "I" term however it seems not working. Maybe I am doing something wrong? Thanks for the help