I have encoders on each wheel of my holonomic X-Drive.
What i would like to do is to be able to keep the robot driging exactly where the joystick is instead of veering off on a tangent. Due to lack of space our robot is terrible unbalanced causing it to veer to the left alot.
Any ideas on how to do this in ROBOTC?
Thanks 
What I think you want is equivalent to making sure that the robot doesn’t rotate when you aren’t telling it to. Having four encoders lets you measure rotation pretty accurately, because you can measure it twice - once by comparing how far the front and back wheels have turned and once by comparing how far the left and right wheels have turned. If the robot hasn’t rotated then the front and back wheels should have traveled the same distance and the left and right wheels should have also traveled the same distance.
If your drive is in an X configuration and you don’t want to use the terms front/back and left/right, you can refer to the diagonals as Trinidadian and Namibian but the code will be the same. What’s important is that comparing the amount of rotation for two wheels opposite each other allows you to measure the rotation of the robot.
Trinidadian diagonal:
and Namibian diagonal:
For this post, I’ll treat clockwise rotation as positive and anticlockwise rotation as negative, as shown:
No rotation is a rotation value of zero, which is our goal.
The encoders can be set up in any orientation, but to simplify the explanation we will assume that they are set up in a particular way.
On both the left and right wheels, the encoder counts upwards (positive) as the robot moves forwards and downwards (negative) as the robot moves backwards:
On both the front and back wheels, the encoder counts upwards (positive) as the robot moves right and downwards (negative) as the robot moves left:
The amount of clockwise rotation can be measured using **rotation **= **(front encoder value) **- (rear encoder value), or using **rotation **= (left encoder value) - (right encoder value). To make the measurement more reliable, you can calculate both and take the average. Both values should be similar, but there will be slight differences because of innacuracies caused mainly by the wheels slipping slightly on the surface.
Once you have a measurement for rotation you can use it to adjust the wheel speeds so that the drive won’t rotate.
To make the X drive move around without rotating, you might use:
motor[front] = VexRT[Ch1];
motor[back] = VexRT[Ch1];
motor[left] = VexRT[Ch2];
motor[right] = VexRT[Ch2];to force the drive not to rotate, you can change that code to:
motor[front] = VexRT[Ch1] - k*rotation;
motor[back] = VexRT[Ch1] + k*rotation;
motor[left] = VexRT[Ch2] - k*rotation;
motor[right] = VexRT[Ch2] + k*rotation;k is a float, which you set to some value that you work out by experimenting. If it is too low, the drive won’t correct itself fast enough. If it is too high, the drive will overcorrect and start to vibrate when it tries to correct itself.
Of course, there will be times when you want the drive to rotate. You can achieve that by doing something like this:
if (abs(VexRT[Ch4]) > 10)
{
while (abs(VexRT[Ch4]) > 5)
{
motor[front] = VexRT[Ch1] - VexRT[Ch4];
motor[back] = VexRT[Ch1] + VexRT[Ch4];
motor[left] = VexRT[Ch2] - VexRT[Ch4];
motor[right] = VexRT[Ch2] + VexRT[Ch4];
}
SensorValue[front_encoder] = 0;
SensorValue[rear_encoder] = 0;
SensorValue[left_encoder] = 0;
SensorValue[right_encoder] = 0;
}
The final part of this post was pretty rushed, so please ask questions if it’s confusing.
Hey, thanks for the info. But either I have misunderstood the meaning of an X-Drive or somebody has but our drive has it’s wheels on a 45 Degree angle to the front of the robot which is different to your diagram.
Sorry for any confusion
If you look closely, his example is exactly what you want. Just rotate it 45 degrees.
Just to specify, rotating it 45 degrees means to use x-drive code in place of the plus drive code shown. I believe Oliver used a plus drive because that is much more easier to think about and understand than x-drives. Also, he stated,
The diagonals will be split into Trinidadian (FrontLeft to BackRight) and Namibian (FrontRight to BackLeft). Here’s the Namibian flag if you’re wondering (because I can’t see it in the other post)
The code for an x-drive is (or at least similar to)
FrontLeft = Ch2 - Ch1 + Ch4
BackLeft = Ch2 + Ch1 + Ch4
FrontRight = -Ch2 - Ch1 + Ch4
BackRight = -Ch2 + Ch1 + Ch4
So, using Oliver’s code as a jumping off point, the drive program will be
if(abs(VexRT[Ch4]) > 10)
{
while (abs(VexRT[Ch4]) > 5)
{
motor[FrontLeft] = Ch2 - Ch1 + Ch4
motor[BackLeft] = Ch2 + Ch1 + Ch4
motor[FrontRight] = -Ch2 - Ch1 + Ch4
motor[BackRight] = -Ch2 + Ch1 + Ch4
}
Reset Your Encoders
}
else
{
FrontLeft = Ch2 - Ch1 - k*rotation; //Trinidadian Diagonal
BackLeft = Ch2 + Ch1 - k*rotation; //Namibian Diagonal
FrontRight = -Ch2 - Ch1 + k*rotation; //Namibian Diagonal
BackRight = -Ch2 + Ch1 + k*rotation; //Trinidadian Diagonal
}
My code is all pseudo code, so it won’t work directly, but it’s just adding motor] and semicolons, so it should be easy enough. You also need to find the rotation, but that’s just subtraction of Trinidadian and Namibian encoder values.
Hey don’t forget Tobago, the other half of Trinidad & Tobago. Ok, it’s hard to say Trinidadian & Tobagian…
Another way of keeping straight rotationally is to have background tasks or function keep the heading via a PID routine while using a gyro.
If the rotate joystick is in a dead zone, let the heading correction function tell you what it wants for a rotation speed in order to stay on the same rotational course. A rotational PID is nice but turn it on and off and reset the errors when doing so or you could be in for a nasty surprise with the integral pre-wound.
Or are you drifting off course in straight lines as a translational drift? Make some test autonomous programs. Set the two wheels on the opposite corners drive while the others are at 0 power. Make sure your motors are driving well on the 45’s without any translational drift. Once that is good, try the other ones.
You may have some weight distribution issues or a motor may be going bad or you may have put the wrong gears in one motor causing drift issues (81M did this in Sack Attack at worlds and it tanked them days 2-3.)
Thanks for that i’ll give that a go when i get to my robot