diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java index 917e5d1..f682301 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java @@ -111,14 +111,15 @@ public RobotMap() { configSPX(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), - leftEncRate, dtLeft); + leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkP", 1), + Robot.getConst("VelocityLeftkI", 0), Robot.getConst("VelocityLeftkD", 0), + 1 / Robot.getConst("Max Low Speed", 84), leftEncRate, dtLeft); leftVelocityController.enable(); - leftVelocityController.setInputRange(0, Double.MAX_VALUE); + leftVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204), + Robot.getConst("Max High Speed", 204)); leftVelocityController.setOutputRange(-1.0, 1.0); leftVelocityController.setContinuous(false); - leftVelocityController.setAbsoluteTolerance(Robot.getConst("MoveToleranceLeft", 2)); + leftVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceLeft", 2)); rightEncPort1 = new DigitalInput(getPort("1RightEnc", 2)); rightEncPort2 = new DigitalInput(getPort("2RightEnc", 3)); @@ -132,16 +133,18 @@ public RobotMap() { configSPX(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), - rightEncRate, dtRight); + rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkP", 1), + Robot.getConst("VelocityRightkI", 0), Robot.getConst("VelocityRightkD", 0), + 1 / Robot.getConst("Max Low Speed", 84), rightEncRate, dtRight); rightVelocityController.enable(); - rightVelocityController.setInputRange(0, Double.MAX_VALUE); + rightVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204), + Robot.getConst("Max High Speed", 204)); rightVelocityController.setOutputRange(-1.0, 1.0); rightVelocityController.setContinuous(false); - rightVelocityController.setAbsoluteTolerance(Robot.getConst("MoveToleranceRight", 2)); + rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight", 2)); robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController); + robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204)); distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist); fancyGyro = new AHRS(SerialPort.Port.kMXP); 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 5355267..416f167 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java @@ -37,7 +37,7 @@ public VelocityPIDController(double kp, double ki, double kd, double kf, PIDSour * Sets the target speed * * @param speed - * the target speed [-1, 1] + * the target speed in inches/second */ @Override public void pidWrite(double output) { @@ -48,7 +48,7 @@ public void pidWrite(double output) { * Sets the target speed * * @param speed - * the target speed [-1, 1] + * the target speed in inches/second */ @Override public void set(double speed) { @@ -68,24 +68,29 @@ public double get() { } /** + * Invert this side of the DT (flip forwards and backwards). * - * */ + * @param isInverted + * invert this side of the DT or not + */ @Override public void setInverted(boolean isInverted) { out.setInverted(isInverted); } /** + * Get whether or not this side of the DT is inverted. * - * */ + * @return is this side of the DT inverted or not + */ @Override public boolean getInverted() { return out.getInverted(); } /** - * - * */ + * Set the output to zero. + */ @Override public void stopMotor() { 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 ce46a05..50be3b3 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java @@ -23,15 +23,18 @@ public PIDMove(double targ, DrivetrainInterface dt, PIDSourceAverage avg) { target = targ; this.dt = dt; requires(Robot.dt); + double kf = 1 / (dt.getCurrentMaxSpeed() * Robot.getConst("Default PID Update Time", 0.05)); moveController = new PIDController(Robot.getConst("MovekP", 1), Robot.getConst("MovekI", 0), - Robot.getConst("MovekD", 0), avg, this); + Robot.getConst("MovekD", 0), kf, avg, this); } // Called just before this Command runs the first time @Override public void initialize() { dt.resetDistEncs(); - moveController.setInputRange(0, Double.MAX_VALUE); + // input is in inches + moveController.setInputRange(-Robot.getConst("Max High Speed", 204), Robot.getConst("Max High Speed", 204)); + // output in "motor units" (arcade and tank only accept values [-1, 1] moveController.setOutputRange(-1.0, 1.0); moveController.setContinuous(false); moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2)); 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 6aa50df..98ec47d 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java @@ -24,8 +24,13 @@ public PIDTurn(double targ, DrivetrainInterface dt, AHRS ahrs) { target = targ; this.dt = dt; requires(Robot.dt); + // calculates the maximum turning speed in degrees/sec based on the max linear + // speed in inches/s and the distance (inches) between sides of the DT + double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 + / (Math.PI * Robot.getConst("Distance Between Wheels", 26.25)); + double kf = 1 / (maxTurnSpeed * Robot.getConst("Default PID Update Time", 0.05)); turnController = new PIDController(Robot.getConst("TurnkP", 1), Robot.getConst("TurnkI", 0), - Robot.getConst("TurnkD", 0), ahrs, this); + Robot.getConst("TurnkD", 0), kf, ahrs, this); } // Called just before this Command runs the first time @@ -33,7 +38,9 @@ public PIDTurn(double targ, DrivetrainInterface dt, AHRS ahrs) { protected void initialize() { dt.resetAHRS(); turnController.disable(); + // input is in degrees turnController.setInputRange(-180, 180); + // output in "motor units" (arcade and tank only accept values [-1, 1] turnController.setOutputRange(-1.0, 1.0); turnController.setContinuous(); turnController.setAbsoluteTolerance(Robot.getConst("TurnTolerance", 1)); 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 6ea12f6..89b81b6 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftHighGear.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftHighGear.java @@ -21,5 +21,6 @@ public ShiftHighGear() { protected void initialize() { Robot.dt.shiftGears(true); SmartDashboard.putBoolean("High Gear", true); + Robot.dt.resetVelocityPIDkFConsts(); } } 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 10c2039..4f403da 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftLowGear.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftLowGear.java @@ -21,5 +21,6 @@ public ShiftLowGear() { protected void initialize() { Robot.dt.shiftGears(false); SmartDashboard.putBoolean("High Gear", false); + Robot.dt.resetVelocityPIDkFConsts(); } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Climber.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Climber.java index e8b5bce..cf24d63 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Climber.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Climber.java @@ -2,13 +2,17 @@ import edu.wpi.first.wpilibj.command.Subsystem; +import org.usfirst.frc.team199.Robot2018.RobotMap; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + /** * */ public class Climber extends Subsystem implements ClimberInterface { - // Put methods for controlling this subsystem - // here. Call these from Commands. + private final WPI_TalonSRX climberMotor = RobotMap.climberMotor; + /** * Set the default command for a subsystem here. @@ -17,5 +21,40 @@ public void initDefaultCommand() { // Set the default command for a subsystem here. //setDefaultCommand(new MySpecialCommand()); } + + + + + /** + * runs the motors + * + */ + public void runClimber(double speed) { + + } + + /** + * attaches the climber hook to the lift. + * Requires that Lift is on the ground + */ + public void attachToLift() { + + } + + /** + * attaches hook to bar and releases it from the lift + */ + public void attachToBar() { + + } + + /** + * stops the climber + */ + public void stopClimber() { + climberMotor.stopMotor(); + } + + } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/ClimberInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/ClimberInterface.java index 2f37fb0..deda661 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/ClimberInterface.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/ClimberInterface.java @@ -7,4 +7,26 @@ public interface ClimberInterface { * */ public void initDefaultCommand(); + /** + * runs the motors + */ + public void runClimber(double speed); + + /** + * attaches the climber hook to the lift. + * Requires that Lift is on the ground + */ + public void attachToLift(); + + /** + * attaches hook to bar and releases it from the lift + */ + public void attachToBar(); + + /** + * stops the climber + */ + public void stopClimber(); + + } 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 e4b316d..7855dfe 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java @@ -111,10 +111,11 @@ public double getDtRightSpeed() { */ @Override public void updatePidConstants() { - leftVelocityController.setPID(Robot.getConst("MoveLeftkP", 1), Robot.getConst("MoveLeftkI", 0), - Robot.getConst("MoveLeftkD", 0), 1 / Robot.getConst("MaxSpeed", 17)); - rightVelocityController.setPID(Robot.getConst("MoveRightkP", 1), Robot.getConst("MoveRightkI", 0), - Robot.getConst("MoveRightkD", 0), 1 / Robot.getConst("MaxSpeed", 17)); + leftVelocityController.setPID(Robot.getConst("VelocityLeftkP", 1), Robot.getConst("VelocityLeftkI", 0), + Robot.getConst("VelocityLeftkD", 0)); + rightVelocityController.setPID(Robot.getConst("VelocityRightkP", 1), Robot.getConst("VelocityRightkI", 0), + Robot.getConst("VelocityRightkD", 0)); + resetVelocityPIDkFConsts(); } /** @@ -209,7 +210,7 @@ public double getRightEncDist() { } /** - * Activates the solenoid to push the drivetrain into high or low gear + * Activates the solenoid to push the drivetrain into high or low gear. * * @param highGear * If the solenoid is to be pushed into high gear (true, kForward) or @@ -231,4 +232,35 @@ public void shiftGears(boolean highGear) { public void shiftGearSolenoidOff() { dtGear.set(DoubleSolenoid.Value.kOff); } + + /** + * Reset the kf constants for both VelocityPIDControllers based on current DT + * gearing (high or low gear). + * + * @param newKF + * the new kF constant based on high and low gear max speeds; should + * be 1 / max speed + * @return the new kF value as 1 / correct max speed + */ + @Override + public double resetVelocityPIDkFConsts() { + double newKF = 1 / getCurrentMaxSpeed(); + leftVelocityController.setF(newKF); + rightVelocityController.setF(newKF); + return newKF; + } + + /** + * Gets the current max speed of the DT based on gearing (high or low gear) + * + * @return the current max speed of the DT in inches/second + */ + @Override + public double getCurrentMaxSpeed() { + if (Robot.getBool("High Gear", true)) { + return Robot.getConst("Max High Speed", 204); + } else { + return Robot.getConst("Max Low Speed", 84); + } + } } 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 91a554f..8d3d842 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/DrivetrainInterface.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/DrivetrainInterface.java @@ -124,4 +124,22 @@ public interface DrivetrainInterface { * Stops the solenoid that pushes the drivetrain into low or high gear */ public void shiftGearSolenoidOff(); + + /** + * Reset the kf constants for both VelocityPIDControllers based on current DT + * gearing (high or low gear). + * + * @param newKF + * the new kF constant based on high and low gear max speeds; should + * be 1 / max speed + * @return the new kF value as 1 / correct max speed + */ + public double resetVelocityPIDkFConsts(); + + /** + * Gets the current max speed of the DT based on gearing (high or low gear) + * + * @return the current max speed of the DT in inches/second + */ + public double getCurrentMaxSpeed(); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java index 768e4e5..e850424 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java @@ -1,14 +1,19 @@ package org.usfirst.frc.team199.Robot2018.subsystems; +import org.usfirst.frc.team199.Robot2018.RobotMap; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + import edu.wpi.first.wpilibj.command.Subsystem; /** * */ public class IntakeEject extends Subsystem implements IntakeEjectInterface { - - // Put methods for controlling this subsystem - // here. Call these from Commands. + + private final WPI_TalonSRX intakeMotor = RobotMap.intakeMotor; + + /** * Set the default command for a subsystem here. @@ -17,5 +22,46 @@ public void initDefaultCommand() { // Set the default command for a subsystem here. //setDefaultCommand(new MySpecialCommand()); } + + /** + * returns current motor value + */ + public double getIntakeSpeed() { + return intakeMotor.get(); + } + + /** + * Uses (insert sensor here) to detect + * a cube in front of the robot. + */ + public boolean seeCube() { + return false; + } + + /** + * Uses (insert sensor here) to detect if + * the cube is currently inside the robot + * + */ + public boolean hasCube() { + return false; + } + + /** + * stops the motors + * + */ + public void stopIntake() { + intakeMotor.stopMotor(); + } + + /** + * Spins the rollers + * @param speed - positive -> rollers in, negative -> rollers out + */ + public void runIntake(double speed) { + + } + } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java index 1a38594..428e1f8 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java @@ -7,4 +7,35 @@ public interface IntakeEjectInterface { * */ public void initDefaultCommand(); + /** + * returns current motor value + */ + public double getIntakeSpeed(); + + /** + * Uses (insert sensor here) to detect + * a cube in front of the robot. + */ + public boolean seeCube(); + + /** + * Uses (insert sensor here) to detect if + * the cube is currently inside the robot + * + */ + public boolean hasCube(); + + /** + * stops the motors + * + */ + public void stopIntake(); + + /** + * Spins the rollers + * @param speed - positive -> rollers in, negative -> rollers out + */ + public void runIntake(double speed); + + } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java index da2d37b..d184723 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java @@ -1,5 +1,9 @@ package org.usfirst.frc.team199.Robot2018.subsystems; +import org.usfirst.frc.team199.Robot2018.RobotMap; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + import edu.wpi.first.wpilibj.command.Subsystem; /** @@ -7,8 +11,9 @@ */ public class Lift extends Subsystem implements LiftInterface { - // Put methods for controlling this subsystem - // here. Call these from Commands. + private final WPI_TalonSRX liftMotor = RobotMap.liftMotor; + + private Position targetPosition = Position.GROUND; /** * Set the default command for a subsystem here. @@ -17,5 +22,41 @@ public void initDefaultCommand() { // Set the default command for a subsystem here. //setDefaultCommand(new MySpecialCommand()); } + + public void setTargetPosition(Position newPosition) { + targetPosition = newPosition; + } + + /** + * Uses (insert sensor here) to detect the current lift position + */ + public double getHeight() { + return -1; + } + + /** + * stops the lift + */ + public void stopLift() { + liftMotor.stopMotor(); + } + + /** + * gets current motor values + */ + public double getLiftSpeed() { + return liftMotor.get(); + } + + /** + * Goes to specified height + * @param position - ground, switch, scale, bar + * @param offset - distance up or down from the position + */ + public void goToPosition(Position position, double offset) { + + } + + } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java index f09e665..30324da 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/LiftInterface.java @@ -7,4 +7,35 @@ public interface LiftInterface { * */ public void initDefaultCommand(); + public enum Position { + GROUND, + SWITCH, + SCALE, + BAR + } + + /** + * Uses (insert sensor here) to detect the current lift position + */ + public double getHeight(); + + /** + * stops the lift + */ + public void stopLift(); + + /** + * gets current motor values + */ + public double getLiftSpeed(); + + + /** + * Goes to specified height + * @param position - ground, switch, scale, bar + * @param offset - distance up or down from position + */ + public void goToPosition(Position position, double offset); + + }