POP32i Macanum with Wireless-X14 Control

#include <POP32.h>
#include <WCX_Soft.h>

WCX mywcx;

#define degToRad 0.0174f
float thetaRad, spd1, spd2, spd3, spd4, omega;
float omega_heading;
uint8_t rxCnt = 0, rxBuf[8];
float pvYaw;
#define head_Kp 2.0f
#define head_Ki 0.0f
#define head_Kd 0.0f
float head_error, head_pError, head_w, head_d, head_i;
int dir = 90;
int spd = 0;

void holonomic(float spd, float theta, float omega) {
  thetaRad = theta * degToRad;
  spd1 = spd * sin(thetaRad - M_PI_4) + omega;
  spd2 = -spd * sin(thetaRad + M_PI_4) + omega;
  spd3 = -spd * sin(thetaRad - M_PI_4) + omega;
  spd4 = spd * sin(thetaRad + M_PI_4) + omega;
  motor(1, spd1);
  motor(2, spd2);
  motor(3, spd3);
  motor(4, spd4);
}

void zeroYaw() {
  Serial1.begin(115200);
  delay(100);
  // Sets data rate to 115200 bps
  Serial1.write(0XA5);
  Serial1.write(0X54);
  delay(100);
  // pitch correction roll angle
  Serial1.write(0XA5);
  Serial1.write(0X55);
  delay(100);
  // zero degree heading
  Serial1.write(0XA5);
  Serial1.write(0X52);
  delay(100);
  // automatic mode
}
bool getIMU() {
  while (Serial1.available()) {
    rxBuf[rxCnt] = Serial1.read();
    if (rxCnt == 0 && rxBuf[0] != 0xAA) return false;
    rxCnt++;
    if (rxCnt == 8) {  // package is complete
      rxCnt = 0;
      if (rxBuf[0] == 0xAA && rxBuf[7] == 0x55) {  // data package is correct
        pvYaw = (int16_t)(rxBuf[1] << 8 | rxBuf[2]) / 100.f;
        return true;
      }
    }
  }
  return false;
}
void Auto_zero() {
  zeroYaw();
  getIMU();
  int timer = millis();
  oled.clear();
  oled.text(1, 2, "Setting zero");
  while (abs(pvYaw) > 0.02) {
    if (getIMU()) {
      oled.text(3, 6, "Yaw: %f  ", pvYaw);
      oled.show();
      beep();
      if (millis() - timer > 5000) {
        zeroYaw();
        timer = millis();
      }
    }
  }
  oled.clear();
  oled.show();
}
void heading(float spd, float theta, float spYaw) {
  head_error = spYaw - pvYaw;
  head_i = head_i + head_error;
  head_i = constrain(head_i, -180, 180);
  head_d = head_error - head_pError;
  head_w = (head_error * head_Kp) + (head_i * head_Ki) + (head_d * head_Kd);
  head_w = constrain(head_w, -100, 100);
  holonomic(spd, theta, head_w);
  head_pError = head_error;
}
void setup() {
  mywcx.begin(A0);
  Auto_zero();
  waitSW_A_bmp();
}
void loop() {
  if (SW_A()) {
    Auto_zero();
  }
  if (mywcx.getButton()) {
    if (mywcx.LL && mywcx.LU) {
      dir = 135;
      spd = 100;
    } else if (mywcx.LR && mywcx.LU) {
      dir = 45;
      spd = 100;
    } else if (mywcx.LL) {
      dir = 180;
      spd = 100;
    } else if (mywcx.LU) {
      dir = 90;
      spd = 100;
    } else if (mywcx.LR) {
      dir = 0;
      spd = 100;
    } else if (mywcx.LD) {
      dir = 270;
      spd = 100;
    } else if (mywcx.RU) {
      omega_heading = 0;
    } else if (mywcx.RL) {
      omega_heading = 45;
    } else if (mywcx.RR) {
      omega_heading = -45;
    } else {
      dir = 90;
      spd = 0;
    }

  } else {
    dir = 90;
    spd = 0;
  }
  getIMU();
  heading(spd, dir, omega_heading);
}