Merge remote-tracking branch 'daremote/master'

Author: MysteryPig

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;
 
@@ -41,22 +47,13 @@ public class Robot extends TimedRobot {
 	public static Listener listen;
 
 	public static OI oi;
-	
+
 	public static Map<String, ArrayList<String[]>> autoScripts;
 
 	Command autonomousCommand;
 	SendableChooser<Position> posChooser = new SendableChooser<Position>();
 	Map<String, SendableChooser<Strategy>> stratChoosers = new HashMap<String, SendableChooser<Strategy>>();
-	String[] fmsPossibilities = {"LL", "LR", "RL", "RR"};
-		
-	public static SmartDashboardInterface sd = new SmartDashboardInterface() {
-		public double getConst(String key, double def) {
-			if (!SmartDashboard.containsKey("Const/" + key)) {
-				SmartDashboard.putNumber("Const/" + key, def);
-			}
-			return SmartDashboard.getNumber("Const/" + key, def);
-		}		
-	};
+	String[] fmsPossibilities = { "LL", "LR", "RL", "RR" };
 
 	public static double getConst(String key, double def) {
 		return sd.getConst(key, def);
@@ -82,13 +79,13 @@ public void robotInit() {
 		lift = new Lift();
 		dt = new Drivetrain();
 		oi = new OI();
-		
+
 		// auto position chooser
 		for (Position p : Position.values()) {
 			posChooser.addObject(p.getSDName(), p);
 		}
 		SmartDashboard.putData("Starting Position", posChooser);
-		
+
 		// auto strategy choosers
 		for (String input : fmsPossibilities) {
 			SendableChooser<Strategy> chooser = new SendableChooser<Strategy>();
@@ -98,11 +95,12 @@ public void robotInit() {
 			SmartDashboard.putData(input, chooser);
 			stratChoosers.put(input, chooser);
 		}
-		
+
 		// auto delay chooser
 		SmartDashboard.putNumber("Auto Delay", 0);
-		
-		// parse scripts from Preferences, which maintains values throughout reboots
+
+		// parse scripts from Preferences, which maintains values throughout
+		// reboots
 		autoScripts = AutoUtils.parseScriptFile(Preferences.getInstance().getString("autoscripts", ""));
 
 		listen = new Listener();
@@ -124,23 +122,23 @@ public void disabledPeriodic() {
 	}
 
 	/**
-	 * This function is called once during the start of autonomous in order to 
-	 * grab values from SmartDashboard and the FMS and call the Autonomous
-	 * command with those values.
+	 * This function is called once during the start of autonomous in order to grab
+	 * values from SmartDashboard and the FMS and call the Autonomous command with
+	 * those values.
 	 */
 	@Override
 	public void autonomousInit() {
 		String fmsInput = DriverStation.getInstance().getGameSpecificMessage();
 		Position startPos = posChooser.getSelected();
 		double autoDelay = SmartDashboard.getNumber("Auto Delay", 0);
-		
+
 		Map<String, Strategy> strategies = new HashMap<String, Strategy>();
 		for (Map.Entry<String, SendableChooser<Strategy>> entry : stratChoosers.entrySet()) {
-		    String key = entry.getKey();
-		    SendableChooser<Strategy> chooser = entry.getValue();
-		    strategies.put(key, chooser.getSelected());
+			String key = entry.getKey();
+			SendableChooser<Strategy> chooser = entry.getValue();
+			strategies.put(key, chooser.getSelected());
 		}
-		
+
 		Autonomous auto = new Autonomous(startPos, strategies, autoDelay, fmsInput, false);
 		auto.start();
 	}

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

@@ -68,11 +68,20 @@ public class RobotMap {
 	 *            the TalonSRX to configure
 	 */
 	private void configSRX(WPI_TalonSRX mc) {
+		// stuff cole said to put
 		int kTimeout = (int) Robot.getConst("kTimeoutMs", 10);
 		mc.configNominalOutputForward(0, kTimeout);
 		mc.configNominalOutputReverse(0, kTimeout);
 		mc.configPeakOutputForward(1, kTimeout);
 		mc.configPeakOutputReverse(-1, kTimeout);
+
+		// current limiting stuff cole said to put
+		mc.configPeakCurrentLimit(0, 0);
+		mc.configPeakCurrentDuration(0, 0);
+		// 40 amps is the amp limit of a CIM. also, the PDP has 40 amp circuit breakers,
+		// so if we went above 40, the motors would stop completely.
+		mc.configContinuousCurrentLimit(40, 0);
+		mc.enableCurrentLimit(true);
 	}
 
 	/**
@@ -83,6 +92,7 @@ private void configSRX(WPI_TalonSRX mc) {
 	 *            the VictorSPX to configure
 	 */
 	private void configSPX(WPI_VictorSPX mc) {
+		// stuff cole said to put
 		int kTimeout = (int) Robot.getConst("kTimeoutMs", 10);
 		mc.configNominalOutputForward(0, kTimeout);
 		mc.configNominalOutputReverse(0, kTimeout);

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

@@ -1,37 +1,51 @@
 package org.usfirst.frc.team199.Robot2018.commands;
 
 import org.usfirst.frc.team199.Robot2018.Robot;
-import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface;
 import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage;
 import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
 
 import edu.wpi.first.wpilibj.PIDController;
-import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.PIDOutput;
 import edu.wpi.first.wpilibj.command.Command;
 
 /**
- *
+ * 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;
 
-	public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) {
+	/**
+	 * 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;
-		if (Robot.dt != null) {
-			requires(Robot.dt);
-		}
-		moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0),
-			sd.getConst("MovekD", 0), avg, this);
+		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();
@@ -45,33 +59,57 @@ public void initialize() {
 		moveController.enable();
 	}
 
-	// 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);

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

@@ -1,36 +1,57 @@
 package org.usfirst.frc.team199.Robot2018.commands;
 
 import org.usfirst.frc.team199.Robot2018.Robot;
-import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface;
 import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
 
+import com.kauailabs.navx.frc.AHRS;
+
 import edu.wpi.first.wpilibj.PIDController;
-import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.PIDOutput;
 import edu.wpi.first.wpilibj.command.Command;
 
 /**
- *
+ * Turns the robot to a certain target bearing using PID. Implements PIDOutput
+ * in order to keep turn PID centralized in this command.
  */
 public class PIDTurn extends Command implements PIDOutput {
 
-	double target;
-	DrivetrainInterface dt;
+	private double target;
+	private DrivetrainInterface dt;
 	private PIDController turnController;
 
-	public PIDTurn(double targ, DrivetrainInterface dt, PIDSource ahrs, SmartDashboardInterface sd) {
+	/**
+	 * 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 AHRS (gyro) object and sets its PIDOutput to this command which
+	 * implements PIDOutput's pidWrite() method.
+	 * 
+	 * @param targ
+	 *            the target bearing (in degrees) to turn to (so negative if turning
+	 *            left, positive if turning right)
+	 * @param dt
+	 *            the Drivetrain (for actual code) or a DrivetrainInterface (for
+	 *            testing)
+	 * @param ahrs
+	 *            the AHRS (gyro)
+	 */
+	public PIDTurn(double targ, DrivetrainInterface dt, AHRS ahrs) {
 		// Use requires() here to declare subsystem dependencies
-		// eg. requires(chassis);
+		requires(Robot.dt);
 		target = targ;
 		this.dt = dt;
-		if (Robot.dt != null) {
-			requires(Robot.dt);
-		}
-		turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0),
-			sd.getConst("TurnkD", 0), ahrs, this);
+		// 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), kf, ahrs, this);
 	}
 
-	// Called just before this Command runs the first time
+	/**
+	 * Called just before this Command runs the first time. Resets the gyro, sets
+	 * the turnController's settings, and then enables it.
+	 */
 	@Override
 	protected void initialize() {
 		dt.resetAHRS();
@@ -45,33 +66,57 @@ protected void initialize() {
 		turnController.enable();
 	}
 
-	// Called repeatedly when this Command is scheduled to run
+	/**
+	 * Called repeatedly when this Command is scheduled to run. This method is empty
+	 * bc the turnController 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 turnController has reached its
+	 * target.
+	 * 
+	 * @return true if the robot has turned to the correct target bearing, false if
+	 *         not
+	 */
 	@Override
 	protected boolean isFinished() {
 		return turnController.onTarget();
 	}
 
-	// Called once after isFinished returns true
+	/**
+	 * Called once after isFinished returns true. Disables and frees the
+	 * turnController, essentially turning it "off" and "deleting" the thread it was
+	 * running on.
+	 */
 	@Override
 	protected void end() {
 		turnController.disable();
 		turnController.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 turnController, 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 turnController's
+	 * output speed to the motors as the turn speed in DifferentialDrive's
+	 * arcadeDrive method.
+	 * 
+	 * @param output
+	 *            the output turn speed [-1, 1], calculated by the turn
+	 *            PIDController, to pass in as the turn speed in arcadeDrive()
+	 */
 	@Override
 	public void pidWrite(double output) {
 		dt.arcadeDrive(0, output);