From 150f450f2b4f9e6094d71007507a7b877e05328a Mon Sep 17 00:00:00 2001 From: Cindy Zhang Date: Thu, 16 Mar 2017 16:46:42 -0700 Subject: [PATCH] competition fixes --- .../usfirst/frc/team3501/robot/Constants.java | 10 ++- .../usfirst/frc/team3501/robot/MathLib.java | 2 +- src/org/usfirst/frc/team3501/robot/OI.java | 4 +- src/org/usfirst/frc/team3501/robot/Robot.java | 20 +++--- .../{driving => climber}/BrakeCANTalons.java | 10 +-- .../{driving => climber}/CoastCANTalons.java | 10 +-- .../climber/MaintainClimbedPosition.java | 62 ------------------- .../robot/commands/climber/RunWinch.java | 10 +-- .../commands/climber/RunWinchContinuous.java | 14 +++-- .../robot/commands/climber/StopWinch.java | 7 ++- .../robot/commands/climber/ToggleWinch.java | 49 --------------- .../commands/driving/ToggleDriveGear.java | 7 +++ .../team3501/robot/subsystems/Climber.java | 48 ++++++++++++++ .../team3501/robot/subsystems/DriveTrain.java | 45 +++++--------- .../team3501/robot/subsystems/Shooter.java | 4 +- 15 files changed, 124 insertions(+), 178 deletions(-) rename src/org/usfirst/frc/team3501/robot/commands/{driving => climber}/BrakeCANTalons.java (62%) rename src/org/usfirst/frc/team3501/robot/commands/{driving => climber}/CoastCANTalons.java (62%) delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/climber/MaintainClimbedPosition.java delete mode 100644 src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java create mode 100644 src/org/usfirst/frc/team3501/robot/subsystems/Climber.java diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index 4efaf21..840c497 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -46,9 +46,10 @@ public class Constants { public static final int PISTON_MODULE = 10; public static final int GEAR_MANIPULATOR_PISTON_FORWARD = 4, GEAR_MANIPULATOR_PISTON_REVERSE = 5; - public static final int LEFT_GEAR_PISTON_FORWARD = 1, - LEFT_GEAR_PISTON_REVERSE = 0, RIGHT_GEAR_PISTON_FORWARD = 3, - RIGHT_GEAR_PISTON_REVERSE = 2; + + public static final int DRIVETRAIN_GEAR_FORWARD = 0, + DRIVETRAIN_GEAR_REVERSE = 1; + public static final Value FORWARD_PISTON_VALUE = DoubleSolenoid.Value.kForward; public static final Value REVERSE_PISTON_VALUE = DoubleSolenoid.Value.kReverse; @@ -69,7 +70,10 @@ public class Constants { public static class Intake { public static final int INTAKE_ROLLER_PORT = 8; + } + public static class Climber { + public static final int WINCH_PORT = 0; } public static enum Direction { diff --git a/src/org/usfirst/frc/team3501/robot/MathLib.java b/src/org/usfirst/frc/team3501/robot/MathLib.java index 5269304..50cdd91 100644 --- a/src/org/usfirst/frc/team3501/robot/MathLib.java +++ b/src/org/usfirst/frc/team3501/robot/MathLib.java @@ -92,7 +92,7 @@ public class MathLib { } } - public static double limitValue(double val, double max, double min) { + public static double limitValue(double val, double min, double max) { if (val > max) { return max; } else if (val < min) { diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java index 61f39c3..e9c0552 100644 --- a/src/org/usfirst/frc/team3501/robot/OI.java +++ b/src/org/usfirst/frc/team3501/robot/OI.java @@ -1,7 +1,7 @@ package org.usfirst.frc.team3501.robot; -import org.usfirst.frc.team3501.robot.commands.driving.BrakeCANTalons; -import org.usfirst.frc.team3501.robot.commands.driving.CoastCANTalons; +import org.usfirst.frc.team3501.robot.commands.climber.BrakeCANTalons; +import org.usfirst.frc.team3501.robot.commands.climber.CoastCANTalons; import org.usfirst.frc.team3501.robot.commands.driving.ToggleDriveGear; import org.usfirst.frc.team3501.robot.commands.driving.ToggleGearManipulatorPiston; import org.usfirst.frc.team3501.robot.commands.intake.ReverseIntakeContinuous; diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index d551429..8151cde 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -2,6 +2,8 @@ package org.usfirst.frc.team3501.robot; import org.usfirst.frc.team3501.robot.commandgroups.AutonMiddleGear; import org.usfirst.frc.team3501.robot.commandgroups.AutonSideGear; +import org.usfirst.frc.team3501.robot.commands.driving.TimeDrive; +import org.usfirst.frc.team3501.robot.subsystems.Climber; import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; import org.usfirst.frc.team3501.robot.subsystems.Intake; import org.usfirst.frc.team3501.robot.subsystems.Shooter; @@ -20,6 +22,7 @@ public class Robot extends IterativeRobot { private static Shooter shooter; private static OI oi; private static Intake intake; + private static Climber climber; Command autonCommand; SendableChooser autonChooser; @@ -30,6 +33,7 @@ public class Robot extends IterativeRobot { oi = OI.getOI(); shooter = Shooter.getShooter(); intake = Intake.getIntake(); + climber = Climber.getClimber(); autonChooser = new SendableChooser(); autonChooser.addDefault("Middle Gear", new AutonMiddleGear()); @@ -46,8 +50,6 @@ public class Robot extends IterativeRobot { CameraServer server = CameraServer.getInstance(); UsbCamera climberCam = server.startAutomaticCapture("climbercam", 0); UsbCamera intakeCam = server.startAutomaticCapture("intakecam", 1); - - driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE); } public static DriveTrain getDriveTrain() { @@ -66,14 +68,17 @@ public class Robot extends IterativeRobot { return Intake.getIntake(); } + public static Climber getClimber() { + return Climber.getClimber(); + } + @Override public void autonomousInit() { - // driveTrain.setLowGear(); - driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE); + driveTrain.setLowGear(); // autonCommand = (Command) autonChooser.getSelected(); - // autonCommand = new TimeDrive(1.5, 0.6); - // Scheduler.getInstance().add(autonCommand); + autonCommand = new TimeDrive(2, 0.6); + Scheduler.getInstance().add(autonCommand); } @Override @@ -83,8 +88,7 @@ public class Robot extends IterativeRobot { @Override public void teleopInit() { - // driveTrain.setHighGear(); - driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE); + driveTrain.setHighGear(); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/BrakeCANTalons.java b/src/org/usfirst/frc/team3501/robot/commands/climber/BrakeCANTalons.java similarity index 62% rename from src/org/usfirst/frc/team3501/robot/commands/driving/BrakeCANTalons.java rename to src/org/usfirst/frc/team3501/robot/commands/climber/BrakeCANTalons.java index 0a19e57..d0cfc34 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/BrakeCANTalons.java +++ b/src/org/usfirst/frc/team3501/robot/commands/climber/BrakeCANTalons.java @@ -1,7 +1,7 @@ -package org.usfirst.frc.team3501.robot.commands.driving; +package org.usfirst.frc.team3501.robot.commands.climber; import org.usfirst.frc.team3501.robot.Robot; -import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; +import org.usfirst.frc.team3501.robot.subsystems.Climber; import edu.wpi.first.wpilibj.command.Command; @@ -9,15 +9,15 @@ import edu.wpi.first.wpilibj.command.Command; * */ public class BrakeCANTalons extends Command { - private DriveTrain driveTrain = Robot.getDriveTrain(); + Climber climber = Robot.getClimber(); public BrakeCANTalons() { - requires(driveTrain); + requires(climber); } @Override protected void initialize() { - driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_BRAKE_MODE); + climber.setCANTalonsBrakeMode(climber.BRAKE_MODE); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/CoastCANTalons.java b/src/org/usfirst/frc/team3501/robot/commands/climber/CoastCANTalons.java similarity index 62% rename from src/org/usfirst/frc/team3501/robot/commands/driving/CoastCANTalons.java rename to src/org/usfirst/frc/team3501/robot/commands/climber/CoastCANTalons.java index ea676f2..a2ecad2 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/CoastCANTalons.java +++ b/src/org/usfirst/frc/team3501/robot/commands/climber/CoastCANTalons.java @@ -1,7 +1,7 @@ -package org.usfirst.frc.team3501.robot.commands.driving; +package org.usfirst.frc.team3501.robot.commands.climber; import org.usfirst.frc.team3501.robot.Robot; -import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; +import org.usfirst.frc.team3501.robot.subsystems.Climber; import edu.wpi.first.wpilibj.command.Command; @@ -9,15 +9,15 @@ import edu.wpi.first.wpilibj.command.Command; * */ public class CoastCANTalons extends Command { - private DriveTrain driveTrain = Robot.getDriveTrain(); + Climber climber = Robot.getClimber(); public CoastCANTalons() { - requires(driveTrain); + requires(climber); } @Override protected void initialize() { - driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE); + climber.setCANTalonsBrakeMode(climber.COAST_MODE); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/MaintainClimbedPosition.java b/src/org/usfirst/frc/team3501/robot/commands/climber/MaintainClimbedPosition.java deleted file mode 100644 index 88ca8e0..0000000 --- a/src/org/usfirst/frc/team3501/robot/commands/climber/MaintainClimbedPosition.java +++ /dev/null @@ -1,62 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.climber; - -import org.usfirst.frc.team3501.robot.Robot; -import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; - -import edu.wpi.first.wpilibj.command.Command; - -/** - * This command runs the winch at a specified speed when the robot has completed - * the climb and when the button triggering it is pressed. This command also - * makes the drive train motors run because the winch is controlled by the drive - * train. - * - * pre-condition: This command is run by a button in OI. The robot must have - * completed climbing. - * - * post-condition: Winch motor set to a specified speed. - * - * @param motorVal - * value range is from -1 to 1 - * - * @author shivanighanta - * - */ -public class MaintainClimbedPosition extends Command { - - /** - * See JavaDoc comment in class for details - * - * @param time - * time in seconds to run the winch - */ - public MaintainClimbedPosition() { - requires(Robot.getDriveTrain()); - } - - @Override - protected void initialize() { - } - - @Override - protected void execute() { - Robot.getDriveTrain().setMotorValues(DriveTrain.MAINTAIN_CLIMBED_POSITION, - DriveTrain.MAINTAIN_CLIMBED_POSITION); - - } - - @Override - protected boolean isFinished() { - return false; - } - - @Override - protected void end() { - Robot.getDriveTrain().stop(); - } - - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinch.java b/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinch.java index 0e980a4..5a76b5c 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinch.java +++ b/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinch.java @@ -1,6 +1,7 @@ package org.usfirst.frc.team3501.robot.commands.climber; import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.subsystems.Climber; import edu.wpi.first.wpilibj.command.Command; @@ -23,6 +24,8 @@ import edu.wpi.first.wpilibj.command.Command; */ public class RunWinch extends Command { + Climber climber = Robot.getClimber(); + private double time; private double motorVal; @@ -35,7 +38,7 @@ public class RunWinch extends Command { * value range is from -1 to 1 */ public RunWinch(double time, double motorVal) { - requires(Robot.getDriveTrain()); + requires(climber); this.time = time; this.motorVal = motorVal; } @@ -46,8 +49,7 @@ public class RunWinch extends Command { @Override protected void execute() { - Robot.getDriveTrain().setMotorValues(motorVal, motorVal); - + climber.setMotorValues(motorVal); } @Override @@ -57,7 +59,7 @@ public class RunWinch extends Command { @Override protected void end() { - Robot.getDriveTrain().stop(); + climber.stop(); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java index 210199d..06fa154 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java +++ b/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java @@ -1,7 +1,8 @@ package org.usfirst.frc.team3501.robot.commands.climber; +import org.usfirst.frc.team3501.robot.OI; import org.usfirst.frc.team3501.robot.Robot; -import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; +import org.usfirst.frc.team3501.robot.subsystems.Climber; import edu.wpi.first.wpilibj.command.Command; @@ -18,7 +19,7 @@ import edu.wpi.first.wpilibj.command.Command; * */ public class RunWinchContinuous extends Command { - DriveTrain driveTrain = Robot.getDriveTrain(); + Climber climber = Robot.getClimber(); private double climbingSpeed; /** @@ -28,8 +29,8 @@ public class RunWinchContinuous extends Command { * value range is from -1 to 1 */ public RunWinchContinuous() { - requires(driveTrain); - climbingSpeed = driveTrain.CLIMBER_SPEED; + requires(climber); + climbingSpeed = climber.CLIMBER_SPEED; } @Override @@ -38,7 +39,8 @@ public class RunWinchContinuous extends Command { @Override protected void execute() { - Robot.getDriveTrain().setMotorValues(climbingSpeed, climbingSpeed); + double thrust = OI.leftJoystick.getY(); + climber.setMotorValues(-thrust); } @Override @@ -48,7 +50,7 @@ public class RunWinchContinuous extends Command { @Override protected void end() { - + climber.stop(); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/StopWinch.java b/src/org/usfirst/frc/team3501/robot/commands/climber/StopWinch.java index 597bd65..0a2b5f9 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/climber/StopWinch.java +++ b/src/org/usfirst/frc/team3501/robot/commands/climber/StopWinch.java @@ -1,6 +1,7 @@ package org.usfirst.frc.team3501.robot.commands.climber; import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.subsystems.Climber; import edu.wpi.first.wpilibj.command.Command; @@ -11,9 +12,10 @@ import edu.wpi.first.wpilibj.command.Command; * */ public class StopWinch extends Command { + Climber climber = Robot.getClimber(); public StopWinch() { - requires(Robot.getDriveTrain()); + requires(climber); } @Override @@ -31,8 +33,7 @@ public class StopWinch extends Command { @Override protected void end() { - Robot.getDriveTrain().stop(); - + climber.stop(); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java b/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java deleted file mode 100644 index 3e60406..0000000 --- a/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java +++ /dev/null @@ -1,49 +0,0 @@ -package org.usfirst.frc.team3501.robot.commands.climber; - -import org.usfirst.frc.team3501.robot.Robot; -import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; - -import edu.wpi.first.wpilibj.command.Command; - -/** - * - */ -public class ToggleWinch extends Command { - DriveTrain driveTrain = Robot.getDriveTrain(); - private double climbingSpeed; - private double maintainPositionSpeed; - - public ToggleWinch() { - requires(driveTrain); - climbingSpeed = driveTrain.CLIMBER_SPEED; - maintainPositionSpeed = driveTrain.MAINTAIN_CLIMBED_POSITION; - } - - @Override - protected void initialize() { - } - - @Override - protected void execute() { - if (driveTrain.shouldBeClimbing) { - driveTrain.setMotorValues(climbingSpeed, climbingSpeed); - } else { - driveTrain.setMotorValues(maintainPositionSpeed, maintainPositionSpeed); - } - } - - @Override - protected boolean isFinished() { - return false; - } - - @Override - protected void end() { - driveTrain.shouldBeClimbing = !driveTrain.shouldBeClimbing; - } - - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleDriveGear.java b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleDriveGear.java index e3c0dcc..281f324 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleDriveGear.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleDriveGear.java @@ -28,11 +28,18 @@ public class ToggleDriveGear extends Command { @Override protected void execute() { Value rightPistonValue = driveTrain.getRightDriveTrainPiston(); + System.out.println("I think i'm at " + rightPistonValue); if (rightPistonValue == Constants.DriveTrain.REVERSE_PISTON_VALUE) { driveTrain.setHighGear(); } else { driveTrain.setLowGear(); } + // boolean leftPistonValue = driveTrain.getLeftDriveTrainPiston(); + // if (leftPistonValue == Constants.DriveTrain.RETRACT_VALUE) { + // driveTrain.setHighGear(); + // } else { + // driveTrain.setLowGear(); + // } } @Override diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Climber.java b/src/org/usfirst/frc/team3501/robot/subsystems/Climber.java new file mode 100644 index 0000000..4cb6c0b --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Climber.java @@ -0,0 +1,48 @@ +package org.usfirst.frc.team3501.robot.subsystems; + +import org.usfirst.frc.team3501.robot.Constants; +import org.usfirst.frc.team3501.robot.MathLib; +import org.usfirst.frc.team3501.robot.commands.climber.RunWinchContinuous; + +import com.ctre.CANTalon; + +import edu.wpi.first.wpilibj.command.Subsystem; + +public class Climber extends Subsystem { + public static Climber climber; + + public static final boolean BRAKE_MODE = true; + public static final boolean COAST_MODE = false; + + public static final double CLIMBER_SPEED = 0; + + private CANTalon winch; + + public Climber() { + winch = new CANTalon(Constants.Climber.WINCH_PORT); + } + + public static Climber getClimber() { + if (climber == null) { + climber = new Climber(); + } + return climber; + } + + public void setMotorValues(double climbingSpeed) { + winch.set(MathLib.limitValue(climbingSpeed, 0.0, 1.0)); + } + + public void stop() { + winch.set(0); + } + + public void setCANTalonsBrakeMode(boolean mode) { + winch.enableBrakeMode(mode); + } + + @Override + protected void initDefaultCommand() { + setDefaultCommand(new RunWinchContinuous()); + } +} diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index b7264f1..383abd6 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -25,25 +25,17 @@ public class DriveTrain extends Subsystem { public static final double INCHES_PER_PULSE = WHEEL_DIAMETER * Math.PI / ENCODER_PULSES_PER_REVOLUTION; - public static final double MAINTAIN_CLIMBED_POSITION = 0; - public static final double TIME_TO_CLIMB_FOR = 0; - public static final double CLIMBER_SPEED = 0; - - public static final boolean DRIVE_BRAKE_MODE = true; - public static final boolean DRIVE_COAST_MODE = false; - private static DriveTrain driveTrain; private final CANTalon frontLeft, frontRight, rearLeft, rearRight; private final RobotDrive robotDrive; private final Encoder leftEncoder, rightEncoder; - private final DoubleSolenoid leftDriveTrainPiston, rightDriveTrainPiston; + private final DoubleSolenoid rightDriveTrainPiston; + // private final Solenoid leftDriveTrainPiston; private final DoubleSolenoid gearManipulatorPiston; private ADXRS450_Gyro imu; - public boolean shouldBeClimbing = false; - private DriveTrain() { // MOTOR CONTROLLERS frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT); @@ -66,14 +58,12 @@ public class DriveTrain extends Subsystem { this.imu = new ADXRS450_Gyro(Constants.DriveTrain.GYRO_PORT); // TODO: Not sure if MODULE_NUMBER should be the same for both - leftDriveTrainPiston = new DoubleSolenoid( - Constants.DriveTrain.PISTON_MODULE, - Constants.DriveTrain.LEFT_GEAR_PISTON_FORWARD, - Constants.DriveTrain.LEFT_GEAR_PISTON_REVERSE); + // leftDriveTrainPiston = new Solenoid(Constants.DriveTrain.PISTON_MODULE, + // Constants.DriveTrain.LEFT_GEAR_PISTON_PORT); rightDriveTrainPiston = new DoubleSolenoid( Constants.DriveTrain.PISTON_MODULE, - Constants.DriveTrain.RIGHT_GEAR_PISTON_FORWARD, - Constants.DriveTrain.RIGHT_GEAR_PISTON_REVERSE); + Constants.DriveTrain.DRIVETRAIN_GEAR_FORWARD, + Constants.DriveTrain.DRIVETRAIN_GEAR_REVERSE); gearManipulatorPiston = new DoubleSolenoid( Constants.DriveTrain.PISTON_MODULE, @@ -169,9 +159,9 @@ public class DriveTrain extends Subsystem { * @return a value that is the current setpoint for the piston kReverse or * KForward */ - public Value getLeftDriveTrainPiston() { - return leftDriveTrainPiston.get(); - } + // public boolean getLeftDriveTrainPiston() { + // return leftDriveTrainPiston.get(); + // } /* * @return a value that is the current setpoint for the piston kReverse or @@ -199,8 +189,15 @@ public class DriveTrain extends Subsystem { * Changes the gear to a DoubleSolenoid.Value */ private void changeGear(DoubleSolenoid.Value gear) { - leftDriveTrainPiston.set(gear); + System.out.println("shifting to " + gear); rightDriveTrainPiston.set(gear); + System.out.println("after: " + this.getRightDriveTrainPiston()); + + // + // if (gear == Constants.DriveTrain.FORWARD_PISTON_VALUE) + // leftDriveTrainPiston.set(Constants.DriveTrain.EXTEND_VALUE); + // else + // leftDriveTrainPiston.set(Constants.DriveTrain.RETRACT_VALUE); } public Value getGearManipulatorPistonValue() { @@ -219,12 +216,4 @@ public class DriveTrain extends Subsystem { protected void initDefaultCommand() { setDefaultCommand(new JoystickDrive()); } - - public void setCANTalonsBrakeMode(boolean mode) { - frontLeft.enableBrakeMode(mode); - rearLeft.enableBrakeMode(mode); - - frontRight.enableBrakeMode(mode); - rearRight.enableBrakeMode(mode); - } } diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java index ab36fdd..5711b26 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java @@ -20,8 +20,8 @@ public class Shooter extends Subsystem { private static final double RPM_THRESHOLD = 10; private static final double DEFAULT_INDEXING_MOTOR_VALUE = 0.75; private static final double REVERSE_FLYWHEEL_MOTOR_VALUE = -0.75; - private static final double DEFAULT_SHOOTING_SPEED = 2700; // rpm - private static final double SHOOTING_SPEED_INCREMENT = 50; + private static final double DEFAULT_SHOOTING_SPEED = 2875; // rpm + private static final double SHOOTING_SPEED_INCREMENT = 25; private double targetShootingSpeed = DEFAULT_SHOOTING_SPEED; private double currentShooterMotorValue = 0; -- 2.30.2