Added lift motors to robotMap and accounted for two CIMs in kD calculations

Author: dfang314

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

@@ -21,6 +21,7 @@
 import edu.wpi.first.wpilibj.PIDSourceType;
 import edu.wpi.first.wpilibj.SerialPort;
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.VictorSP;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
@@ -32,10 +33,12 @@
  */
 public class RobotMap {
 
-	public static WPI_TalonSRX intakeMotor;
 	public static WPI_TalonSRX liftMotor;
 	public static WPI_TalonSRX climberMotor;
 
+	public static VictorSP leftIntakeMotor;
+	public static VictorSP rightIntakeMotor;
+
 	public static DigitalSource leftEncPort1;
 	public static DigitalSource leftEncPort2;
 	public static Encoder leftEncDist;
@@ -102,13 +105,14 @@ private void configSPX(WPI_VictorSPX mc) {
 
 	public RobotMap() {
 
-		intakeMotor = new WPI_TalonSRX(getPort("IntakeTalonSRX", 4));
-		configSRX(intakeMotor);
 		liftMotor = new WPI_TalonSRX(getPort("LiftTalonSRX", 5));
 		configSRX(liftMotor);
 		climberMotor = new WPI_TalonSRX(getPort("ClimberTalonSRX", 6));
 		configSRX(climberMotor);
 
+		leftIntakeMotor = new VictorSP(getPort("IntakeLeftVictorSP", 0));
+		rightIntakeMotor = new VictorSP(getPort("IntakeRightVictorSP", 1));
+
 		leftEncPort1 = new DigitalInput(getPort("1LeftEnc", 0));
 		leftEncPort2 = new DigitalInput(getPort("2LeftEnc", 1));
 		leftEncDist = new Encoder(leftEncPort1, leftEncPort2);
@@ -191,15 +195,14 @@ public double calcDefkD() {
 		 * half of the drivetrain only has to support half of the robot), and radius of
 		 * the drivetrain wheels squared. It's inversely proportional to the stall
 		 * torque of the shaft, which is found by multiplying the stall torque of the
-		 * motor with the gear reduction.
+		 * motor with the gear reduction by the amount of motors.
 		 */
 		double gearReduction = Robot.getBool("High Gear", false) ? Robot.getConst("High Gear Gear Reduction", 5.392)
 				: Robot.getConst("Low Gear Gear Reduction", 12.255);
+		double radius = Robot.getConst("Radius of Drivetrain Wheel", 0.0635);
 		double timeConstant = Robot.getConst("Omega Max", 5330) / gearReduction
-				* convertLbsTokG(Robot.getConst("Mass of Robot", 150)) / 2
-				* Robot.getConst("Radius of Drivetrain Wheel", 0.0635)
-				* Robot.getConst("Radius of Drivetrain Wheel", 0.0635)
-				/ (Robot.getConst("Stall Torque", 2.41) * gearReduction);
+				* convertLbsTokG(Robot.getConst("Mass of Robot", 150)) / 2 * radius * radius
+				/ (Robot.getConst("Stall Torque", 2.41) * gearReduction * 2);
 		double cycleTime = Robot.getConst("Code cycle time", 0.1);
 		/*
 		 * The denominator of kD is 1-(e ^ -cycleTime / timeConstant). The numerator is