diff --git a/wpilibjExamples/build.gradle b/wpilibjExamples/build.gradle index d57e143928..7b65086ab1 100644 --- a/wpilibjExamples/build.gradle +++ b/wpilibjExamples/build.gradle @@ -33,7 +33,7 @@ if (!project.hasProperty('skipPMD')) { gradle.projectsEvaluated { tasks.withType(JavaCompile) { - options.compilerArgs << "-Xlint:unchecked" << "-Xlint:deprecation" + options.compilerArgs << "-Xlint:unchecked" << "-Xlint:deprecation" << "-Werror" } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java index ba8feeb450..c93ceab2df 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,10 +7,8 @@ package edu.wpi.first.wpilibj.examples.gearsbot.commands; -import edu.wpi.first.wpilibj.PIDController; -import edu.wpi.first.wpilibj.PIDSource; -import edu.wpi.first.wpilibj.PIDSourceType; import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.examples.gearsbot.Robot; @@ -21,7 +19,7 @@ import edu.wpi.first.wpilibj.examples.gearsbot.Robot; * encoders. */ public class DriveStraight extends Command { - private final PIDController m_pid; + private final PIDController m_pid = new PIDController(4, 0, 0); /** * Create a new DriveStraight command. @@ -29,24 +27,6 @@ public class DriveStraight extends Command { */ public DriveStraight(double distance) { requires(Robot.m_drivetrain); - m_pid = new PIDController(4, 0, 0, new PIDSource() { - PIDSourceType m_sourceType = PIDSourceType.kDisplacement; - - @Override - public double pidGet() { - return Robot.m_drivetrain.getDistance(); - } - - @Override - public void setPIDSourceType(PIDSourceType pidSource) { - m_sourceType = pidSource; - } - - @Override - public PIDSourceType getPIDSourceType() { - return m_sourceType; - } - }, d -> Robot.m_drivetrain.drive(d, d)); m_pid.setAbsoluteTolerance(0.01); m_pid.setSetpoint(distance); @@ -58,20 +38,25 @@ public class DriveStraight extends Command { // Get everything in a safe starting state. Robot.m_drivetrain.reset(); m_pid.reset(); - m_pid.enable(); + } + + @Override + protected void execute() { + double pidOut = m_pid.calculate(Robot.m_drivetrain.getDistance()); + + Robot.m_drivetrain.drive(pidOut, pidOut); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return m_pid.onTarget(); + return m_pid.atSetpoint(); } // Called once after isFinished returns true @Override protected void end() { - // Stop PID and the wheels - m_pid.disable(); + // Stop the wheels Robot.m_drivetrain.drive(0, 0); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java index 71f03dce8a..ab8655bd0e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,10 +7,8 @@ package edu.wpi.first.wpilibj.examples.gearsbot.commands; -import edu.wpi.first.wpilibj.PIDController; -import edu.wpi.first.wpilibj.PIDSource; -import edu.wpi.first.wpilibj.PIDSourceType; import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.examples.gearsbot.Robot; @@ -21,7 +19,7 @@ import edu.wpi.first.wpilibj.examples.gearsbot.Robot; * encoders. */ public class SetDistanceToBox extends Command { - private final PIDController m_pid; + private final PIDController m_pid = new PIDController(-2, 0, 0); /** * Create a new set distance to box command. @@ -29,24 +27,6 @@ public class SetDistanceToBox extends Command { */ public SetDistanceToBox(double distance) { requires(Robot.m_drivetrain); - m_pid = new PIDController(-2, 0, 0, new PIDSource() { - PIDSourceType m_sourceType = PIDSourceType.kDisplacement; - - @Override - public double pidGet() { - return Robot.m_drivetrain.getDistanceToObstacle(); - } - - @Override - public void setPIDSourceType(PIDSourceType pidSource) { - m_sourceType = pidSource; - } - - @Override - public PIDSourceType getPIDSourceType() { - return m_sourceType; - } - }, d -> Robot.m_drivetrain.drive(d, d)); m_pid.setAbsoluteTolerance(0.01); m_pid.setSetpoint(distance); @@ -58,20 +38,25 @@ public class SetDistanceToBox extends Command { // Get everything in a safe starting state. Robot.m_drivetrain.reset(); m_pid.reset(); - m_pid.enable(); + } + + @Override + protected void execute() { + double pidOut = m_pid.calculate(Robot.m_drivetrain.getDistanceToObstacle()); + + Robot.m_drivetrain.drive(pidOut, pidOut); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return m_pid.onTarget(); + return m_pid.atSetpoint(); } // Called once after isFinished returns true @Override protected void end() { - // Stop PID and the wheels - m_pid.disable(); + // Stop the wheels Robot.m_drivetrain.drive(0, 0); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java index d27347d070..9b165de221 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -11,7 +11,6 @@ import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.CounterBase.EncodingType; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.PIDSourceType; import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.Victor; @@ -58,10 +57,6 @@ public class DriveTrain extends Subsystem { m_drive.setExpiration(0.1); m_drive.setMaxOutput(1.0); - // Configure encoders - m_rightEncoder.setPIDSourceType(PIDSourceType.kDisplacement); - m_leftEncoder.setPIDSourceType(PIDSourceType.kDisplacement); - if (Robot.isReal()) { // Converts to feet m_rightEncoder.setDistancePerPulse(0.0785398); m_leftEncoder.setDistancePerPulse(0.0785398); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java index 302ce1674d..9559e07e53 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -9,10 +9,10 @@ package edu.wpi.first.wpilibj.examples.potentiometerpid; import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.PWMVictorSPX; import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.controller.PIDController; /** * This is a sample program to demonstrate how to use a soft potentiometer and a @@ -36,9 +36,7 @@ public class Robot extends TimedRobot { private static final double kD = -2.0; private PIDController m_pidController; - @SuppressWarnings("PMD.SingularField") private AnalogInput m_potentiometer; - @SuppressWarnings("PMD.SingularField") private SpeedController m_elevatorMotor; private Joystick m_joystick; @@ -51,17 +49,17 @@ public class Robot extends TimedRobot { m_elevatorMotor = new PWMVictorSPX(kMotorChannel); m_joystick = new Joystick(kJoystickChannel); - m_pidController = new PIDController(kP, kI, kD, m_potentiometer, m_elevatorMotor); + m_pidController = new PIDController(kP, kI, kD); m_pidController.setInputRange(0, 5); } - @Override - public void teleopInit() { - m_pidController.enable(); - } - @Override public void teleopPeriodic() { + // Run the PID Controller + double pidOut + = m_pidController.calculate(m_potentiometer.getAverageVoltage()); + m_elevatorMotor.set(pidOut); + // when the button is pressed once, the selected elevator setpoint // is incremented boolean currentButtonValue = m_joystick.getTrigger(); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java index cc80432477..6b727be03e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -8,10 +8,9 @@ package edu.wpi.first.wpilibj.examples.ultrasonicpid; import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.PIDController; -import edu.wpi.first.wpilibj.PIDOutput; import edu.wpi.first.wpilibj.PWMVictorSPX; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.drive.DifferentialDrive; /** @@ -45,27 +44,26 @@ public class Robot extends TimedRobot { private final DifferentialDrive m_robotDrive = new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort), new PWMVictorSPX(kRightMotorPort)); - private final PIDController m_pidController - = new PIDController(kP, kI, kD, m_ultrasonic, new MyPidOutput()); + private final PIDController m_pidController = new PIDController(kP, kI, kD); - /** - * Drives the robot a set distance from an object using PID control and the - * ultrasonic sensor. - */ @Override - public void teleopInit() { + public void robotInit() { // Set expected range to 0-24 inches; e.g. at 24 inches from object go // full forward, at 0 inches from object go full backward. m_pidController.setInputRange(0, kMaxDistance * kValueToInches); - // Set setpoint of the pid controller - m_pidController.setSetpoint(kHoldDistance * kValueToInches); - m_pidController.enable(); // begin PID control } - private class MyPidOutput implements PIDOutput { - @Override - public void pidWrite(double output) { - m_robotDrive.arcadeDrive(output, 0); - } + @Override + public void teleopInit() { + // Set setpoint of the pid controller + m_pidController.setSetpoint(kHoldDistance * kValueToInches); + } + + @Override + public void teleopPeriodic() { + double pidOutput + = m_pidController.calculate(m_ultrasonic.getAverageVoltage()); + + m_robotDrive.arcadeDrive(pidOutput, 0); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java index 7f38b8adf9..630be9a4d7 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; * creating this project, you must also update the build.gradle file in the * project. */ +@SuppressWarnings("deprecation") public class Robot extends IterativeRobot { private static final String kDefaultAuto = "Default"; private static final String kCustomAuto = "My Auto";