Quad-Encoder Error-Correction PID on Holonomic X-Drive?

Hi! I am programming a Vex competion robot for Starstruck and currently have a 4 motor holonomic x-drive with a shaft encoder on each wheel axle. When testing, it seems that one motor is running a bit slower than the rest, and causing the robot to not drive straight (checked for friction problems/ weight distribution already). What I would like to do is create a system that would compensate for the error in the motor while driving, so there is no "drifting, " except I have no idea how to code this. I have been told I can use a PID loop with the encoders, but since I have never done PID or encoder code before, I would appreciate some help. I would also like help on how to use the encoders to precisely measure position during auton/programming skills. I am using PROS to program. Thanks!

This is my x-drive code as of now:

int getChannelValue(int channel) { 
	//Sets channel values based on joystick input
	if (abs(joystickGetAnalog(1, channel))>15){
		return joystickGetAnalog(1, channel);
	else {
		return 0;

void setDriveMotors(int flSpeed, frSpeed, blSpeed, brSpeed) {
	motorSet(front_left_drive, flSpeed);        
	motorSet(front_right_drive, frSpeed);
	motorSet(back_left_drive, blSpeed);        
	motorSet(back_right_drive, brSpeed);

//Drive Code (in opcontrol while loop)
    	ch4 = getChannelValue(4); //Channel 4 = Forward/Backward (Y-Component)
    	ch3 = getChannelValue(3); //Channel 3 = Left/Right Strafing (X-Component)
    	ch1 = getChannelValue(1); //Channel 1 = Left/Right Turning (Rotation)
    	frontLeftSpeed = ch3 + ch4 + ch1 ; //Speed of motors for the drive = Y-Component + X-Component + Rotation
    	frontRightSpeed = -ch3 + ch4  + ch1;
    	backLeftSpeed = ch3 - ch4 + ch1;
    	backRightSpeed = -ch3 - ch4 + ch1;
    	setDriveMotors(frontLeftSpeed, frontRightSpeed, backLeftSpeed, backRightSpeed);

It’s going to get very complicated very fast…

I can think of 3 ways to do this effectively, both using several PID loops concurrently.

First method is to check each motor against each other, 4 PID loops for each motor, comparing its encoder value to the average values of the other 3 motors. A 5th loop is used to track overall distance.

Second method is to do the same thing except track motor velocity rather than distance.

Third way is to use PID loops tracking the distance moved diagonally (2 PID loops for this), how much the robot has turned, and a final one for how far the robot has moved. If you need help with actual PID code, there are a lot of resources available already on that. Getting an X-drive to drive straight using PID like this is difficult, but you will definitely learn a lot from the experience.

An added benefit from adding this code is that the X-drive will automatically correct its course if bumped in autonomous. However, this takes a long time to perfect; try replacing that slower motor first to see if that fixes the issue.

It is much easier to shift your coordinate system to be 45 degrees offset so the NE/SW wheels are one direction (x) and NW/SE is the other direction (y).

Measure your distances in each of those new coordinates and a PID for managing x and another PID for managing y error works beautifully. Do this separately from rotation unless you want to get more complicated. Having gyro and controlling rotation rotation can be added in there to keep rotation to non-existent. But in order to do so you have to have X Y and rotation in one task.

You get these curve like motions where the one direction will finish before the other one. It is not the maximum torque of motors but it works very accurately for position.