PIDController help: robot not turning far enough

Technical Java
1

I’m having an issue with my PID control where no matter how high I set the P value the Robot does not turn far enough to the set angle. Currently I have the angle set at 90 degrees and what is happening is that the robot turns and then oscillates at around 20-40 degrees, all at the max speeds that I set in the output range (-0.52 to 0.52). Any idea what could be the issue? The gyro sensor that I’m using is the ADXRS450 gyro sensor.

package org.usfirst.frc.team3205.robot.commands;

import org.usfirst.frc.team3205.robot.Robot;
import org.usfirst.frc.team3205.robot.RobotMap;

import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 */
public class autoAlign extends Command implements PIDOutput{
	private PIDController controller; 
	private double endTime; 
	private double angle; 
	private double speed; 
	
	private double pidOut; 
	
	private long onTargetMillis; 
	private long startTimeMillis; 
	
	
    public autoAlign() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.driveTrain); 
    	angle = 90; 
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	double kP = RobotMap.kP;
    	double kI = RobotMap.kI; 
    	double kD = RobotMap.kD; 
    	
    	controller = new PIDController(kP, kI, kD, Robot.driveTrain.gyroSensor, this);
    	controller.setInputRange(20.0, 150.0);
    	controller.setOutputRange(-RobotMap.SPEED, RobotMap.SPEED); 
    	controller.setAbsoluteTolerance(RobotMap.TOLERANCE); 
    	controller.setContinuous();
    	controller.setSetpoint(angle); 
    	controller.enable(); 
    	startTimeMillis = System.currentTimeMillis(); 
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	Robot.driveTrain.driveCertainAmounts(pidOut, -pidOut); 
    	SmartDashboard.putBoolean("onTarget:", controller.onTarget()); 
    	SmartDashboard.putNumber("angle:", Robot.driveTrain.getAngle()); 
    	SmartDashboard.putData("pid:", controller);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
//        if(!controller.onTarget()){
//        	onTargetMillis = 0; 
//        	return false; 
//        }
//        if(onTargetMillis == 0){
//        	onTargetMillis = System.currentTimeMillis(); 
//        }
        
//        return System.currentTimeMillis() > onTargetMillis + 100; 
        return controller.onTarget(); 
    }

    // Called once after isFinished returns true
    protected void end() {
    	controller.disable(); 
    	Robot.driveTrain.stop(); 
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
    
    public void pidWrite(double output){
    	synchronized(this){
    		pidOut = output; 
    		SmartDashboard.putNumber("pidOut:", pidOut); 
    		
    		
    	}
    }
}
2

Did you reset the gyro to 0? And can I see the PID values

Sent from my iPad using Tapatalk

3

First, as Peteman said, please post your P, I , and D values, and make sure to reset your gyro. Most don’t reset unless you either hit a button on them or power cycle.

Second, check your calibration and units. This sounds like your setpoint isn’t where you think it is, so you’re going there and then oscillating due to a large P gain.

Beyond that, you’re going to have to elaborate a bit. How did you zero your gyro? Is it even turning in the direction you want? Does it overshoot 90 degrees at all?

4

Your sensor is out of phase with your output, and your robot is oscillating around the antipode of your setpoint; when it crosses, the “shortest path” changes direction (and, thus, so too does the output).

Invert either the sensor input or loop output to fix this.

5

Right now I have P set to 0.2 and I and D set to 0.0. I zero gyro each time I activate the controller by having a button on the smart dashboard that resets the gyro (uses the .reset() method). I also output the current angle of the robot on dashboard and it never over shoots 90 degrees, just goes to 20-40 and oscillates around there.

6

It’s strange, I have a command for comparison purposes that turns the robot until it reaches past 90 degrees and it works fine. I’ll see if inverting the inputs and outputs will help.

7

Found the issue! I just set the input range to -180 to 180 and raised the P value to 0.4 and it works fine.

8