Merge remote-tracking branch 'dadavid/master'

Author: caengineers

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

@@ -15,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;
@@ -108,6 +109,7 @@ public void robotInit() {
 		// auto delay chooser
 		SmartDashboard.putNumber("Auto Delay", 0);
 
+
 		// parse scripts from Preferences, which maintains values throughout
 		// reboots
 		autoScripts = AutoUtils.parseScriptFile(Preferences.getInstance().getString("autoscripts", ""));
@@ -137,6 +139,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);

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

@@ -122,7 +122,7 @@ public RobotMap() {
 		dtLeft = new SpeedControllerGroup(dtLeftMaster, dtLeftSlave);
 
 		leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkP", 1),
-				Robot.getConst("VelocityLeftkI", 0), Robot.getConst("VelocityLeftkD", 0),
+				Robot.getConst("VelocityLeftkI", 0), Robot.getConst("VelocityLeftkD", calcDefkD()),
 				1 / Robot.getConst("Max Low Speed", 84), leftEncRate, dtLeft);
 		leftVelocityController.enable();
 		leftVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
@@ -144,7 +144,7 @@ public RobotMap() {
 		dtRight = new SpeedControllerGroup(dtRightMaster, dtRightSlave);
 
 		rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkP", 1),
-				Robot.getConst("VelocityRightkI", 0), Robot.getConst("VelocityRightkD", 0),
+				Robot.getConst("VelocityRightkI", 0), Robot.getConst("VelocityRightkD", calcDefkD()),
 				1 / Robot.getConst("Max Low Speed", 84), rightEncRate, dtRight);
 		rightVelocityController.enable();
 		rightVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
@@ -181,4 +181,15 @@ public int getPort(String key, int def) {
 		return (int) SmartDashboard.getNumber("Port/" + key, def);
 	}
 
+	/**
+	 * 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;
+	}
 }

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

@@ -0,0 +1,41 @@
+package org.usfirst.frc.team199.Robot2018.commands;
+
+import org.usfirst.frc.team199.Robot2018.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class TalonDrive extends Command {
+
+	public TalonDrive() {
+		// Use requires() here to declare subsystem dependencies
+		// eg. requires(chassis);
+	}
+
+	// Called just before this Command runs the first time
+	protected void initialize() {
+	}
+
+	// Called repeatedly when this Command is scheduled to run
+	protected void execute() {
+		Robot.dt.dtRightPIDDrive(Robot.oi.leftJoy.getY());
+		Robot.dt.dtLeftPIDDrive(Robot.oi.rightJoy.getY());
+	}
+
+	// Make this return true when this Command no longer needs to run execute()
+	protected boolean isFinished() {
+		return false;
+	}
+
+	// Called once after isFinished returns true
+	protected void end() {
+	}
+
+	// Called when another command which requires one or more of the same
+	// subsystems is scheduled to run
+	protected void interrupted() {
+		end();
+	}
+}

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

@@ -13,6 +13,9 @@
 import org.usfirst.frc.team199.Robot2018.autonomous.VelocityPIDController;
 import org.usfirst.frc.team199.Robot2018.commands.TeleopDrive;
 
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
 import com.kauailabs.navx.frc.AHRS;
 
 import edu.wpi.first.wpilibj.DoubleSolenoid;
@@ -25,6 +28,10 @@ public class Drivetrain extends Subsystem implements DrivetrainInterface {
 	// Put methods for controlling this subsystem
 	// here. Call these from Commands.
 
+	private final WPI_TalonSRX dtLeftMaster = RobotMap.dtLeftMaster;
+	private final WPI_VictorSPX dtLeftSlave = RobotMap.dtLeftSlave;
+	private final WPI_TalonSRX dtRightMaster = RobotMap.dtRightMaster;
+	private final WPI_VictorSPX dtRightSlave = RobotMap.dtRightSlave;
 	private final Encoder leftEncDist = RobotMap.leftEncDist;
 	private final Encoder rightEncDist = RobotMap.rightEncDist;
 	private final PIDSourceAverage distEncAvg = RobotMap.distEncAvg;
@@ -42,6 +49,32 @@ public void initDefaultCommand() {
 		setDefaultCommand(new TeleopDrive());
 	}
 
+	/**
+	 * Sets the left side of the drivetrain to use the talons' pids to run at the
+	 * specified speed.
+	 * 
+	 * @param value
+	 *            The speed to run at
+	 */
+	public void dtLeftPIDDrive(double value) {
+		double setValue = value * Robot.getConst("Units per 100ms", 3413);
+		dtLeftMaster.set(ControlMode.Velocity, setValue);
+		dtLeftSlave.set(ControlMode.Velocity, setValue);
+	}
+
+	/**
+	 * Sets the right side of the drivetrain to use the talons' pids to run at the
+	 * specified speed.
+	 * 
+	 * @param value
+	 *            The speed to run at
+	 */
+	public void dtRightPIDDrive(double value) {
+		double setValue = value * Robot.getConst("Units per 100ms", 3413);
+		dtRightMaster.set(ControlMode.Velocity, setValue);
+		dtRightSlave.set(ControlMode.Velocity, setValue);
+	}
+
 	/**
 	 * Drives based on joystick input and SmartDashboard values
 	 */