Bidirectional PID Steering Wheel Control

#include "EBYTE.h"

#define ESerial Serial1
#define PIN_M0 8
#define PIN_M1 9
#define PIN_AX 10 

#define ENCODER0PINA         20      // this pin needs to support interrupts
#define ENCODER0PINB         21      // no interrupt required
#define CPR                  1250    // encoder cycles per revolution
#define CLOCKWISE            1       // direction constant
#define COUNTER_CLOCKWISE    2       // direction constant

#define RPWM 5
#define LPWM 7
#define REN 4
#define LEN 6

#define Button 44
int buttonstate = LOW;

EBYTE Transceiver(&ESerial, PIN_M0, PIN_M1, PIN_AX);

// variables modified by interrupt handler must be declared as volatile
volatile long encoder0Position = 0;
volatile long interruptsReceived = 0;

// track last position on encoder so we know whether it's worth printing new output
long previousPosition = 0;

//int pastPosition = 0; //stored last encoder position for error; different because of type. only used for Derror

// track direction: 0 = counter-clockwise; 1 = clockwise
short currentDirection = CLOCKWISE;


float Pgain = .02;
float Igain = 0.005;
float Dgain = 0.01;
int Ierror = 0;
int Pasterror = 0;

struct DATA1{
  int IncomingRadio;
};

struct DATA2{
  int OutgoingRadio;
};
DATA1 MyData1;
DATA2 MyData2;

void setup()
{
  Serial.begin(9600);
  ESerial.begin(9600);
  Serial.println("Starting Sender");
  Transceiver.init(); 
  Transceiver.SetAddressH(3);
  Transceiver.SetAddressL(2);
  Transceiver.SetChannel(5);
  Transceiver.SetMode(MODE_NORMAL);
  Transceiver.SetAirDataRate(ADR_1200);
  Transceiver.SetOptions(0b01000100);
  Transceiver.SaveParameters(PERMANENT);
  Serial.println(Transceiver.GetAirDataRate());
  Serial.println(Transceiver.GetChannel());
  Transceiver.PrintParameters();
  
  //Motor Driver outputs
   for(int i = 4; i <= 7; i++){
    pinMode(i, OUTPUT);
    }
  digitalWrite(LEN, HIGH);
  digitalWrite(REN, HIGH);
 
  // Encoder inputs
  pinMode(ENCODER0PINA, INPUT);
  pinMode(ENCODER0PINB, INPUT);

  pinMode(Button, INPUT);

  // interrupts
  attachInterrupt(3, onInterrupt, RISING);

}

void loop()
{
  buttonstate = digitalRead(Button);
  Transceiver.GetStruct(&MyData1, sizeof(MyData1));
  //Serial.print("Recieve: ");Serial.println(MyData1.IncomingRadio);
  int inputsignal = MyData1.IncomingRadio;
  int currentPosition = encoder0Position;
  
  
  //SERIALPLOTTER
  Serial.print(inputsignal);
  Serial.print(",");
  Serial.println(currentPosition);
  
  //Serial.print("Curret: ");Serial.println(currentPosition);
  
  int Perror = abs(inputsignal - currentPosition);
  Ierror = Ierror + Perror;
  int Derror = abs(Perror - Pasterror);
  if (Ierror > 25000)
  {
    Ierror = 25000;
  }
  int motorspeed = (Pgain * Perror) + (Igain * Ierror) + (Derror * Dgain);
  if (motorspeed > 50)
  {
    motorspeed=50; //this defines the max speed
  }
  //Serial.println(Derror);
  if (inputsignal > currentPosition+250)
  {
    //Serial.println("LEFT"); //motor moving to the right
    analogWrite(RPWM, 0);
    analogWrite(LPWM,motorspeed);
    //delay(100);  
  }
  else if (inputsignal < currentPosition-250)
  {
    //Serial.println("HIGH"); //motor moving to the left
    analogWrite(RPWM, motorspeed);
    analogWrite(LPWM, 0);
    //delay(100);
  }
  else
  {
    //Serial.println("center");
    analogWrite(RPWM, 0);
    analogWrite(LPWM, 0);
    Ierror=0;
    //delay(100);
  }
  
  
  if (buttonstate == HIGH){
    encoder0Position = 0;
  }
  delay(20);

  MyData2.OutgoingRadio = currentPosition;
  Transceiver.SendStruct(&MyData2, sizeof(MyData2));
  //Serial.print("Send: ");Serial.println(MyData2.OutgoingRadio);
  Pasterror = Perror;
  delay(20);
  
  if (encoder0Position != previousPosition)
  {
    previousPosition = encoder0Position;
  }
 
  
}

void onInterrupt()
{
  // read both inputs
  int a = digitalRead(ENCODER0PINA);
  int b = digitalRead(ENCODER0PINB);

  if (a == b )
  {
    // b is leading a (counter-clockwise)
    encoder0Position--;
    currentDirection = COUNTER_CLOCKWISE;
  }
  else
  {
    // a is leading b (clockwise)
    encoder0Position++;
    currentDirection = CLOCKWISE;
  }
}