Field Centric X Drive Coding HELP!

Hi, thanks for stopping by this post. I need help with some coding for an field centric X Drive as I have been working for hours with no luck. I am using the starting location of the gyro and comparing the value to new values, but I can’t get it to not drive the robot when it’s rotated. I understand that adding the gyro value to x,z inputs will automatically drive it, but I don’t know how to separate it. If you have a better method and/better way of doing this. Feel free to post below. Thanks for your help!

please don’t ask for help with code without posting the code you need help with.

I was asking for how to do the code, like post an example. I’m trying to do trig, but I’m new to coding. I was wondering if you guys had code to compare it to.

The code is non existent as I keep changing it trying to find the solution.

Ok, lets assume that you use one joystick to drive your robot, where Forward/Backward direction is mapped to Y and Left/Right - to X, and you already figured out how to use X and Y to control X Drive motors in robot-centric frame.

To get to the field-centric frame, first, you need to convert X and Y inputs into power magnitude P = sqrt(XX+YY) and angle theta = atan2(y,x).

Then you add (or subtract) your Gyro reading from theta and convert the power levels into robot-centric X2 and Y2 using Pcos(theta2) and Psin(theta2).

If you just doing it to learn how this works, the above code should be enough, however, if you need to depend on this to play 2v2 games, you will need to add a lot of the code to ensure that you handle Gyro drift and could reset it (using line trackers when crossing white lines) after the error gets too large.

Drift error will grow gradually with time or jump suddenly after robot collides with a wall or another robot.

I wouldn’t recommend field-centric driving controls for a season with heavy defense and frequent robot-to-robot interactions.

When we were using holonomic drive in Skyrise we put the strips of the colored tape on sides of the robot to help drivers orient themselves in robot-centric frame and it worked well…

Thanks for the response. It was very helpful. I will try to code it and I’ll ask you some more questions if I can’t figure it out. Thanks again.

What do you mean by theta2 is it multiplied or squared?

It would be theta2=theta+gyroAngle.

Depending on how gyro is mounted you may need to subtract its output instead.

Ok, I’ll try it. I got it to move, but it’s finicky. Thank you so much. Should I divide the gyro value by something. It’s working, but when I slightly move it its switches direction of wheels very quickly until I stop moving it.

Yes, I totally forgot about scaling. Take a look at these threads:

The shaking was caused by the gyrosenser value being to high. Thank you so much! It works, I’ll fine tune it to have smoother transitions from rotation. When I rotate it and move forward, the motors start at like 20-30 power when they reverse and I don’t know how to fix it. You are amazing for helping me as I worked on this for 4 hours without luck, but thanks to you I have it moving the same direction as it rotates.

I’m having problems with the values meshing. When it’s 0 or 180 it works perfectly, but rotate it 90 and move forward, there’s a point where only 2, diagonal motors turn on and Vise versa. Here’s a photo of the coding used.

Hm, I never used graphical RobotC, but the code looks right to me. I am not sure what the issue might be, but X Drive is supposed to use only two motors when you drive along it diagonal. It is going to be both slower and weaker when it is using only two motors:

Try to set theta2 = theta, ignoring Gyro output, and see if robot drives the same way it would drive without extra transformations.

I separated the Gyro from the equations and it works perfectly. I’ll describe what was happening more precisely. Any point that is 0 or 180 from the starting position the commands work really well, but when I turn 90 or -90 it does not add both of the vectors together as 2 motors turn on and go diagonal, but its suppose to move straight away from me. A more descriptive version would be if Gyro < 90 then 2 motors activate,but if Gyro > 90 then the other two motors turn on and the previous motors turn off, but within this region all four motors should activate. The only way to get them to both turn on at the same time is to have the joystick perfectly straight with 3 and 4 channel. This happens with channel 3 and 4, but it does not happen at all when originated 0, 180 from starting position with channel 3 and 4. I have another question, with this new code setup. Is there a way to lock rotation as I drive to stabilize accuracy of X drive. I did it before by using > < commands that set the gyro to zero, but I cant use this because of the field oriented driving. If you don’t understand the behavior of the robot from my description. I can make a video of it.

More behaviors I noticed is that from the starting point, 0, if I turn 10 degrees or any degree close to zero. The motor speeds do not change until they reach 45 degrees, then instantly reverse. Is there a way to make this transition smoother. For instance, if I the robot is at 0 degrees and I move joystick forward, it moves away from me, but if I rotate it by ten degrees, it moves away from me diagonally to the right. Then once it passes 45 degrees. The motors reverse and it moves diagonally away from me to the left until it passes 90, then it switches diagonally towards me, then smooths out to normal driving again at 180.

If somebody can help me figure this part out it would be appreciated. The robot does not smoothly transition motor power when rotating. They kick on from 0 to 30, but they should slowly do so?

The issue might be that SensorValue[Gyro] is returning as integer and when you divide it by another int to get to radians it gets truncated. Try to divide it by floating point number i.e. 522.0 to ensure floating point math.

Also, try to send power values to motors using true speed mapping for keeping best ratio between X and Y.

It is very easy to add to your program - one global array and a function with a couple of if() statements to check for sign and upper array bound. Just make sure you follow this workaround or latest version of RobotC may have issues uploading the code:

This was from a Plus configured holonomic. For an X you just have to adjust the gyro offset by 45 degrees. I can’t find the blog post this was taken from.

Some elements are left to you. This uses slew rate (hence the motorReq]), and a logarithmic drive function to smooth out the joystick.

There was also some code to get the rotation to happen automatically but that can overpower things. Manual rotation worked well enough in drive control.

		NewNorth = (SensorValue[Gyro1]);
		// New North is where the robot belives north is.

		// use the log drive to ease the driving but still keep the 127 max
		x_joy= log_drive(vexRT[Ch4]);
		y_joy= log_drive(vexRT[Ch3]);
		rotatey_joy= log_drive(vexRT[Ch2]);
		rotatex_joy= log_drive(vexRT[Ch1]);

		//The 8UP button resets the gyro so the
		//current heading is considered forward.
		//This is helpful because we can reset North if we change our position.
		if (vexRT[Btn7U] == 1) {
			SensorValue[Gyro1] = 0;
			NewNorth = 0;

		direction_vector= sqrt((x_joy*x_joy)+(y_joy*y_joy));
		if(y_joy!=0 && y_joy>0)
			direction_angle = 10*radiansToDegrees(atan((float)x_joy/(float)y_joy));
		else if(y_joy !=0 && y_joy< 0)

			direction_angle = 10*radiansToDegrees(atan((float)(x_joy*-1)/(float)(y_joy*-1))+1800);
			//writeDebugStreamLine("special x_joy: %d y_joy %d  dir ang %f", x_joy, y_joy, direction_angle);
			if(x_joy==0)direction_angle = 0;
			if(x_joy<0)direction_angle = 1800;
		/*writeDebugStreamLine("x_joy: %d y_joy %d ", x_joy, y_joy);
		writeDebugStreamLine("direction_angle: %f ", direction_angle);
		writeDebugStreamLine("direction_vector: %d ", direction_vector);*/

		cosX = cosDegrees(NewNorth/10);
		sinY = sinDegrees(NewNorth/10);
		/*writeDebugStreamLine("gyro: %f ", NewNorth/10);
		writeDebugStreamLine("cos gyro: %f \n", cosX);
		writeDebugStreamLine("sin gyro: %f \n", sinY);*/

		drive_drift = (direction_angle - NewNorth)/10;  // divie by 10 since the angles are in gyro *10 range

		cosX = cosDegrees(drive_drift);
		sinY = sinDegrees(drive_drift);
		driveX = direction_vector * cosX;
		driveY = direction_vector * sinY;

		rotation = rotatex_joy;

		motorReq[East_drive]= limitMotor((driveX) + rotation);
		motorReq[West_drive] = limitMotor((driveX*-1) + rotation);
		motorReq[North_drive] = limitMotor((driveY*-1) + rotation);
		motorReq[South_drive] = limitMotor((driveY) + rotation);