PID input/output ranges and un-overriding all of kate's stuff

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);

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();

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));

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java

@@ -24,16 +24,23 @@ 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
 	@Override
 	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));

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();
 	}
 }

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();
 	}
 }

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();
+	}
+
+
 }
 

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();
+
+
 }

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);
+		}
+	}
 }

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();
 }

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) {
+
+	}
+
 }