Using NavX MXP to rotate to angle using arcade drive

[drplis]

Hi all,
This year, our team is attempting to use a navX MXP to rotate to a specific angle. We’ve tried implementing this into our program but we had no luck. Here is the code which we attempted to use.

package frc.robot.autonomous;

import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
import com.kauailabs.navx.frc.AHRS;

import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import frc.robot.Motors;
import frc.robot.Robot;

/**
 * RotateToAngle
 */
public class RotateToAngle {

    private Robot robot = new Robot();

    AHRS ahrs;
    PIDController turnController;
    DifferentialDrive myRobot;
    double rotateToAngleRate = robot.getRotateAngleRate();

    public RotateToAngle(AHRS ahrs, PIDController turnController) {
        this.ahrs = ahrs;
        this.turnController = turnController;
        myRobot = Motors.drive;
    }

    static final double kToleranceDegrees = 2.0f;

    static final double kTargetAngleDegrees = 90.0f;

    // Channels for the wheels
    final static int leftChannel = 0;
    final static int rightChannel = 1;

    WPI_VictorSPX leftMotor;
    WPI_VictorSPX rightMotor;

    public void operatorControl(XboxController operatorController, XboxController driverController) {
        if (operatorController.getAButtonPressed()) {
            System.out.println("1");
            if (!turnController.isEnabled()) {
                System.out.println("2");
                turnController.setSetpoint(kTargetAngleDegrees);
                System.out.println("3");
                rotateToAngleRate = 0; // This value will be updated in the pidWrite() method.
                turnController.enable();
                System.out.println("4");
            }
            System.out.println("5");
            double leftStickValue = rotateToAngleRate;
            double rightStickValue = rotateToAngleRate;
            System.out.println("6");
            myRobot.tankDrive(leftStickValue, rightStickValue);
            System.out.println("7. Current Rotate Angle Rate: " + rotateToAngleRate);
        } else if (operatorController.getXButtonPressed()) {
            /*
             * "Zero" the yaw (whatever direction the sensor is pointing now will become the
             * new "Zero" degrees.
             */
            ahrs.zeroYaw();
        } else if (operatorController.getYButtonPressed()) {
            /*
             * While this button is held down, the robot is in "drive straight" mode.
             * Whatever direction the robot was heading when "drive straight" mode was
             * entered will be maintained. The average speed of both joysticks is the
             * magnitude of motion.
             */
            if (!turnController.isEnabled()) {
                // Acquire current yaw angle, using this as the target angle.
                turnController.setSetpoint(ahrs.getYaw());
                rotateToAngleRate = 0; // This value will be updated in the pidWrite() method.
                turnController.enable();
            }
            double magnitude = (driverController.getRawAxis(2) + driverController.getRawAxis(3)) / 2;
            double leftStickValue = magnitude + rotateToAngleRate;
            double rightStickValue = magnitude - rotateToAngleRate;
            myRobot.tankDrive(leftStickValue, rightStickValue);
        } else {
            /* If the turn controller had been enabled, disable it now. */
            if (turnController.isEnabled()) {
                turnController.disable();
            }
            /* Standard arcade drive, no driver assistance. */
            driveControl(driverController);
        }
        Timer.delay(0.005); // wait for a motor update time
    }

    private void driveControl(XboxController driverController) {
        double deadZone = 0.15;
        double dif = Math.signum(driverController.getRawAxis(2) - driverController.getRawAxis(3))
            * ((driverController.getRawAxis(2) - driverController.getRawAxis(3))
                * (driverController.getRawAxis(2) - driverController.getRawAxis(3)));
        if (Math.abs(dif) <= deadZone)
          dif = 0.0;
    
        double turn = Math.signum(driverController.getRawAxis(0))
            * (driverController.getRawAxis(0) * driverController.getRawAxis(0));
        if (Math.abs(turn) <= deadZone)
          turn = 0.0;
    
        Motors.drive.arcadeDrive(dif * 1, (turn) * 0.6); // 0.8
        // Fist num: Driving speed
        // Second num: Turning speed
      }
}

Any help would be appreciated.

[NewtonCrosby]

What is the actual problem you are having when you say “no luck”?

[Oblarg]

The structure of your robot program is highly confusing. Consider basing your program on one of the official WPILib examples, such as the GyroDriveCommands project.

[drplis]

When we hold down the A button on the controller, it doesn’t do anything. Also, the rotateToAngleRate variable doesn’t seem to change.

[NewtonCrosby]

Maybe a github link would be better, but I’m not actually seeing where you are updating the the variable. There is a comment that says the pid controller will change the output but in my quick scrolling I didn’t see where that happens.

Oblarg’s reference to the new example code for the updated command based robot is more or less out of the box what you want to do.

[drplis]

I took some time to recreate our auto turning class and in the process fixed the problem where our robot would not turn. But now we’re having another problem where the robot over shoots and attempts to correct itself indefinitely. Changing the P, I, and D values doesn’t seem to do much in terms of lowering or stopping the over shooting.
package frc.robot.autonomous;

import com.kauailabs.navx.frc.AHRS;

import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import utils.Motors;

/**
 * Drive
 */
public class Drive {

    double P = 0.150;
    double I = 0.1;
    double D = 1;
    double integral, previous_error, setpoint = 0;
    double rcw = 0;
    AHRS gyro;
    DifferentialDrive robotDrive = Motors.drive;

    public Drive(AHRS gyro) {
        this.gyro = gyro;
    }

    /**
     * 
     * @apiNote Set the target angle you want to turn to
     */
    public void setSetpoint(double setpoint) {
        this.setpoint = setpoint;
    }

    public void PID () {
        double error = setpoint - gyro.getAngle();
        this.integral += (error * .02);
        double derivative = (error - this.previous_error) / 0.02;
        this.rcw = ((P * error) + (I * this.integral) + (D * derivative));
    }

    public void execute() {
        PID();
        double rotateSpeed = this.rcw;
        if(Math.abs(rotateSpeed) > 0.5) {
            if(rotateSpeed < 0) {
                rotateSpeed = -0.5;
            } else {
                rotateSpeed = 0.5;
            }
        } else {
            rotateSpeed = this.rcw;
        }
        robotDrive.arcadeDrive(0, rotateSpeed);
    }
}

[Oblarg]

You’re probably oscillating around the antipode because your output is in the wrong direction.

[gerthworm]

A few thoughts:

1. I highly recommend using wpi’s PID classes over rolling your own. I speak from experience.
2. Smoke test: You say changing PID values doesn’t “do much”. You should be able to set all PID gains to zero, and observe no motion. If this isn’t true, there’s definitely something wrong with your math.
3. Confirm that applying positive control effort (this.rcw) causes the actual value (gyro.getAngle()) to increase. Standard definitions for how P, I, and D are calculated assume this.
4. Do not attempt to select PID values at random. Use an algorithmic process, such as the one I detail here.