I am currently stuck on how to program my holonomic drive in autonomous.

There is an encoder on each wheel, with 2 I2Cs on the front wheels and 2 Quadrature encoders in the rear wheels.

There is so much variation in each run that it is driving me nuts. I do not think that the proper way is to add the encoders together and divide by 4…

Any help/tips is appreciated!

Many thanks,

Timothy Leung

```
int mySensorValue(tSensors sensorName)
{
if(sensorName == EncoderFrontLeft || sensorName == EncoderFrontRight)
{
float QuadEncoderValue;
QuadEncoderValue = SensorValue[sensorName] * 627.2 / 360;
return (int) QuadEncoderValue;
}
else
{
return SensorValue[sensorName];
}
}
// Ensure motor power is within -127 and 127 range
int motorCap(int motorPowervalue)
{
if (motorPowervalue >= 127)
{
return (127);
}
else if (motorPowervalue <= -127)
{
return (-127);
}
return (motorPowervalue);
}
//// This function drives the robot
//// notes:
//// a) 1 rotation of a wheel is 360 ticks.
////
//// EncoderTicks - distance to travel - 360 ticks = 1 rotation of the wheel
//// Target Power - speed to be applied
//// direction - 0 - 8 - N, NE, E, SE, S, SW, W, NW, TURN RIGHT, TURN LEFT
void drive(int EncoderTicks, int TargetPower = 50, int direction)
{
int joy_1_x = 0;
int joy_1_y = 0;
int joy_2_x = 0;
clearDriveEncoders();
switch(direction)
{
case 0: joy_1_x = 0; joy_1_y = 1; joy_2_x = 0; // N
break;
case 1: joy_1_x = 1; joy_1_y = 1; joy_2_x = 0; // NE
break;
case 2: joy_1_x = 1; joy_1_y = 0; joy_2_x = 0; // E
break;
case 3: joy_1_x = 1; joy_1_y = -1; joy_2_x = 0; // SE
break;
case 4: joy_1_x = 0; joy_1_y = -1; joy_2_x = 0; // S
break;
case 5: joy_1_x = -1; joy_1_y = -1; joy_2_x = 0; // SW
break;
case 6: joy_1_x = -1; joy_1_y = 0; joy_2_x = 0; // W
break;
case 7: joy_1_x = -1; joy_1_y = 1; joy_2_x = 0; // NW
break;
case 8: joy_1_x = 0; joy_1_y = 0; joy_2_x = 1; // turn right
break;
case 9: joy_1_x = 0; joy_1_y = 0; joy_2_x = -1; // turn left
break;
}
// apply power
joy_1_x = joy_1_x * abs(TargetPower);
joy_1_y = joy_1_y * abs(TargetPower);
joy_2_x = joy_2_x * abs(TargetPower);
while(((abs(mySensorValue(EncoderRearRight)) + abs(mySensorValue(EncoderRearLeft)) + abs(mySensorValue(EncoderFrontRight)) + abs(mySensorValue(EncoderFrontLeft))) / 4) < EncoderTicks)
{
motor[FrontDriveL] = motorCap((1*joy_1_y) + (joy_1_x) + (joy_2_x));
motor[FrontDriveR] = motorCap((1*joy_1_y) + (-1*joy_1_x) + (-1*joy_2_x));
motor[BackDriveROne] = motor[BackDriveRTwo] = motorCap((1*joy_1_y) + (joy_1_x) + (-1*joy_2_x))
motor[BackDriveLOne] = motor[BackDriveLTwo] = motorCap((1*joy_1_y) + (-1*joy_1_x) + (joy_2_x))
}
motor[FrontDriveL] = 0;
motor[FrontDriveR] = 0;
motor[BackDriveROne] = motor[BackDriveRTwo] = 0;
motor[BackDriveLOne] = motor[BackDriveLTwo] = 0;
clearDriveEncoders();
}
```