How do I get started with Ramsete in Vexcode V5 Pro

For some context, for our robot, we are currently using Odom and PID for our autonomous and programming skills but it doesn’t always work with 100% accuracy. I have been researching ways to make it more consistent and came up on the topic of control algorithms and have been looking at Ramsete for a while now. I took a look at Purdue Sigbots Page on Ramsete and got a basic understanding of what it is and coded the basic equations on the page. I want to start using it for our robot this year but all the documentation and posts about it are for FRC using the Okapilib imported library. If anyone has any suggestions or documentation on how to get it working for VRC I would greatly appreciate it.

Here is my code so far.

namespace Ramsete {
  double ErrorX, ErrorY, ErrorAngle;
  double GlobalX, GlobalY, GlobalAngle;
  double TargetX,TargetY, TargetAngle;
  double K, kB, kC, LinVel, AngVel;
  double DownScaler, Linear, Angular;
  double LinearMotor, AngularMotor, WheelCir = 2.75;
  double LeftPower, RightPower;
  void Ramsete(void);
}

void Ramsete::Ramsete(void) {
  kB = 1; 
  /* Roughly a proportional term for the controller.
   Large values of  will result in a more aggressive controller. 
   Must be > 0. */
  kC = 1;
  /* Roughly a damping term (like the D term of a PID controller).
   Must be between 0 and 1. */

  while(true) {
  GlobalX = Odom::X;
  GlobalY = Odom::Y;
  GlobalAngle = Odom::Angle;
  
  ErrorX = (cos(GlobalAngle) * (TargetX - GlobalX)) + (sin(GlobalAngle) * (TargetY - GlobalY));
  ErrorY = (cos(GlobalAngle) * (TargetX - GlobalX)) + ((sin(GlobalAngle) * -1) * (TargetY - GlobalY) );
  ErrorAngle = TargetAngle - GlobalAngle;

  DownScaler = .01;
  LinVel = ErrorX * DownScaler;
  AngVel = ErrorY * ErrorAngle;

  K = 2 * kC * sqrt((pow(AngVel, 2) + kB) * pow(LinVel, 2));

  Linear = LinVel * cos(ErrorAngle) + K * ErrorX;
  Angular = AngVel + K * ErrorAngle + ((kB * LinVel * sin(ErrorAngle) * ErrorY) / ErrorAngle);

  LinearMotor = Linear / WheelCir;
  AngularMotor = Angular / WheelCir;

  LeftPower = LinearMotor + AngularMotor;
  RightPower = LinearMotor - AngularMotor;
  wait(5, msec);
  }
}