Autosteer_USB_v5_0.ino

 /*
  * USB Autosteer code For AgOpenGPS
  * 4 Feb 2021, Brian Tischler
  * Like all Arduino code - copied from somewhere else :)
  * So don't claim it as your own
  */
  
////////////////// User Settings /////////////////////////  

  //How many degrees before decreasing Max PWM
  #define LOW_HIGH_DEGREES 5.0

  /*  PWM Frequency -> 
   *   490hz (default) = 0
   *   122hz = 1
   *   3921hz = 2
   */
  #define PWM_Frequency 0
  
  #define NUMPIXELS  35                 // Odd number dont use =0 
  #define Neopixel_Pin  9                 // Set this to the pin number you are using for the Neopixel strip controll line
  #define Neopixel_Brightness  35        // brightness value between 0 and 255
  #define mmPerLightbarPixel  20         // 40 = 4cm
  #include <Adafruit_NeoPixel.h>
  Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, Neopixel_Pin, NEO_GRB + NEO_KHZ800);
  const byte centerpixel = (NUMPIXELS-1)/2;
  byte levelcolor[NUMPIXELS][3];
  int16_t distanceFromLine = 32020;  // cross track error - Lightbar PGN (249) bytes 5 * 6. Start at 32020 so it is ignored untill a value is received from AOG
  int16_t prev_display_dist = 32000;  // used in the lightbar function to check if the lightbar actually needs updating

/////////////////////////////////////////////

  // if not in eeprom, overwrite 
  #define EEP_Ident 5100 

  // Address of CMPS14 shifted right one bit for arduino wire library
  #define CMPS14_ADDRESS 0x60

  // BNO08x definitions
  #define REPORT_INTERVAL 90 //Report interval in ms (same as the delay at the bottom)

  //   ***********  Motor drive connections  **************888
  //Connect ground only for cytron, Connect Ground and +5v for IBT2
    
  //Dir1 for Cytron Dir, Both L and R enable for IBT2
  #define DIR1_RL_ENABLE  4  //PD4

  //PWM1 for Cytron PWM, Left PWM for IBT2
  #define PWM1_LPWM  3  //PD3

  //Not Connected for Cytron, Right PWM for IBT2
  #define PWM2_RPWM  9 //D9

  //--------------------------- Switch Input Pins ------------------------
  #define STEERSW_PIN 6 //PD6
  #define WORKSW_PIN 7  //PD7
  #define REMOTE_PIN 8  //PB0

  //Define sensor pin for current or pressure sensor
  #define ANALOG_SENSOR_PIN A0
  
  #define CONST_180_DIVIDED_BY_PI 57.2957795130823

  #include <Wire.h>
  #include <EEPROM.h> 
  #include "zADS1115.h"
  ADS1115_lite adc(ADS1115_DEFAULT_ADDRESS);     // Use this for the 16-bit version ADS1115
  #include "BNO08x_AOG.h"
 
  //loop time variables in milliseconds  
  const uint16_t LOOP_TIME = 50;  //20Hz
  const uint32_t serialTimeoutms = 5; // ms  (takes just under 3 ms to receive a full PGN from the PC if it desnt get garbled by the Neopixels function
  uint32_t lastTime = LOOP_TIME;
  uint32_t currentTime = LOOP_TIME;
  uint32_t last249Time = 1000;  // ms
  uint32_t headerFoundTime = 1000;  // ms
  const uint32_t lightbarShowDelay = 20;  // ms  - This is used to prevent clashes with further PGNs still coming into the serial port from the PC
  bool pgn249timeout = true;
  bool prev249timeoutstate = false;

  const uint16_t WATCHDOG_THRESHOLD = 100;
  const uint16_t WATCHDOG_FORCE_VALUE = WATCHDOG_THRESHOLD + 2; // Should be greater than WATCHDOG_THRESHOLD
  uint8_t watchdogTimer = WATCHDOG_FORCE_VALUE;
  const int8_t BUFFER_BYTE_LIMIT = 30;
  
   //Parsing PGN
  bool isPGNFound = false, isHeaderFound = false;
  uint8_t pgn = 0, pgn_start = 0, dataLength = 0, idx = 0;
  int16_t tempHeader = 0;

  //show life in AgIO
  uint8_t helloAgIO[] = {0x80,0x81, 0x7f, 0xC7, 1, 0, 0x47 };
  uint8_t helloCounter=0;

  //fromAutoSteerData FD 253 - ActualSteerAngle*100 -5,6, Heading-7,8, 
        //Roll-9,10, SwitchByte-11, pwmDisplay-12, CRC 13
  uint8_t PGN_253[] = {0x80,0x81, 0x7f, 0xFD, 8, 0, 0, 0, 0, 0,0,0,0, 0xCC };
  int8_t PGN_253_Size = sizeof(PGN_253) - 1;

  //fromAutoSteerData FD 250 - sensor values etc
  uint8_t PGN_250[] = {0x80,0x81, 0x7f, 0xFA, 8, 0, 0, 0, 0, 0,0,0,0, 0xCC }; 
  int8_t PGN_250_Size = sizeof(PGN_250) - 1;
  uint8_t aog2Count = 0;
  float sensorReading, sensorSample;

  // booleans to see if we are using CMPS or BNO08x
  bool useCMPS = false;
  bool useBNO08x = false;

  // BNO08x address variables to check where it is
  const uint8_t bno08xAddresses[] = {0x4A,0x4B};
  const int16_t nrBNO08xAdresses = sizeof(bno08xAddresses)/sizeof(bno08xAddresses[0]);
  uint8_t bno08xAddress;
  BNO080 bno08x;

  float bno08xHeading = 0;
  double bno08xRoll = 0;
  double bno08xPitch = 0;

  int16_t bno08xHeading10x = 0;
  int16_t bno08xRoll10x = 0;
  
  //EEPROM
  int16_t EEread = 0;
 
  //Relays
  bool isRelayActiveHigh = true;
  uint8_t relay = 0, relayHi = 0, uTurn = 0;
  uint8_t tram = 0;

  //Switches
  uint8_t remoteSwitch = 0, workSwitch = 0, steerSwitch = 1, switchByte = 0;

  //On Off
  uint8_t guidanceStatus = 0;
  uint8_t prevGuidanceStatus = 0;
  bool guidanceStatusChanged = false;

  //speed sent as *10
  float gpsSpeed = 0;
  
  //steering variables
  float steerAngleActual = 0;
  float steerAngleSetPoint = 0; //the desired angle from AgOpen
  int16_t steeringPosition = 0; //from steering sensor
  float steerAngleError = 0; //setpoint - actual
  
  //pwm variables
  int16_t pwmDrive = 0, pwmDisplay = 0;
  float pValue = 0;
  float errorAbs = 0;
  float highLowPerDeg = 0; 
 
  //Steer switch button  ***********************************************************************************************************
  uint8_t currentState = 1, reading, previous = 0;
  uint8_t pulseCount = 0; // Steering Wheel Encoder
  bool encEnable = false; //debounce flag
  uint8_t thisEnc = 0, lastEnc = 0;

   //Variables for settings  
   struct Storage {
      uint8_t Kp = 40;  //proportional gain
      uint8_t lowPWM = 10;  //band of no action
      int16_t wasOffset = 0;
      uint8_t minPWM = 9;
      uint8_t highPWM = 60;//max PWM value
      float steerSensorCounts = 30;        
      float AckermanFix = 1;     //sent as percent
  };  Storage steerSettings;  //14 bytes

   //Variables for settings - 0 is false  
   struct Setup {
      uint8_t InvertWAS = 0;
      uint8_t IsRelayActiveHigh = 0; //if zero, active low (default)
      uint8_t MotorDriveDirection = 0;
      uint8_t SingleInputWAS = 1;
      uint8_t CytronDriver = 1;
      uint8_t SteerSwitch = 0;  //1 if switch selected
      uint8_t SteerButton = 0;  //1 if button selected
      uint8_t ShaftEncoder = 0;
      uint8_t PressureSensor = 0;
      uint8_t CurrentSensor = 0;
      uint8_t PulseCountMax = 5;
      uint8_t IsDanfoss = 0; 
  };  Setup steerConfig;          //9 bytes

  //reset function
  void(* resetFunc) (void) = 0;

  void setup()
  { 
    //PWM rate settings. Set them both the same!!!!
  
    if (PWM_Frequency == 1) 
    {
      TCCR2B = TCCR2B & B11111000 | B00000110;    // set timer 2 to 256 for PWM frequency of   122.55 Hz
      TCCR1B = TCCR1B & B11111000 | B00000100;    // set timer 1 to 256 for PWM frequency of   122.55 Hz
    }
  
    else if (PWM_Frequency == 2)
    {
      TCCR1B = TCCR1B & B11111000 | B00000010;    // set timer 1 to 8 for PWM frequency of  3921.16 Hz
      TCCR2B = TCCR2B & B11111000 | B00000010;    // set timer 2 to 8 for PWM frequency of  3921.16 Hx
  
    }
    
    //keep pulled high and drag low to activate, noise free safe   
    pinMode(WORKSW_PIN, INPUT_PULLUP); 
    pinMode(STEERSW_PIN, INPUT_PULLUP); 
    pinMode(REMOTE_PIN, INPUT_PULLUP); 
    pinMode(DIR1_RL_ENABLE, OUTPUT);
    
    if (steerConfig.CytronDriver) pinMode(PWM2_RPWM, OUTPUT); 
    
    //set up communication
    Wire.begin();
    Serial.begin(38400);
  
    //test if CMPS working
    uint8_t error;
    Wire.beginTransmission(CMPS14_ADDRESS);
    error = Wire.endTransmission();

    if (error == 0)
    {
      Serial.println(F("Error = 0"));
      Serial.print(F("CMPS14 ADDRESs: 0x"));
      Serial.println(CMPS14_ADDRESS, HEX);
      Serial.println(F("CMPS14 Ok."));
      useCMPS = true;
    }
    else 
    {
      Serial.println(F("Error = 4"));
      Serial.println(F("CMPS not Connected or Found"));
      useCMPS = false;
    }

    // Check for BNO08x
    if(!useCMPS)
    {
      for(int16_t i = 0; i < nrBNO08xAdresses; i++)
      {
        bno08xAddress = bno08xAddresses[i];
        
        Serial.print(F("\r\nChecking for BNO08X on "));
        Serial.println(bno08xAddress, HEX);
        Wire.beginTransmission(bno08xAddress);
        error = Wire.endTransmission();
    
        if (error == 0)
        {
          Serial.println(F("Error = 0"));
          Serial.print(F("BNO08X ADDRESs: 0x"));
          Serial.println(bno08xAddress, HEX);
          Serial.println(F("BNO08X Ok."));
          
          // Initialize BNO080 lib        
          if (bno08x.begin(bno08xAddress))
          {
            Wire.setClock(400000); //Increase I2C data rate to 400kHz
  
            // Use gameRotationVector
            bno08x.enableGameRotationVector(REPORT_INTERVAL); //Send data update every REPORT_INTERVAL in ms for BNO085
  
            // Retrieve the getFeatureResponse report to check if Rotation vector report is corectly enable
            if (bno08x.getFeatureResponseAvailable() == true)
            {
              if (bno08x.checkReportEnable(SENSOR_REPORTID_GAME_ROTATION_VECTOR, REPORT_INTERVAL) == false) bno08x.printGetFeatureResponse();

              // Break out of loop
              useBNO08x = true;
              break;
            }
            else 
            {
              Serial.println(F("BNO08x init fails!!"));
            }
          }
          else
          {
            Serial.println(F("BNO080 not detected at given I2C address."));
          }
        }
        else 
        {
          Serial.println(F("Error = 4"));
          Serial.println(F("BNO08X not Connected or Found")); 
        }
      }
    }
    
    EEPROM.get(0, EEread);              // read identifier
      
    if (EEread != EEP_Ident)   // check on first start and write EEPROM
    {           
      EEPROM.put(0, EEP_Ident);
      EEPROM.put(10, steerSettings);   
      EEPROM.put(40, steerConfig);
    }
    else 
    { 
      EEPROM.get(10, steerSettings);     // read the Settings
      EEPROM.get(40, steerConfig);
    }
    
    // for PWM High to Low interpolator
    highLowPerDeg = ((float)(steerSettings.highPWM - steerSettings.lowPWM)) / LOW_HIGH_DEGREES;

    adc.setSampleRate(ADS1115_REG_CONFIG_DR_128SPS); //128 samples per second
    adc.setGain(ADS1115_REG_CONFIG_PGA_6_144V);

  #if NUMPIXELS > 0
    pixels.begin();
    for (int i =0 ;i < centerpixel;i++){ //Right
      levelcolor[i][0]=0; levelcolor[i][1]=Neopixel_Brightness; levelcolor[i][2]=0; // Green (Right)
    }
    for (int i = centerpixel+1;i < NUMPIXELS;i++){ //Left
      levelcolor[i][0]=Neopixel_Brightness; levelcolor[i][1]=0; levelcolor[i][2]=0; // Red (Left)
    }
    levelcolor[centerpixel][0]=20; levelcolor[centerpixel][1]=20;levelcolor[centerpixel][2]=0;  // Yellow (centerpixel)
  #endif

  Serial.flush(); // Check all serial transmits are complete before entering the main program loop
  while (Serial.available()) {Serial.read();}  // Clear the serial buffer before we enter the main program loop

  }// End of Setup

  void loop()
  {
    // Loop triggers every 100 msec and sends back steer angle etc   
    currentTime = millis();
   
    if (currentTime - lastTime >= LOOP_TIME)
    {
      lastTime = currentTime;
  
      //reset debounce
      encEnable = true;
     
      //If connection lost to AgOpenGPS, the watchdog will count up and turn off steering
      if (watchdogTimer++ > 250) watchdogTimer = WATCHDOG_FORCE_VALUE;
  
      //read all the switches
      workSwitch = digitalRead(WORKSW_PIN);  // read work switch
      
      if (steerConfig.SteerSwitch == 1)         //steer switch on - off
      {
        steerSwitch = digitalRead(STEERSW_PIN); //read auto steer enable switch open = 0n closed = Off
      }
      else if (steerConfig.SteerButton == 1)    //steer Button momentary
      {
        reading = digitalRead(STEERSW_PIN);      
        if (reading == LOW && previous == HIGH) 
        {
          if (currentState == 1)
          {
            currentState = 0;
            steerSwitch = 0;
          }
          else
          {
            currentState = 1;
            steerSwitch = 1;
          }
        }      
        previous = reading;
      }
      else                                      // No steer switch and no steer button
      {
          // So set the correct value. When guidanceStatus = 1, 
          // it should be on because the button is pressed in the GUI
          // But the guidancestatus should have set it off first
          if (guidanceStatusChanged && guidanceStatus == 1 && steerSwitch == 1 && previous == 0)
          {
              steerSwitch = 0;
              previous = 1;
          }

          // This will set steerswitch off and make the above check wait until the guidanceStatus has gone to 0
          if (guidanceStatusChanged && guidanceStatus == 0 && steerSwitch == 0 && previous == 1)
          {
              steerSwitch = 1;
              previous = 0;
          }
      }
      
      if (steerConfig.ShaftEncoder && pulseCount >= steerConfig.PulseCountMax) 
      {
        steerSwitch = 1; // reset values like it turned off
        currentState = 1;
        previous = 0;
      }

      // Pressure sensor?
      if (steerConfig.PressureSensor)
      {
          sensorSample = (float)analogRead(ANALOG_SENSOR_PIN);
          sensorSample *= 0.25;
          sensorReading = sensorReading * 0.6 + sensorSample * 0.4;
          if (sensorReading >= steerConfig.PulseCountMax)
          {
              steerSwitch = 1; // reset values like it turned off
              currentState = 1;
              previous = 0;
          }
      }

      //Current sensor?
      if ( steerConfig.CurrentSensor)
      {
          sensorSample = (float)analogRead(ANALOG_SENSOR_PIN);
          sensorSample = (abs(512 - sensorSample)) * 0.5;
          sensorReading = sensorReading * 0.7 + sensorSample * 0.3;
          if (sensorReading >= steerConfig.PulseCountMax)
          {
              steerSwitch = 1; // reset values like it turned off
              currentState = 1;
              previous = 0;
          }
      }

      remoteSwitch = digitalRead(REMOTE_PIN); //read auto steer enable switch open = 0n closed = Off
      switchByte = 0;
      switchByte |= (remoteSwitch << 2); //put remote in bit 2
      switchByte |= (steerSwitch << 1);   //put steerswitch status in bit 1 position
      switchByte |= workSwitch;   
    
      //get steering position       
      if (steerConfig.SingleInputWAS)   //Single Input ADS
      {
        adc.setMux(ADS1115_REG_CONFIG_MUX_SINGLE_0);        
        steeringPosition = adc.getConversion();    
        adc.triggerConversion();//ADS1115 Single Mode 
        
         steeringPosition = (steeringPosition >> 1); //bit shift by 2  0 to 13610 is 0 to 5v
      }    
      else    //ADS1115 Differential Mode
      {
        adc.setMux(ADS1115_REG_CONFIG_MUX_DIFF_0_1);
        steeringPosition = adc.getConversion();    
        adc.triggerConversion();        
              
        steeringPosition = (steeringPosition >> 1); //bit shift by 2  0 to 13610 is 0 to 5v
      }
     
      //DETERMINE ACTUAL STEERING POSITION
            
        //convert position to steer angle. 32 counts per degree of steer pot position in my case
        //  ***** make sure that negative steer angle makes a left turn and positive value is a right turn *****
      if (steerConfig.InvertWAS)
      {
          steeringPosition = (steeringPosition - 6805  - steerSettings.wasOffset);   // 1/2 of full scale
          steerAngleActual = (float)(steeringPosition) / -steerSettings.steerSensorCounts;
      }
      else
      {
          steeringPosition = (steeringPosition - 6805  + steerSettings.wasOffset);   // 1/2 of full scale
          steerAngleActual = (float)(steeringPosition) / steerSettings.steerSensorCounts; 
      }
        
      //Ackerman fix
      if (steerAngleActual < 0) steerAngleActual = (steerAngleActual * steerSettings.AckermanFix);
      
      if (watchdogTimer < WATCHDOG_THRESHOLD)
      { 
       //Enable H Bridge for IBT2, hyd aux, etc for cytron
        if (steerConfig.CytronDriver) 
        {
          if (steerConfig.IsRelayActiveHigh) 
          {
            digitalWrite(PWM2_RPWM, 0); 
          }
          else  
          {
            digitalWrite(PWM2_RPWM, 1);       
          }          
        }
        else digitalWrite(DIR1_RL_ENABLE, 1);     
        
        steerAngleError = steerAngleActual - steerAngleSetPoint;   //calculate the steering error
        //if (abs(steerAngleError)< steerSettings.lowPWM) steerAngleError = 0;
        
        calcSteeringPID();  //do the pid
        motorDrive();       //out to motors the pwm value
      }
    else
      {
        //we've lost the comm to AgOpenGPS, or just stop request
        //Disable H Bridge for IBT2, hyd aux, etc for cytron
        if (steerConfig.CytronDriver) 
        {
          if (steerConfig.IsRelayActiveHigh) 
          {
            digitalWrite(PWM2_RPWM, 1); 
          }
          else  
          {
            digitalWrite(PWM2_RPWM, 0);       
          }
        }
        else digitalWrite(DIR1_RL_ENABLE, 0); //IBT2
                
        pwmDrive = 0; //turn off steering motor
        motorDrive(); //out to motors the pwm value
        pulseCount=0;
      }

      //send empty pgn to AgIO to show activity
      if (++helloCounter > 5)
      {
        Serial.write(helloAgIO,sizeof(helloAgIO));
        helloCounter = 0;
      }
    
    } //end of timed loop
  
    //This runs continuously, not timed //// Serial Receive Data/Settings /////////////////
 
    // Serial Receive
    //Do we have a match with 0x8081?    
    if (Serial.available() > 1 && !isHeaderFound && !isPGNFound) 
    {
      uint8_t temp = Serial.read();
      if (tempHeader == 0x80 && temp == 0x81) 
      {
        isHeaderFound = true;
        tempHeader = 0;
        headerFoundTime = currentTime;      
      }
      else  
      {
        tempHeader = temp;     //save for next time
        //return;  // skips the rest of the loop to try and receive the next byte      
      }
    }
  
    //Find Source, PGN, and Length
    if (Serial.available() > 2 && isHeaderFound && !isPGNFound)
    {
      pgn_start = Serial.read(); //The 7F or less
      pgn = Serial.read();
      dataLength = Serial.read();
      isPGNFound = true;
      idx=0;
    }
    
    //The data package
    if (Serial.available() > dataLength && isHeaderFound && isPGNFound)
    {
        if (pgn == 254) //FE AutoSteerData
        {
            //bit 5,6
            gpsSpeed = ((float)(Serial.read() | Serial.read() << 8)) * 0.1;

            prevGuidanceStatus = guidanceStatus;

            //bit 7
            guidanceStatus = Serial.read();
            guidanceStatusChanged = (guidanceStatus != prevGuidanceStatus);

            //Bit 8,9    set point steer angle * 100 is sent
            steerAngleSetPoint = ((float)(Serial.read() | Serial.read() << 8)) * 0.01; //high low bytes

            if ((bitRead(guidanceStatus, 0) == 0) || (gpsSpeed < 0.1) || (steerSwitch == 1))
            {
                watchdogTimer = WATCHDOG_FORCE_VALUE; //turn off steering motor
            }
            else          //valid conditions to turn on autosteer
            {
                watchdogTimer = 0;  //reset watchdog
            }

            //Bit 10 Tram 
            tram = Serial.read();

            //Bit 11 section 1 to 8
            relay = Serial.read();

            //Bit 12 section 9 to 16
            relayHi = Serial.read();


            //Bit 13 CRC
            Serial.read();

            //reset for next pgn sentence
            isHeaderFound = isPGNFound = false;
            pgn = dataLength = 0;

            //----------------------------------------------------------------------------
            //Serial Send to agopenGPS
            // Steer Data to AOG
            int16_t sa = (int16_t)(steerAngleActual * 100);
            PGN_253[5] = (uint8_t)sa;
            PGN_253[6] = sa >> 8;


            if (useCMPS)
            {
                Wire.beginTransmission(CMPS14_ADDRESS);
                Wire.write(0x02);
                Wire.endTransmission();

                Wire.requestFrom(CMPS14_ADDRESS, 2);
                while (Wire.available() < 2);

                //the heading x10
                PGN_253[8] = Wire.read();
                PGN_253[7] = Wire.read();

                Wire.beginTransmission(CMPS14_ADDRESS);
                Wire.write(0x1C);
                Wire.endTransmission();

                Wire.requestFrom(CMPS14_ADDRESS, 2);
                while (Wire.available() < 2);

                //the roll x10
                PGN_253[10] = Wire.read();
                PGN_253[9] = Wire.read();
            }
            else if (useBNO08x)
            {
                if (bno08x.dataAvailable() == true)
                {
                    bno08xHeading = (bno08x.getYaw()) * CONST_180_DIVIDED_BY_PI; // Convert yaw / heading to degrees
                    bno08xHeading = -bno08xHeading; //BNO085 counter clockwise data to clockwise data

                    if (bno08xHeading < 0 && bno08xHeading >= -180) //Scale BNO085 yaw from [-180°;180°] to [0;360°]
                    {
                        bno08xHeading = bno08xHeading + 360;
                    }

                    bno08xRoll = (bno08x.getRoll()) * CONST_180_DIVIDED_BY_PI; //Convert roll to degrees
                    //bno08xPitch = (bno08x.getPitch())* CONST_180_DIVIDED_BY_PI; // Convert pitch to degrees

                    bno08xHeading10x = (int16_t)(bno08xHeading * 10);
                    bno08xRoll10x = (int16_t)(bno08xRoll * 10);

                    //Serial.print(bno08xHeading10x);
                    //Serial.print(F",");
                    //Serial.println(bno08xRoll10x); 

                    //the heading x10
                    PGN_253[7] = (uint8_t)bno08xHeading10x;
                    PGN_253[8] = bno08xHeading10x >> 8;


                    //the roll x10
                    PGN_253[9] = (uint8_t)bno08xRoll10x;
                    PGN_253[10] = bno08xRoll10x >> 8;
                }
            }
            else
            {
                //heading         
                PGN_253[7] = (uint8_t)9999;
                PGN_253[8] = 9999 >> 8;

                //roll
                PGN_253[9] = (uint8_t)8888;
                PGN_253[10] = 8888 >> 8;
            }

            PGN_253[11] = switchByte;
            PGN_253[12] = (uint8_t)pwmDisplay;

            //add the checksum for AOG
            int16_t CK_A = 0;
            for (uint8_t i = 2; i < PGN_253_Size; i++)
            {
                CK_A = (CK_A + PGN_253[i]);
            }

            PGN_253[PGN_253_Size] = CK_A;

            //send to AOG
            Serial.write(PGN_253, sizeof(PGN_253));

            //Steer Data 2 -------------------------------------------------
            if (steerConfig.PressureSensor || steerConfig.CurrentSensor)
            {
              if (aog2Count++ > 2)
              {
                //Send fromAutosteer2
                PGN_250[5] = (byte)sensorReading;

                //add the checksum for AOG2
                CK_A = 0;
                for (uint8_t i = 2; i < PGN_250_Size; i++)
                {
                    CK_A = (CK_A + PGN_250[i]);
                }
                PGN_250[PGN_250_Size] = CK_A;

                Serial.write(PGN_250, sizeof(PGN_250));
                aog2Count = 0;
              }
            }

        // Stop sending the helloAgIO message
        if(helloCounter) helloCounter = 0;
        //--------------------------------------------------------------------------              
      }
              
      else if (pgn==252) //FC AutoSteerSettings
      {         
        //PID values
        steerSettings.Kp = ((float)Serial.read());   // read Kp from AgOpenGPS
        steerSettings.Kp*=0.5;
        
        steerSettings.highPWM = Serial.read();
        
        steerSettings.lowPWM = (float)Serial.read();   // read lowPWM from AgOpenGPS
                
        steerSettings.minPWM = Serial.read(); //read the minimum amount of PWM for instant on
        
        steerSettings.steerSensorCounts = Serial.read(); //sent as setting displayed in AOG
        
        steerSettings.wasOffset = (Serial.read());  //read was zero offset Hi
               
        steerSettings.wasOffset |= (Serial.read() << 8);  //read was zero offset Lo
        
        steerSettings.AckermanFix = (float)Serial.read() * 0.01; 

        //crc
        //udpData[13];        //crc
        Serial.read();
    
        //store in EEPROM
        EEPROM.put(10, steerSettings);           
    
        // for PWM High to Low interpolator
        highLowPerDeg = ((float)(steerSettings.highPWM - steerSettings.lowPWM)) / LOW_HIGH_DEGREES;
        
        //reset for next pgn sentence
        isHeaderFound = isPGNFound = false;
        pgn=dataLength=0;
      }
    
      else if (pgn == 251) //FB - steerConfig
      {       
        uint8_t sett = Serial.read();
         
        if (bitRead(sett,0)) steerConfig.InvertWAS = 1; else steerConfig.InvertWAS = 0;
        if (bitRead(sett,1)) steerConfig.IsRelayActiveHigh = 1; else steerConfig.IsRelayActiveHigh = 0;
        if (bitRead(sett,2)) steerConfig.MotorDriveDirection = 1; else steerConfig.MotorDriveDirection = 0;
        if (bitRead(sett,3)) steerConfig.SingleInputWAS = 1; else steerConfig.SingleInputWAS = 0;
        if (bitRead(sett,4)) steerConfig.CytronDriver = 1; else steerConfig.CytronDriver = 0;
        if (bitRead(sett,5)) steerConfig.SteerSwitch = 1; else steerConfig.SteerSwitch = 0;
        if (bitRead(sett,6)) steerConfig.SteerButton = 1; else steerConfig.SteerButton = 0;
        if (bitRead(sett,7)) steerConfig.ShaftEncoder = 1; else steerConfig.ShaftEncoder = 0;
        
        steerConfig.PulseCountMax = Serial.read();

        //was speed
        Serial.read(); 
        
        sett = Serial.read(); //byte 8 - setting1 - Danfoss valve etc

        if (bitRead(sett, 0)) steerConfig.IsDanfoss = 1; else steerConfig.IsDanfoss = 0;
        if (bitRead(sett, 1)) steerConfig.PressureSensor = 1; else steerConfig.PressureSensor = 0;
        if (bitRead(sett, 2)) steerConfig.CurrentSensor = 1; else steerConfig.CurrentSensor = 0;
              
        Serial.read(); //byte 9
        Serial.read(); //byte 10
         
        Serial.read(); //byte 11
        Serial.read(); //byte 12
      
        //crc byte 13
        Serial.read();
                
        EEPROM.put(40, steerConfig);
      
        //reset for next pgn sentence
        isHeaderFound = isPGNFound = false;
        pgn=dataLength=0; 
      
        //reset the arduino
        resetFunc();
      }

      else if (pgn==249) //FC Light bar PGN
      {
        pgn249timeout = false;
        last249Time = millis();
        //cross track error byte 5 & 6
        distanceFromLine = ((int16_t)(Serial.read() << 8 | Serial.read()));

        //crc byte 7
        Serial.read();

        //reset for next pgn sentence
        isHeaderFound = isPGNFound = false;
        pgn=dataLength=0;
      }

      //clean up strange pgns
      else
      {
        //reset for next pgn sentence
        isHeaderFound = isPGNFound = false;
        for (uint8_t i=0; i<=dataLength; i++){  // '<=' so that we catch the CRC byte too
          Serial.read();  // Read and discard the strange PGNs
        }
        pgn=dataLength=0;
      }

    } //end if (Serial.available() > dataLength && isHeaderFound && isPGNFound)      

    if (((Serial.available() > BUFFER_BYTE_LIMIT) && !isHeaderFound) ||
        ((Serial.available() > BUFFER_BYTE_LIMIT) && (currentTime - headerFoundTime > serialTimeoutms) && isHeaderFound)
        )
    {  // if our Serial buffer is getting too full and we arent in the middle of a PGN, or we are but it is taking too long
      while(Serial.available()){
        uint8_t temp = Serial.read();
        if (temp == 0x80){
          tempHeader = temp;  // if we find what might be the start of a PGN, stop emptying it and set up for the next loop
          break;
        }
      }
      isHeaderFound = isPGNFound = false;
      pgn = dataLength = 0;
    }
    

    if (encEnable)
    {
      thisEnc = digitalRead(REMOTE_PIN);
      if (thisEnc != lastEnc)
      {
        lastEnc = thisEnc;
        if ( lastEnc) EncoderFunc();
      }
    }

    #if NUMPIXELS >0
      if ((currentTime - last249Time >= (1000 + lightbarShowDelay)) and !pgn249timeout){  // if we havent received the xtrack error for over 1000ms
        pgn249timeout = true;  // gets reset to false when we receive the 249 PGN above
        distanceFromLine = 32000;
        digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));  // TODO - remove - for debugging only
      }
      if (currentTime - last249Time >= lightbarShowDelay) {   // if the current loop time is > the last time 249 PGN was received plus the show delay
        lightbar(distanceFromLine);
        prev249timeoutstate = pgn249timeout;
      }
    #endif

  } // end of main loop

  //ISR Steering Wheel Encoder
  void EncoderFunc()
  {        
     if (encEnable) 
     {
        pulseCount++; 
        encEnable = false;
     }            
  }

#if NUMPIXELS >0
  void lightbar(int16_t distanceFromLine ){
    int16_t display_dist = constrain (distanceFromLine / mmPerLightbarPixel , -centerpixel ,centerpixel);
    if ((display_dist != prev_display_dist) || (pgn249timeout != prev249timeoutstate)){   // if the display value is different or the timeout status has changed
      byte n = display_dist + centerpixel;
      for (int i = 0 ;i < NUMPIXELS; i++){
        if ( (i == centerpixel && i == n)|| //Center
             (display_dist < 0 && i >= n && i <= centerpixel && distanceFromLine != 32020 && distanceFromLine != 32000)|| //Right Bar
             (display_dist > 0 && i <= n && i > centerpixel && distanceFromLine != 32020 && distanceFromLine != 32000) ) //Left Bar
        {
          pixels.setPixelColor(i,levelcolor[i][0],levelcolor[i][1],levelcolor[i][2]);
        }else{
          if (i == centerpixel){
            if (pgn249timeout){
              pixels.setPixelColor(i,25,0,25); // Magenta (we havent received xtrack error for a long time)
            } else {
              pixels.setPixelColor(i,10,10,0); // Dimmer yellow (we arent on centre)
            }
          } else {
            pixels.setPixelColor(i,0,0,0); //Clear
          }
        }
      }
    prev_display_dist = display_dist;
    pixels.show();
    //delayMicroseconds(50);  // tiny delay after sending the colours to allow the colours to latch in the LEDs
    }
  }
#endif

  //TCCR2B = TCCR2B & B11111000 | B00000001;    // set timer 2 divisor to     1 for PWM frequency of 31372.55 Hz
  //TCCR2B = TCCR2B & B11111000 | B00000010;    // set timer 2 divisor to     8 for PWM frequency of  3921.16 Hz
  //TCCR2B = TCCR2B & B11111000 | B00000011;    // set timer 2 divisor to    32 for PWM frequency of   980.39 Hz
  //TCCR2B = TCCR2B & B11111000 | B00000100;    // set timer 2 divisor to    64 for PWM frequency of   490.20 Hz (The DEFAULT)
  //TCCR2B = TCCR2B & B11111000 | B00000101;    // set timer 2 divisor to   128 for PWM frequency of   245.10 Hz
  //TCCR2B = TCCR2B & B11111000 | B00000110;    // set timer 2 divisor to   256 for PWM frequency of   122.55 Hz
  //TCCR2B = TCCR2B & B11111000 | B00000111;    // set timer 2 divisor to  1024 for PWM frequency of    30.64 Hz

  //TCCR1B = TCCR1B & B11111000 | B00000001;    // set timer 1 divisor to     1 for PWM frequency of 31372.55 Hz
  //TCCR1B = TCCR1B & B11111000 | B00000010;    // set timer 1 divisor to     8 for PWM frequency of  3921.16 Hz
  //TCCR1B = TCCR1B & B11111000 | B00000011;    // set timer 1 divisor to    64 for PWM frequency of   490.20 Hz (The DEFAULT)
  //TCCR1B = TCCR1B & B11111000 | B00000100;    // set timer 1 divisor to   256 for PWM frequency of   122.55 Hz
  //TCCR1B = TCCR1B & B11111000 | B00000101;    // set timer 1 divisor to  1024 for PWM frequency of    30.64 Hz