PID by math setup

implemented getLiftTimeConstant and methods in it for Lift
still need to implement default constant values (using more math, see
PIDMove constructor)
commented out stuff in testPeriodic bc don't need them

Author: lhmcgann

Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java

@@ -290,7 +290,7 @@ public void testInit() {
 	@Override
 	public void testPeriodic() {
 		// Scheduler.getInstance().run();
-		lift.disable();
-		lift.runMotor(SmartDashboard.getNumber("Voltage to Lift", 0));
+		// lift.disable();
+		// lift.runMotor(SmartDashboard.getNumber("Voltage to Lift", 0));
 	}
 }

Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java

@@ -43,7 +43,7 @@ public class RobotMap {
 	public static DigitalSource liftEncPort1;
 	public static DigitalSource liftEncPort2;
 
-	//public static WPI_TalonSRX climberMotor;
+	// public static WPI_TalonSRX climberMotor;
 
 	public static VictorSP leftIntakeMotor;
 	public static VictorSP rightIntakeMotor;
@@ -123,7 +123,7 @@ private void configSPX(WPI_VictorSPX mc) {
 
 	public RobotMap() {
 		pdp = new PowerDistributionPanel();
-		
+
 		liftMotorA = new WPI_VictorSPX(getPort("LiftVictorSPX", 5));
 		configSPX(liftMotorA);
 		liftMotorB = new WPI_TalonSRX(getPort("1LiftTalonSRX", 6));
@@ -252,7 +252,10 @@ public double getRadius() {
 		return Robot.getConst("Radius of Drivetrain Wheel", 0.0635);
 	}
 
-	public double getOmegaMax() {
+	/**
+	 * @return the RPM max of a CIM
+	 */
+	public static double getOmegaMax() {
 		return Robot.getConst("Omega Max", 5330);
 	}
 
@@ -264,7 +267,10 @@ public double getCycleTime() {
 		return Robot.getConst("Code cycle time", 0.05);
 	}
 
-	public double getStallTorque() {
+	/**
+	 * @return the stall torque of a CIM
+	 */
+	public static double getStallTorque() {
 		return Robot.getConst("Stall Torque", 2.41);
 	}
 
@@ -273,7 +279,12 @@ private double convertLbsTokG(double lbs) {
 		return lbs * 0.45359237;
 	}
 
-	private double convertNtokG(double newtons) {
+	/**
+	 * @param the
+	 *            weight (in Newtons)
+	 * @return the equivalent mass (in kg)
+	 */
+	public static double convertNtokG(double newtons) {
 		// weight / accel due to grav = kg
 		return newtons / 9.81;
 	}

Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java

@@ -155,6 +155,14 @@ public double getWiggleRoom() {
 		return WIGGLE_ROOM;
 	}
 
+	/**
+	 * @return the max speed of the lift
+	 */
+	public double getLiftMaxSpeed() {
+		/** @todo find lift max speed and set default below */
+		return Robot.getConst("Lift Max Speed", 0);
+	}
+
 	/**
 	 * Uses AMT103 Encoder to detect the current lift height with respect to the
 	 * lift's min height (inches)
@@ -176,4 +184,42 @@ protected void usePIDOutput(double output) {
 		out += Robot.getConst("Lift: Necessary Voltage", 0);
 		runMotor(out);
 	}
+
+	/**
+	 * Gets the time constant of the drivetrain, which is used to calculate PID
+	 * constants.
+	 * 
+	 * @return time constant
+	 */
+	public double getLiftTimeConstant() {
+		double gearReduction = getLiftGearRatio();
+		double radius = getLiftRadius();
+		double timeConstant = RobotMap.getOmegaMax() / gearReduction / 60 * 2 * Math.PI
+				* RobotMap.convertNtokG(getLiftedWeight()) * radius * radius
+				/ (RobotMap.getStallTorque() * gearReduction);
+		return timeConstant;
+	}
+
+	/**
+	 * @return the gear ratio of the lift gearbox
+	 */
+	public double getLiftGearRatio() {
+		return Robot.getConst("Lift Gear Reduction", 10);
+	}
+
+	/**
+	 * @return the lift sprocket's radius
+	 */
+	public double getLiftRadius() {
+		// 15 tooth, #35 sprocket
+		return Robot.getConst("Lift Sprocket Pitch Diam", 1.79) / 2;
+	}
+
+	/**
+	 * @return the weight (in Newtons) of everything that is being lifted: intake,
+	 *         lift components, NOT cube
+	 */
+	public double getLiftedWeight() {
+		return Robot.getConst("Weight of Lifted Stuff", 342);
+	}
 }