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:
http://www.theflagshop.co.uk/ekmps/shops/speed/images/namibia-flag-3ft-x-2ft-4090-p.jpg
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.