Help with Holonomoic Autonomous

VEX V5 General Discussion Technical Discussion
#1

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();



}
#2

Yes, adding them all and dividing by 4 probably will not work for all directions.

Think about what each wheel is doing. Driving forwards and backwards, ok that will work, all encoders either count up or down. But what about strafing, left wheel encoders cancel, right wheel encoders also cancel, add them all together and I think the result is probably no change at all.

I would simplify the code, just program forwards, backwards, left and right to start with. Forget about diagonal moves. Then start by just using two encoders, perhaps the ones on the front wheels. Then think about the change in encoder values.

Forwards - left increases - right increases
Backwards - left decreases - right decreases
Right - left increases - right decreases (may have that backwards)
Left - left decreases - right increases

Anyway, you get the idea. I’ve never actually programmed a mecanum in autonomous, need to tackle it in the next couple of weeks but I was planing to use a gyro.

I would also perhaps use more meaningful names for the variables, this is an excerpt from my standard code for mecanum.

    // Get joystick values
    DriveSystemGetControlValues( &forward, &right, &turn );

    if( theDrive.type == Mecanum4MotorFieldCentric )
    {
        // Get gyro angle in radians
        theta = degreesToRadians( GyroGetAngle() );

        // rotate coordinate system
        temp  = forward * cos(theta) - right * sin(theta);
        right = forward * sin(theta) + right * cos(theta);
        forward = temp;
    }

    // Set drive
    drive_l_front = forward + turn + right;
    drive_l_back  = forward + turn - right;

    drive_r_front = forward - turn - right;
    drive_r_back  = forward - turn + right;

    // followed by some value limiting and then the motors are set.

using forward, turn and right rather than joy_1x etc. really helps clarify what you want when thinking about the logic of the code.

Hope some of that helps.

#3

In regards to this, I am using



while(((abs(mySensorValue(EncoderRearRight)) + abs(mySensorValue(EncoderRearLeft)) + abs(mySensorValue(EncoderFrontRight)) + abs(mySensorValue(EncoderFrontLeft))) / 4) < EncoderTicks)

which would give me a positive encoder count no matter what.

Need to figure out the other half of your post

EDIT: Still confused.

#4

One of my teams was very successful last season using a gyro with mecanums. They built their autonomous up from short segments of turning with the gyro and driving straight forward with encoders. This was perfect for the tight quarters in the isolation zone.

Jay

#5

I was going to mention gyro…

This has been an age-old problem with Holo’s, because of the slip and the various ways in which it can cause the robot to move, it can become very problematic to program a Holo for autonomous.

Having put a fair amount of thought into this topic, here’s some food for thought.

First off, I like to think of a Holo as an X/Y scanner. This makes sense if you consider one axle as X and the other as Y, and consider that power can be applied to either X or Y independently.

Problem is, the robot can also rotate so the vector of this X/Y motion can vary and it quickly becomes apparent that knowing the robot’s heading with a good degree of accuracy, and high sample-rate is almost imperative in order to do any tracking.

I think the compass shines here. It’s a quick and dirty way of getting the robot’s heading without any physics, or crazy math or even that much code.

Once you have the heading, if you apply power evenly to the motors on a given axis (or axle) then tracking should become much more simplified. So none of those complex translate while rotating moves, but you can still move in any direction. I would suggest pausing to rotate, execute the rotate, then let the compass grab it’s new heading before continuing.

With this simplification or limit on the robot’s motion, creating a vector for the robot is possible, and adding all those up should produce the robot’s current position so long as you sample at a consistent rate and compensate for speed.

All in all, this is all theory and none of it is really easy. You may be better off using just a compass and accelerometer and not using the encoder values at all to calculate position.

Hope that maybe sparks some ideas, good luck.
-Cody

#6

Thanks for the help :smiley:

I guess it’s time I pickup a gyro…

#7