# Can I use gyro to drive straight in teleop period?

I wanted to know if you can drive straight using the gyro sensor because we don’t have encoder to correct the speed of each motor. We’re using tankdrive and right now the problem we’re facing is that it’s not going in the direction we wanted. For instance, if we move left and right stick to up direction the robot doesn’t go straight, and for some reason, I think the speed of left side of the robot is going faster then the right side.

Also, I wanted to know if we can use it in autonomous mode. Like I need help with turning the robot with gyro (corresponding the wheel with the gyro angle.)

This is our first year so we’re rookie, and their no programming mentor to mentor us.

It would be very helpful with encoders but it is possible to do it without. Consider the differential of the two tank sticks is directly proportional to the turning rate of the robot. The bigger the difference, the faster the robot should turn. And the turning rate of the robot is the raw output of the gyro (rotational velocity). So in theory, you can do this:

``````
double drivePower = (leftStick + rightStick)/2.0;
double stickDifferential = leftStick - rightStick;
double motorDifferential = Kp*(stickDifferential - gyro.getScaledRotationRate());
leftMotor.set(drivePower + motorDifferential);
rightMotor.set(drivePower - motorDifferential);

``````

Note: this is pseudocode, not real code. It is intending to demonstrate the concept. If stickDifferential is zero, that means you want to go straight. If the gyro rotation rate is zero, that means your robot is going straight. Then motorDifferential is zero. If stickDifferential is zero but the robot is turning left, then gyro rotation rate is negative, then motorDifferential will be a positive number. Then the left motor will get more power than the right thus correcting the left drift. Does that make sense?

Our team reuses this class as a basic implementation of a pi controller attempting to keep rotation to a minimum. We only usually only use it for holonomic drive trains, but it could easily be used for a tank drive. We only use a pi controller here, but you could easily modify this for a more accurate pid controller, or use the wpilib class.

EDIT:
I didn’t internalize that you were using tank drive. You could still try to use this, but it might be a little harder to adapt. I suppose if you run this method only when both joystick values are within some value of each other, it might work.

I’d suggest here a reasonably stiff P controller. That’ll get the OP most of the gain for fewer corner cases. We use essentially P gyro compensation.

Here is the code from our FTC library.

``````
/**
* This method implements tank drive where leftPower controls the left motors and right power controls the right
* motors.
*
* @param leftPower specifies left power value.
* @param rightPower specifies right power value.
* @param inverted specifies true to invert control (i.e. robot front becomes robot back).
*/
public void tankDrive(double leftPower, double rightPower, boolean inverted)
{
leftPower = TrcUtil.clipRange(leftPower);
rightPower = TrcUtil.clipRange(rightPower);

if (inverted)
{
double swap = leftPower;
leftPower = -rightPower;
rightPower = -swap;
}

if (gyro != null)
{
//
// Gyro assist is enabled.
//
double drivePower = (leftPower + rightPower)/2.0;
double diffPower = (leftPower - rightPower)/2.0;
double turnPower = gyroAssistKp*(diffPower - gyroRateScale*gyro.getZRotationRate().value);
leftPower = drivePower + turnPower;
rightPower = drivePower - turnPower;
double maxMag = Math.max(Math.abs(leftPower), Math.abs(rightPower));
if (maxMag > 1.0)
{
leftPower /= maxMag;
rightPower /= maxMag;
}
}

if (frontLeftMotor != null)
{
frontLeftMotor.setPower(leftPower);
}

if (frontRightMotor != null)
{
frontRightMotor.setPower(rightPower);
}

if (rearLeftMotor != null)
{
rearLeftMotor.setPower(leftPower);
}

if (rearRightMotor != null)
{
rearRightMotor.setPower(rightPower);
}
}   //tankDrive

``````

I would look at this problem first. If your robot is pulling badly enough that the driver is having trouble going straight, something is amiss. Check out this recent thread for things to try.

I have written working code for driving straight with the gyro in teleop. This works by holding a button and you can drive straight in the direction that you are facing when the button is pressed.

``````
@Override
public void operatorControl() {
drive.setSafetyEnabled(true);
gyro.reset();
drive.setMaxOutput(0.75);
int reset = 0;

while (isOperatorControl() && isEnabled()) {
angle = gyro.getAngle() * kP;

/*
* If the button is pressed (and held), drive straight based on the
* angle the robot was facing when the button was initially pressed.
*/
if (logitech.getRawButton(6)) {
if (reset < 1) {
gyro.reset();
reset++;
System.out.println("Success! Gyro was reset.");
}
if (Math.abs(logitech.getRawAxis(1)) > 0.15) {
drive.mecanumDrive_Cartesian(logitech.getRawAxis(1), 0, angle, 0);
} else if (Math.abs(logitech.getRawAxis(0)) > 0.15) {
drive.mecanumDrive_Cartesian(0, logitech.getRawAxis(0), angle, 0);
}

/*
* If the button is not pressed, drive normally.
*/
} else {
reset = 0;
if (Math.abs(logitech.getRawAxis(1)) > 0.15 || Math.abs(logitech.getRawAxis(0)) > 0.15
|| Math.abs(logitech.getRawAxis(2)) > 0.15) {
drive.mecanumDrive_Cartesian(logitech.getRawAxis(1), logitech.getRawAxis(0),
-logitech.getRawAxis(2) * 0.5, 0);
}
}

``````

This is using mecanum drive, but you could edit it to work with your drive train.

Another thing I would recommend for teleop driving straight is to put a deadband on your joystick turning amount so that you automatically engage gyro code to attempt to drive straight when your inputs are within a certain range of straight ahead. On a single joystick this might mean assuming straight whenever the y axis is within ±0.2 of zero, on a dual joystick setup it would be when left right differential from mikets’ example is within ±0.2 of zero. This helps to avoid drift if you have joysticks that don’t perfectly center or if your driver doesn’t consistently push straight ahead on the stick when trying to drive straight. That last point is especially pertinent for a rookie since you will have inexperienced drivers and they tend to do this more under match pressure in my experience.

I like to always engage this code when driving and have a button or switch to disable when something breaks. If you have to remember to push a button to engage it many will forget to use the assist button consistently.

These are the two very relevant points.

1. If the robot doesn’t go straight, there is something wrong with the mechanical part of the robot. You must fix that. A gyro or encoder or any programming solution will only reduce the robots performance.

2. As rookies, with no programming mentor, stay away from the complex and advanced solution and go towards the simple. Not to discourage you, but get the mechanicals right first.

It is not unusual for rookies to make this mistake. Experienced teams know this already.

I’m using ADXRS450 Gyro, and it don’t have this feature “gyro.getZRotationRate().value”

Sorry, that code was using our library. The corrected code is below:

``````
/**
* This method implements tank drive where leftPower controls the left motors and right power controls the right
* motors.
*
* @param leftPower specifies left power value.
* @param rightPower specifies right power value.
* @param inverted specifies true to invert control (i.e. robot front becomes robot back).
*/
public void tankDrive(double leftPower, double rightPower, boolean inverted)
{
leftPower = TrcUtil.clipRange(leftPower);
rightPower = TrcUtil.clipRange(rightPower);

if (inverted)
{
double swap = leftPower;
leftPower = -rightPower;
rightPower = -swap;
}

if (gyroAssistEnabled)
{
double diffPower = (leftPower - rightPower)/2.0;
double assistPower =
TrcUtil.clipRange(gyroAssistKp*(diffPower - gyroRateScale*gyro.getRate()));
leftPower += assistPower;
rightPower -= assistPower;
double maxMag = Math.max(Math.abs(leftPower), Math.abs(rightPower));
if (maxMag > 1.0)
{
leftPower /= maxMag;
rightPower /= maxMag;
}
}

leftPower = TrcUtil.clipRange(leftPower, -maxOutput, maxOutput);
rightPower = TrcUtil.clipRange(rightPower, -maxOutput, maxOutput);

if (leftFrontMotor != null) leftFrontMotor.setPower(leftPower);
if (rightFrontMotor != null) rightFrontMotor.setPower(rightPower);
if (leftRearMotor != null) leftRearMotor.setPower(leftPower);
if (rightRearMotor != null) rightRearMotor.setPower(rightPower);
}   //tankDrive

``````