From 4f6a659015a46925c8a2a4c447e23b7c192a9793 Mon Sep 17 00:00:00 2001 From: lhmcgann Date: Sun, 28 Jan 2018 20:17:00 -0800 Subject: [PATCH 1/7] PID, Drivetrain, cleanup, other little things implemented velocity PID controllers: class, RobotMap, DT centralized move and turn PID controllers in respective commands --> deleted them from RobotMap and their methods from Drivetrain made some more things InstantCommands tests for velocity controllers and new PIDSourceAverage class no tests for PIDMove or PIDTurn commands bc don't know how to get around need for SmartDashboard/NetworkTables deleted gyro stuff bc we don't need/use it: have the ahrs deleted LeftDrive and RightDrive classes and references in Robot general cleanup of Drivetrain and its interface --- .../org/usfirst/frc/team199/Robot2018/OI.java | 31 +- .../usfirst/frc/team199/Robot2018/Robot.java | 7 - .../frc/team199/Robot2018/RobotMap.java | 110 +++--- .../autonomous/VelocityPIDController.java | 54 ++- .../team199/Robot2018/commands/PIDMove.java | 46 ++- .../team199/Robot2018/commands/PIDTurn.java | 39 +- .../commands/SetDistancePerPulse.java | 31 +- .../Robot2018/commands/ShiftDriveType.java | 3 +- .../Robot2018/commands/ShiftHighGear.java | 36 +- .../Robot2018/commands/ShiftLowGear.java | 36 +- .../Robot2018/commands/TeleopDrive.java | 2 +- .../commands/UpdatePIDConstants.java | 31 +- .../Robot2018/subsystems/Drivetrain.java | 346 ++++-------------- .../subsystems/DrivetrainInterface.java | 160 +++----- .../Robot2018/subsystems/LeftDrive.java | 30 -- .../Robot2018/subsystems/RightDrive.java | 30 -- .../Robot2018/PIDSourceAverageTest.java | 59 +++ .../Robot2018/VelocityPIDControllerTest.java | 14 +- 18 files changed, 364 insertions(+), 701 deletions(-) delete mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LeftDrive.java delete mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/RightDrive.java create mode 100644 Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDSourceAverageTest.java diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java index 99e73c5..6f5d33f 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java @@ -24,33 +24,6 @@ * interface to the commands and command groups that allow control of the robot. */ public class OI { - //// CREATING BUTTONS - // One type of button is a joystick button which is any button on a - //// joystick. - // You create one by telling it which joystick it's on and which button - // number it is. - // Joystick stick = new Joystick(port); - // Button button = new JoystickButton(stick, buttonNumber); - - // There are a few additional built in buttons you can use. Additionally, - // by subclassing Button you can create custom triggers and bind those to - // commands the same as any other Button. - - //// TRIGGERING COMMANDS WITH BUTTONS - // Once you have a button, it's trivial to bind it to a button in one of - // three ways: - - // Start the command when the button is pressed and let it run the command - // until it is finished as determined by it's isFinished method. - // button.whenPressed(new ExampleCommand()); - - // Run the command while the button is being held down and interrupt it once - // the button is released. - // button.whileHeld(new ExampleCommand()); - - // Start the command when the button is released and let it run the command - // until it is finished as determined by it's isFinished method. - // button.whenReleased(new ExampleCommand()); public Joystick leftJoy; private JoystickButton shiftLowGear; @@ -79,9 +52,9 @@ public OI() { shiftDriveType = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2)); shiftDriveType.whenPressed(new ShiftDriveType()); PIDMove = new JoystickButton(leftJoy, getButton("PID Move", 7)); - PIDMove.whenPressed(new PIDMove(40, Robot.dt)); + PIDMove.whenPressed(new PIDMove(40, Robot.dt, RobotMap.distEncAvg)); PIDTurn = new JoystickButton(leftJoy, getButton("PID Turn", 8)); - PIDTurn.whenPressed(new PIDTurn(30, Robot.dt)); + PIDTurn.whenPressed(new PIDTurn(30, Robot.dt, RobotMap.fancyGyro)); rightJoy = new Joystick(1); updatePidConstants = new JoystickButton(rightJoy, getButton("Get PID Constants", 8)); diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java index 147d6f9..481cf8f 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java @@ -5,9 +5,7 @@ import org.usfirst.frc.team199.Robot2018.subsystems.ClimberAssist; import org.usfirst.frc.team199.Robot2018.subsystems.Drivetrain; import org.usfirst.frc.team199.Robot2018.subsystems.IntakeEject; -import org.usfirst.frc.team199.Robot2018.subsystems.LeftDrive; import org.usfirst.frc.team199.Robot2018.subsystems.Lift; -import org.usfirst.frc.team199.Robot2018.subsystems.RightDrive; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.command.Command; @@ -30,8 +28,6 @@ public class Robot extends TimedRobot { public static Lift lift; public static RobotMap rmap; public static Drivetrain dt; - public static LeftDrive ld; - public static RightDrive rd; public static Listener listen; public static OI oi; @@ -65,9 +61,6 @@ public void robotInit() { intakeEject = new IntakeEject(); lift = new Lift(); dt = new Drivetrain(); - rmap.initPIDControllers(); - ld = new LeftDrive(); - rd = new RightDrive(); oi = new OI(); listen = new Listener(); // chooser.addObject("My Auto", new MyAutoCommand()); diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java index 7e24f58..402e2f2 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java @@ -7,14 +7,19 @@ package org.usfirst.frc.team199.Robot2018; +import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage; +import org.usfirst.frc.team199.Robot2018.autonomous.VelocityPIDController; + import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DigitalSource; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.PIDSourceType; import edu.wpi.first.wpilibj.SerialPort; import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.drive.DifferentialDrive; @@ -27,32 +32,28 @@ * floating around. */ public class RobotMap { - // For example to map the left and right motors, you could define the - // following variables to use with your drivetrain subsystem. - // public static int leftMotor = 1; - // public static int rightMotor = 2; - - // If you are using multiple modules, make sure to define both the port - // number and the module. For example you with a rangefinder: - // public static int rangefinderPort = 1; - // public static int rangefinderModule = 1; - - public static Encoder leftEnc; + public static DigitalSource leftEncPort1; + public static DigitalSource leftEncPort2; + public static Encoder leftEncDist; + public static Encoder leftEncRate; public static WPI_TalonSRX dtLeftDrive; public static WPI_VictorSPX dtLeftSlave; public static SpeedControllerGroup dtLeft; + public static VelocityPIDController leftVelocityController; - public static Encoder rightEnc; + public static DigitalSource rightEncPort1; + public static DigitalSource rightEncPort2; + public static Encoder rightEncDist; + public static Encoder rightEncRate; public static WPI_TalonSRX dtRightDrive; public static WPI_VictorSPX dtRightSlave; public static SpeedControllerGroup dtRight; + public static VelocityPIDController rightVelocityController; + public static DifferentialDrive robotDrive; - public static PIDController turnController; - // public static PIDController moveController; - public static PIDController moveLeftController; - public static PIDController moveRightController; + public static PIDSourceAverage distEncAvg; - public static AHRS ahrs; + public static AHRS fancyGyro; public static AnalogGyro dtGyro; public static DoubleSolenoid dtGear; @@ -88,61 +89,52 @@ private void configSPX(WPI_VictorSPX mc) { public RobotMap() { - leftEnc = new Encoder(getPort("1LeftEnc", 0), getPort("2LeftEnc", 1)); + leftEncPort1 = new DigitalInput(getPort("1LeftEnc", 0)); + leftEncPort2 = new DigitalInput(getPort("2LeftEnc", 1)); + leftEncDist = new Encoder(leftEncPort1, leftEncPort2); + leftEncDist.setPIDSourceType(PIDSourceType.kDisplacement); + leftEncRate = new Encoder(leftEncPort1, leftEncPort2); + leftEncRate.setPIDSourceType(PIDSourceType.kRate); dtLeftDrive = new WPI_TalonSRX(getPort("LeftTalonSRXDrive", 0)); configSRX(dtLeftDrive); dtLeftSlave = new WPI_VictorSPX(getPort("LeftTalonSPXSlave", 1)); configSPX(dtLeftSlave); dtLeft = new SpeedControllerGroup(dtLeftDrive, dtLeftSlave); - rightEnc = new Encoder(getPort("1RightEnc", 2), getPort("2RightEnc", 3)); + leftVelocityController = new VelocityPIDController(Robot.getConst("MoveLeftkP", 1), + Robot.getConst("MoveLeftkI", 0), Robot.getConst("MoveLeftkD", 0), leftEncRate, dtLeft); + leftVelocityController.enable(); + leftVelocityController.setInputRange(0, Double.MAX_VALUE); + leftVelocityController.setOutputRange(-1.0, 1.0); + leftVelocityController.setContinuous(false); + leftVelocityController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceLeft", 2)); + + rightEncPort1 = new DigitalInput(getPort("1RightEnc", 2)); + rightEncPort2 = new DigitalInput(getPort("2RightEnc", 3)); + rightEncDist = new Encoder(leftEncPort1, leftEncPort2); + rightEncDist.setPIDSourceType(PIDSourceType.kDisplacement); + rightEncRate = new Encoder(leftEncPort1, leftEncPort2); + rightEncRate.setPIDSourceType(PIDSourceType.kRate); dtRightDrive = new WPI_TalonSRX(getPort("RightTalonSRXDrive", 2)); configSRX(dtRightDrive); dtRightSlave = new WPI_VictorSPX(getPort("RightTalonSPXSlave", 3)); configSPX(dtRightSlave); dtRight = new SpeedControllerGroup(dtRightDrive, dtRightSlave); - robotDrive = new DifferentialDrive(dtLeft, dtRight); - - // moveController = new PIDController(Robot.getConst("MovekP", 1), - // Robot.getConst("MovekI", 0), - // Robot.getConst("MovekD", 0), Robot.dt, Robot.dt); - // moveController.disable(); - // moveController.setInputRange(0, Double.MAX_VALUE); - // moveController.setOutputRange(-1.0, 1.0); - // moveController.setContinuous(false); - // moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2)); - - - ahrs = new AHRS(SerialPort.Port.kMXP); + rightVelocityController = new VelocityPIDController(Robot.getConst("ConstMoveRightkP", 1), + Robot.getConst("ConstMoveRightkI", 0), Robot.getConst("ConstMoveRightkD", 0), rightEncRate, dtRight); + rightVelocityController.enable(); + rightVelocityController.setInputRange(0, Double.MAX_VALUE); + rightVelocityController.setOutputRange(-1.0, 1.0); + rightVelocityController.setContinuous(false); + rightVelocityController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceRight", 2)); + + robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController); + + distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist); + fancyGyro = new AHRS(SerialPort.Port.kMXP); dtGyro = new AnalogGyro(getPort("Gyro", 0)); dtGear = new DoubleSolenoid(getPort("1dtGearSolenoid", 0), getPort("2dtGearSolenoid", 1)); - - } - - public void initPIDControllers() { - turnController = new PIDController(Robot.getConst("TurnkP", 1), Robot.getConst("TurnkI", 0), - Robot.getConst("TurnkD", 0), ahrs, Robot.dt); - turnController.disable(); - turnController.setInputRange(-180, 180); - turnController.setOutputRange(-1.0, 1.0); - turnController.setContinuous(); - turnController.setAbsoluteTolerance(Robot.getConst("TurnTolerance", 1)); - - moveLeftController = new PIDController(Robot.getConst("MoveLeftkP", 1), Robot.getConst("MoveLeftkI", 0), - Robot.getConst("MoveLeftkD", 0), Robot.ld, Robot.ld); - moveLeftController.disable(); - moveLeftController.setInputRange(0, Double.MAX_VALUE); - moveLeftController.setOutputRange(-1.0, 1.0); - moveLeftController.setContinuous(false); - moveLeftController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceLeft", 2)); - moveRightController = new PIDController(Robot.getConst("ConstMoveRightkP", 1), - Robot.getConst("ConstMoveRightkI", 0), Robot.getConst("ConstMoveRightkD", 0), Robot.rd, Robot.rd); - moveRightController.disable(); - moveRightController.setInputRange(0, Double.MAX_VALUE); - moveRightController.setOutputRange(-1.0, 1.0); - moveRightController.setContinuous(false); - moveRightController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceRight", 2)); } /** diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java index 01ca5c8..8b24f04 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java @@ -3,56 +3,88 @@ import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.SpeedControllerGroup; public class VelocityPIDController extends PIDController implements SpeedController { - private SpeedController out; + private SpeedControllerGroup out; /** - * @param kp the proportional PID constant - * @param ki the integral PID constant - * @param kd the derivative PID constant + * Constructs a VelocityPIDContoller and invokes the super constructor + * (PIDController), setting the three PID constants and the source and output + * for this VelocityPIDController. + * + * @param kp + * the proportional PID constant + * @param ki + * the integral PID constant + * @param kd + * the derivative PID constant * @param source - * the SpeedController you are reading from + * the sensor (e.g. velocity encoder) you are reading from * @param output * the SpeedController you are telling what to do */ - public VelocityPIDController(double kp, double ki, double kd, PIDSource source, SpeedController output) { + public VelocityPIDController(double kp, double ki, double kd, PIDSource source, SpeedControllerGroup output) { super(kp, ki, kd, source, output); out = output; } + /** + * Sets the target speed + * + * @param speed + * the target speed [-1, 1] + */ @Override public void pidWrite(double output) { setSetpoint(output); } + /** + * Sets the target speed + * + * @param speed + * the target speed [-1, 1] + */ @Override public void set(double speed) { - // TODO Auto-generated method stub setSetpoint(speed); } + /** + * Gets the current set voltage (setpoint) sent to the output + * SpeedControllerGroup + * + * @return the current set voltage (setpoint/target/goal) sent to the output + * SpeedControllerGroup + */ @Override public double get() { - // TODO Auto-generated method stub - // should possibly be actual spdCtr value instead??? - return out.get(); + return getSetpoint(); } + /** + * + * */ @Override public void setInverted(boolean isInverted) { out.setInverted(isInverted); } + /** + * + * */ @Override public boolean getInverted() { return out.getInverted(); } + /** + * + * */ @Override public void stopMotor() { - // TODO Auto-generated method stub out.stopMotor(); } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java index eba6048..ce46a05 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java @@ -1,53 +1,73 @@ package org.usfirst.frc.team199.Robot2018.commands; import org.usfirst.frc.team199.Robot2018.Robot; +import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage; import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface; +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.PIDOutput; import edu.wpi.first.wpilibj.command.Command; /** * */ -public class PIDMove extends Command { +public class PIDMove extends Command implements PIDOutput { - double target; - DrivetrainInterface dt; + private double target; + private DrivetrainInterface dt; + private PIDController moveController; - public PIDMove(double targ, DrivetrainInterface dt) { + public PIDMove(double targ, DrivetrainInterface dt, PIDSourceAverage avg) { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); target = targ; this.dt = dt; requires(Robot.dt); + moveController = new PIDController(Robot.getConst("MovekP", 1), Robot.getConst("MovekI", 0), + Robot.getConst("MovekD", 0), avg, this); } // Called just before this Command runs the first time - protected void initialize() { - dt.resetEnc(); - dt.enableMovePid(); - dt.setMoveSetpointLeft(target); - dt.setMoveSetpointRight(target); + @Override + public void initialize() { + dt.resetDistEncs(); + moveController.setInputRange(0, Double.MAX_VALUE); + moveController.setOutputRange(-1.0, 1.0); + moveController.setContinuous(false); + moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2)); + moveController.setSetpoint(target); + moveController.enable(); } // Called repeatedly when this Command is scheduled to run + @Override protected void execute() { - dt.tankDrive(dt.getLeftPidOut(), dt.getRightPidOut()); + // This method is empty bc the moveController runs on a different thread as soon + // as it is enabled. } // Make this return true when this Command no longer needs to run execute() + @Override protected boolean isFinished() { - return dt.onDriveTarg(); + return moveController.onTarget(); } // Called once after isFinished returns true + @Override protected void end() { - dt.disableMovePid(); - dt.stopDrive(); + moveController.disable(); + moveController.free(); } // Called when another command which requires one or more of the same // subsystems is scheduled to run + @Override protected void interrupted() { end(); } + + @Override + public void pidWrite(double output) { + dt.arcadeDrive(output, 0); + } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java index 9806897..6aa50df 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java @@ -3,50 +3,73 @@ import org.usfirst.frc.team199.Robot2018.Robot; import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface; +import com.kauailabs.navx.frc.AHRS; + +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.PIDOutput; import edu.wpi.first.wpilibj.command.Command; /** * */ -public class PIDTurn extends Command { +public class PIDTurn extends Command implements PIDOutput { double target; DrivetrainInterface dt; + private PIDController turnController; - public PIDTurn(double targ, DrivetrainInterface dt) { + public PIDTurn(double targ, DrivetrainInterface dt, AHRS ahrs) { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); target = targ; this.dt = dt; requires(Robot.dt); + turnController = new PIDController(Robot.getConst("TurnkP", 1), Robot.getConst("TurnkI", 0), + Robot.getConst("TurnkD", 0), ahrs, this); } // Called just before this Command runs the first time + @Override protected void initialize() { dt.resetAHRS(); - dt.setTurnSetpoint(target); - dt.enableTurnPid(); + turnController.disable(); + turnController.setInputRange(-180, 180); + turnController.setOutputRange(-1.0, 1.0); + turnController.setContinuous(); + turnController.setAbsoluteTolerance(Robot.getConst("TurnTolerance", 1)); + turnController.setSetpoint(target); + turnController.enable(); } // Called repeatedly when this Command is scheduled to run + @Override protected void execute() { - dt.arcadeDrive(0, dt.getAnglePidOut()); + // This method is empty bc the moveController runs on a different thread as soon + // as it is enabled. } // Make this return true when this Command no longer needs to run execute() + @Override protected boolean isFinished() { - return dt.onTurnTarg(); + return turnController.onTarget(); } // Called once after isFinished returns true + @Override protected void end() { - dt.disableTurnPid(); - dt.stopDrive(); + turnController.disable(); + turnController.free(); } // Called when another command which requires one or more of the same // subsystems is scheduled to run + @Override protected void interrupted() { end(); } + + @Override + public void pidWrite(double output) { + dt.arcadeDrive(0, output); + } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/SetDistancePerPulse.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/SetDistancePerPulse.java index d3fdde6..597a0bb 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/SetDistancePerPulse.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/SetDistancePerPulse.java @@ -2,46 +2,23 @@ import org.usfirst.frc.team199.Robot2018.Robot; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.InstantCommand; /** * */ -public class SetDistancePerPulse extends Command { - - Timer tim; +public class SetDistancePerPulse extends InstantCommand { public SetDistancePerPulse() { + super(); // Use requires() here to declare subsystem dependencies // eg. requires(chassis); } // Called just before this Command runs the first time + @Override protected void initialize() { - tim = new Timer(); - tim.reset(); - tim.start(); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { Robot.dt.setDistancePerPulseLeft(Robot.getConst("Distance Per Pulse Left", 0.184)); Robot.dt.setDistancePerPulseRight(Robot.getConst("Distance Per Pulse Right", 0.184)); } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return tim.get() > Robot.getConst("Update Constants Time", 0.1); - } - - // Called once after isFinished returns true - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftDriveType.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftDriveType.java index c37af49..ef5673b 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftDriveType.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftDriveType.java @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** - * + * Toggles between arcade and tank drive. */ public class ShiftDriveType extends InstantCommand { @@ -17,6 +17,7 @@ public ShiftDriveType() { } // Called once when the command executes + @Override protected void initialize() { SmartDashboard.putBoolean("Bool/Arcade Drive", !Robot.getBool("Arcade Drive", true)); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftHighGear.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftHighGear.java index 6c99348..6ea12f6 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftHighGear.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftHighGear.java @@ -2,48 +2,24 @@ import org.usfirst.frc.team199.Robot2018.Robot; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.InstantCommand; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** - * + * Shifts the DT to high gear. */ -public class ShiftHighGear extends Command { - - Timer tim; +public class ShiftHighGear extends InstantCommand { public ShiftHighGear() { + super(); // Use requires() here to declare subsystem dependencies // eg. requires(chassis); } // Called just before this Command runs the first time + @Override protected void initialize() { - tim = new Timer(); - tim.reset(); - tim.start(); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - Robot.dt.changeShiftGear(true); + Robot.dt.shiftGears(true); SmartDashboard.putBoolean("High Gear", true); } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return tim.get() > Robot.getConst("Piston Act Time", 0.1); - } - - // Called once after isFinished returns true - protected void end() { - Robot.dt.turnGearSolenoidOff(); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftLowGear.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftLowGear.java index c009660..10c2039 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftLowGear.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftLowGear.java @@ -2,48 +2,24 @@ import org.usfirst.frc.team199.Robot2018.Robot; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.InstantCommand; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** - * + * Shifts the DT to low gear. */ -public class ShiftLowGear extends Command { - - Timer tim; +public class ShiftLowGear extends InstantCommand { public ShiftLowGear() { + super(); // Use requires() here to declare subsystem dependencies // eg. requires(chassis); } // Called just before this Command runs the first time + @Override protected void initialize() { - tim = new Timer(); - tim.reset(); - tim.start(); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - Robot.dt.changeShiftGear(false); + Robot.dt.shiftGears(false); SmartDashboard.putBoolean("High Gear", false); } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return tim.get() > Robot.getConst("Piston Act Time", 0.1); - } - - // Called once after isFinished returns true - protected void end() { - Robot.dt.turnGearSolenoidOff(); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/TeleopDrive.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/TeleopDrive.java index 4bdf576..fb35327 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/TeleopDrive.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/TeleopDrive.java @@ -40,7 +40,7 @@ protected boolean isFinished() { // Called once after isFinished returns true @Override protected void end() { - Robot.dt.stopDrive(); + Robot.dt.disableVelocityPIDs(); } // Called when another command which requires one or more of the same diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdatePIDConstants.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdatePIDConstants.java index 63f0e10..1748df3 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdatePIDConstants.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdatePIDConstants.java @@ -2,45 +2,22 @@ import org.usfirst.frc.team199.Robot2018.Robot; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.InstantCommand; /** * */ -public class UpdatePIDConstants extends Command { - - Timer tim; +public class UpdatePIDConstants extends InstantCommand { public UpdatePIDConstants() { + super(); // Use requires() here to declare subsystem dependencies // eg. requires(chassis); } // Called just before this Command runs the first time + @Override protected void initialize() { - tim = new Timer(); - tim.reset(); - tim.start(); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { Robot.dt.updatePidConstants(); } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return tim.get() > Robot.getConst("Update Constants Time", 0.1); - } - - // Called once after isFinished returns true - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java index f776767..a7c2ae5 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java @@ -9,143 +9,39 @@ import org.usfirst.frc.team199.Robot2018.Robot; import org.usfirst.frc.team199.Robot2018.RobotMap; +import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage; +import org.usfirst.frc.team199.Robot2018.autonomous.VelocityPIDController; import org.usfirst.frc.team199.Robot2018.commands.TeleopDrive; import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.PIDController; -import edu.wpi.first.wpilibj.PIDOutput; import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.drive.DifferentialDrive; -public class Drivetrain extends Subsystem implements PIDOutput, DrivetrainInterface { +public class Drivetrain extends Subsystem implements DrivetrainInterface { // Put methods for controlling this subsystem // here. Call these from Commands. - private final Encoder leftEnc = RobotMap.leftEnc; - private final Encoder rightEnc = RobotMap.rightEnc; + private final Encoder leftEncDist = RobotMap.leftEncDist; + private final Encoder rightEncDist = RobotMap.rightEncDist; + private final PIDSourceAverage distEncAvg = RobotMap.distEncAvg; private final SpeedControllerGroup dtLeft = RobotMap.dtLeft; private final SpeedControllerGroup dtRight = RobotMap.dtRight; private final DifferentialDrive robotDrive = RobotMap.robotDrive; - private final PIDController turnController = RobotMap.turnController; - // private final PIDController moveController = RobotMap.moveController; - private final PIDController moveLeftController = RobotMap.moveLeftController; - private final PIDController moveRightController = RobotMap.moveRightController; + private final VelocityPIDController leftVelocityController = RobotMap.leftVelocityController; + private final VelocityPIDController rightVelocityController = RobotMap.rightVelocityController; - private final AHRS ahrs = RobotMap.ahrs; - private final AnalogGyro dtGyro = RobotMap.dtGyro; + private final AHRS fancyGyro = RobotMap.fancyGyro; private final DoubleSolenoid dtGear = RobotMap.dtGear; - private double anglePidOut = 0; - private double leftPidOut = 0; - private double rightPidOut = 0; - @Override public void initDefaultCommand() { setDefaultCommand(new TeleopDrive()); } - /** - * Updates the PIDControllers' PIDConstants based on SmartDashboard values - */ - @Override - public void updatePidConstants() { - turnController.setPID(Robot.getConst("TurnkP", 1), Robot.getConst("TurnkI", 0), Robot.getConst("TurnkD", 0)); - moveLeftController.setPID(Robot.getConst("MoveLeftkP", 1), Robot.getConst("MoveLeftkI", 0), - Robot.getConst("MoveLeftkD", 0)); - moveRightController.setPID(Robot.getConst("MoveRightkP", 1), Robot.getConst("MoveRightkI", 0), - Robot.getConst("MoveRightkD", 0)); - } - - /** - * Activates the solenoid to push the drivetrain into low or high gear - * - * @param forw - * If the solenoid is to be pushed forward or not (backwards) - */ - @Override - public void changeShiftGear(boolean forw) { - if (forw ^ Robot.getBool("Drivetrain Gear Shift Backwards", false)) { - dtGear.set(DoubleSolenoid.Value.kForward); - } else { - dtGear.set(DoubleSolenoid.Value.kReverse); - } - } - - /** - * Stops the solenoid that pushes the drivetrain into low or high gear - */ - @Override - public void turnGearSolenoidOff() { - dtGear.set(DoubleSolenoid.Value.kOff); - } - - /** - * Resets the AHRS value - */ - @Override - public void resetAHRS() { - ahrs.reset(); - } - - /** - * Runs the left side of the drivetrain at the specified speed - * - * @param value - * Value for the motor(s) to run at - */ - @Override - public void setLeftMotor(double value) { - dtLeft.set(value); - } - - /** - * Tells the left side of the drivetrain to stop running - */ - private void stopLeftMotor() { - dtLeft.stopMotor(); - } - - /** - * Runs the right side of the drivetrain at the specified speed - * - * @param value - * Value for the motor(s) to run at - */ - @Override - public void setRightMotor(double value) { - dtRight.set(value); - } - - /** - * Tells the right side of the drivetrain to stop running - */ - private void stopRightMotor() { - dtRight.stopMotor(); - } - - /** - * Tells the drivetrain to stop running - */ - @Override - public void stopDrive() { - stopRightMotor(); - stopLeftMotor(); - } - - /** - * Resets the encoders' distances to zero - */ - @Override - public void resetEnc() { - leftEnc.reset(); - rightEnc.reset(); - } - /** * Drives based on joystick input and SmartDashboard values */ @@ -190,245 +86,149 @@ public void tankDrive(double leftSpeed, double rightSpeed) { /** * Used for getting the speed at which the left side of the drivetrain is - * currently running + * currently set to. Gets data straight from SpeedControllerGroup. * - * @return The speed that the left side of the drivetrain is set to + * @return The speed that the left side of the drivetrain is set to [-1, 1] */ @Override - public double getDtLeft() { + public double getDtLeftSpeed() { return dtLeft.get(); } /** * Used for getting the speed at which the right side of the drivetrain is - * currently running + * currently set to. Gets data straight from SpeedControllerGroup. * - * @return The speed that the right side of the drivetrain is set to + * @return The speed that the right side of the drivetrain is set to [-1, 1] */ @Override - public double getDtRight() { + public double getDtRightSpeed() { return dtRight.get(); } /** - * Used to get the angle that the gyro currently reads - * - * @return The angle that the gyro reads - */ - @Override - public double getGyroAngle() { - return dtGyro.getAngle(); - } - - /** - * Resets the gyro to 0 - */ - @Override - public void resetGyro() { - dtGyro.reset(); - } - - /** - * Disables the turnPID PIDController used for turning - */ - @Override - public void disableTurnPid() { - turnController.disable(); - } - - /** - * Enables the turnPID PIDController used for turning + * Updates the PIDControllers' PIDConstants based on SmartDashboard values */ @Override - public void enableTurnPid() { - turnController.enable(); + public void updatePidConstants() { + leftVelocityController.setPID(Robot.getConst("MoveLeftkP", 1), Robot.getConst("MoveLeftkI", 0), + Robot.getConst("MoveLeftkD", 0)); + rightVelocityController.setPID(Robot.getConst("MoveRightkP", 1), Robot.getConst("MoveRightkI", 0), + Robot.getConst("MoveRightkD", 0)); } /** - * Sets the setPoint of the turnPID PIDController - * - * @param set - * The value to set the setPoint at + * Enable the VelocityPIDControllers used for velocity control on each side of + * the DT */ @Override - public void setTurnSetpoint(double set) { - turnController.setSetpoint(set); + public void enableVelocityPIDs() { + leftVelocityController.enable(); + rightVelocityController.enable(); } /** - * Enable the movePID PIDController used for moving + * Disables the VelocityPIDControllers used for velocity control on each side of + * the DT */ @Override - public void enableMovePid() { - moveLeftController.enable(); - moveRightController.enable(); + public void disableVelocityPIDs() { + leftVelocityController.disable(); + rightVelocityController.disable(); } /** - * Disables the movePID PIDController used for moving + * Resets the AHRS value */ @Override - public void disableMovePid() { - moveLeftController.disable(); - moveRightController.disable(); + public void resetAHRS() { + fancyGyro.reset(); } /** - * Sets the setPoint of the moveLeftPID PIDController + * Used to get the yaw angle (Z-axis in degrees) that the ahrs currently reads * - * @param set - * The value to set the setPoint at + * @return The angle that the ahrs reads (in degrees) */ @Override - public void setMoveSetpointLeft(double set) { - moveLeftController.setSetpoint(set); + public double getAHRSAngle() { + return fancyGyro.getAngle(); } /** - * Sets the setPoint of the moveRightPID PIDController - * - * @param set - * The value to set the setPoint at + * Resets the encoders' distances to zero */ @Override - public void setMoveSetpointRight(double set) { - moveRightController.setSetpoint(set); + public void resetDistEncs() { + leftEncDist.reset(); + rightEncDist.reset(); } /** * Sets the distancePerPulse property on the left encoder * - * @param dist - * The distance to set the distancePerPulse at + * @param ratio + * The ratio to set the distancePerPulse to (real dist units/encoder + * pulses) */ @Override - public void setDistancePerPulseLeft(double dist) { - leftEnc.setDistancePerPulse(dist); + public void setDistancePerPulseLeft(double ratio) { + leftEncDist.setDistancePerPulse(ratio); } /** * Sets the distancePerPulse property on the right encoder * - * @param dist - * The distance to set the distancePerPulse at - */ - @Override - public void setDistancePerPulseRight(double dist) { - rightEnc.setDistancePerPulse(dist); - } - - /** - * Returns the value that Drivetrain receives due to implementing PIDOutput - * - * @return The value that is written by PIDControllers - */ - @Override - public double getAnglePidOut() { - return anglePidOut; - } - - /** - * Returns the value that leftdrive should be set to according to PIDControllers - * - * @return The value that is written by PIDControllers + * @param ratio + * The ratio to set the distancePerPulse to (real dist units/encoder + * pulses) */ @Override - public double getLeftPidOut() { - return leftPidOut; + public void setDistancePerPulseRight(double ratio) { + rightEncDist.setDistancePerPulse(ratio); } /** - * Returns the value that rightdrive should be set to according to - * PIDControllers + * Returns the distance (in real units) that the left encoder reads * - * @return The value that is written by PIDControllers + * @return How far the left encoder has traveled in real units since last reset */ @Override - public double getRightPidOut() { - return rightPidOut; + public double getLeftEncDist() { + return leftEncDist.getDistance(); } /** - * Returns the distance that the left encoder reads + * Returns the distance (in real units) that the right encoder reads * - * @return How much the left encoder has travelled + * @return How far the right encoder has traveled in real units since last reset */ - public double getLeftEnc() { - return leftEnc.getDistance(); - } - - /** - * Returns the distance that the right encoder reads - * - * @return How much the right encoder has travelled - */ - public double getRightEnc() { - return rightEnc.getDistance(); - } - // public boolean onTargetLeft() { - // return moveLeftController.onTarget(); - // } - // public boolean onTargetRight() { - // return moveRightController.onTarget(); - // } - @Override - public void pidWrite(double output) { - anglePidOut = output; - } - - /** - * Sets the leftPidOutput - * - * @param output - * The value to set the output to - */ - public void pidLeftWrite(double output) { - leftPidOut = output; + public double getRightEncDist() { + return rightEncDist.getDistance(); } /** - * Sets the rightPidOutput + * Activates the solenoid to push the drivetrain into high or low gear * - * @param output - * The value to set the output to - */ - public void pidRightWrite(double output) { - rightPidOut = output; - } - - /** - * Returns the average of two numbers - * - * @param a - * First number to average - * @param b - * Second number to average - * @return The arithmetic mean of a and b - */ - public static double average(double a, double b) { - return (a + b) / 2; - } - - /** - * Returns whether the turnController PIDController senses that it's on target - * - * @return Whether the turnController PIDController is on target + * @param highGear + * If the solenoid is to be pushed into high gear (true, kForward) or + * low gear (false, kReverse) */ @Override - public boolean onTurnTarg() { - return turnController.onTarget(); + public void shiftGears(boolean highGear) { + if (highGear ^ Robot.getBool("Drivetrain Gear Shift Low", false)) { + dtGear.set(DoubleSolenoid.Value.kForward); + } else { + dtGear.set(DoubleSolenoid.Value.kReverse); + } } /** - * Returns whether the moveLeftController and moveRightController PIDController - * sense that their on target - * - * @return Whether the moveController PIDControllers are on target + * Stops the solenoid that pushes the drivetrain into low or high gear */ @Override - public boolean onDriveTarg() { - return moveLeftController.onTarget() && moveRightController.onTarget(); + public void shiftGearSolenoidOff() { + dtGear.set(DoubleSolenoid.Value.kOff); } - } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/DrivetrainInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/DrivetrainInterface.java index 64b92ac..91a554f 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/DrivetrainInterface.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/DrivetrainInterface.java @@ -1,54 +1,8 @@ package org.usfirst.frc.team199.Robot2018.subsystems; public interface DrivetrainInterface { - /** - * Updates the PIDControllers' PIDConstants based on SmartDashboard values - */ - public void updatePidConstants(); - - /** - * Activates the solenoid to push the drivetrain into low or high gear - * - * @param forw - * If the solenoid is to be pushed forward or not (backwards) - */ - public void changeShiftGear(boolean forw); - - /** - * Stops the solenoid that pushes the drivetrain into low or high gear - */ - public void turnGearSolenoidOff(); - - /** - * Resets the AHRS value - */ - public void resetAHRS(); - - /** - * Runs the left side of the drivetrain at the specified speed - * - * @param value - * Value for the motor(s) to run at - */ - public void setLeftMotor(double value); - - /** - * Runs the right side of the drivetrain at the specified speed - * - * @param value - * Value for the motor(s) to run at - */ - public void setRightMotor(double value); - - /** - * Tells the drivetrain to stop running - */ - public void stopDrive(); - /** - * Resets the encoders' distances to zero - */ - public void resetEnc(); + public void initDefaultCommand(); /** * Drives based on joystick input and SmartDashboard values @@ -77,125 +31,97 @@ public interface DrivetrainInterface { /** * Used for getting the speed at which the left side of the drivetrain is - * currently running + * currently set to. Gets data straight from SpeedControllerGroup. * - * @return The speed that the left side of the drivetrain is set to + * @return The speed that the left side of the drivetrain is set to [-1, 1] */ - public double getDtLeft(); + public double getDtLeftSpeed(); /** * Used for getting the speed at which the right side of the drivetrain is - * currently running - * - * @return The speed that the right side of the drivetrain is set to - */ - public double getDtRight(); - - /** - * Used to get the angle that the gyro currently reads + * currently set to. Gets data straight from SpeedControllerGroup. * - * @return The angle that the gyro reads - */ - public double getGyroAngle(); - - /** - * Resets the gyro to 0 - */ - public void resetGyro(); - - /** - * Disables the turnPID PIDController used for turning + * @return The speed that the right side of the drivetrain is set to [-1, 1] */ - public void disableTurnPid(); + public double getDtRightSpeed(); /** - * Enables the turnPID PIDController used for turning + * Updates the PIDControllers' PIDConstants based on SmartDashboard values */ - public void enableTurnPid(); + public void updatePidConstants(); /** - * Sets the setPoint of the turnPID PIDController - * - * @param set - * The value to set the setPoint at + * Enable the VelocityPIDControllers used for velocity control on each side of + * the DT */ - public void setTurnSetpoint(double set); + public void enableVelocityPIDs(); /** - * Enable the movePID PIDController used for moving + * Disables the VelocityPIDControllers used for velocity control on each side of + * the DT */ - public void enableMovePid(); + public void disableVelocityPIDs(); /** - * Disables the movePID PIDController used for moving + * Resets the AHRS value */ - public void disableMovePid(); + public void resetAHRS(); /** - * Sets the setPoint of the moveLeftPID PIDController + * Used to get the yaw angle (Z-axis in degrees) that the ahrs currently reads * - * @param set - * The value to set the setPoint at + * @return The angle that the ahrs reads (in degrees) */ - public void setMoveSetpointLeft(double set); + public double getAHRSAngle(); /** - * Sets the setPoint of the moveRightPID PIDController - * - * @param set - * The value to set the setPoint at + * Resets the encoders' distances to zero */ - public void setMoveSetpointRight(double set); + public void resetDistEncs(); /** * Sets the distancePerPulse property on the left encoder * - * @param dist - * The distance to set the distancePerPulse at + * @param ratio + * The ratio to set the distancePerPulse to (real dist units/encoder + * pulses) */ - public void setDistancePerPulseLeft(double dist); + public void setDistancePerPulseLeft(double ratio); /** * Sets the distancePerPulse property on the right encoder * - * @param dist - * The distance to set the distancePerPulse at + * @param ratio + * The ratio to set the distancePerPulse to (real dist units/encoder + * pulses) */ - public void setDistancePerPulseRight(double dist); + public void setDistancePerPulseRight(double ratio); /** - * Returns the value that Drivetrain receives due to implementing PIDOutput + * Returns the distance (in real units) that the left encoder reads * - * @return The value that is written by PIDControllers + * @return How far the left encoder has traveled in real units since last reset */ - public double getAnglePidOut(); + public double getLeftEncDist(); /** - * Returns the value that leftdrive should be set to according to PIDControllers + * Returns the distance (in real units) that the right encoder reads * - * @return The value that is written by PIDControllers + * @return How far the right encoder has traveled in real units since last reset */ - public double getLeftPidOut(); + public double getRightEncDist(); /** - * Returns the value that rightdrive should be set to according to - * PIDControllers + * Activates the solenoid to push the drivetrain into high or low gear * - * @return The value that is written by PIDControllers + * @param highGear + * If the solenoid is to be pushed into high gear (true, kForward) or + * low gear (false, kReverse) */ - public double getRightPidOut(); + public void shiftGears(boolean highGear); /** - * Returns whether the turnController PIDController senses that it's on target - * - * @return Whether the turnController PIDController is on target - */ - public boolean onTurnTarg(); - - /** - * Returns whether the moveController PIDController senses that it's on target - * - * @return Whether the moveController PIDController is on target + * Stops the solenoid that pushes the drivetrain into low or high gear */ - public boolean onDriveTarg(); + public void shiftGearSolenoidOff(); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LeftDrive.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LeftDrive.java deleted file mode 100644 index 55dce68..0000000 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LeftDrive.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.usfirst.frc.team199.Robot2018.subsystems; - -import org.usfirst.frc.team199.Robot2018.Robot; - -import edu.wpi.first.wpilibj.PIDOutput; -import edu.wpi.first.wpilibj.PIDSource; -import edu.wpi.first.wpilibj.PIDSourceType; - -public class LeftDrive implements PIDOutput, PIDSource{ - - @Override - public PIDSourceType getPIDSourceType() { - return PIDSourceType.kDisplacement; - } - - @Override - public double pidGet() { - return Robot.dt.getLeftEnc(); - } - - @Override - public void setPIDSourceType(PIDSourceType arg0) { - } - - @Override - public void pidWrite(double arg0) { - Robot.dt.pidLeftWrite(arg0); - } - -} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/RightDrive.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/RightDrive.java deleted file mode 100644 index d79c4a0..0000000 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/RightDrive.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.usfirst.frc.team199.Robot2018.subsystems; - -import org.usfirst.frc.team199.Robot2018.Robot; - -import edu.wpi.first.wpilibj.PIDOutput; -import edu.wpi.first.wpilibj.PIDSource; -import edu.wpi.first.wpilibj.PIDSourceType; - -public class RightDrive implements PIDOutput, PIDSource { - - @Override - public PIDSourceType getPIDSourceType() { - return PIDSourceType.kDisplacement; - } - - @Override - public double pidGet() { - return Robot.dt.getRightEnc(); - } - - @Override - public void setPIDSourceType(PIDSourceType arg0) { - } - - @Override - public void pidWrite(double arg0) { - Robot.dt.pidRightWrite(arg0); - } - -} diff --git a/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDSourceAverageTest.java b/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDSourceAverageTest.java new file mode 100644 index 0000000..57d69fd --- /dev/null +++ b/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDSourceAverageTest.java @@ -0,0 +1,59 @@ +package org.usfirst.frc.team199.Robot2018; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.mockito.Mockito.mock; +import static org.mockito.Mockito.when; + +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage; + +import edu.wpi.first.wpilibj.HLUsageReporting; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.internal.HardwareTimer; + +class PIDSourceAverageTest { + + @BeforeEach + void setUp() { + // Since VelocityPIDController extends PIDController and PIDController calls + // static methods in wpilib that only work on robot, + // we setup these mocks to allow the tests to run off robot. + HardwareTimer tim = mock(HardwareTimer.class); + Timer.Interface timerInstance = mock(Timer.Interface.class); + when(tim.newTimer()).thenReturn(timerInstance); + Timer.SetImplementation(tim); + HLUsageReporting.Interface usageReporter = mock(HLUsageReporting.Interface.class); + HLUsageReporting.SetImplementation(usageReporter); + } + + @Test + void test1() { + PIDSource lEnc = mock(PIDSource.class); + when(lEnc.getPIDSourceType()).thenReturn(PIDSourceType.kDisplacement); + + PIDSource rEnc = mock(PIDSource.class); + when(rEnc.getPIDSourceType()).thenReturn(PIDSourceType.kDisplacement); + PIDSourceAverage avg = new PIDSourceAverage(lEnc, rEnc); + assertEquals(avg.getPIDSourceType(), PIDSourceType.kDisplacement); + } + + @Test + void test2() { + PIDSource lEnc = mock(PIDSource.class); + when(lEnc.getPIDSourceType()).thenReturn(PIDSourceType.kDisplacement); + + PIDSource rEnc = mock(PIDSource.class); + when(rEnc.getPIDSourceType()).thenReturn(PIDSourceType.kRate); + PIDSourceAverage avg; + try { + avg = new PIDSourceAverage(lEnc, rEnc); + } catch (IllegalArgumentException e) { + avg = new PIDSourceAverage(rEnc, rEnc); + } + assertEquals(avg.getPIDSourceType(), PIDSourceType.kRate); + } + +} diff --git a/Robot2018/test/org/usfirst/frc/team199/Robot2018/VelocityPIDControllerTest.java b/Robot2018/test/org/usfirst/frc/team199/Robot2018/VelocityPIDControllerTest.java index fbca124..81a8975 100644 --- a/Robot2018/test/org/usfirst/frc/team199/Robot2018/VelocityPIDControllerTest.java +++ b/Robot2018/test/org/usfirst/frc/team199/Robot2018/VelocityPIDControllerTest.java @@ -2,7 +2,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.mockito.Mockito.mock; -import static org.mockito.Mockito.verify; import static org.mockito.Mockito.when; import org.junit.jupiter.api.BeforeEach; @@ -11,7 +10,7 @@ import edu.wpi.first.wpilibj.HLUsageReporting; import edu.wpi.first.wpilibj.PIDSource; -import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.internal.HardwareTimer; @@ -33,21 +32,20 @@ void setUp() { @Test void test1() { PIDSource source = mock(PIDSource.class); - SpeedController out = mock(SpeedController.class); + SpeedControllerGroup out = mock(SpeedControllerGroup.class); double p = 1; double i = 0.5; double d = 0.0037; VelocityPIDController vPID = new VelocityPIDController(p, i, d, source, out); - when(out.get()).thenReturn(6.5); - assertEquals(6.5, vPID.get()); - verify(out).get(); + vPID.set(20); + assertEquals(vPID.get(), 20); } @Test void test2() { PIDSource source = mock(PIDSource.class); - SpeedController out = mock(SpeedController.class); + SpeedControllerGroup out = mock(SpeedControllerGroup.class); double p = 1; double i = 0.5; double d = 0.0037; @@ -60,7 +58,7 @@ void test2() { @Test void test3() { PIDSource source = mock(PIDSource.class); - SpeedController out = mock(SpeedController.class); + SpeedControllerGroup out = mock(SpeedControllerGroup.class); double p = 1; double i = 0.5; double d = 0.0037; From 5ffaed1a243f851054d8c63a8abc590d6f088fe3 Mon Sep 17 00:00:00 2001 From: lhmcgann Date: Mon, 29 Jan 2018 19:54:56 -0800 Subject: [PATCH 2/7] Update .gitignore --- .gitignore | 2 -- 1 file changed, 2 deletions(-) diff --git a/.gitignore b/.gitignore index 0e8305b..ac314b5 100644 --- a/.gitignore +++ b/.gitignore @@ -2,5 +2,3 @@ bin/ build/ dist/ .DS_Store -*auto* -*Auto* From 536e79282c6ecd0168d1ef4e40eeafb4a3a3d088 Mon Sep 17 00:00:00 2001 From: lhmcgann Date: Mon, 29 Jan 2018 20:00:31 -0800 Subject: [PATCH 3/7] got PIDSourceAverage back bc .gitignore stuff from testing-no-auto --- .../autonomous/PIDSourceAverage.java | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/PIDSourceAverage.java diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/PIDSourceAverage.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/PIDSourceAverage.java new file mode 100644 index 0000000..14b9ef2 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/PIDSourceAverage.java @@ -0,0 +1,37 @@ +package org.usfirst.frc.team199.Robot2018.autonomous; + +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; + +public class PIDSourceAverage implements PIDSource { + + private PIDSource lSrc; + private PIDSource rSrc; + private PIDSourceType type; + + public PIDSourceAverage(PIDSource leftSource, PIDSource rightSource) { + lSrc = leftSource; + rSrc = rightSource; + if (leftSource.getPIDSourceType().equals(rightSource.getPIDSourceType())) { + type = leftSource.getPIDSourceType(); + } else { + throw new IllegalArgumentException(); + } + } + + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + type = pidSource; + } + + @Override + public PIDSourceType getPIDSourceType() { + return type; + } + + @Override + public double pidGet() { + return (lSrc.pidGet() + rSrc.pidGet()) / 2; + } + +} From f7c3e56b06211d1f34f445154880da49a816cdc1 Mon Sep 17 00:00:00 2001 From: lhmcgann Date: Mon, 29 Jan 2018 20:39:29 -0800 Subject: [PATCH 4/7] added kF VelocityPIDController: added constructor param and javadoc RobotMap: initial instantiation Drivetrain: updatePIDConstants method --- .../org/usfirst/frc/team199/Robot2018/RobotMap.java | 12 +++++++----- .../Robot2018/autonomous/VelocityPIDController.java | 7 +++++-- .../frc/team199/Robot2018/subsystems/Drivetrain.java | 4 ++-- 3 files changed, 14 insertions(+), 9 deletions(-) diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java index 3bbc21c..4b20617 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java @@ -114,12 +114,13 @@ public RobotMap() { dtLeft = new SpeedControllerGroup(dtLeftDrive, dtLeftSlave); leftVelocityController = new VelocityPIDController(Robot.getConst("MoveLeftkP", 1), - Robot.getConst("MoveLeftkI", 0), Robot.getConst("MoveLeftkD", 0), leftEncRate, dtLeft); + Robot.getConst("MoveLeftkI", 0), Robot.getConst("MoveLeftkD", 0), 1 / Robot.getConst("MaxSpeed", 17), + leftEncRate, dtLeft); leftVelocityController.enable(); leftVelocityController.setInputRange(0, Double.MAX_VALUE); leftVelocityController.setOutputRange(-1.0, 1.0); leftVelocityController.setContinuous(false); - leftVelocityController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceLeft", 2)); + leftVelocityController.setAbsoluteTolerance(Robot.getConst("MoveToleranceLeft", 2)); rightEncPort1 = new DigitalInput(getPort("1RightEnc", 2)); rightEncPort2 = new DigitalInput(getPort("2RightEnc", 3)); @@ -133,13 +134,14 @@ public RobotMap() { configSPX(dtRightSlave); dtRight = new SpeedControllerGroup(dtRightDrive, dtRightSlave); - rightVelocityController = new VelocityPIDController(Robot.getConst("ConstMoveRightkP", 1), - Robot.getConst("ConstMoveRightkI", 0), Robot.getConst("ConstMoveRightkD", 0), rightEncRate, dtRight); + rightVelocityController = new VelocityPIDController(Robot.getConst("MoveRightkP", 1), + Robot.getConst("MoveRightkI", 0), Robot.getConst("MoveRightkD", 0), 1 / Robot.getConst("MaxSpeed", 17), + rightEncRate, dtRight); rightVelocityController.enable(); rightVelocityController.setInputRange(0, Double.MAX_VALUE); rightVelocityController.setOutputRange(-1.0, 1.0); rightVelocityController.setContinuous(false); - rightVelocityController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceRight", 2)); + rightVelocityController.setAbsoluteTolerance(Robot.getConst("MoveToleranceRight", 2)); robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController); diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java index 8b24f04..5355267 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java @@ -20,13 +20,16 @@ public class VelocityPIDController extends PIDController implements SpeedControl * the integral PID constant * @param kd * the derivative PID constant + * @param kf + * the feed forward value: should be 1/MaxSpeed * @param source * the sensor (e.g. velocity encoder) you are reading from * @param output * the SpeedController you are telling what to do */ - public VelocityPIDController(double kp, double ki, double kd, PIDSource source, SpeedControllerGroup output) { - super(kp, ki, kd, source, output); + public VelocityPIDController(double kp, double ki, double kd, double kf, PIDSource source, + SpeedControllerGroup output) { + super(kp, ki, kd, kf, source, output); out = output; } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java index a7c2ae5..e4b316d 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java @@ -112,9 +112,9 @@ public double getDtRightSpeed() { @Override public void updatePidConstants() { leftVelocityController.setPID(Robot.getConst("MoveLeftkP", 1), Robot.getConst("MoveLeftkI", 0), - Robot.getConst("MoveLeftkD", 0)); + Robot.getConst("MoveLeftkD", 0), 1 / Robot.getConst("MaxSpeed", 17)); rightVelocityController.setPID(Robot.getConst("MoveRightkP", 1), Robot.getConst("MoveRightkI", 0), - Robot.getConst("MoveRightkD", 0)); + Robot.getConst("MoveRightkD", 0), 1 / Robot.getConst("MaxSpeed", 17)); } /** From 652686273728d071f95c15a674c48e3887c06b55 Mon Sep 17 00:00:00 2001 From: lhmcgann Date: Mon, 29 Jan 2018 21:54:48 -0800 Subject: [PATCH 5/7] fixed tests added f in VelocityPIDController constructors to resolve errors added test to PIDSourceAverageTest to make sure it actually returns the average --- .../Robot2018/PIDSourceAverageTest.java | 29 +++++++++++++++++++ .../Robot2018/VelocityPIDControllerTest.java | 9 ++++-- 2 files changed, 35 insertions(+), 3 deletions(-) diff --git a/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDSourceAverageTest.java b/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDSourceAverageTest.java index 57d69fd..b7e68b4 100644 --- a/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDSourceAverageTest.java +++ b/Robot2018/test/org/usfirst/frc/team199/Robot2018/PIDSourceAverageTest.java @@ -56,4 +56,33 @@ void test2() { assertEquals(avg.getPIDSourceType(), PIDSourceType.kRate); } + @Test + void test3() { + + PIDSource lEnc = mock(PIDSource.class); + when(lEnc.getPIDSourceType()).thenReturn(PIDSourceType.kDisplacement); + + PIDSource rEnc = mock(PIDSource.class); + when(rEnc.getPIDSourceType()).thenReturn(PIDSourceType.kDisplacement); + + PIDSourceAverage avg; + try { + avg = new PIDSourceAverage(lEnc, rEnc); + } catch (IllegalArgumentException e) { + avg = new PIDSourceAverage(rEnc, rEnc); + } + assertEquals(avg.getPIDSourceType(), PIDSourceType.kDisplacement); + + double leftVal = 2.0; + double rightVal = 3.0; + double avgVal = (leftVal + rightVal) / 2; + + assertEquals(avgVal, 2.5); + + when(lEnc.pidGet()).thenReturn(leftVal); + when(rEnc.pidGet()).thenReturn(rightVal); + + assertEquals(avg.pidGet(), avgVal); + } + } diff --git a/Robot2018/test/org/usfirst/frc/team199/Robot2018/VelocityPIDControllerTest.java b/Robot2018/test/org/usfirst/frc/team199/Robot2018/VelocityPIDControllerTest.java index 81a8975..fdcbb77 100644 --- a/Robot2018/test/org/usfirst/frc/team199/Robot2018/VelocityPIDControllerTest.java +++ b/Robot2018/test/org/usfirst/frc/team199/Robot2018/VelocityPIDControllerTest.java @@ -36,7 +36,8 @@ void test1() { double p = 1; double i = 0.5; double d = 0.0037; - VelocityPIDController vPID = new VelocityPIDController(p, i, d, source, out); + double f = 1.0 / 17; + VelocityPIDController vPID = new VelocityPIDController(p, i, d, f, source, out); vPID.set(20); assertEquals(vPID.get(), 20); @@ -49,7 +50,8 @@ void test2() { double p = 1; double i = 0.5; double d = 0.0037; - VelocityPIDController vPID = new VelocityPIDController(p, i, d, source, out); + double f = 1.0 / 17; + VelocityPIDController vPID = new VelocityPIDController(p, i, d, f, source, out); vPID.pidWrite(20); assertEquals(vPID.getSetpoint(), 20); @@ -62,7 +64,8 @@ void test3() { double p = 1; double i = 0.5; double d = 0.0037; - VelocityPIDController vPID = new VelocityPIDController(p, i, d, source, out); + double f = 1.0 / 17; + VelocityPIDController vPID = new VelocityPIDController(p, i, d, f, source, out); vPID.set(20); assertEquals(vPID.getSetpoint(), 20); From e584ed71edab7767550a5ba7778e472b0dcf8457 Mon Sep 17 00:00:00 2001 From: lhmcgann Date: Mon, 29 Jan 2018 22:01:53 -0800 Subject: [PATCH 6/7] changed button names in OI per Corvin's suggestion in order to clarify and leave no room for confusion about what is what (e.g. button was called PIDMove but so was the command it was calling; changed to PIDMoveButton, etc.) yeah, I know the names are long, but you know exactly what they are :) --- .../org/usfirst/frc/team199/Robot2018/OI.java | 42 +++++++++---------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java index 6f5d33f..452606c 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java @@ -26,14 +26,14 @@ public class OI { public Joystick leftJoy; - private JoystickButton shiftLowGear; - private JoystickButton shiftHighGear; - private JoystickButton shiftDriveType; - private JoystickButton PIDMove; - private JoystickButton PIDTurn; + private JoystickButton shiftLowGearButton; + private JoystickButton shiftHighGearButton; + private JoystickButton shiftDriveTypeButton; + private JoystickButton PIDMoveButton; + private JoystickButton PIDTurnButton; public Joystick rightJoy; - private JoystickButton updatePidConstants; - private JoystickButton updateEncoderDPP; + private JoystickButton updatePIDConstantsButton; + private JoystickButton updateEncoderDPPButton; public Joystick manipulator; public int getButton(String key, int def) { @@ -45,22 +45,22 @@ public int getButton(String key, int def) { public OI() { leftJoy = new Joystick(0); - shiftLowGear = new JoystickButton(leftJoy, getButton("Shift Low Gear", 3)); - shiftLowGear.whenPressed(new ShiftLowGear()); - shiftHighGear = new JoystickButton(leftJoy, getButton("Shift High Gear", 5)); - shiftHighGear.whenPressed(new ShiftHighGear()); - shiftDriveType = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2)); - shiftDriveType.whenPressed(new ShiftDriveType()); - PIDMove = new JoystickButton(leftJoy, getButton("PID Move", 7)); - PIDMove.whenPressed(new PIDMove(40, Robot.dt, RobotMap.distEncAvg)); - PIDTurn = new JoystickButton(leftJoy, getButton("PID Turn", 8)); - PIDTurn.whenPressed(new PIDTurn(30, Robot.dt, RobotMap.fancyGyro)); + shiftLowGearButton = new JoystickButton(leftJoy, getButton("Shift Low Gear", 3)); + shiftLowGearButton.whenPressed(new ShiftLowGear()); + shiftHighGearButton = new JoystickButton(leftJoy, getButton("Shift High Gear", 5)); + shiftHighGearButton.whenPressed(new ShiftHighGear()); + shiftDriveTypeButton = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2)); + shiftDriveTypeButton.whenPressed(new ShiftDriveType()); + PIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7)); + PIDMoveButton.whenPressed(new PIDMove(40, Robot.dt, RobotMap.distEncAvg)); + PIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8)); + PIDTurnButton.whenPressed(new PIDTurn(30, Robot.dt, RobotMap.fancyGyro)); rightJoy = new Joystick(1); - updatePidConstants = new JoystickButton(rightJoy, getButton("Get PID Constants", 8)); - updatePidConstants.whenPressed(new UpdatePIDConstants()); - updateEncoderDPP = new JoystickButton(rightJoy, getButton("Get Encoder Dist Per Pulse", 9)); - updateEncoderDPP.whenPressed(new SetDistancePerPulse()); + updatePIDConstantsButton = new JoystickButton(rightJoy, getButton("Get PID Constants", 8)); + updatePIDConstantsButton.whenPressed(new UpdatePIDConstants()); + updateEncoderDPPButton = new JoystickButton(rightJoy, getButton("Get Encoder Dist Per Pulse", 9)); + updateEncoderDPPButton.whenPressed(new SetDistancePerPulse()); manipulator = new Joystick(2); } From 9753c5704b06da3085f502d727a88d535c978d2c Mon Sep 17 00:00:00 2001 From: lhmcgann Date: Mon, 29 Jan 2018 22:10:09 -0800 Subject: [PATCH 7/7] changed drive -> master, deleted AnalogGyro remnants all in RobotMap changed TalonSRX dt motor controller names from Drive -> Master per request by Dean forgot to delete actual AnalogGyro in RobotMap, so did that --- .../frc/team199/Robot2018/RobotMap.java | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java index 4b20617..917e5d1 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java @@ -14,7 +14,6 @@ import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DigitalSource; import edu.wpi.first.wpilibj.DoubleSolenoid; @@ -41,7 +40,7 @@ public class RobotMap { public static DigitalSource leftEncPort2; public static Encoder leftEncDist; public static Encoder leftEncRate; - public static WPI_TalonSRX dtLeftDrive; + public static WPI_TalonSRX dtLeftMaster; public static WPI_VictorSPX dtLeftSlave; public static SpeedControllerGroup dtLeft; public static VelocityPIDController leftVelocityController; @@ -50,7 +49,7 @@ public class RobotMap { public static DigitalSource rightEncPort2; public static Encoder rightEncDist; public static Encoder rightEncRate; - public static WPI_TalonSRX dtRightDrive; + public static WPI_TalonSRX dtRightMaster; public static WPI_VictorSPX dtRightSlave; public static SpeedControllerGroup dtRight; public static VelocityPIDController rightVelocityController; @@ -59,7 +58,6 @@ public class RobotMap { public static PIDSourceAverage distEncAvg; public static AHRS fancyGyro; - public static AnalogGyro dtGyro; public static DoubleSolenoid dtGear; /** @@ -107,11 +105,11 @@ public RobotMap() { leftEncDist.setPIDSourceType(PIDSourceType.kDisplacement); leftEncRate = new Encoder(leftEncPort1, leftEncPort2); leftEncRate.setPIDSourceType(PIDSourceType.kRate); - dtLeftDrive = new WPI_TalonSRX(getPort("LeftTalonSRXDrive", 0)); - configSRX(dtLeftDrive); + dtLeftMaster = new WPI_TalonSRX(getPort("LeftTalonSRXMaster", 0)); + configSRX(dtLeftMaster); dtLeftSlave = new WPI_VictorSPX(getPort("LeftTalonSPXSlave", 1)); configSPX(dtLeftSlave); - dtLeft = new SpeedControllerGroup(dtLeftDrive, dtLeftSlave); + dtLeft = new SpeedControllerGroup(dtLeftMaster, dtLeftSlave); leftVelocityController = new VelocityPIDController(Robot.getConst("MoveLeftkP", 1), Robot.getConst("MoveLeftkI", 0), Robot.getConst("MoveLeftkD", 0), 1 / Robot.getConst("MaxSpeed", 17), @@ -128,11 +126,11 @@ public RobotMap() { rightEncDist.setPIDSourceType(PIDSourceType.kDisplacement); rightEncRate = new Encoder(leftEncPort1, leftEncPort2); rightEncRate.setPIDSourceType(PIDSourceType.kRate); - dtRightDrive = new WPI_TalonSRX(getPort("RightTalonSRXDrive", 2)); - configSRX(dtRightDrive); + dtRightMaster = new WPI_TalonSRX(getPort("RightTalonSRXMaster", 2)); + configSRX(dtRightMaster); dtRightSlave = new WPI_VictorSPX(getPort("RightTalonSPXSlave", 3)); configSPX(dtRightSlave); - dtRight = new SpeedControllerGroup(dtRightDrive, dtRightSlave); + dtRight = new SpeedControllerGroup(dtRightMaster, dtRightSlave); rightVelocityController = new VelocityPIDController(Robot.getConst("MoveRightkP", 1), Robot.getConst("MoveRightkI", 0), Robot.getConst("MoveRightkD", 0), 1 / Robot.getConst("MaxSpeed", 17), @@ -147,7 +145,6 @@ public RobotMap() { distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist); fancyGyro = new AHRS(SerialPort.Port.kMXP); - dtGyro = new AnalogGyro(getPort("Gyro", 0)); dtGear = new DoubleSolenoid(getPort("1dtGearSolenoid", 0), getPort("2dtGearSolenoid", 1)); }