From 247a513e9ae1afc378466bf3360958f73ca29f02 Mon Sep 17 00:00:00 2001 From: Rishabh <57200@psdschools.org> Date: Tue, 11 Feb 2020 17:35:16 -0700 Subject: [PATCH 01/24] Start shooter stuff --- build.gradle | 2 +- src/main/java/frc4388/robot/subsystems/Drive.java | 2 +- src/main/java/frc4388/robot/subsystems/Storage.java | 12 ++++++++++++ 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/build.gradle b/build.gradle index e00a044..e0194f3 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2020.1.2" + id "edu.wpi.first.GradleRIO" version "2020.2.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 997f16e..f23bad4 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -101,7 +101,7 @@ public class Drive extends ProfiledPIDSubsystem { m_driveTrain.arcadeDrive(move, steer); } - public double getGyroYaw() { +public double getGyroYaw() { double[] ypr = new double[3]; m_pigeon.getYawPitchRoll(ypr); return ypr[0]; diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 7b85a0d..d3defc1 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -45,4 +45,16 @@ public class Storage extends SubsystemBase { m_storageMotor.set(input); boolean beam_on = m_beamSensors[0].get(); } + + /** + * Prepares storage for shooting + */ + public void storageAim() { + + } + /* + *If shooting move storage motor until top sensor is tripped + *If intaking move storage motor until bottom sensor is tripped + * + */ } From ee9bd4d3017fdcf5857798d3292dbdc1d5ba6873 Mon Sep 17 00:00:00 2001 From: Rishabh <57200@psdschools.org> Date: Tue, 11 Feb 2020 20:07:35 -0700 Subject: [PATCH 02/24] Shooter gains fix --- src/main/java/frc4388/robot/Constants.java | 9 ++------- .../java/frc4388/robot/RobotContainer.java | 2 +- .../frc4388/robot/subsystems/Storage.java | 20 ++++++++++--------- 3 files changed, 14 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bcd15f8..318ac48 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -108,13 +108,8 @@ public final class Constants { public static final int PID_PRIMARY = 0; /* PID Gains */ - public static final double storP = 0.1; - public static final double storI = 1e-4; - public static final double storD = 1.0; - public static final double storIz = 0.0; - public static final double storF = 0.0; - public static final double storkmaxOutput = 1.0; - public static final double storkminOutput = -1.0; + public static final double STORAGE_MIN_OUTPUT = -1.0; + public static final Gains STORAGE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); } public static final class LEDConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2585fbc..bf4a506 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -137,7 +137,7 @@ public class RobotContainer { /* Storage Neo PID Test */ new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5))); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 5fbcd1c..be3f97f 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -19,7 +19,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj2.command.SubsystemBase; - +import frc4388.robot.Gains; import frc4388.robot.Constants.StorageConstants; public class Storage extends SubsystemBase { @@ -30,6 +30,8 @@ public class Storage extends SubsystemBase { CANEncoder m_encoder = m_storageMotor.getEncoder(); + Gains storageGains = StorageConstants.STORAGE_GAINS; + /** * Creates a new Storage. */ @@ -72,15 +74,15 @@ public class Storage extends SubsystemBase { } /* Storage PID Control */ - public void runStoragePositionPID(double targetPos, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput) + public void runStoragePositionPID(double targetPos) { // Set PID Coefficients - m_storagePIDController.setP(kP); - m_storagePIDController.setI(kI); - m_storagePIDController.setD(kD); - m_storagePIDController.setIZone(kIz); - m_storagePIDController.setFF(kF); - m_storagePIDController.setOutputRange(kminOutput, kmaxOutput); + m_storagePIDController.setP(storageGains.m_kP); + m_storagePIDController.setI(storageGains.m_kI); + m_storagePIDController.setD(storageGains.m_kD); + m_storagePIDController.setIZone(storageGains.m_kIzone); + m_storagePIDController.setFF(storageGains.m_kF); + m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kPeakOutput); m_storagePIDController.setReference(targetPos, ControlType.kPosition); } @@ -94,7 +96,7 @@ public class Storage extends SubsystemBase { * Prepares storage for shooting */ public void storageAim() { - + //runStoragePositionPID(targetPos, kP, kI, kD, kIz, kF, kmaxOutput, kminOutput); } /* *If shooting move storage motor until top sensor is tripped From 1463bd209472d311be0b74d078e9ad772c0fde3b Mon Sep 17 00:00:00 2001 From: Rishabh <57200@psdschools.org> Date: Thu, 13 Feb 2020 19:07:36 -0700 Subject: [PATCH 03/24] Add up and down for storage thingy --- .../java/frc4388/robot/RobotContainer.java | 3 +++ .../frc4388/robot/subsystems/Storage.java | 19 +++++++++++++++++-- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bf4a506..5ad26e8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -138,6 +138,9 @@ public class RobotContainer { /* Storage Neo PID Test */ new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5))); + + new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) + .whileHeld(new RunCommand(() -> m_robotStorage.storageIntake())); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index be3f97f..5f4d610 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -65,7 +65,7 @@ public class Storage extends SubsystemBase { } else { System.err.println("Beam off"); } - + } public void resetEncoder() @@ -96,7 +96,21 @@ public class Storage extends SubsystemBase { * Prepares storage for shooting */ public void storageAim() { - //runStoragePositionPID(targetPos, kP, kI, kD, kIz, kF, kmaxOutput, kminOutput); + if (m_beamSensors[0].get() == false){ + m_storageMotor.set(0.5); + } + else{ + m_storageMotor.set(0); + } + } + +public void storageIntake() { + if (m_beamSensors[2].get() == false){ + m_storageMotor.set(-0.5); + } + else{ + m_storageMotor.set(0); + } /* *If shooting move storage motor until top sensor is tripped @@ -104,3 +118,4 @@ public class Storage extends SubsystemBase { * */ } +} From 0cf61e189337aa233ee5dc2d2041b3c52ad4dbc7 Mon Sep 17 00:00:00 2001 From: Sebastian <66741@psdschools.org> Date: Sat, 15 Feb 2020 11:16:12 -0800 Subject: [PATCH 04/24] Update Storage.java --- .../frc4388/robot/subsystems/Storage.java | 33 +++++++++---------- 1 file changed, 16 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 5f4d610..280b3f9 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -32,6 +32,8 @@ public class Storage extends SubsystemBase { Gains storageGains = StorageConstants.STORAGE_GAINS; + Intake m_intake; + /** * Creates a new Storage. */ @@ -56,17 +58,7 @@ public class Storage extends SubsystemBase { * * @param input the voltage to run motor at */ - public void runStorage(final double input) { - m_storageMotor.set(input); - final boolean beam_on = m_beamSensors[0].get(); - - if (beam_on) { - System.err.println("Beam on"); - } else { - System.err.println("Beam off"); - } - - } + public void resetEncoder() { @@ -95,8 +87,8 @@ public class Storage extends SubsystemBase { /** * Prepares storage for shooting */ - public void storageAim() { - if (m_beamSensors[0].get() == false){ + public void storageAim() { + if (m_beamSensors[2].get() == false){ m_storageMotor.set(0.5); } else{ @@ -104,13 +96,21 @@ public class Storage extends SubsystemBase { } } -public void storageIntake() { - if (m_beamSensors[2].get() == false){ +public void storageIntake(Intake intake) { + m_intake = intake; + if (m_beamSensors[1].get() == false){ m_storageMotor.set(-0.5); } else{ m_storageMotor.set(0); - + } + if (m_beamSensors[0].get()){ + m_intake.runExtender(-0.3); + m_storagePIDController.setReference(10, ControlType.kPosition); + } +} +public void storageOuttake() { + m_storageMotor.set(1); } /* *If shooting move storage motor until top sensor is tripped @@ -118,4 +118,3 @@ public void storageIntake() { * */ } -} From 57cc8a26e6fa921cb851a98332a710fb7bc1409b Mon Sep 17 00:00:00 2001 From: Sebastian <66741@psdschools.org> Date: Sat, 15 Feb 2020 16:01:28 -0800 Subject: [PATCH 05/24] Even More Janky Code Storage --- src/main/java/frc4388/robot/RobotContainer.java | 10 +++++++--- .../java/frc4388/robot/subsystems/Intake.java | 12 ++++++++---- .../java/frc4388/robot/subsystems/Storage.java | 15 ++++++++------- 3 files changed, 23 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5ad26e8..389c553 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -79,9 +79,8 @@ public class RobotContainer { m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.15), m_robotShooter)); // drives the leveler with an axis input from the driver controller m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); - // runs storage motor at 50 percent - m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); } + /** * Use this method to define your button->command mappings. Buttons can be created by @@ -139,8 +138,13 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5))); + //Prepares storage for intaking new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) - .whileHeld(new RunCommand(() -> m_robotStorage.storageIntake())); + .whileHeld(new RunCommand(() -> m_robotStorage.storageIntake(m_robotIntake))); + + //Runs storage to outtake + new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) + .whileHeld(new RunCommand(() -> m_robotStorage.storageOuttake())); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 62bb30a..679aac3 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -17,16 +17,20 @@ import edu.wpi.first.wpilibj.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.IntakeConstants; + + public class Intake extends SubsystemBase { CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless); CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless); - CANDigitalInput m_extenderForwardLimit; - CANDigitalInput m_extenderReverseLimit; + /** * Creates a new Intake. */ public Intake() { + CANDigitalInput m_extenderForwardLimit; + CANDigitalInput m_extenderReverseLimit; + m_intakeMotor.restoreFactoryDefaults(); m_extenderMotor.restoreFactoryDefaults(); @@ -50,7 +54,7 @@ public class Intake extends SubsystemBase { * Runs intake motor * @param input the percent output to run motor at */ - public void runIntake(double input) { + public void runIntake(final double input) { m_intakeMotor.set(input); } @@ -58,7 +62,7 @@ public class Intake extends SubsystemBase { * Runs extender motor * @param input the percent output to run motor at */ - public void runExtender(double input) { + public void runExtender(final double input) { m_extenderMotor.set(input); } } diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 280b3f9..7c35066 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -9,12 +9,14 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.revrobotics.CANDigitalInput; import com.revrobotics.CANEncoder; import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; import com.revrobotics.ControlType; import com.revrobotics.SparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; @@ -95,7 +97,6 @@ public class Storage extends SubsystemBase { m_storageMotor.set(0); } } - public void storageIntake(Intake intake) { m_intake = intake; if (m_beamSensors[1].get() == false){ @@ -110,11 +111,11 @@ public void storageIntake(Intake intake) { } } public void storageOuttake() { - m_storageMotor.set(1); + m_storageMotor.set(1); + + /* + *If shooting move storage motor until top sensor is tripped + *If intaking move storage motor until bottom sensor is tripped + */ } - /* - *If shooting move storage motor until top sensor is tripped - *If intaking move storage motor until bottom sensor is tripped - * - */ } From 86d6d5d946c548da2510d4458e1817f90bd6fe65 Mon Sep 17 00:00:00 2001 From: Sebastian <66741@psdschools.org> Date: Tue, 18 Feb 2020 18:12:06 -0800 Subject: [PATCH 06/24] Limit switches for extenders Co-Authored-By: llamakingker #janky --- .../java/frc4388/robot/subsystems/Intake.java | 23 +++++++++++++++---- .../frc4388/robot/subsystems/Storage.java | 2 +- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 679aac3..63cb587 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -22,14 +22,15 @@ import frc4388.robot.Constants.IntakeConstants; public class Intake extends SubsystemBase { CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless); CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless); - + CANDigitalInput m_extenderForwardLimit; + CANDigitalInput m_extenderReverseLimit; + boolean extended = false; /** * Creates a new Intake. */ public Intake() { - CANDigitalInput m_extenderForwardLimit; - CANDigitalInput m_extenderReverseLimit; + m_intakeMotor.restoreFactoryDefaults(); m_extenderMotor.restoreFactoryDefaults(); @@ -63,6 +64,18 @@ public class Intake extends SubsystemBase { * @param input the percent output to run motor at */ public void runExtender(final double input) { - m_extenderMotor.set(input); + if (m_extenderForwardLimit.get()) { + extended = true; + } + if (m_extenderReverseLimit.get()) { + extended = false; + } + + if (extended == false) { + m_extenderMotor.set(0.5); + } + if (extended == true) { + m_extenderMotor.set(-0.5); + } } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 7c35066..db3d2fe 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -106,7 +106,7 @@ public void storageIntake(Intake intake) { m_storageMotor.set(0); } if (m_beamSensors[0].get()){ - m_intake.runExtender(-0.3); + m_intake.runExtender(-0.3); m_storagePIDController.setReference(10, ControlType.kPosition); } } From d89f9da745ce7ab33e0bc3f7defb5c0b8488d084 Mon Sep 17 00:00:00 2001 From: Sebastian <66741@psdschools.org> Date: Tue, 18 Feb 2020 18:45:07 -0800 Subject: [PATCH 07/24] Changed Limit Switch on Extender Co-Authored-By: llamakingker #janky --- .../robot/commands/RunExtenderOutIn.java | 30 ++++++++++++++++--- 1 file changed, 26 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java index b0bb140..6cfd9c4 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -7,14 +7,25 @@ package frc4388.robot.commands; +import com.revrobotics.CANDigitalInput; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.subsystems.Intake; import frc4388.utility.controller.IHandController; public class RunExtenderOutIn extends CommandBase { private Intake m_intake; private boolean isOut = false; - private long startTime; + CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless); + CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless); + CANDigitalInput m_extenderForwardLimit; + CANDigitalInput m_extenderReverseLimit; + + /** * Uses input from opperator to run the extender motor. @@ -25,13 +36,17 @@ public class RunExtenderOutIn extends CommandBase { // Use addRequirements() here to declare subsystem dependencies. m_intake = subsystem; addRequirements(m_intake); + + m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + m_extenderForwardLimit.enableLimitSwitch(false); + m_extenderReverseLimit.enableLimitSwitch(false); } // Called when the command is initially scheduled. @Override public void initialize() { isOut = !isOut; - startTime = System.currentTimeMillis(); } // Called every time the scheduler runs while the command is scheduled. @@ -54,9 +69,16 @@ public class RunExtenderOutIn extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (startTime + 3000 < System.currentTimeMillis()) { + if (isOut && m_extenderForwardLimit.get() == true){ return true; } - return false; + + else if(isOut && m_extenderReverseLimit.get() == true){ + return true; + } + + else{ + return false; + } } } From b0f420dc692cb06998c485971a219316b67ee9dc Mon Sep 17 00:00:00 2001 From: Kerem <81587@psdschools.org> Date: Tue, 18 Feb 2020 19:56:03 -0700 Subject: [PATCH 08/24] Added buttons --- src/main/java/frc4388/robot/RobotContainer.java | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 50cedc1..0a735c2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -82,7 +82,6 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // runs the drum shooter in idle mode - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter)); // drives the leveler with an axis input from the driver controller // m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); @@ -110,10 +109,10 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000)); - new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileHeld(new TrackTarget(m_robotShooter)); - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .whenPressed(new RunExtenderOutIn(m_robotIntake)); /* PID Test Command */ @@ -135,11 +134,11 @@ public class RobotContainer { // sets solenoids into high gear - new JoystickButton(getDriverJoystick(), XboxController.START_BUTTON) + new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); // sets solenoids into low gear - new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON) + new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); // interrupts any running command From 6742028ba9e742587f01e16be5f715004c6d13c0 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Wed, 19 Feb 2020 20:48:34 -0700 Subject: [PATCH 09/24] Add Shooting capabilities --- src/main/java/frc4388/robot/Constants.java | 1 + .../java/frc4388/robot/RobotContainer.java | 8 +- .../frc4388/robot/commands/ShootShooter.java | 75 +++++++++++++++++++ .../frc4388/robot/commands/TrackTarget.java | 16 ++++ 4 files changed, 98 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/ShootShooter.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c4e5917..a1a5901 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -137,6 +137,7 @@ public final class Constants { public static final double MOTOR_DEAD_ZONE = 0.3; public static final double DISTANCE_ERROR_EQUATION_M = 1.1279; public static final double DISTANCE_ERROR_EQUATION_B = -15.0684; + public static final double GRAV = 385.83; } public static final class OIConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0a735c2..493ab99 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -30,6 +30,7 @@ import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; +import frc4388.robot.commands.ShootShooter; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -106,8 +107,11 @@ public class RobotContainer { // .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) // .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000)); + new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5)); + + new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1)); new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileHeld(new TrackTarget(m_robotShooter)); diff --git a/src/main/java/frc4388/robot/commands/ShootShooter.java b/src/main/java/frc4388/robot/commands/ShootShooter.java new file mode 100644 index 0000000..370c4b5 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShootShooter.java @@ -0,0 +1,75 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 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 frc4388.robot.commands; + +import edu.wpi.first.wpilibj.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.commands.TrackTarget; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.Storage; + +public class ShootShooter extends CommandBase { + Shooter m_shooter; + Storage m_storage; + private long startTime; + private int ballNum; + /** + * Creates a new ShootAlll. + */ + public ShootShooter(Shooter shootSub, Storage storeSub, int numBall) { + ballNum = numBall; + m_shooter = shootSub; + m_storage = storeSub; + addRequirements(m_shooter); + addRequirements(m_storage); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + startTime = System.currentTimeMillis(); + //new InstantCommand(() -> m_storage.storageAiming()); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + TrackTarget track = new TrackTarget(m_shooter); //Used to pull velocity and angle from TrackTarget.java + + //Aiming + if (startTime + 3000 > System.currentTimeMillis()) + { + new ShooterVelocityControlPID(m_shooter, track.addFireVel()); + } + + else if(3000 + startTime + (1000 * ballNum) > System.currentTimeMillis()){ + new RunCommand(() -> m_storage.runStorage(0.5)); + new ShooterVelocityControlPID(m_shooter, track.addFireVel()); + new RunCommand(() -> m_shooter.runAngleAdjustPID(track.addFireAngle())); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (3000 + startTime + (1000 * ballNum) > System.currentTimeMillis()){ + return true; + } + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 2853242..3f806d4 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -7,6 +7,7 @@ package frc4388.robot.commands; +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Shooter; @@ -28,6 +29,8 @@ public class TrackTarget extends CommandBase { double yAngle = 0; double target = 0; public double distance; + public static double fireVel; + public static double fireAngle; /** * Uses the Limelight to track the target @@ -46,6 +49,13 @@ public class TrackTarget extends CommandBase { NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); } + public double addFireVel() { + return fireVel; + } + public double addFireAngle() { + return fireAngle; + } + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { @@ -65,6 +75,12 @@ public class TrackTarget extends CommandBase { //Finding Distance distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180)); SmartDashboard.putNumber("Distance to Target", distance); + + double yVel = Math.sqrt(2*VisionConstants.GRAV*VisionConstants.TARGET_HEIGHT); + double xVel = (distance*VisionConstants.GRAV)/(yVel); + + fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2))); + fireAngle = Math.atan(yVel/xVel); } } From 7667182b012dd0b87299ae6d142c50370528536e Mon Sep 17 00:00:00 2001 From: Aarav Shah <91212@psdschools.org> Date: Thu, 20 Feb 2020 17:00:50 -0700 Subject: [PATCH 10/24] Changes to isExtended boolean Refactored name according to boolean convention and combined two different booleans doing the same thing. --- .../java/frc4388/robot/commands/RunExtenderOutIn.java | 10 +++++----- src/main/java/frc4388/robot/subsystems/Intake.java | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java index 6cfd9c4..3c062a0 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -19,7 +19,7 @@ import frc4388.utility.controller.IHandController; public class RunExtenderOutIn extends CommandBase { private Intake m_intake; - private boolean isOut = false; + CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless); CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless); CANDigitalInput m_extenderForwardLimit; @@ -46,13 +46,13 @@ public class RunExtenderOutIn extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - isOut = !isOut; + m_intake.isExtended = !m_intake.isExtended; } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (isOut){ + if (m_intake.isExtended){ m_intake.runExtender(0.3); } else { m_intake.runExtender(-0.3); @@ -69,11 +69,11 @@ public class RunExtenderOutIn extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (isOut && m_extenderForwardLimit.get() == true){ + if (m_intake.isExtended && m_extenderForwardLimit.get() == true){ return true; } - else if(isOut && m_extenderReverseLimit.get() == true){ + else if(m_intake.isExtended && m_extenderReverseLimit.get() == true){ return true; } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 63cb587..d6b9b32 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -24,7 +24,7 @@ public class Intake extends SubsystemBase { CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless); CANDigitalInput m_extenderForwardLimit; CANDigitalInput m_extenderReverseLimit; - boolean extended = false; + public boolean isExtended = false; /** * Creates a new Intake. @@ -65,16 +65,16 @@ public class Intake extends SubsystemBase { */ public void runExtender(final double input) { if (m_extenderForwardLimit.get()) { - extended = true; + isExtended = true; } if (m_extenderReverseLimit.get()) { - extended = false; + isExtended = false; } - if (extended == false) { + if (isExtended == false) { m_extenderMotor.set(0.5); } - if (extended == true) { + if (isExtended == true) { m_extenderMotor.set(-0.5); } } From 01a0327cf3fe4f321b5f7eaf98a5ce0cc832287c Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Thu, 20 Feb 2020 20:21:28 -0700 Subject: [PATCH 11/24] Try to make shooter work --- src/main/java/frc4388/robot/Constants.java | 7 +---- .../frc4388/robot/commands/ShootShooter.java | 28 +++++++++---------- .../frc4388/robot/commands/TrackTarget.java | 9 ++---- .../frc4388/robot/subsystems/Shooter.java | 23 ++++++++++++++- .../frc4388/robot/subsystems/Storage.java | 22 ++++++++------- 5 files changed, 52 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a1a5901..3f9f627 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -114,13 +114,8 @@ public final class Constants { public static final int PID_PRIMARY = 0; /* PID Gains */ - public static final double storP = 0.1; - public static final double storI = 1e-4; - public static final double storD = 1.0; - public static final double storIz = 0.0; - public static final double storF = 0.0; - public static final double storkmaxOutput = 1.0; public static final double storkminOutput = -1.0; + public static final Gains STORAGE_GAINS = new Gains(0.1, 0, 1, 0, 0, 1); } public static final class LEDConstants { diff --git a/src/main/java/frc4388/robot/commands/ShootShooter.java b/src/main/java/frc4388/robot/commands/ShootShooter.java index 370c4b5..d9b2d1f 100644 --- a/src/main/java/frc4388/robot/commands/ShootShooter.java +++ b/src/main/java/frc4388/robot/commands/ShootShooter.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.commands.TrackTarget; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Storage; @@ -23,6 +22,7 @@ public class ShootShooter extends CommandBase { Storage m_storage; private long startTime; private int ballNum; + public boolean velReach = false; /** * Creates a new ShootAlll. */ @@ -30,35 +30,35 @@ public class ShootShooter extends CommandBase { ballNum = numBall; m_shooter = shootSub; m_storage = storeSub; - addRequirements(m_shooter); - addRequirements(m_storage); } // Called when the command is initially scheduled. @Override public void initialize() { - startTime = System.currentTimeMillis(); - //new InstantCommand(() -> m_storage.storageAiming()); + //new InstantCommand(() -> m_storage.storageAim()); + velReach = false; } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - TrackTarget track = new TrackTarget(m_shooter); //Used to pull velocity and angle from TrackTarget.java //Aiming - if (startTime + 3000 > System.currentTimeMillis()) + if (!m_shooter.velReached && velReach == false) //If the shooter is spooled { - new ShooterVelocityControlPID(m_shooter, track.addFireVel()); + new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()); + startTime = System.currentTimeMillis(); } - else if(3000 + startTime + (1000 * ballNum) > System.currentTimeMillis()){ - new RunCommand(() -> m_storage.runStorage(0.5)); - new ShooterVelocityControlPID(m_shooter, track.addFireVel()); - new RunCommand(() -> m_shooter.runAngleAdjustPID(track.addFireAngle())); + else { + velReach = true; + new ParallelCommandGroup( + new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()), + new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())), + new RunCommand(() -> m_storage.runStoragePositionPID(m_storage.getEncoderPos() + (2*ballNum))) + ); } } - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { @@ -67,7 +67,7 @@ public class ShootShooter extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (3000 + startTime + (1000 * ballNum) > System.currentTimeMillis()){ + if (startTime + (1000 * ballNum) < System.currentTimeMillis()){ return true; } return false; diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 3f806d4..8102f13 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -49,12 +49,7 @@ public class TrackTarget extends CommandBase { NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); } - public double addFireVel() { - return fireVel; - } - public double addFireAngle() { - return fireAngle; - } + // Called every time the scheduler runs while the command is scheduled. @Override @@ -81,6 +76,8 @@ public class TrackTarget extends CommandBase { fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2))); fireAngle = Math.atan(yVel/xVel); + m_shooter.m_fireVel = fireVel; + m_shooter.m_fireAngle = fireAngle; } } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index a18aca5..c4487ce 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -48,6 +48,9 @@ public class Shooter extends SubsystemBase { double velP; double input; + public boolean velReached; + public double m_fireVel; + public double m_fireAngle; /* * Creates a new Shooter subsystem. @@ -79,6 +82,13 @@ public class Shooter extends SubsystemBase { SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV); } + public double addFireVel() { + return m_fireVel; + } + public double addFireAngle() { + return m_fireAngle; + } + /** * Runs drum shooter motor. * @param speed Speed to set the motor at @@ -105,15 +115,26 @@ public class Shooter extends SubsystemBase { public void runDrumShooterVelocityPID(double targetVel, double actualVel) { velP = actualVel/targetVel; //Percent of target double runSpeed = actualVel + (12000*velP); //Ramp up equation - //if (runSpeed > targetVel) {runSpeed = targetVel;} SmartDashboard.putNumber("shoot", actualVel); runSpeed = runSpeed/targetVel; //Convert to percent + if (actualVel < targetVel - 1000){ //PID Based on ramp up amount m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed); } else{ //PID Based on targetVel m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } + + //Tells wether the target velocity has been reached + double upperBound = targetVel + 300; + double lowerBound = targetVel - 300; + if (actualVel < upperBound && actualVel > lowerBound) + { + velReached = true; + } + else{ + velReached = false; + } } diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 790e59e..50628c6 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -18,7 +18,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj2.command.SubsystemBase; - +import frc4388.robot.Gains; import frc4388.robot.Constants.StorageConstants; public class Storage extends SubsystemBase { @@ -29,6 +29,8 @@ public class Storage extends SubsystemBase { CANEncoder m_encoder = m_storageMotor.getEncoder(); + public static Gains m_storageGains = StorageConstants.STORAGE_GAINS; + /** * Creates a new Storage. */ @@ -65,21 +67,21 @@ public class Storage extends SubsystemBase { } /* Storage PID Control */ - public void runStoragePositionPID(double targetPos, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput) + public void runStoragePositionPID(double targetPos) { // Set PID Coefficients - m_storagePIDController.setP(kP); - m_storagePIDController.setI(kI); - m_storagePIDController.setD(kD); - m_storagePIDController.setIZone(kIz); - m_storagePIDController.setFF(kF); - m_storagePIDController.setOutputRange(kminOutput, kmaxOutput); + m_storagePIDController.setP(m_storageGains.m_kP); + m_storagePIDController.setI(m_storageGains.m_kI); + m_storagePIDController.setD(m_storageGains.m_kD); + m_storagePIDController.setIZone(m_storageGains.m_kIzone); + m_storagePIDController.setFF(m_storageGains.m_kF); + m_storagePIDController.setOutputRange(StorageConstants.storkminOutput, m_storageGains.m_kmaxOutput); m_storagePIDController.setReference(targetPos, ControlType.kPosition); } - public void getEncoderPos() + public double getEncoderPos() { - m_encoder.getPosition(); + return m_encoder.getPosition(); } } From 7cdeb09908a6a89854045d321e7f4482feb7e5bc Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 21 Feb 2020 16:59:05 -0700 Subject: [PATCH 12/24] TODO Shooter --- .../java/frc4388/robot/RobotContainer.java | 9 +-- .../frc4388/robot/commands/ShootShooter.java | 75 ------------------- 2 files changed, 4 insertions(+), 80 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/ShootShooter.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 493ab99..77dc2a9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -30,7 +30,6 @@ import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; -import frc4388.robot.commands.ShootShooter; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -107,11 +106,11 @@ public class RobotContainer { // .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) // .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5)); + //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5)); - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1)); + //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1)); new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileHeld(new TrackTarget(m_robotShooter)); diff --git a/src/main/java/frc4388/robot/commands/ShootShooter.java b/src/main/java/frc4388/robot/commands/ShootShooter.java deleted file mode 100644 index d9b2d1f..0000000 --- a/src/main/java/frc4388/robot/commands/ShootShooter.java +++ /dev/null @@ -1,75 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 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 frc4388.robot.commands; - -import edu.wpi.first.wpilibj.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.Storage; - -public class ShootShooter extends CommandBase { - Shooter m_shooter; - Storage m_storage; - private long startTime; - private int ballNum; - public boolean velReach = false; - /** - * Creates a new ShootAlll. - */ - public ShootShooter(Shooter shootSub, Storage storeSub, int numBall) { - ballNum = numBall; - m_shooter = shootSub; - m_storage = storeSub; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - //new InstantCommand(() -> m_storage.storageAim()); - velReach = false; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - //Aiming - if (!m_shooter.velReached && velReach == false) //If the shooter is spooled - { - new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()); - startTime = System.currentTimeMillis(); - } - - else { - velReach = true; - new ParallelCommandGroup( - new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()), - new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())), - new RunCommand(() -> m_storage.runStoragePositionPID(m_storage.getEncoderPos() + (2*ballNum))) - ); - } - } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (startTime + (1000 * ballNum) < System.currentTimeMillis()){ - return true; - } - return false; - } -} From e7e91d9c4cde502ca53414e8a4b412c74a48c943 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 21 Feb 2020 17:02:21 -0700 Subject: [PATCH 13/24] add thing for later --- src/main/java/frc4388/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 77dc2a9..3015bac 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -113,7 +113,8 @@ public class RobotContainer { // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1)); new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new TrackTarget(m_robotShooter)); + .whileHeld(new TrackTarget(m_robotShooter)) + //.whileHeld(new RunCommand(() -> m_robotStorage.storeAim())); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .whenPressed(new RunExtenderOutIn(m_robotIntake)); From 84c83dd3e56c824540879bcda5357aecbf7c6e6f Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 21 Feb 2020 17:16:16 -0700 Subject: [PATCH 14/24] Fixed gradle fail --- src/main/java/frc4388/robot/RobotContainer.java | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3015bac..293123e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -113,7 +113,7 @@ public class RobotContainer { // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1)); new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new TrackTarget(m_robotShooter)) + .whileHeld(new TrackTarget(m_robotShooter)); //.whileHeld(new RunCommand(() -> m_robotStorage.storeAim())); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) @@ -154,9 +154,6 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber)); - /* Storage Neo PID Test */ - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new TrackTarget(m_robotShooter)); } /** From 9983b9354b5bb105e60a49bac9b776e95fbd6b9d Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 21 Feb 2020 17:25:17 -0700 Subject: [PATCH 15/24] FIX --- .../java/frc4388/robot/RobotContainer.java | 62 ++++++------------- 1 file changed, 20 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4d5a858..265f57b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -112,46 +112,7 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(0), m_robotDrive)); - - /* Operator Buttons */ - // activates "Lit Mode" - //new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - // .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) - // .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - - //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5)); - - //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1)); - - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new TrackTarget(m_robotShooter)); - //.whileHeld(new RunCommand(() -> m_robotStorage.storeAim())); - new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whenPressed(new RunExtenderOutIn(m_robotIntake)); - - /* PID Test Command */ - // runs velocity PID while driving straight - new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500)) - .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); - - //new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileHeld(new DriveWithJoystickAuxPID(m_robotDrive, getDriverController())); - - // resets the yaw of the pigeon - new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whileHeld(new RunCommand(() -> m_robotDrive.tankDriveVelocity(9, 3), m_robotDrive)); - - // turn 45 degrees - new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new RunCommand(() -> m_robotDrive.driveWithInputAux(0.2, 0), m_robotDrive)); - - // sets solenoids into high gear new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); @@ -159,10 +120,27 @@ public class RobotContainer { // sets solenoids into low gear new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); + + + /* Operator Buttons */ + + //TODO: Shooter Buttons + // shoots until released + //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5)); + + // shoots one ball + //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1)); + + // aims the turret + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + .whileHeld(new TrackTarget(m_robotShooter)); + //.whenPressed(new RunCommand(() -> m_robotStorage.storeAim())); - // interrupts any running command - new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) - .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); + // extends or retracts the extender + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + .whenPressed(new RunExtenderOutIn(m_robotIntake)); // safety for climber and leveler new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) From 629046c0329094a6373f70ecb4ee3e2f8b006b44 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 21 Feb 2020 20:41:36 -0700 Subject: [PATCH 16/24] aaaaaaaaaaaa --- .../java/frc4388/robot/RobotContainer.java | 6 +- .../robot/commands/StorageIntakeFinal.java | 46 +++++++++++++++ .../robot/commands/StorageIntakeGroup.java | 27 +++++++++ .../frc4388/robot/commands/storageIntake.java | 58 +++++++++++++++++++ .../frc4388/robot/commands/storageOutake.java | 44 ++++++++++++++ .../robot/commands/storagePrepAim.java | 52 +++++++++++++++++ .../robot/commands/storagePrepIntake.java | 56 ++++++++++++++++++ .../frc4388/robot/subsystems/Storage.java | 51 +++++----------- 8 files changed, 300 insertions(+), 40 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/StorageIntakeFinal.java create mode 100644 src/main/java/frc4388/robot/commands/StorageIntakeGroup.java create mode 100644 src/main/java/frc4388/robot/commands/storageIntake.java create mode 100644 src/main/java/frc4388/robot/commands/storageOutake.java create mode 100644 src/main/java/frc4388/robot/commands/storagePrepAim.java create mode 100644 src/main/java/frc4388/robot/commands/storagePrepIntake.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 26d1a69..ac85461 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -39,6 +39,7 @@ import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.commands.ShooterVelocityControlPID; +import frc4388.robot.commands.StorageIntakeGroup; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -49,6 +50,7 @@ import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; import frc4388.robot.commands.TrackTarget; +import frc4388.robot.commands.storageOutake; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; import frc4388.robot.subsystems.Storage; @@ -171,11 +173,11 @@ public class RobotContainer { //Prepares storage for intaking new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) - .whileHeld(new RunCommand(() -> m_robotStorage.storageIntake(m_robotIntake))); + .whileHeld(new StorageIntakeGroup(m_robotIntake, m_robotStorage)); //Runs storage to outtake new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) - .whileHeld(new RunCommand(() -> m_robotStorage.storageOuttake())); + .whileHeld(new storageOutake(m_robotStorage)); } /** diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java new file mode 100644 index 0000000..9576f35 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 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 frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Storage; + +public class StorageIntakeFinal extends CommandBase { + Storage m_storage; + /** + * Creates a new StorageIntakeFinal. + */ + public StorageIntakeFinal(Storage subsystem) { + m_storage = subsystem; + addRequirements(m_storage); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (m_storage.getBeam(1)){ + m_storage.setStoragePID(m_storage.getEncoderPos() + 5); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java new file mode 100644 index 0000000..767ed69 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 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 frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Storage; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class StorageIntakeGroup extends SequentialCommandGroup { + /** + * Creates a new StorageIntakeGroup. + */ + public StorageIntakeGroup(Intake m_intake, Storage m_storage) { + addCommands( + new storagePrepIntake(m_intake, m_storage), + new storageIntake(m_intake, m_storage), + new StorageIntakeFinal(m_storage)); + } +} diff --git a/src/main/java/frc4388/robot/commands/storageIntake.java b/src/main/java/frc4388/robot/commands/storageIntake.java new file mode 100644 index 0000000..ee30708 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/storageIntake.java @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 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 frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Storage; + +public class storageIntake extends CommandBase { + public Intake m_intake; + public Storage m_storage; + /** + * Creates a new storageIntake. + */ + public storageIntake(Intake inSub, Storage storeSub) { + m_intake = inSub; + m_storage = storeSub; + addRequirements(m_intake); + addRequirements(m_storage); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + if (m_storage.getBeam(0)){ + m_storage.setStoragePID(m_storage.getEncoderPos() + 2); + m_intake.runExtender(-0.3); + } + else{ + m_intake.runExtender(0.3); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_storage.getBeam(0)){ + return true; + } + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/storageOutake.java b/src/main/java/frc4388/robot/commands/storageOutake.java new file mode 100644 index 0000000..4820dc0 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/storageOutake.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 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 frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Storage; + +public class storageOutake extends CommandBase { + Storage m_storage; + /** + * Creates a new storageOutake. + */ + public storageOutake(Storage storeSub) { + m_storage = storeSub; + addRequirements(m_storage); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_storage.runStorage(-0.5); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/storagePrepAim.java b/src/main/java/frc4388/robot/commands/storagePrepAim.java new file mode 100644 index 0000000..c8b3bad --- /dev/null +++ b/src/main/java/frc4388/robot/commands/storagePrepAim.java @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 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 frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Storage; + +public class storagePrepAim extends CommandBase { + Storage m_storage; + /** + * Creates a new storagePrepAim. + */ + public storagePrepAim(Storage storeSub) { + m_storage = storeSub; + addRequirements(m_storage); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (m_storage.getBeam(2) == false){ + m_storage.runStorage(0.5); + } + else{ + m_storage.runStorage(0); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_storage.getBeam(2)){ + return true; + } + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/storagePrepIntake.java b/src/main/java/frc4388/robot/commands/storagePrepIntake.java new file mode 100644 index 0000000..7fab016 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/storagePrepIntake.java @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 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 frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Storage; + +public class storagePrepIntake extends CommandBase { + public Intake m_intake; + public Storage m_storage; + /** + * Creates a new storagePrepIntake. + */ + public storagePrepIntake(Intake inSub, Storage storeSub) { + m_intake = inSub; + m_storage = storeSub; + addRequirements(m_intake); + addRequirements(m_storage); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (m_storage.getBeam(1) == false){ + m_storage.runStorage(-0.5); + } + else{ + m_storage.runStorage(0); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_storage.getBeam(1)){ + return true; + } + return false; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index c71355a..24422c9 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -19,6 +19,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Gains; import frc4388.robot.Constants.StorageConstants; @@ -35,12 +36,14 @@ public class Storage extends SubsystemBase { Intake m_intake; + public boolean botReached; + /** * Creates a new Storage. */ public Storage() { resetEncoder(); - + boolean botReached = false; m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0); m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1); m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2); @@ -65,14 +68,12 @@ public class Storage extends SubsystemBase { final boolean beam_on = m_beamSensors[0].get(); } - public void resetEncoder() - { + public void resetEncoder(){ m_encoder.setPosition(0); } /* Storage PID Control */ - public void runStoragePositionPID(double targetPos) - { + public void runStoragePositionPID(double targetPos){ // Set PID Coefficients m_storagePIDController.setP(storageGains.m_kP); m_storagePIDController.setI(storageGains.m_kI); @@ -84,41 +85,15 @@ public class Storage extends SubsystemBase { m_storagePIDController.setReference(targetPos, ControlType.kPosition); } - public void getEncoderPos() - { - m_encoder.getPosition(); + public double getEncoderPos(){ + return m_encoder.getPosition(); } - /** - * Prepares storage for shooting - */ - public void storageAim() { - if (m_beamSensors[2].get() == false){ - m_storageMotor.set(0.5); - } - else{ - m_storageMotor.set(0); - } + public boolean getBeam(int id){ + return m_beamSensors[id].get(); } -public void storageIntake(Intake intake) { - m_intake = intake; - if (m_beamSensors[1].get() == false){ - m_storageMotor.set(-0.5); - } - else{ - m_storageMotor.set(0); - } - if (m_beamSensors[0].get()){ - m_intake.runExtender(-0.3); - m_storagePIDController.setReference(10, ControlType.kPosition); - } -} -public void storageOuttake() { - m_storageMotor.set(1); - - /* - *If shooting move storage motor until top sensor is tripped - *If intaking move storage motor until bottom sensor is tripped - */ + + public void setStoragePID(double position){ + m_storagePIDController.setReference(position , ControlType.kPosition); } } From 80dfa95f79824de4db5755a5441dd772025aa746 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 21 Feb 2020 20:51:09 -0700 Subject: [PATCH 17/24] fox --- src/main/java/frc4388/robot/subsystems/Storage.java | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index aed7e4e..5a7c7a0 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -64,7 +64,6 @@ public class Storage extends SubsystemBase { public void runStorage(final double input) { m_storageMotor.set(input); - final boolean beam_on = m_beamSensors[0].get(); } public void resetEncoder(){ @@ -74,12 +73,12 @@ public class Storage extends SubsystemBase { /* Storage PID Control */ public void runStoragePositionPID(double targetPos){ // Set PID Coefficients - m_storagePIDController.setP(m_storageGains.m_kP); - m_storagePIDController.setI(m_storageGains.m_kI); - m_storagePIDController.setD(m_storageGains.m_kD); - m_storagePIDController.setIZone(m_storageGains.m_kIzone); - m_storagePIDController.setFF(m_storageGains.m_kF); - m_storagePIDController.setOutputRange(StorageConstants.storkminOutput, m_storageGains.m_kmaxOutput); + m_storagePIDController.setP(storageGains.m_kP); + m_storagePIDController.setI(storageGains.m_kI); + m_storagePIDController.setD(storageGains.m_kD); + m_storagePIDController.setIZone(storageGains.m_kIzone); + m_storagePIDController.setFF(storageGains.m_kF); + m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput); m_storagePIDController.setReference(targetPos, ControlType.kPosition); } From d8e35e77df459adb8aa1a71cc6a8dfb5e06303a8 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 21 Feb 2020 21:11:43 -0700 Subject: [PATCH 18/24] Requested reviews --- src/main/java/frc4388/robot/subsystems/Intake.java | 4 ++-- src/main/java/frc4388/robot/subsystems/Storage.java | 3 +-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index d6b9b32..cbcd617 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -55,7 +55,7 @@ public class Intake extends SubsystemBase { * Runs intake motor * @param input the percent output to run motor at */ - public void runIntake(final double input) { + public void runIntake(double input) { m_intakeMotor.set(input); } @@ -63,7 +63,7 @@ public class Intake extends SubsystemBase { * Runs extender motor * @param input the percent output to run motor at */ - public void runExtender(final double input) { + public void runExtender() { if (m_extenderForwardLimit.get()) { isExtended = true; } diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 5a7c7a0..3d460fe 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -42,7 +42,6 @@ public class Storage extends SubsystemBase { */ public Storage() { resetEncoder(); - boolean botReached = false; m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0); m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1); m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2); @@ -62,7 +61,7 @@ public class Storage extends SubsystemBase { * @param input the voltage to run motor at */ - public void runStorage(final double input) { + public void runStorage(double input) { m_storageMotor.set(input); } From ac1bb02a7a28cabd0b9aff57f18670aca91bb7a4 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 21 Feb 2020 21:18:12 -0700 Subject: [PATCH 19/24] Fix input for extender --- src/main/java/frc4388/robot/subsystems/Intake.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index cbcd617..6b716f4 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -63,7 +63,7 @@ public class Intake extends SubsystemBase { * Runs extender motor * @param input the percent output to run motor at */ - public void runExtender() { + public void runExtender(double input) { if (m_extenderForwardLimit.get()) { isExtended = true; } @@ -72,10 +72,10 @@ public class Intake extends SubsystemBase { } if (isExtended == false) { - m_extenderMotor.set(0.5); + m_extenderMotor.set(input); } if (isExtended == true) { - m_extenderMotor.set(-0.5); + m_extenderMotor.set(-input); } } } \ No newline at end of file From b741417e0b8f423d630b059201a1b5c543c519d1 Mon Sep 17 00:00:00 2001 From: Elijah Price Date: Tue, 25 Feb 2020 17:12:56 -0700 Subject: [PATCH 20/24] initial testing --- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 7 +- .../robot/commands/DriveWithJoystick.java | 6 +- .../java/frc4388/robot/subsystems/Drive.java | 69 ++++++++++++------- 4 files changed, 56 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 5ada949..ba82993 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -106,7 +106,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { - m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); + m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); m_robotContainer.setDriveGearState(true); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f14c5d9..f46ff66 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -117,11 +117,14 @@ public class RobotContainer { // sets solenoids into high gear new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); + .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); // sets solenoids into low gear new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); + .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); + + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .whileHeld(new InstantCommand(() -> m_robotDrive.driveWithInput(0, 0), m_robotDrive)); /* Operator Buttons */ diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 5387e8d..42aedfc 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -48,12 +48,14 @@ public class DriveWithJoystick extends CommandBase { moveOutput = Math.cos(1.571*moveInput)-1; } - double cosMultiplier = .45; + double cosMultiplier = .55; double deadzone = .2; if (steerInput > 0){ steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone); - } else { + } else if (steerInput < 0) { steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone); + } else { + steerOutput = 0; } m_drive.driveWithInput(moveOutput, steerOutput); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 1902e0d..d0e7601 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -26,6 +26,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.SensorTerm; import com.ctre.phoenix.motorcontrol.StatusFrame; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.music.Orchestra; @@ -33,6 +34,7 @@ import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.RobotDrive; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.drive.DifferentialDrive; @@ -69,6 +71,7 @@ public class Drive extends SubsystemBase { public double m_rightFrontMotorVel; public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + //public RobotDrive m_driveTrain2 = new RobotDrive(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor) SendableChooser m_chooser = new SendableChooser(); public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS; @@ -109,21 +112,31 @@ public class Drive extends SubsystemBase { coolFalcon(false); /* set back motors as followers */ - m_leftBackMotor.follow(m_leftFrontMotor); - m_rightBackMotor.follow(m_rightFrontMotor); + //m_leftBackMotor.follow(m_leftFrontMotor); + //m_rightBackMotor.follow(m_rightFrontMotor); /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); - m_rightFrontMotor.setInverted(true); + m_rightFrontMotor.setInverted(false); //m_driveTrain.setRightSideInverted(false); - m_leftBackMotor.setInverted(InvertType.FollowMaster); - m_rightBackMotor.setInverted(InvertType.FollowMaster); + //m_leftBackMotor.setInverted(InvertType.FollowMaster); + //m_rightBackMotor.setInverted(InvertType.FollowMaster); + + float rampRate = 0.1f; + m_rightFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); + + SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 20, 25, 0.01); + m_rightFrontMotor.configSupplyCurrentLimit(c); + m_leftFrontMotor.configSupplyCurrentLimit(c); setDriveTrainNeutralMode(NeutralMode.Coast); /* deadbands */ - m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE - m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed + //m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE + //m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed //m_leftFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE //m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed @@ -322,26 +335,36 @@ public class Drive extends SubsystemBase { m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity(); try { - SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); + //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); + SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); + SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); - - SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); - SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); + //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); + //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - + /*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); + SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); + SmartDashboard.putNumber("Right Back Velocity", m_rightBackMotor.getSensorCollection().getIntegratedSensorVelocity()); + SmartDashboard.putNumber("Left Back Velocity", m_leftBackMotor.getSensorCollection().getIntegratedSensorVelocity()); + */ SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature()); SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature()); - //SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent()); - //SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent()); - //SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent()); - //SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getSupplyCurrent()); + + + SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent()); + + SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent()); + SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent()); + SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent()); + SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent()); //SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); @@ -350,10 +373,10 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); - SmartDashboard.putString("Odometry Values Meters", getPose().toString()); - SmartDashboard.putNumber("Odometry Heading", getHeading()); + //SmartDashboard.putString("Odometry Values Meters", getPose().toString()); + //SmartDashboard.putNumber("Odometry Heading", getHeading()); - SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); + //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); //SmartDashboard.putNumber("Delta Time", m_deltaTime); if (currentSong != m_songChooser.getSelected()){ @@ -389,8 +412,8 @@ public class Drive extends SubsystemBase { */ public void driveWithInput(double move, double steer) { m_driveTrain.arcadeDrive(move, steer); - m_leftBackMotor.follow(m_leftFrontMotor); - m_rightBackMotor.follow(m_rightFrontMotor); + m_leftBackMotor.follow(m_leftFrontMotor, FollowerType.PercentOutput); + m_rightBackMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput); } /** From 7339658077b70d486beb1edcb2d10d6197660de8 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 19:15:15 -0700 Subject: [PATCH 21/24] Fix watchdog feeds and steer output curving --- .../java/frc4388/robot/commands/DriveWithJoystick.java | 4 ++-- src/main/java/frc4388/robot/subsystems/Drive.java | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 42aedfc..fe1c8e5 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -51,9 +51,9 @@ public class DriveWithJoystick extends CommandBase { double cosMultiplier = .55; double deadzone = .2; if (steerInput > 0){ - steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone); + steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier; } else if (steerInput < 0) { - steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone); + steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier; } else { steerOutput = 0; } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index d0e7601..9f2639b 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -119,8 +119,8 @@ public class Drive extends SubsystemBase { m_leftFrontMotor.setInverted(false); m_rightFrontMotor.setInverted(false); //m_driveTrain.setRightSideInverted(false); - //m_leftBackMotor.setInverted(InvertType.FollowMaster); - //m_rightBackMotor.setInverted(InvertType.FollowMaster); + m_leftBackMotor.setInverted(InvertType.FollowMaster); + m_rightBackMotor.setInverted(InvertType.FollowMaster); float rampRate = 0.1f; m_rightFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); @@ -128,9 +128,9 @@ public class Drive extends SubsystemBase { m_leftFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); m_leftBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); - SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 20, 25, 0.01); - m_rightFrontMotor.configSupplyCurrentLimit(c); - m_leftFrontMotor.configSupplyCurrentLimit(c); + //SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 40, 35, 0.01); + //m_rightFrontMotor.configSupplyCurrentLimit(c); + //m_leftFrontMotor.configSupplyCurrentLimit(c); setDriveTrainNeutralMode(NeutralMode.Coast); From ab6cc48c461052da84743ffeefa1871695ee52bb Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 20:45:29 -0700 Subject: [PATCH 22/24] Reduce deadband for greater range of motion --- src/main/java/frc4388/robot/commands/DriveWithJoystick.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index fe1c8e5..f51621a 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -49,7 +49,7 @@ public class DriveWithJoystick extends CommandBase { } double cosMultiplier = .55; - double deadzone = .2; + double deadzone = .1; if (steerInput > 0){ steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier; } else if (steerInput < 0) { From f7dc572a4463d88d58f47f5ebf5358dd83b87953 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 20:52:20 -0700 Subject: [PATCH 23/24] Minor Cleanup Removed set up of Neutral Mode in the constructor because Neutral Mode is controlled in Robot.java --- src/main/java/frc4388/robot/Robot.java | 1 - src/main/java/frc4388/robot/subsystems/Drive.java | 2 -- 2 files changed, 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index ba82993..feab056 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -64,7 +64,6 @@ public class Robot extends TimedRobot { @Override public void disabledInit() { m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); - //m_robotContainer.setDriveGearState(true); } @Override diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 9f2639b..74b9ef9 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -132,8 +132,6 @@ public class Drive extends SubsystemBase { //m_rightFrontMotor.configSupplyCurrentLimit(c); //m_leftFrontMotor.configSupplyCurrentLimit(c); - setDriveTrainNeutralMode(NeutralMode.Coast); - /* deadbands */ //m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE //m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed From 3e829b4db8e2b13ce0cd94bda172ed84b1b7829b Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 21:12:02 -0700 Subject: [PATCH 24/24] Remove extraneous code --- src/main/java/frc4388/robot/RobotContainer.java | 6 +----- src/main/java/frc4388/robot/subsystems/Drive.java | 9 ++++----- 2 files changed, 5 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f46ff66..f57b496 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -123,13 +123,9 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whileHeld(new InstantCommand(() -> m_robotDrive.driveWithInput(0, 0), m_robotDrive)); - - /* Operator Buttons */ - //TODO: Shooter Buttons + //TODO: Shooter Buttons // shoots until released //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5)); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 74b9ef9..0dae7c8 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -71,7 +71,6 @@ public class Drive extends SubsystemBase { public double m_rightFrontMotorVel; public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); - //public RobotDrive m_driveTrain2 = new RobotDrive(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor) SendableChooser m_chooser = new SendableChooser(); public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS; @@ -112,8 +111,8 @@ public class Drive extends SubsystemBase { coolFalcon(false); /* set back motors as followers */ - //m_leftBackMotor.follow(m_leftFrontMotor); - //m_rightBackMotor.follow(m_rightFrontMotor); + m_leftBackMotor.follow(m_leftFrontMotor); + m_rightBackMotor.follow(m_rightFrontMotor); /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); @@ -410,8 +409,8 @@ public class Drive extends SubsystemBase { */ public void driveWithInput(double move, double steer) { m_driveTrain.arcadeDrive(move, steer); - m_leftBackMotor.follow(m_leftFrontMotor, FollowerType.PercentOutput); - m_rightBackMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput); + m_leftBackMotor.follow(m_leftFrontMotor); + m_rightBackMotor.follow(m_rightFrontMotor); } /**