Kugoo S1 Pro Dual Motor Arduino Serial MiddleMan

#include <SoftwareSerial.h>

SoftwareSerial portOne(4, 7);
SoftwareSerial portTwo(4, 5);
SoftwareSerial portThree(8, 9);

uint32_t LastControllerTimeStamp = 0;
uint32_t TimeBetweenLastTwoMessages = 100;
uint32_t LastScreenTimeStamp = 0;
bool bIsListening = false;

uint8_t bufferRead[32];
uint8_t bufferRead2[32];

float CurrentInitValue = 0;

void setup() {
  portOne.begin(9600);
  portTwo.begin(9600);
  portThree.begin(9600);
  portThree.listen();

  for(int i = 0; i < 5; i++)
  {
    CurrentInitValue += analogRead(1);
  }
  CurrentInitValue /= 5;
}

int getCRC(uint8_t *Array, int ByteCount)
{
  uint16_t CRC = 0;
  Array++;
  for(int i = 1; i < ByteCount-2; i++)
  {
    CRC += *Array;
    Array++;
  }
  return (CRC>>8) | (CRC<<8);
}

void loop() {

  if((millis() - LastControllerTimeStamp) < (TimeBetweenLastTwoMessages - 55))
  {
    if(!bIsListening)
    {
      bIsListening = true;
      portOne.listen();
    }
  }
  else
  {
    if(bIsListening)
    {
      bIsListening = false;
      portThree.listen();
    }
  }


  if(bIsListening)
  {
    if(portOne.available() > 0)
    {
      int ByteCount = 0;
      while (portOne.available() > 0) {
        bufferRead[ByteCount] = portOne.read();
        ByteCount++;
        if(portOne.available() == 0)
        {
          delay(2);
        }
      }
      memcpy(bufferRead2, bufferRead, ByteCount);

      if(ByteCount == 8)
      {
        bufferRead[5] = map(bufferRead[5], 0x00, 0xD3, 0x0A, 0xB0);
        bufferRead2[5] = bufferRead[5];
        if(bufferRead[5] > 0x0A)
        {
            if(bufferRead[5] < 120)
            {
              bufferRead[5] = 0;
              bufferRead[4] = 0;
            }
            else
            {
              bufferRead[5] = 0x0A + ((bufferRead[5]-121) * 3);
              bufferRead[4] = 0;
            }
        }

        uint16_t CRC = getCRC(bufferRead, ByteCount);
        uint16_t CRC2 = getCRC(bufferRead2, ByteCount);
        memcpy(&bufferRead[ByteCount-2], &CRC, sizeof(uint16_t));
        memcpy(&bufferRead2[ByteCount-2], &CRC2, sizeof(uint16_t));

        LastScreenTimeStamp = millis();
      }

      portOne.write(bufferRead2, ByteCount);
      portTwo.write(bufferRead, ByteCount);

    }
  }
  else
  {
    // CONTROLLER

    if(portThree.available() > 0)
    {
      int ByteCount = 0;
      while (portThree.available() > 0) {
        bufferRead[ByteCount] = portThree.read();
        ByteCount++;
        if(portThree.available() == 0)
        {
          delay(2);
        }
      }

      if(ByteCount == 11)
      {
        float ReadCurrent = abs(analogRead(1) - CurrentInitValue) + 7;
        bufferRead[4] = uint8_t( (float) ( ReadCurrent / float(15.06) ) );
        uint16_t CRC = getCRC(bufferRead, ByteCount);
        memcpy(&bufferRead[ByteCount-2], &CRC, sizeof(uint16_t));
      }

      portThree.write(bufferRead, ByteCount);

      if(ByteCount == 11)
      {
        TimeBetweenLastTwoMessages = millis() - LastControllerTimeStamp;
        if(TimeBetweenLastTwoMessages > 530)
        {
          TimeBetweenLastTwoMessages = 469;
        }
        LastControllerTimeStamp = millis();
      }
    }
  }
}