Merge branch 'master' of https://github.com/doawelul/RobotCode2018.git

Conflicts:
	Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
	Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java

	merging, most of this stuff not working super well rn

Author: DriverStationComputer

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

@@ -35,11 +35,14 @@ public class OI {
 	private JoystickButton updatePIDConstantsButton;
 	private JoystickButton updateEncoderDPPButton;
 	public Joystick manipulator;
-
+	
 	public int getButton(String key, int def) {
 		if (!SmartDashboard.containsKey("Button/" + key)) {
-			SmartDashboard.putNumber("Button/" + key, def);
-		}
+			if (!SmartDashboard.putNumber("Button/" + key, def)) {
+				System.err.println("SmartDashboard Key" + "Button/" + key + "already taken by a different type");
+				return def;
+			}
+		}		
 		return (int) SmartDashboard.getNumber("Button/" + key, def);
 	}
 

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

@@ -1,3 +1,9 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
 
 package org.usfirst.frc.team199.Robot2018;
 
@@ -9,6 +15,7 @@
 import org.usfirst.frc.team199.Robot2018.commands.Autonomous;
 import org.usfirst.frc.team199.Robot2018.commands.Autonomous.Position;
 import org.usfirst.frc.team199.Robot2018.commands.Autonomous.Strategy;
+import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
 import org.usfirst.frc.team199.Robot2018.subsystems.Climber;
 import org.usfirst.frc.team199.Robot2018.subsystems.ClimberAssist;
 import org.usfirst.frc.team199.Robot2018.subsystems.Drivetrain;
@@ -51,14 +58,20 @@ public class Robot extends TimedRobot {
 
 	public static double getConst(String key, double def) {
 		if (!SmartDashboard.containsKey("Const/" + key)) {
-			SmartDashboard.putNumber("Const/" + key, def);
+			if (!SmartDashboard.putNumber("Const/" + key, def)) {
+				System.err.println("SmartDashboard Key" + "Const/" + key + "already taken by a different type");
+				return def;
+			}
 		}
 		return SmartDashboard.getNumber("Const/" + key, def);
 	}
 
 	public static boolean getBool(String key, boolean def) {
 		if (!SmartDashboard.containsKey("Bool/" + key)) {
-			SmartDashboard.putBoolean("Bool/" + key, def);
+			if (!SmartDashboard.putBoolean("Bool/" + key, def)) {
+				System.err.println("SmartDashboard Key" + "Bool/" + key + "already taken by a different type");
+				return def;
+			}
 		}
 		return SmartDashboard.getBoolean("Bool/" + key, def);
 	}
@@ -124,6 +137,7 @@ public void disabledPeriodic() {
 	 */
 	@Override
 	public void autonomousInit() {
+		Scheduler.getInstance().add(new ShiftLowGear());
 		String fmsInput = DriverStation.getInstance().getGameSpecificMessage();
 		Position startPos = posChooser.getSelected();
 		double autoDelay = SmartDashboard.getNumber("Auto Delay", 0);
@@ -155,6 +169,7 @@ public void teleopInit() {
 		// this line or comment it out.
 		if (autonomousCommand != null)
 			autonomousCommand.cancel();
+		dt.putVelocityControllersToDashboard();
 	}
 
 	/**
@@ -177,16 +192,14 @@ public void testPeriodic() {
 		// firstTime = false;
 		//// }
 		// Robot.dt.setVPIDs(Robot.getConst("VPID Test Set", 0.5));
-		// SmartDashboard.putNumber("Drivetrain/Left VPID Targ",
-		// Robot.dt.getLeftVPIDSetpoint());
-		// SmartDashboard.putNumber("Drivetrain/Right VPID Targ",
-		// Robot.dt.getRightVPIDSetpoint());
-		// SmartDashboard.putNumber("Left VPID Error", Robot.dt.getLeftVPIDerror());
-		// SmartDashboard.putNumber("Right VPID Error", Robot.dt.getRightVPIDerror());
-		// SmartDashboard.putNumber("Left Enc Rate", Robot.dt.getLeftEncRate());
-		// SmartDashboard.putNumber("Right Enc Rate", Robot.dt.getRightEncRate());
-
-		dt.dtLeft.set(0.1);
+		SmartDashboard.putNumber("Drivetrain/Left VPID Targ", Robot.dt.getLeftVPIDSetpoint());
+		SmartDashboard.putNumber("Drivetrain/Right VPID Targ", Robot.dt.getRightVPIDSetpoint());
+		SmartDashboard.putNumber("Left VPID Error", Robot.dt.getLeftVPIDerror());
+		SmartDashboard.putNumber("Right VPID Error", Robot.dt.getRightVPIDerror());
+		SmartDashboard.putNumber("Left Enc Rate", Robot.dt.getLeftEncRate());
+		SmartDashboard.putNumber("Right Enc Rate", Robot.dt.getRightEncRate());
+
+		// dt.dtLeft.set(0.1);
 		// dt.dtRight.set(-oi.rightJoy.getY());
 		// dt.dtLeft.set(-oi.leftJoy.getY());
 		// dt.dtRight.set(-oi.rightJoy.getY());

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,18 +105,6 @@ private void configSPX(WPI_VictorSPX mc) {
 		mc.configPeakOutputReverse(-1, kTimeout);
 	}
 
-	/**
-	 * Uses SmartDashboard and math to calculate a *great* default kD
-	 */
-	public double calcDefkD() {
-		double timeConstant = Robot.getConst("Omega Max", 5330) * Robot.getConst("Mass of Robot", 54.4311)
-				* Robot.getConst("Radius of Drivetrain Wheel", 0.0635)
-				* Robot.getConst("Radius of Drivetrain Wheel", 0.0635) / Robot.getConst("Stall Torque", 2.41);
-		double cycleTime = Robot.getConst("Code cycle time", 0.1);
-		double denominator = 1 - Math.pow(Math.E, -1 * cycleTime / timeConstant);
-		return 1 / denominator;
-	}
-
 	public RobotMap() {
 
 		// intakeMotor = new WPI_TalonSRX(getPort("IntakeTalonSRX", 4));
@@ -123,14 +114,17 @@ public RobotMap() {
 		// 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", 2));
 		leftEncPort2 = new DigitalInput(getPort("2LeftEnc", 3));
 		leftEncDist = new Encoder(leftEncPort1, leftEncPort2);
 		leftEncDist.setPIDSourceType(PIDSourceType.kDisplacement);
 		leftEncRate = new Encoder(leftEncPort1, leftEncPort2);
 		leftEncRate.setPIDSourceType(PIDSourceType.kRate);
-		leftEncDist.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO));
-		leftEncRate.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO));
+		leftEncDist.setDistancePerPulse(Robot.getConst("DPP", 0.013908));
+		leftEncRate.setDistancePerPulse(Robot.getConst("DPP", 0.013908));
 
 		dtLeftMaster = new WPI_TalonSRX(getPort("LeftTalonSRXMaster", 1));
 		configSRX(dtLeftMaster);
@@ -140,9 +134,9 @@ public RobotMap() {
 		// inverted bc gear boxes invert from input to output
 		dtLeft.setInverted(true);
 
-		leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkP", 0),
-				Robot.getConst("VelocityLeftkI", 0), Robot.getConst("VelocityLeftkD", 0),
-				1 / Robot.getConst("Max Low Speed", 84), leftEncRate, dtLeft);
+		leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkI", 0), 0,
+				Robot.getConst("VelocityLeftkD", calcDefkD()), 1 / Robot.getConst("Max Low Speed", 84), leftEncRate,
+				dtLeft);
 		leftVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
 				Robot.getConst("Max High Speed", 204));
 		leftVelocityController.setOutputRange(-1.0, 1.0);
@@ -156,8 +150,8 @@ public RobotMap() {
 		rightEncDist.setPIDSourceType(PIDSourceType.kDisplacement);
 		rightEncRate = new Encoder(rightEncPort1, rightEncPort2);
 		rightEncRate.setPIDSourceType(PIDSourceType.kRate);
-		rightEncDist.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO));
-		rightEncRate.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO));
+		rightEncDist.setDistancePerPulse(Robot.getConst("DPP", 0.013908));
+		rightEncRate.setDistancePerPulse(Robot.getConst("DPP", 0.013908));
 
 		dtRightMaster = new WPI_TalonSRX(getPort("RightTalonSRXMaster", 4));
 		configSRX(dtRightMaster);
@@ -167,18 +161,19 @@ public RobotMap() {
 		// inverted bc gear boxes invert from input to output
 		dtRight.setInverted(true);
 
-		rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkP", 0),
-				Robot.getConst("VelocityRightkI", 0), Robot.getConst("VelocityRightkD", 0),
-				1 / Robot.getConst("Max Low Speed", 84), rightEncRate, dtRight);
+		rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkI", 0), 0,
+				Robot.getConst("VelocityRightkD", calcDefkD()), 1 / Robot.getConst("Max Low Speed", 84), rightEncRate,
+				dtRight);
 		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("VelocityToleranceRight", 2));
 
-		robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
-		robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
-		// robotDrive = new DifferentialDrive(dtLeft, dtRight);
+		// robotDrive = new DifferentialDrive(leftVelocityController,
+		// rightVelocityController);
+		// robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
+		robotDrive = new DifferentialDrive(dtLeft, dtRight);
 
 		distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist);
 		fancyGyro = new AHRS(SerialPort.Port.kMXP);
@@ -197,9 +192,48 @@ public RobotMap() {
 	 */
 	public int getPort(String key, int def) {
 		if (!SmartDashboard.containsKey("Port/" + key)) {
-			SmartDashboard.putNumber("Port/" + key, def);
+			if (!SmartDashboard.putNumber("Port/" + key, def)) {
+				System.err.println("SmartDashboard Key" + "Port/" + key + "already taken by a different type");
+				return def;
+			}
 		}
 		return (int) SmartDashboard.getNumber("Port/" + key, def);
 	}
 
+	/**
+	 * Uses SmartDashboard and math to calculate a *great* default kD
+	 */
+	public double calcDefkD() {
+		/*
+		 * timeConstant is proportional to max speed of the shaft (which is the max
+		 * speed of the cim divided by the gear reduction), half the mass (because the
+		 * 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 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
+				* convertNtokG(Robot.getConst("Weight of Robot", 342)) / 2 * radius * radius
+				/ (Robot.getConst("Stall Torque", 2.41) * gearReduction * 2);
+		double cycleTime = Robot.getConst("Code cycle time", 0.05);
+		/*
+		 * The denominator of kD is 1-(e ^ -cycleTime / timeConstant). The numerator is
+		 * one.
+		 */
+		double denominator = 1 - Math.pow(Math.E, -1 * cycleTime / timeConstant);
+		return 1 / denominator;
+	}
+
+	private double convertLbsTokG(double lbs) {
+		// number from google ;)
+		return lbs * 0.45359237;
+	}
+
+	private double convertNtokG(double newtons) {
+		// weight / accel due to grav = kg
+		return newtons / 9.81;
+	}
 }

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

@@ -10,26 +10,43 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
- *
+ * Drives the robot a certain target distance using PID. Implements PIDOutput in
+ * order to keep move PID centralized in this command.
  */
 public class PIDMove extends Command implements PIDOutput {
 
 	private double target;
 	private DrivetrainInterface dt;
 	private PIDController moveController;
 
+	/**
+	 * Constructs this command with a new PIDController. Sets all of the
+	 * controller's PID constants based on SD prefs. Sets the controller's PIDSource
+	 * to the encoder average object and sets its PIDOutput to this command which
+	 * implements PIDOutput's pidWrite() method.
+	 * 
+	 * @param targ
+	 *            the target distance (in inches) to move to
+	 * @param dt
+	 *            the Drivetrain (for actual code) or a DrivetrainInterface (for
+	 *            testing)
+	 * @param avg
+	 *            the PIDSourceAverage of the DT's two Encoders
+	 */
 	public PIDMove(double targ, DrivetrainInterface dt, PIDSourceAverage avg) {
 		// Use requires() here to declare subsystem dependencies
-		// eg. requires(chassis);
+		requires(Robot.dt);
 		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), kf, avg, this);
 	}
 
-	// Called just before this Command runs the first time
+	/**
+	 * Called just before this Command runs the first time. Resets the distance
+	 * encoders, sets the moveController's settings, and then enables it.
+	 */
 	@Override
 	public void initialize() {
 		dt.resetDistEncs();
@@ -44,33 +61,57 @@ public void initialize() {
 		SmartDashboard.putData(moveController);
 	}
 
-	// Called repeatedly when this Command is scheduled to run
+	/**
+	 * Called repeatedly when this Command is scheduled to run. This method is empty
+	 * bc the moveController runs on a different thread as soon as it is enabled in
+	 * initialize().
+	 */
 	@Override
 	protected void execute() {
-		// 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()
+	/**
+	 * Tells this command to terminate when the moveController has reached its
+	 * target.
+	 * 
+	 * @return true if the robot has moved the correct distance, false if not
+	 */
 	@Override
 	protected boolean isFinished() {
 		return moveController.onTarget();
 	}
 
-	// Called once after isFinished returns true
+	/**
+	 * Called once after isFinished returns true. Disables and frees the
+	 * moveController, essentially turning it "off" and "deleting" the thread it was
+	 * running on.
+	 */
 	@Override
 	protected void end() {
 		moveController.disable();
 		moveController.free();
 	}
 
-	// Called when another command which requires one or more of the same
-	// subsystems is scheduled to run
+	/**
+	 * Called when another command which requires one or more of the same subsystems
+	 * is scheduled to run. Disables and frees the moveController, essentially
+	 * turning it "off" and "deleting" the thread it was running on.
+	 */
 	@Override
 	protected void interrupted() {
 		end();
 	}
 
+	/**
+	 * Implementation of PIDOutput's pidWrite method. Sends the moveController's
+	 * output speed to the motors as the drive/forward speed in DifferentialDrive's
+	 * arcadeDrive method.
+	 * 
+	 * @param output
+	 *            the output drive/forward speed [-1, 1], calculated by the move
+	 *            PIDController, to pass in as the drive/forward speed in
+	 *            arcadeDrive()
+	 */
 	@Override
 	public void pidWrite(double output) {
 		dt.arcadeDrive(output, 0);