From c7e50a36203fa9ef7f4ad59abbfae177d6b02c14 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sun, 1 Mar 2020 14:51:26 -0700 Subject: [PATCH 01/21] Reverse the DriveWithJoysticks move axis --- 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 54bc129..d0bdf96 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -39,7 +39,7 @@ public class DriveWithJoystick extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double moveInput = -m_controller.getLeftYAxis(); + double moveInput = m_controller.getLeftYAxis(); double steerInput = m_controller.getRightXAxis(); double moveOutput = 0; double steerOutput = 0; From 36e1534a8ffe9a48326839f5b1ee0c592235bbdb Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 2 Mar 2020 16:57:16 -0700 Subject: [PATCH 02/21] Added high gear for shtuff --- .../DriveWithJoystickDriveStraight.java | 26 ++++++++++++++++--- .../java/frc4388/robot/subsystems/Drive.java | 2 +- 2 files changed, 24 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java index ef9c1b9..8de8d2f 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java @@ -7,14 +7,18 @@ package frc4388.robot.commands; +import java.security.PublicKey; + import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpiutil.math.MathUtil; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; import frc4388.utility.controller.IHandController; public class DriveWithJoystickDriveStraight extends CommandBase { Drive m_drive; + Pneumatics m_pneumatics; double m_targetGyro, m_currentGyro; double m_stopPos; long m_currTime, m_deltaTime; @@ -22,6 +26,8 @@ public class DriveWithJoystickDriveStraight extends CommandBase { long m_deadTimeout = 100; IHandController m_controller; boolean m_isInterrupted; + double highGearMultiplier = 1; + double lowGearMultiplier = 1; /** * Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller. @@ -35,6 +41,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase { public DriveWithJoystickDriveStraight(Drive subsystem, IHandController controller) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; + m_pneumatics = m_drive.m_pneumaticsSubsystem; m_controller = controller; addRequirements(m_drive); } @@ -84,9 +91,17 @@ public class DriveWithJoystickDriveStraight extends CommandBase { } private void runDriveWithInput(double move, double steer) { - double cosMultiplier = .7; + + double cosMultiplier = 0.7; double steerOutput = 0; double deadzone = .1; + + if (m_pneumatics.m_isSpeedShiftHigh) { + cosMultiplier = 0.7; + } else { + cosMultiplier = 0.7; + } + /* Curves the steer output to be similarily gradual */ if (steer > 0) { steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steer) + cosMultiplier; @@ -107,8 +122,13 @@ public class DriveWithJoystickDriveStraight extends CommandBase { */ private void resetGyroTarget() { //m_targetGyro = m_currentGyro; - m_targetGyro = m_currentGyro - + m_drive.getTurnRate(); + if (m_pneumatics.m_isSpeedShiftHigh) { + m_targetGyro = m_currentGyro + + highGearMultiplier * m_drive.getTurnRate(); + } else { + m_targetGyro = m_currentGyro + + lowGearMultiplier * m_drive.getTurnRate(); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 059d3c6..599e5e7 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -53,7 +53,7 @@ public class Drive extends SubsystemBase { public Orchestra m_orchestra; /* Pneumatics Subsystem */ - Pneumatics m_pneumaticsSubsystem; + public Pneumatics m_pneumaticsSubsystem; /* Low Gear Gains */ public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW; From 85e0e798a22e9261e4ba1890225a79c47cc69c2a Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 2 Mar 2020 17:12:20 -0700 Subject: [PATCH 03/21] h --- src/main/java/frc4388/robot/Constants.java | 2 ++ .../robot/commands/DriveWithJoystickDriveStraight.java | 6 +++--- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bf12a83..acc92c8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -42,6 +42,8 @@ public final class Constants { public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(false, 60, 40, 2); public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops + public static final double COS_MULTIPLIER_LOW_DRIVE_STRAIGHT = 0.7; + public static final double COS_MULTIPLIER_HIGH_DRIVE_STRAIGHT = 0.7; /* Drive Train Characteristics */ public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13; diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java index 8de8d2f..d6a3390 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java @@ -92,14 +92,14 @@ public class DriveWithJoystickDriveStraight extends CommandBase { private void runDriveWithInput(double move, double steer) { - double cosMultiplier = 0.7; + double cosMultiplier; double steerOutput = 0; double deadzone = .1; if (m_pneumatics.m_isSpeedShiftHigh) { - cosMultiplier = 0.7; + cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH_DRIVE_STRAIGHT; } else { - cosMultiplier = 0.7; + cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW_DRIVE_STRAIGHT; } /* Curves the steer output to be similarily gradual */ From 8c23043afb20b4296b57d6f5e44c946f1ef35e79 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 2 Mar 2020 17:34:10 -0700 Subject: [PATCH 04/21] Made Drive WIth Hoytstjcj sdefautl fcommd --- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../java/frc4388/robot/commands/DriveWithJoystick.java | 7 ++++--- .../robot/commands/DriveWithJoystickDriveStraight.java | 4 ++-- 4 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index acc92c8..ee7a165 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -42,8 +42,8 @@ public final class Constants { public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(false, 60, 40, 2); public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops - public static final double COS_MULTIPLIER_LOW_DRIVE_STRAIGHT = 0.7; - public static final double COS_MULTIPLIER_HIGH_DRIVE_STRAIGHT = 0.7; + public static final double COS_MULTIPLIER_LOW = 1.0; + public static final double COS_MULTIPLIER_HIGH = 0.8; /* Drive Train Characteristics */ public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b488752..4793fc2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -127,7 +127,7 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller - m_robotDrive.setDefaultCommand(new DriveWithJoystickDriveStraight(m_robotDrive, getDriverController())); + m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController())); //m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, getDriverController())); // drives intake with input from triggers on the opperator controller diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 3c6e88f..e09140e 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -9,6 +9,7 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Pneumatics; import frc4388.utility.controller.IHandController; @@ -52,13 +53,13 @@ public class DriveWithJoystick extends CommandBase { moveOutput = Math.cos(1.571*moveInput)-1; } - double cosMultiplier = 1.0; + double cosMultiplier; double deadzone = .1; if (m_pneumatics.m_isSpeedShiftHigh) { - cosMultiplier = 0.8; + cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH; } else { - cosMultiplier = 1.0; + cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW; } if (steerInput > 0){ diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java index d6a3390..c69d4e9 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java @@ -97,9 +97,9 @@ public class DriveWithJoystickDriveStraight extends CommandBase { double deadzone = .1; if (m_pneumatics.m_isSpeedShiftHigh) { - cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH_DRIVE_STRAIGHT; + cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH; } else { - cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW_DRIVE_STRAIGHT; + cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW; } /* Curves the steer output to be similarily gradual */ From 923c04bf34a7be207f0bd5c51a5c120cedf082e4 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 2 Mar 2020 18:04:08 -0700 Subject: [PATCH 05/21] Fixes of intake and beams (untested) --- src/main/java/frc4388/robot/Constants.java | 10 ++--- .../java/frc4388/robot/RobotContainer.java | 34 ++++++++++------ .../robot/commands/RunExtenderOutIn.java | 6 +-- .../robot/commands/RunIntakeWithTriggers.java | 12 ++---- .../frc4388/robot/commands/StorageIntake.java | 4 +- .../robot/commands/StorageIntakeFinal.java | 40 +++++++++++++++++++ .../robot/commands/StoragePrepIntake.java | 4 +- .../java/frc4388/robot/subsystems/Intake.java | 19 +++++++-- .../frc4388/robot/subsystems/Storage.java | 10 ++--- 9 files changed, 95 insertions(+), 44 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/StorageIntakeFinal.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e09b86a..75703cc 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -159,12 +159,10 @@ public final class Constants { public static final double STORAGE_TIMEOUT = 2000; /* Ball Indexes */ - public static final int BEAM_SENSOR_DIO_0 = 0; - public static final int BEAM_SENSOR_DIO_1 = 1; - public static final int BEAM_SENSOR_DIO_2 = 2; - public static final int BEAM_SENSOR_DIO_3 = 3; - public static final int BEAM_SENSOR_DIO_4 = 4; - public static final int BEAM_SENSOR_DIO_5 = 5; + public static final int BEAM_SENSOR_SHOOTER = 1; + public static final int BEAM_SENSOR_USELESS = 2; + public static final int BEAM_SENSOR_STORAGE = 3; + public static final int BEAM_SENSOR_INTAKE = 4; /* PID Values */ public static final int SLOT_DISTANCE = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 718abcb..3afc1a3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -136,7 +136,8 @@ public class RobotContainer { m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); + // runs the storage not + m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); } @@ -188,8 +189,8 @@ public class RobotContainer { // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whenPressed(new RunExtenderOutIn(m_robotIntake)); - + //.whenPressed(new RunExtenderOutIn(m_robotIntake)); + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.3))); // safety for climber and leveler new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber)) @@ -197,27 +198,34 @@ public class RobotContainer { // starts tracking target new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) - .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); + //.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) + //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.3))); //Prepares storage for intaking - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) - .whileHeld(new StorageIntake(m_robotIntake, m_robotStorage)); + //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) + // .whileHeld(new StorageIntake(m_robotIntake, m_robotStorage)); //Runs storage to outtake - new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) - .whileHeld(new StorageOutake(m_robotStorage)); + //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) + // .whileHeld(new StorageOutake(m_robotStorage)); //TEST FOR HOOD new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.3))) - .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0))); + //.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.3))) + //.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0))); + .whileHeld(new RunCommand(() -> m_robotIntake.runIntakeOut(0.1))) + .whenReleased(new RunCommand(() -> m_robotIntake.runIntakeOut(0.0))); + //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.3))); //TEST FOR HOOD new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.3))) - .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0))); + //.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.3))) + //.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0))); + .whileHeld(new RunCommand(() -> m_robotIntake.runIntakeIn(0.1))) + .whenReleased(new RunCommand(() -> m_robotIntake.runIntakeIn(0.0))); + //Trims shooter new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS) diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java index 3c062a0..64139a0 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -38,9 +38,9 @@ public class RunExtenderOutIn extends CommandBase { 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); + m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_extenderForwardLimit.enableLimitSwitch(true); + m_extenderReverseLimit.enableLimitSwitch(true); } // Called when the command is initially scheduled. diff --git a/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java b/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java index 5f9dc4a..b9f6298 100644 --- a/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java +++ b/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java @@ -41,16 +41,12 @@ public class RunIntakeWithTriggers extends CommandBase { double rightTrigger = m_controller.getRightTriggerAxis(); double leftTrigger = m_controller.getLeftTriggerAxis(); double output = 0; - if (leftTrigger < .5) { - if(leftTrigger > rightTrigger) { - output = leftTrigger; - } - if (rightTrigger > leftTrigger) { - output = -leftTrigger; - } - } else { + if(leftTrigger >= rightTrigger) { output = leftTrigger; } + if (rightTrigger > leftTrigger) { + output = -rightTrigger; + } m_intake.runIntake(output); } diff --git a/src/main/java/frc4388/robot/commands/StorageIntake.java b/src/main/java/frc4388/robot/commands/StorageIntake.java index baf1630..2a1123a 100644 --- a/src/main/java/frc4388/robot/commands/StorageIntake.java +++ b/src/main/java/frc4388/robot/commands/StorageIntake.java @@ -39,7 +39,7 @@ public class StorageIntake extends CommandBase { @Override public void execute() { - if (!m_storage.getBeam(0)){ + if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){ m_storage.runStorage(StorageConstants.STORAGE_SPEED); intook = true; } @@ -56,7 +56,7 @@ public class StorageIntake extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (m_storage.getBeam(0) && intook){ + if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && intook){ return true; } return false; 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..2a7d10a --- /dev/null +++ b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +public class StorageIntakeFinal extends CommandBase { + /** + * Creates a new StorageIntakeFinal. + */ + public StorageIntakeFinal() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // 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() { + } + + // 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/StoragePrepIntake.java b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java index 44a2d2e..2fa52e1 100644 --- a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java +++ b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java @@ -38,7 +38,7 @@ public class StoragePrepIntake extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (m_storage.getBeam(0)){ + if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE)){ m_storage.runStorage(-StorageConstants.STORAGE_SPEED); } else{ @@ -54,7 +54,7 @@ public class StoragePrepIntake extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (!m_storage.getBeam(0) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){ + if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){ return true; } return false; diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index f074b33..1de90f4 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -14,6 +14,7 @@ import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.IntakeConstants; @@ -38,10 +39,10 @@ public class Intake extends SubsystemBase { m_intakeMotor.setIdleMode(IdleMode.kCoast); m_extenderMotor.setIdleMode(IdleMode.kBrake); m_intakeMotor.setInverted(false); - m_extenderMotor.setInverted(false); + m_extenderMotor.setInverted(true); - m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed); - m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); m_extenderForwardLimit.enableLimitSwitch(true); m_extenderReverseLimit.enableLimitSwitch(true); } @@ -58,7 +59,14 @@ public class Intake extends SubsystemBase { public void runIntake(double input) { m_intakeMotor.set(input); } + + public void runIntakeIn(double input){ + m_extenderMotor.set(-input); + } + public void runIntakeOut(double input){ + m_extenderMotor.set(input); + } /** * Runs extender motor * @param input the percent output to run motor at @@ -67,9 +75,12 @@ public class Intake extends SubsystemBase { if (m_extenderForwardLimit.get()) { isExtended = true; } - if (m_extenderReverseLimit.get()) { + else if (m_extenderReverseLimit.get()) { isExtended = false; } + else{ + m_extenderMotor.set(-input); + } if (isExtended == false) { 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 7bcc411..604176c 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -43,12 +43,10 @@ public class Storage extends SubsystemBase { */ public Storage() { resetEncoder(); - 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); - m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_3); - m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_4); - m_beamSensors[5] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_5); + m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); + m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_USELESS); + m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE); + m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); } @Override From ddf18c3a9394aa8846b04ce47163a17804c8cb4c Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 2 Mar 2020 21:03:39 -0700 Subject: [PATCH 06/21] attempt at fix all notes: failed --- src/main/deploy/Robot Data - Angles.csv | 5 +- src/main/deploy/Robot Data - Distances.csv | 8 ++- src/main/java/frc4388/robot/Constants.java | 8 +-- .../java/frc4388/robot/RobotContainer.java | 45 ++++++++-------- .../frc4388/robot/commands/HoldTarget.java | 54 ++++++++++--------- .../robot/commands/HoodPositionPID.java | 6 +-- .../robot/commands/RunExtenderOutIn.java | 5 -- .../robot/commands/ShootFireGroup.java | 2 +- .../robot/commands/ShootPrepGroup.java | 8 +-- .../commands/ShooterVelocityControlPID.java | 2 +- .../frc4388/robot/commands/TrackTarget.java | 16 +++--- .../java/frc4388/robot/subsystems/Drive.java | 2 +- .../java/frc4388/robot/subsystems/Intake.java | 10 ++-- .../frc4388/robot/subsystems/Shooter.java | 34 ++++++------ 14 files changed, 100 insertions(+), 105 deletions(-) diff --git a/src/main/deploy/Robot Data - Angles.csv b/src/main/deploy/Robot Data - Angles.csv index 273d4ba..5f3965e 100644 --- a/src/main/deploy/Robot Data - Angles.csv +++ b/src/main/deploy/Robot Data - Angles.csv @@ -1,6 +1,3 @@ Angle (deg),Displacement (deg) --20,-5 --10,-2 0,0 -10,2 -20,5 \ No newline at end of file +0,0 \ No newline at end of file diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv index e058b92..5ff8a86 100644 --- a/src/main/deploy/Robot Data - Distances.csv +++ b/src/main/deploy/Robot Data - Distances.csv @@ -1,6 +1,4 @@ Distance (in),Hood Ext. (u),Drum Velocity (u/ds) -21,10,10000 -100,23,11000 -200,30,14000 -300,56,17000 -480,100,20000 \ No newline at end of file +21,3,10000 +262,30,15000 +492,30,15000 diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index fe6bedf..3685fd5 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -121,8 +121,8 @@ public final class Constants { public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final int SHOOTER_TIMEOUT_MS = 30; - //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); - public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.2, 0.0, 0, 0.0453, 0, 1.0); + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); + //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0.0, 0.05, 0, 1.0); public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 1.0); public static final double SHOOTER_TURRET_MIN = -1.0; @@ -139,7 +139,7 @@ public final class Constants { public static final int HOOD_DOWN_SOFT_LIMIT = 3; public static final double HOOD_CONVERT_SLOPE = 0.47; public static final double HOOD_CONVERT_B = 40.5; - public static final double HOOD_CALIBRATE_SPEED = 0.1; + public static final double HOOD_CALIBRATE_SPEED = 0.2; public static final double DRUM_RAMP_LIMIT = 1000; public static final double DRUM_VELOCITY_BOUND = 300; @@ -194,7 +194,7 @@ public final class Constants { public static final class VisionConstants { public static final double FOV = 29.8; //Field of view of limelight - public static final double TARGET_HEIGHT = 64; + public static final double TARGET_HEIGHT = 71; public static final double LIME_ANGLE = 25; public static final double TURN_P_VALUE = 0.65; public static final double X_ANGLE_ERROR = 1.3; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6c88f5c..08a5321 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -181,22 +181,31 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); + + /* Operator Buttons */ // shoots until released new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false) - .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + //.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false) + //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8))); // shoots one ball new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false) - .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + //.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false) + //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8))); // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - //.whenPressed(new RunExtenderOutIn(m_robotIntake)); - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.3))); + .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))) + .whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0))); + + new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))) + .whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0))); + // safety for climber and leveler new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber)) @@ -206,9 +215,9 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) //.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.3))); - - + //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); + .whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) + .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30))); //Prepares storage for intaking //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) // .whileHeld(new StorageIntake(m_robotIntake, m_robotStorage)); @@ -217,21 +226,11 @@ public class RobotContainer { //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) // .whileHeld(new StorageOutake(m_robotStorage)); - //TEST FOR HOOD - new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - //.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.3))) - //.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0))); - .whileHeld(new RunCommand(() -> m_robotIntake.runIntakeOut(0.1))) - .whenReleased(new RunCommand(() -> m_robotIntake.runIntakeOut(0.0))); - //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.3))); - - //TEST FOR HOOD + //Run drum new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) - //.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.3))) - //.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0))); - .whileHeld(new RunCommand(() -> m_robotIntake.runIntakeIn(0.1))) - .whenReleased(new RunCommand(() -> m_robotIntake.runIntakeIn(0.0))); - + //.whenPressed(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)); + .whenPressed(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); + //.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); //Trims shooter new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS) diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java index cbdee25..fa39663 100644 --- a/src/main/java/frc4388/robot/commands/HoldTarget.java +++ b/src/main/java/frc4388/robot/commands/HoldTarget.java @@ -13,6 +13,7 @@ import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.LimeLight; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.utility.ShooterTables; import frc4388.utility.controller.IHandController; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.networktables.NetworkTable; @@ -56,7 +57,7 @@ public class HoldTarget extends CommandBase { //Vision Processing Mode NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); - } + } @@ -67,39 +68,44 @@ public class HoldTarget extends CommandBase { xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); - if (target == 1.0){ //If target in view - //Aiming Left/Right - turnAmount = (xAngle/VisionConstants.FOV)*VisionConstants.TURN_P_VALUE; - if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR){turnAmount = 0;} //Angle Error Zone - //Deadzones - else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;} - else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;} + if (target == 1.0) { // If target in view + // Aiming Left/Right + turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE; + if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) { + turnAmount = 0; + } // Angle Error Zone + // Deadzones + else if (turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE) { + turnAmount = VisionConstants.MOTOR_DEAD_ZONE; + } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) { + turnAmount = -VisionConstants.MOTOR_DEAD_ZONE; + } m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim); - //Finding Distance - distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180)); + // 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); + //START Equation Code + /* + 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) * (180/Math.PI); - m_shooter.m_fireVel = fireVel; - m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim; + */ + //END Equation Code + + //START CSV Code + fireVel = m_shooter.m_shooterTable.getVelocity(distance); + fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different + //fireAngle = 33; + //END CSV Code - }/* - else{ - System.err.println("Shooter Pos: " + m_shooterAim.getShooterRotatePosition()); - double curveInput = -Math.abs(-Math.cos(Math.PI * ((2*m_shooterAim.getShooterRotatePosition())/55))+1) * 0.1; - if (m_shooterAim.getShooterRotatePosition() >= -3 || m_shooterAim.getShooterRotatePosition() <= -54){ - curveInput = -curveInput; - } - System.err.println("Curve Input: " + curveInput); - m_shooterAim.runShooterWithInput(curveInput); + m_shooter.m_fireVel = fireVel; + m_shooter.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; } - */ } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/HoodPositionPID.java index efecb58..b48a3c4 100644 --- a/src/main/java/frc4388/robot/commands/HoodPositionPID.java +++ b/src/main/java/frc4388/robot/commands/HoodPositionPID.java @@ -31,9 +31,9 @@ public class HoodPositionPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double slope = ShooterConstants.HOOD_CONVERT_SLOPE; + /*double slope = ShooterConstants.HOOD_CONVERT_SLOPE; double b = ShooterConstants.HOOD_CONVERT_B; - firingAngle = (-slope*m_shooter.addFireAngle())+b; + firingAngle = (-slope*m_shooter.addFireAngle())+b;*/ SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle()); SmartDashboard.putNumber("Fire Angle", firingAngle); m_shooter.runAngleAdjustPID(firingAngle); @@ -49,7 +49,7 @@ public class HoodPositionPID extends CommandBase { public boolean isFinished() { double encoderPos = m_shooter.m_angleAdjustMotor.getEncoder().getPosition(); if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){ - return true; + return false; } return false; } diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java index 64139a0..37d7ae1 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -36,11 +36,6 @@ 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.kNormallyOpen); - m_extenderForwardLimit.enableLimitSwitch(true); - m_extenderReverseLimit.enableLimitSwitch(true); } // Called when the command is initially scheduled. diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java index 7d23c65..59bcf9c 100644 --- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java +++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java @@ -25,7 +25,7 @@ public class ShootFireGroup extends ParallelRaceGroup { */ public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) { addCommands( - new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())), + new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())), new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())), new HoldTarget(m_shooter, m_shooterAim), new StorageRun(m_storage) diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java index 5c20683..d234d7a 100644 --- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java @@ -25,10 +25,10 @@ public class ShootPrepGroup extends ParallelCommandGroup { */ public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) { addCommands( - new TrackTarget(m_shooter, m_shooterAim), - new ShooterVelocityControlPID(m_shooter), - new StoragePrepAim(m_storage), - new HoodPositionPID(m_shooter) + //new TrackTarget(m_shooter, m_shooterAim), + //new ShooterVelocityControlPID(m_shooter) + new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())) + //new StoragePrepAim(m_storage) ); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index 52dea7f..9dbd4f4 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -34,7 +34,7 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity()); + m_shooter.runDrumShooterVelocityPID(/*m_shooter.addFireVel()*/13000); m_shooter.runAngleAdjustPID(m_shooter.addFireAngle()); SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel()); SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle()); diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 8bdae59..197dc72 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -43,8 +43,6 @@ public class TrackTarget extends CommandBase { public double m_hoodTrim; public double m_turretTrim; - ShooterTables m_shooterTable; - /** * Uses the Limelight to track the target * @param shooterSubsystem The Shooter subsystem @@ -63,7 +61,6 @@ public class TrackTarget extends CommandBase { // Vision Processing Mode NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); - m_shooterTable = new ShooterTables(); } // Called every time the scheduler runs while the command is scheduled. @@ -92,21 +89,24 @@ public class TrackTarget extends CommandBase { SmartDashboard.putNumber("Distance to Target", distance); //START Equation Code + /* 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) * (180/Math.PI); + */ //END Equation Code - /*//START CSV Code - fireVel = m_shooterTable.getVelocity(distance); - fireAngle = m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different - //END CSV Code*/ + //START CSV Code + fireVel = m_shooter.m_shooterTable.getVelocity(distance); + fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different + //fireAngle = 33; + //END CSV Code m_shooter.m_fireVel = fireVel; - m_shooter.m_fireAngle = fireAngle + m_shooter.shooterTrims.m_hoodTrim; + m_shooter.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; } } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 599e5e7..2d02a42 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -786,7 +786,7 @@ public class Drive extends SubsystemBase { //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_deltaTimeMs); if (m_currentSong != m_songChooser.getSelected()){ diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 1de90f4..11507ef 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -60,18 +60,14 @@ public class Intake extends SubsystemBase { m_intakeMotor.set(input); } - public void runIntakeIn(double input){ - m_extenderMotor.set(-input); - } - - public void runIntakeOut(double input){ + public void runExtender(double input){ m_extenderMotor.set(input); } /** * Runs extender motor * @param input the percent output to run motor at */ - public void runExtender(double input) { + /*public void runExtender(double input) { if (m_extenderForwardLimit.get()) { isExtended = true; } @@ -88,5 +84,5 @@ public class Intake extends SubsystemBase { if (isExtended == true) { m_extenderMotor.set(-input); } - } + }*/ } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index cd0dda3..ae10d75 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -47,7 +47,7 @@ public class Shooter extends SubsystemBase { double velP; double input; - ShooterTables m_shooterTable; + public ShooterTables m_shooterTable; public boolean velReached; public double m_fireVel; @@ -69,6 +69,7 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.setInverted(true); m_angleAdjustMotor.setIdleMode(IdleMode.kBrake); m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.configClosedloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); setShooterGains(); m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); @@ -77,7 +78,6 @@ public class Shooter extends SubsystemBase { int closedLoopTimeMs = 1; m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterTable = new ShooterTables(); @@ -91,8 +91,6 @@ public class Shooter extends SubsystemBase { SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5)); SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30)); - - m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); m_hoodUpLimit.enableLimitSwitch(true); @@ -108,7 +106,19 @@ public class Shooter extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV); + try{ + SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity()); + + SmartDashboard.putNumber("Fire Velocity From CSV", m_fireVel); + SmartDashboard.putNumber("Fire Angle From CSV", m_fireAngle); + + //SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition()); + } + + catch(Exception e) + { + + } } public double addFireVel() { @@ -157,15 +167,9 @@ public class Shooter extends SubsystemBase { * @param falcon Motor to use * @param targetVel Target velocity to run motor at */ - public void runDrumShooterVelocityPID(double targetVel, double actualVel) { - SmartDashboard.putNumber("shoot", actualVel); - if (actualVel < targetVel - ShooterConstants.DRUM_RAMP_LIMIT){ - m_shooterFalcon.set(TalonFXControlMode.PercentOutput, 1);//Ramp up - } - else{ //PID Based on targetVel - m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID - } - m_shooterFalcon.feed(); + public void runDrumShooterVelocityPID(double targetVel) { + System.out.println("dddddddddddddddddddddddd" + targetVel); + m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } public void resetGyroAngleAdj(){ @@ -173,6 +177,6 @@ public class Shooter extends SubsystemBase { } public double getAnglePosition(){ - return m_angleAdjustMotor.getEncoder().getPosition(); + return m_angleEncoder.getPosition(); } } \ No newline at end of file From d4a18c9c57fa4625f763e35e332bf3b655fe28b7 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 2 Mar 2020 21:23:33 -0700 Subject: [PATCH 07/21] Move Gains and Trims to Utilities --- src/main/java/frc4388/robot/Constants.java | 1 + .../frc4388/robot/commands/TrimShooter.java | 6 +----- .../java/frc4388/robot/subsystems/Drive.java | 5 +---- .../java/frc4388/robot/subsystems/Shooter.java | 17 +++++++---------- .../frc4388/robot/subsystems/ShooterAim.java | 8 +++----- .../java/frc4388/robot/subsystems/Storage.java | 12 +++--------- .../java/frc4388/{robot => utility}/Gains.java | 2 +- .../java/frc4388/{robot => utility}/Trims.java | 2 +- 8 files changed, 18 insertions(+), 35 deletions(-) rename src/main/java/frc4388/{robot => utility}/Gains.java (98%) rename src/main/java/frc4388/{robot => utility}/Trims.java (96%) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3685fd5..2dd226d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -10,6 +10,7 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; /** diff --git a/src/main/java/frc4388/robot/commands/TrimShooter.java b/src/main/java/frc4388/robot/commands/TrimShooter.java index b77a4ba..84daecd 100644 --- a/src/main/java/frc4388/robot/commands/TrimShooter.java +++ b/src/main/java/frc4388/robot/commands/TrimShooter.java @@ -7,14 +7,10 @@ package frc4388.robot.commands; -import edu.wpi.first.wpilibj.Joystick; -import frc4388.utility.controller.XboxController; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.RobotContainer; -import frc4388.robot.Trims; import frc4388.robot.Constants.OIConstants; import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; +import frc4388.utility.controller.XboxController; public class TrimShooter extends CommandBase { private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 2d02a42..300359d 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -24,7 +24,6 @@ import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; @@ -34,10 +33,8 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; - import frc4388.robot.Constants.DriveConstants; -import frc4388.robot.Constants.PneumaticsConstants; -import frc4388.robot.Gains; +import frc4388.utility.Gains; public class Drive extends SubsystemBase { /* Create Motors, Gyros, etc */ diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index ae10d75..b4f1403 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -10,25 +10,22 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; - +import com.revrobotics.CANDigitalInput; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANEncoder; +import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax.SoftLimitDirection; -import com.revrobotics.CANDigitalInput; -import com.revrobotics.CANEncoder; -import com.revrobotics.CANPIDController; -import com.revrobotics.ControlType; -import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.ControlType; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Gains; -import frc4388.robot.Trims; import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.Gains; import frc4388.utility.ShooterTables; +import frc4388.utility.Trims; import frc4388.utility.controller.IHandController; public class Shooter extends SubsystemBase { diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index 1111749..07b04f5 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -8,20 +8,18 @@ package frc4388.robot.subsystems; import com.revrobotics.CANDigitalInput; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import com.revrobotics.CANEncoder; import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; -import com.revrobotics.ControlType; -import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.ControlType; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Gains; import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.Gains; public class ShooterAim extends SubsystemBase { public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 604176c..a014544 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -7,23 +7,17 @@ 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 com.revrobotics.ControlType; + import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.command.WaitUntilCommand; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Gains; import frc4388.robot.Constants.StorageConstants; +import frc4388.utility.Gains; public class Storage extends SubsystemBase { public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); diff --git a/src/main/java/frc4388/robot/Gains.java b/src/main/java/frc4388/utility/Gains.java similarity index 98% rename from src/main/java/frc4388/robot/Gains.java rename to src/main/java/frc4388/utility/Gains.java index 7d2b3d7..f1b78f9 100644 --- a/src/main/java/frc4388/robot/Gains.java +++ b/src/main/java/frc4388/utility/Gains.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot; +package frc4388.utility; /** * Add your docs here. diff --git a/src/main/java/frc4388/robot/Trims.java b/src/main/java/frc4388/utility/Trims.java similarity index 96% rename from src/main/java/frc4388/robot/Trims.java rename to src/main/java/frc4388/utility/Trims.java index 859794f..b97791d 100644 --- a/src/main/java/frc4388/robot/Trims.java +++ b/src/main/java/frc4388/utility/Trims.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot; +package frc4388.utility; public class Trims{ public double m_turretTrim; From 08d6a3605aa3d9f02ccbddfcd63a65d436984d5c Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 2 Mar 2020 21:45:38 -0700 Subject: [PATCH 08/21] Sort Commands and Organise Imports --- .../java/frc4388/robot/RobotContainer.java | 73 +++++-------------- .../{ => auto}/AutoPath1FromCenter.java | 3 +- .../{ => auto}/AutoPath2FromRight.java | 3 +- .../robot/commands/{ => auto}/Wait.java | 2 +- .../{ => climber}/RunClimberWithTriggers.java | 2 +- .../{ => climber}/RunLevelerWithJoystick.java | 2 +- .../{ => drive}/DrivePositionMPAux.java | 2 +- .../DriveStraightAtVelocityPID.java | 3 +- .../DriveStraightToPositionMM.java | 2 +- .../DriveStraightToPositionPID.java | 3 +- .../{ => drive}/DriveWithJoystick.java | 3 +- .../{ => drive}/DriveWithJoystickAuxPID.java | 4 +- .../DriveWithJoystickDriveStraight.java | 5 +- .../DriveWithJoystickUsingDeadAssistPID.java | 3 +- .../commands/{ => drive}/GotoCoordinates.java | 4 +- .../commands/{ => drive}/PlaySongDrive.java | 2 +- .../commands/{ => drive}/TurnDegrees.java | 2 +- .../{ => intake}/RunExtenderOutIn.java | 4 +- .../{ => intake}/RunIntakeWithTriggers.java | 2 +- .../{ => shooter}/CalibrateShooter.java | 3 +- .../commands/{ => shooter}/HoldTarget.java | 17 ++--- .../{ => shooter}/HoodPositionPID.java | 3 +- .../{ => shooter}/ShootFireGroup.java | 3 +- .../{ => shooter}/ShootFullGroup.java | 2 +- .../{ => shooter}/ShootPrepGroup.java | 2 +- .../ShooterVelocityControlPID.java | 2 +- .../commands/{ => shooter}/TrackTarget.java | 21 ++---- .../commands/{ => shooter}/TrimShooter.java | 2 +- .../commands/{ => storage}/StorageIntake.java | 3 +- .../{ => storage}/StorageIntakeFinal.java | 2 +- .../commands/{ => storage}/StorageOutake.java | 2 +- .../{ => storage}/StoragePositionPID.java | 2 +- .../{ => storage}/StoragePrepAim.java | 2 +- .../{ => storage}/StoragePrepIntake.java | 2 +- .../commands/{ => storage}/StorageRun.java | 2 +- .../java/frc4388/robot/subsystems/Camera.java | 2 - .../java/frc4388/robot/subsystems/Intake.java | 4 +- .../frc4388/robot/subsystems/Leveler.java | 5 -- .../frc4388/robot/subsystems/Pneumatics.java | 2 - 39 files changed, 69 insertions(+), 138 deletions(-) rename src/main/java/frc4388/robot/commands/{ => auto}/AutoPath1FromCenter.java (95%) rename src/main/java/frc4388/robot/commands/{ => auto}/AutoPath2FromRight.java (95%) rename src/main/java/frc4388/robot/commands/{ => auto}/Wait.java (98%) rename src/main/java/frc4388/robot/commands/{ => climber}/RunClimberWithTriggers.java (97%) rename src/main/java/frc4388/robot/commands/{ => climber}/RunLevelerWithJoystick.java (98%) rename src/main/java/frc4388/robot/commands/{ => drive}/DrivePositionMPAux.java (98%) rename src/main/java/frc4388/robot/commands/{ => drive}/DriveStraightAtVelocityPID.java (97%) rename src/main/java/frc4388/robot/commands/{ => drive}/DriveStraightToPositionMM.java (98%) rename src/main/java/frc4388/robot/commands/{ => drive}/DriveStraightToPositionPID.java (97%) rename src/main/java/frc4388/robot/commands/{ => drive}/DriveWithJoystick.java (97%) rename src/main/java/frc4388/robot/commands/{ => drive}/DriveWithJoystickAuxPID.java (96%) rename src/main/java/frc4388/robot/commands/{ => drive}/DriveWithJoystickDriveStraight.java (97%) rename src/main/java/frc4388/robot/commands/{ => drive}/DriveWithJoystickUsingDeadAssistPID.java (98%) rename src/main/java/frc4388/robot/commands/{ => drive}/GotoCoordinates.java (96%) rename src/main/java/frc4388/robot/commands/{ => drive}/PlaySongDrive.java (97%) rename src/main/java/frc4388/robot/commands/{ => drive}/TurnDegrees.java (98%) rename src/main/java/frc4388/robot/commands/{ => intake}/RunExtenderOutIn.java (94%) rename src/main/java/frc4388/robot/commands/{ => intake}/RunIntakeWithTriggers.java (98%) rename src/main/java/frc4388/robot/commands/{ => shooter}/CalibrateShooter.java (96%) rename src/main/java/frc4388/robot/commands/{ => shooter}/HoldTarget.java (94%) rename src/main/java/frc4388/robot/commands/{ => shooter}/HoodPositionPID.java (95%) rename src/main/java/frc4388/robot/commands/{ => shooter}/ShootFireGroup.java (94%) rename src/main/java/frc4388/robot/commands/{ => shooter}/ShootFullGroup.java (97%) rename src/main/java/frc4388/robot/commands/{ => shooter}/ShootPrepGroup.java (97%) rename src/main/java/frc4388/robot/commands/{ => shooter}/ShooterVelocityControlPID.java (98%) rename src/main/java/frc4388/robot/commands/{ => shooter}/TrackTarget.java (91%) rename src/main/java/frc4388/robot/commands/{ => shooter}/TrimShooter.java (97%) rename src/main/java/frc4388/robot/commands/{ => storage}/StorageIntake.java (96%) rename src/main/java/frc4388/robot/commands/{ => storage}/StorageIntakeFinal.java (96%) rename src/main/java/frc4388/robot/commands/{ => storage}/StorageOutake.java (97%) rename src/main/java/frc4388/robot/commands/{ => storage}/StoragePositionPID.java (97%) rename src/main/java/frc4388/robot/commands/{ => storage}/StoragePrepAim.java (97%) rename src/main/java/frc4388/robot/commands/{ => storage}/StoragePrepIntake.java (97%) rename src/main/java/frc4388/robot/commands/{ => storage}/StorageRun.java (97%) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 08a5321..f9bba13 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -11,76 +11,43 @@ import java.util.List; import com.ctre.phoenix.motorcontrol.NeutralMode; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.trajectory.Trajectory; import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; - -import frc4388.robot.Constants.*; -import frc4388.robot.commands.AutoPath1FromCenter; -import frc4388.robot.commands.AutoPath2FromRight; -import frc4388.robot.commands.CalibrateShooter; -import frc4388.robot.commands.DrivePositionMPAux; -import frc4388.robot.commands.DriveStraightAtVelocityPID; -import frc4388.robot.commands.DriveStraightToPositionMM; -import frc4388.robot.commands.DriveStraightToPositionPID; -import frc4388.robot.commands.DriveWithJoystick; -import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID; -import frc4388.robot.commands.DriveStraightToPositionMM; -import frc4388.robot.commands.DriveStraightToPositionPID; -import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID; -import frc4388.robot.commands.HoldTarget; -import frc4388.robot.commands.HoodPositionPID; -import frc4388.robot.commands.DriveWithJoystickDriveStraight; -import frc4388.robot.commands.RunClimberWithTriggers; -import frc4388.robot.commands.RunExtenderOutIn; -import frc4388.robot.commands.RunIntakeWithTriggers; -import frc4388.robot.commands.ShooterVelocityControlPID; -import frc4388.robot.commands.StorageIntake; -import frc4388.robot.commands.GotoCoordinates; -import frc4388.robot.commands.RunClimberWithTriggers; -import frc4388.robot.commands.RunExtenderOutIn; -import frc4388.robot.commands.RunIntakeWithTriggers; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.Climber; -import frc4388.robot.commands.RunLevelerWithJoystick; -import frc4388.robot.commands.ShootFireGroup; -import frc4388.robot.commands.ShootFullGroup; -import frc4388.robot.commands.ShootPrepGroup; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.LED; -import frc4388.robot.commands.TrackTarget; -import frc4388.robot.commands.TrimShooter; -import frc4388.robot.commands.StorageOutake; -import frc4388.robot.commands.StoragePrepAim; -import frc4388.robot.commands.StoragePrepIntake; -import frc4388.robot.commands.StorageRun; +import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.Constants.OIConstants; +import frc4388.robot.commands.auto.AutoPath1FromCenter; +import frc4388.robot.commands.auto.Wait; +import frc4388.robot.commands.climber.RunClimberWithTriggers; +import frc4388.robot.commands.climber.RunLevelerWithJoystick; +import frc4388.robot.commands.drive.DriveStraightToPositionMM; +import frc4388.robot.commands.drive.DriveWithJoystick; +import frc4388.robot.commands.drive.TurnDegrees; +import frc4388.robot.commands.intake.RunIntakeWithTriggers; +import frc4388.robot.commands.shooter.CalibrateShooter; +import frc4388.robot.commands.shooter.HoldTarget; +import frc4388.robot.commands.shooter.TrimShooter; import frc4388.robot.subsystems.Camera; +import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Leveler; import frc4388.robot.subsystems.LimeLight; -import frc4388.robot.commands.TurnDegrees; -import frc4388.robot.commands.Wait; -import frc4388.robot.commands.StorageOutake; -import frc4388.robot.subsystems.Camera; -import frc4388.robot.subsystems.Leveler; import frc4388.robot.subsystems.Pneumatics; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; import frc4388.robot.subsystems.Storage; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; diff --git a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java b/src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java similarity index 95% rename from src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java rename to src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java index 4ec080a..b749d57 100644 --- a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java +++ b/src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java @@ -5,9 +5,10 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.auto; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.commands.drive.GotoCoordinates; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Pneumatics; diff --git a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java similarity index 95% rename from src/main/java/frc4388/robot/commands/AutoPath2FromRight.java rename to src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java index 43657c3..19d0b7e 100644 --- a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java +++ b/src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java @@ -5,9 +5,10 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.auto; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.commands.drive.GotoCoordinates; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Pneumatics; diff --git a/src/main/java/frc4388/robot/commands/Wait.java b/src/main/java/frc4388/robot/commands/auto/Wait.java similarity index 98% rename from src/main/java/frc4388/robot/commands/Wait.java rename to src/main/java/frc4388/robot/commands/auto/Wait.java index 580d279..9280310 100644 --- a/src/main/java/frc4388/robot/commands/Wait.java +++ b/src/main/java/frc4388/robot/commands/auto/Wait.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.auto; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java b/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java similarity index 97% rename from src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java rename to src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java index 9ab4285..be72d49 100644 --- a/src/main/java/frc4388/robot/commands/RunClimberWithTriggers.java +++ b/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.climber; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Climber; diff --git a/src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java b/src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java similarity index 98% rename from src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java rename to src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java index 0b91068..7cf5b2f 100644 --- a/src/main/java/frc4388/robot/commands/RunLevelerWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.climber; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Leveler; diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/drive/DrivePositionMPAux.java similarity index 98% rename from src/main/java/frc4388/robot/commands/DrivePositionMPAux.java rename to src/main/java/frc4388/robot/commands/drive/DrivePositionMPAux.java index 68d8390..901fdcb 100644 --- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java +++ b/src/main/java/frc4388/robot/commands/drive/DrivePositionMPAux.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.DriveConstants; diff --git a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java b/src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java similarity index 97% rename from src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java rename to src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java index c79ccbc..e63a5bf 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java @@ -4,8 +4,7 @@ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ /*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands; +package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.DriveConstants; diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java similarity index 98% rename from src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java rename to src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java index c9aa927..f72a223 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java similarity index 97% rename from src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java rename to src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java index a457e26..337ff6d 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java @@ -5,9 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java similarity index 97% rename from src/main/java/frc4388/robot/commands/DriveWithJoystick.java rename to src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java index b7d823f..75ab279 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java @@ -5,9 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java similarity index 96% rename from src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java rename to src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java index 2531847..3a958a9 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickAuxPID.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java @@ -5,9 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; - -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpiutil.math.MathUtil; diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java similarity index 97% rename from src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java rename to src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java index c69d4e9..345b160 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java @@ -5,12 +5,9 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; - -import java.security.PublicKey; +package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpiutil.math.MathUtil; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Pneumatics; diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java similarity index 98% rename from src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java rename to src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java index 0b154e7..3b2ef73 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java @@ -5,9 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpiutil.math.MathUtil; import frc4388.robot.Constants.DriveConstants; diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java similarity index 96% rename from src/main/java/frc4388/robot/commands/GotoCoordinates.java rename to src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java index 6ca3deb..33de918 100644 --- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java +++ b/src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java @@ -5,10 +5,10 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.commands.auto.Wait; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Pneumatics; diff --git a/src/main/java/frc4388/robot/commands/PlaySongDrive.java b/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java similarity index 97% rename from src/main/java/frc4388/robot/commands/PlaySongDrive.java rename to src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java index 5e0e6b4..eaa0f07 100644 --- a/src/main/java/frc4388/robot/commands/PlaySongDrive.java +++ b/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Drive; diff --git a/src/main/java/frc4388/robot/commands/TurnDegrees.java b/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java similarity index 98% rename from src/main/java/frc4388/robot/commands/TurnDegrees.java rename to src/main/java/frc4388/robot/commands/drive/TurnDegrees.java index 83630dd..b9f7d35 100644 --- a/src/main/java/frc4388/robot/commands/TurnDegrees.java +++ b/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java similarity index 94% rename from src/main/java/frc4388/robot/commands/RunExtenderOutIn.java rename to src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java index 37d7ae1..c222ad6 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java @@ -5,17 +5,15 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.intake; 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; diff --git a/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java b/src/main/java/frc4388/robot/commands/intake/RunIntakeWithTriggers.java similarity index 98% rename from src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java rename to src/main/java/frc4388/robot/commands/intake/RunIntakeWithTriggers.java index b9f6298..7f90f63 100644 --- a/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java +++ b/src/main/java/frc4388/robot/commands/intake/RunIntakeWithTriggers.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.intake; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Intake; diff --git a/src/main/java/frc4388/robot/commands/CalibrateShooter.java b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java similarity index 96% rename from src/main/java/frc4388/robot/commands/CalibrateShooter.java rename to src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java index 1b766d7..b2a54ac 100644 --- a/src/main/java/frc4388/robot/commands/CalibrateShooter.java +++ b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java @@ -5,11 +5,10 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; import com.revrobotics.CANSparkMax.SoftLimitDirection; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.subsystems.Shooter; diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java similarity index 94% rename from src/main/java/frc4388/robot/commands/HoldTarget.java rename to src/main/java/frc4388/robot/commands/shooter/HoldTarget.java index fa39663..a91fb49 100644 --- a/src/main/java/frc4388/robot/commands/HoldTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java @@ -5,21 +5,16 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.LimeLight; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.utility.ShooterTables; -import frc4388.utility.controller.IHandController; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.VisionConstants; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; +import frc4388.utility.controller.IHandController; public class HoldTarget extends CommandBase { //Setup diff --git a/src/main/java/frc4388/robot/commands/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java similarity index 95% rename from src/main/java/frc4388/robot/commands/HoodPositionPID.java rename to src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java index b48a3c4..980c9ee 100644 --- a/src/main/java/frc4388/robot/commands/HoodPositionPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java @@ -5,11 +5,10 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.subsystems.Shooter; public class HoodPositionPID extends CommandBase { diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java similarity index 94% rename from src/main/java/frc4388/robot/commands/ShootFireGroup.java rename to src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java index 59bcf9c..2adcbda 100644 --- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java @@ -5,10 +5,11 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; +import frc4388.robot.commands.storage.StorageRun; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; import frc4388.robot.subsystems.Storage; diff --git a/src/main/java/frc4388/robot/commands/ShootFullGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java similarity index 97% rename from src/main/java/frc4388/robot/commands/ShootFullGroup.java rename to src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java index 55557b8..e124093 100644 --- a/src/main/java/frc4388/robot/commands/ShootFullGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.subsystems.Shooter; diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java similarity index 97% rename from src/main/java/frc4388/robot/commands/ShootPrepGroup.java rename to src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java index d234d7a..ef8714d 100644 --- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java similarity index 98% rename from src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java rename to src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java index 9dbd4f4..8411aeb 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java similarity index 91% rename from src/main/java/frc4388/robot/commands/TrackTarget.java rename to src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index 197dc72..857e003 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -5,25 +5,16 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.subsystems.Camera; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.LimeLight; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.commands.TrimShooter; -import frc4388.utility.ShooterTables; -import frc4388.utility.controller.IHandController; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.VisionConstants; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; +import frc4388.utility.controller.IHandController; public class TrackTarget extends CommandBase { // Setup diff --git a/src/main/java/frc4388/robot/commands/TrimShooter.java b/src/main/java/frc4388/robot/commands/shooter/TrimShooter.java similarity index 97% rename from src/main/java/frc4388/robot/commands/TrimShooter.java rename to src/main/java/frc4388/robot/commands/shooter/TrimShooter.java index 84daecd..2c673b8 100644 --- a/src/main/java/frc4388/robot/commands/TrimShooter.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrimShooter.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.OIConstants; diff --git a/src/main/java/frc4388/robot/commands/StorageIntake.java b/src/main/java/frc4388/robot/commands/storage/StorageIntake.java similarity index 96% rename from src/main/java/frc4388/robot/commands/StorageIntake.java rename to src/main/java/frc4388/robot/commands/storage/StorageIntake.java index 2a1123a..9ead11f 100644 --- a/src/main/java/frc4388/robot/commands/StorageIntake.java +++ b/src/main/java/frc4388/robot/commands/storage/StorageIntake.java @@ -5,10 +5,9 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.storage; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.Storage; diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java b/src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java similarity index 96% rename from src/main/java/frc4388/robot/commands/StorageIntakeFinal.java rename to src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java index 2a7d10a..0d8c447 100644 --- a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java +++ b/src/main/java/frc4388/robot/commands/storage/StorageIntakeFinal.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.storage; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/frc4388/robot/commands/StorageOutake.java b/src/main/java/frc4388/robot/commands/storage/StorageOutake.java similarity index 97% rename from src/main/java/frc4388/robot/commands/StorageOutake.java rename to src/main/java/frc4388/robot/commands/storage/StorageOutake.java index 3231600..658d8e5 100644 --- a/src/main/java/frc4388/robot/commands/StorageOutake.java +++ b/src/main/java/frc4388/robot/commands/storage/StorageOutake.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.storage; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.StorageConstants; diff --git a/src/main/java/frc4388/robot/commands/StoragePositionPID.java b/src/main/java/frc4388/robot/commands/storage/StoragePositionPID.java similarity index 97% rename from src/main/java/frc4388/robot/commands/StoragePositionPID.java rename to src/main/java/frc4388/robot/commands/storage/StoragePositionPID.java index bc607e5..2a27b41 100644 --- a/src/main/java/frc4388/robot/commands/StoragePositionPID.java +++ b/src/main/java/frc4388/robot/commands/storage/StoragePositionPID.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.storage; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.StorageConstants; diff --git a/src/main/java/frc4388/robot/commands/StoragePrepAim.java b/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java similarity index 97% rename from src/main/java/frc4388/robot/commands/StoragePrepAim.java rename to src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java index f0e52c1..268f39e 100644 --- a/src/main/java/frc4388/robot/commands/StoragePrepAim.java +++ b/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.storage; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java b/src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java similarity index 97% rename from src/main/java/frc4388/robot/commands/StoragePrepIntake.java rename to src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java index 2fa52e1..c935623 100644 --- a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java +++ b/src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.storage; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.StorageConstants; diff --git a/src/main/java/frc4388/robot/commands/StorageRun.java b/src/main/java/frc4388/robot/commands/storage/StorageRun.java similarity index 97% rename from src/main/java/frc4388/robot/commands/StorageRun.java rename to src/main/java/frc4388/robot/commands/storage/StorageRun.java index 91632f5..11dbc4b 100644 --- a/src/main/java/frc4388/robot/commands/StorageRun.java +++ b/src/main/java/frc4388/robot/commands/storage/StorageRun.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.storage; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.StorageConstants; diff --git a/src/main/java/frc4388/robot/subsystems/Camera.java b/src/main/java/frc4388/robot/subsystems/Camera.java index 332bb46..58bd8d1 100644 --- a/src/main/java/frc4388/robot/subsystems/Camera.java +++ b/src/main/java/frc4388/robot/subsystems/Camera.java @@ -7,11 +7,9 @@ package frc4388.robot.subsystems; -import edu.wpi.cscore.MjpegServer; import edu.wpi.cscore.UsbCamera; import edu.wpi.cscore.VideoSource; import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 11507ef..109b412 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -8,13 +8,11 @@ package frc4388.robot.subsystems; import com.revrobotics.CANDigitalInput; -import com.revrobotics.CANSparkMax; import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import edu.wpi.first.wpilibj.Spark; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.IntakeConstants; diff --git a/src/main/java/frc4388/robot/subsystems/Leveler.java b/src/main/java/frc4388/robot/subsystems/Leveler.java index 0db0562..d29c748 100644 --- a/src/main/java/frc4388/robot/subsystems/Leveler.java +++ b/src/main/java/frc4388/robot/subsystems/Leveler.java @@ -7,17 +7,12 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import edu.wpi.first.wpilibj.Talon; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.LevelerConstants; -import frc4388.robot.subsystems.*; public class Leveler extends SubsystemBase { CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless); diff --git a/src/main/java/frc4388/robot/subsystems/Pneumatics.java b/src/main/java/frc4388/robot/subsystems/Pneumatics.java index 1c373ed..6b7327b 100644 --- a/src/main/java/frc4388/robot/subsystems/Pneumatics.java +++ b/src/main/java/frc4388/robot/subsystems/Pneumatics.java @@ -8,9 +8,7 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.RobotContainer; import frc4388.robot.Constants.PneumaticsConstants; public class Pneumatics extends SubsystemBase { From 8d5563af7f0bd43824a5d0cd1128c3af9c7270d1 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 2 Mar 2020 21:56:57 -0700 Subject: [PATCH 09/21] Delete WPILibOldCommands.json --- vendordeps/WPILibOldCommands.json | 37 ------------------------------- 1 file changed, 37 deletions(-) delete mode 100644 vendordeps/WPILibOldCommands.json diff --git a/vendordeps/WPILibOldCommands.json b/vendordeps/WPILibOldCommands.json deleted file mode 100644 index acc8879..0000000 --- a/vendordeps/WPILibOldCommands.json +++ /dev/null @@ -1,37 +0,0 @@ -{ - "fileName": "WPILibOldCommands.json", - "name": "WPILib-Old-Commands", - "version": "2020.0.0", - "uuid": "b066afc2-5c18-43c4-b758-43381fcb275e", - "mavenUrls": [], - "jsonUrl": "", - "javaDependencies": [ - { - "groupId": "edu.wpi.first.wpilibOldCommands", - "artifactId": "wpilibOldCommands-java", - "version": "wpilib" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "edu.wpi.first.wpilibOldCommands", - "artifactId": "wpilibOldCommands-cpp", - "version": "wpilib", - "libName": "wpilibOldCommands", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxraspbian", - "linuxaarch64bionic", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxx86-64" - ] - } - ] -} \ No newline at end of file From 97aa73d69a616cb834da5c03afd09ffd2e2e0888 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 2 Mar 2020 22:32:29 -0700 Subject: [PATCH 10/21] Disable all that Smart Dashboard Stuff --- .../java/frc4388/robot/RobotContainer.java | 1 - .../frc4388/robot/commands/auto/Wait.java | 4 +-- .../drive/DriveStraightToPositionMM.java | 4 +-- .../drive/DriveWithJoystickAuxPID.java | 6 ++-- .../drive/DriveWithJoystickDriveStraight.java | 4 +-- .../DriveWithJoystickUsingDeadAssistPID.java | 6 ++-- .../robot/commands/drive/TurnDegrees.java | 4 +-- .../commands/shooter/HoodPositionPID.java | 4 +-- .../shooter/ShooterVelocityControlPID.java | 4 +-- .../java/frc4388/robot/subsystems/Drive.java | 36 +++++++++---------- .../java/frc4388/robot/subsystems/LED.java | 2 +- .../frc4388/robot/subsystems/Shooter.java | 20 +++++------ .../frc4388/robot/subsystems/Storage.java | 4 +-- .../java/frc4388/utility/ShooterTables.java | 12 +++---- 14 files changed, 55 insertions(+), 56 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f9bba13..5bda228 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -206,7 +206,6 @@ public class RobotContainer { //Calibrates turret and hood new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim)); - } /** diff --git a/src/main/java/frc4388/robot/commands/auto/Wait.java b/src/main/java/frc4388/robot/commands/auto/Wait.java index 9280310..2b06ae3 100644 --- a/src/main/java/frc4388/robot/commands/auto/Wait.java +++ b/src/main/java/frc4388/robot/commands/auto/Wait.java @@ -46,11 +46,11 @@ public class Wait extends CommandBase { public void execute() { if (counter == 0) { - SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime); + //SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime); } m_currentTime = System.currentTimeMillis(); - SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime)); + //SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime)); counter ++; } diff --git a/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java index f72a223..a2be80e 100644 --- a/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java @@ -61,7 +61,7 @@ public class DriveStraightToPositionMM extends CommandBase { //System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); //System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro); - SmartDashboard.putBoolean("MM Run", true); + //SmartDashboard.putBoolean("MM Run", true); i++; } @@ -74,7 +74,7 @@ public class DriveStraightToPositionMM extends CommandBase { @Override public boolean isFinished() { if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){ - SmartDashboard.putBoolean("MM Run", false); + //SmartDashboard.putBoolean("MM Run", false); return true; } else { if ((m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100)) { diff --git a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java index 3a958a9..6444141 100644 --- a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java @@ -59,9 +59,9 @@ public class DriveWithJoystickAuxPID extends CommandBase { m_drive.driveWithInputAux(moveOutput, m_targetGyro); - System.err.println("Target: " + m_targetGyro); - System.err.println("Current: " + currentGyro); - System.err.println(); + //System.err.println("Target: " + m_targetGyro); + //System.err.println("Current: " + currentGyro); + //System.err.println(); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java index 345b160..56d2f96 100644 --- a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java @@ -106,12 +106,12 @@ public class DriveWithJoystickDriveStraight extends CommandBase { steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steer) - cosMultiplier; } m_drive.driveWithInput(move, steerOutput); - System.out.println("Driving With Input"); + //System.out.println("Driving With Input"); } private void runDriveStraight(double move) { m_drive.driveWithInputAux(move * 3/4, m_targetGyro); - System.out.println("Driving Straight with Target: " + m_targetGyro); + //System.out.println("Driving Straight with Target: " + m_targetGyro); } /** diff --git a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java index 3b2ef73..13523af 100644 --- a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java @@ -132,12 +132,12 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { } m_drive.driveWithInput(move, steerOutput); - System.out.println("Driving With Input"); + //System.out.println("Driving With Input"); } private void runDriveStraight(double move) { m_drive.driveWithInputAux(move * 3/4, m_targetGyro); - System.out.println("Driving Straight with Target: " + m_targetGyro); + //System.out.println("Driving Straight with Target: " + m_targetGyro); } private void runStoppedTurn(double steer) { @@ -158,7 +158,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { } else { m_drive.driveWithInputAux(0, m_targetGyro); } - System.out.println("Turning with Target: " + m_targetGyro); + //System.out.println("Turning with Target: " + m_targetGyro); } /** diff --git a/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java b/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java index b9f7d35..e0999e6 100644 --- a/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java +++ b/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java @@ -50,8 +50,8 @@ public class TurnDegrees extends CommandBase { m_drive.runTurningPID(m_targetAngleTicksOut); - SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut)); - SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut); + //SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut)); + //SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut); i++; } diff --git a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java index 980c9ee..405530d 100644 --- a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java @@ -33,8 +33,8 @@ public class HoodPositionPID extends CommandBase { /*double slope = ShooterConstants.HOOD_CONVERT_SLOPE; double b = ShooterConstants.HOOD_CONVERT_B; firingAngle = (-slope*m_shooter.addFireAngle())+b;*/ - SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle()); - SmartDashboard.putNumber("Fire Angle", firingAngle); + //SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle()); + //SmartDashboard.putNumber("Fire Angle", firingAngle); m_shooter.runAngleAdjustPID(firingAngle); } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java index 8411aeb..f806eb4 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java @@ -36,8 +36,8 @@ public class ShooterVelocityControlPID extends CommandBase { public void execute() { m_shooter.runDrumShooterVelocityPID(/*m_shooter.addFireVel()*/13000); m_shooter.runAngleAdjustPID(m_shooter.addFireAngle()); - SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel()); - SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle()); + //SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel()); + //SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 300359d..f040c4f 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -733,16 +733,16 @@ public class Drive extends SubsystemBase { //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 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()); double leftRPM = ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); double rightRPM = ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Motor RPM", leftRPM); - SmartDashboard.putNumber("Right Motor RPM", rightRPM); + //SmartDashboard.putNumber("Left Motor RPM", leftRPM); + //SmartDashboard.putNumber("Right Motor RPM", rightRPM); //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); @@ -751,8 +751,8 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); //SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); - SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); + //SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); + //SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); /*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); @@ -760,23 +760,23 @@ public class Drive extends SubsystemBase { 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 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 Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent()); - SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.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("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)); + //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); //SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); - SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); + //SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 6ef70e5..cbad21c 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -50,6 +50,6 @@ public class LED extends SubsystemBase { @Override public void periodic(){ - SmartDashboard.putNumber("LED", currentLED); + //SmartDashboard.putNumber("LED", currentLED); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index b4f1403..e16cabe 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -78,15 +78,15 @@ public class Shooter extends SubsystemBase { m_shooterTable = new ShooterTables(); - SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10)); - SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200)); - SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250)); - SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500)); + //SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10)); + //SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200)); + //SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250)); + //SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500)); - SmartDashboard.putNumber("CSV A -30", m_shooterTable.getAngleDisplacement(-30)); - SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10)); - SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5)); - SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30)); + //SmartDashboard.putNumber("CSV A -30", m_shooterTable.getAngleDisplacement(-30)); + //SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10)); + //SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5)); + //SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30)); m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); @@ -106,8 +106,8 @@ public class Shooter extends SubsystemBase { try{ SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity()); - SmartDashboard.putNumber("Fire Velocity From CSV", m_fireVel); - SmartDashboard.putNumber("Fire Angle From CSV", m_fireAngle); + SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel); + SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle); //SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition()); } diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index a014544..67d515d 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -77,8 +77,8 @@ public class Storage extends SubsystemBase { m_storagePIDController.setFF(storageGains.m_kF); m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput); - SmartDashboard.putNumber("Storage Position PID Target", targetPos); - SmartDashboard.putNumber("Storage Position Pos", getEncoderPos()); + //SmartDashboard.putNumber("Storage Position PID Target", targetPos); + //SmartDashboard.putNumber("Storage Position Pos", getEncoderPos()); m_storagePIDController.setReference(targetPos, ControlType.kPosition); } diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java index dc7fa83..85244ce 100644 --- a/src/main/java/frc4388/utility/ShooterTables.java +++ b/src/main/java/frc4388/utility/ShooterTables.java @@ -85,12 +85,12 @@ public class ShooterTables { } catch (IOException e) { e.printStackTrace(); } - SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]); - SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]); - SmartDashboard.putNumber("m_distanceLength", m_distanceLength); - SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]); - SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]); - SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]); + //SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]); + //SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]); + //SmartDashboard.putNumber("m_distanceLength", m_distanceLength); + //SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]); + //SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]); + //SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]); } public double getHood(double distance) { //Rotations of motor From 6d501b6bb985580f29210db36dc8869dbc85fc83 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 2 Mar 2020 23:08:15 -0700 Subject: [PATCH 11/21] Update Buttons --- .../java/frc4388/robot/RobotContainer.java | 62 ++++++++++--------- 1 file changed, 34 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5bda228..02032f9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -37,7 +37,10 @@ import frc4388.robot.commands.drive.TurnDegrees; import frc4388.robot.commands.intake.RunIntakeWithTriggers; import frc4388.robot.commands.shooter.CalibrateShooter; import frc4388.robot.commands.shooter.HoldTarget; +import frc4388.robot.commands.shooter.ShootFullGroup; +import frc4388.robot.commands.shooter.ShootPrepGroup; import frc4388.robot.commands.shooter.TrimShooter; +import frc4388.robot.commands.storage.StoragePrepIntake; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Drive; @@ -51,6 +54,7 @@ import frc4388.robot.subsystems.ShooterAim; import frc4388.robot.subsystems.Storage; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; +import frc4388.utility.controller.XboxTriggerButton; /** * This class is where the bulk of the robot should be declared. Since @@ -138,6 +142,8 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new InstantCommand()); + + /* Driver Buttons */ // sets solenoids into high gear new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) @@ -149,29 +155,26 @@ public class RobotContainer { - /* Operator Buttons */ - // shoots until released new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - //.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false) - //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8))); + .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false) + .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8))); // shoots one ball new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - //.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false) - //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8))); + .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false) + .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8))); // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))) - .whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0))); - + .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))) - .whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0))); + .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); // safety for climber and leveler new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) @@ -180,24 +183,11 @@ public class RobotContainer { // starts tracking target new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - //.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) - //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); + .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) + .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); - .whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) - .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30))); - //Prepares storage for intaking - //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) - // .whileHeld(new StorageIntake(m_robotIntake, m_robotStorage)); - - //Runs storage to outtake - //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) - // .whileHeld(new StorageOutake(m_robotStorage)); - - //Run drum - new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) - //.whenPressed(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)); - .whenPressed(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); - //.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); + //.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) + //.whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30))); //Trims shooter new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS) @@ -206,6 +196,22 @@ public class RobotContainer { //Calibrates turret and hood new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim)); + + //Prepares storage for intaking + new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.LEFT_TRIGGER) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8))) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + + //Runs storage to outtake + new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.RIGHT_TRIGGER) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8))) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + + //Run drum + //new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) + //.whenPressed(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)); + //.whenPressed(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); + //.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); } /** From 8f6578a47b1f0da9e17e762f9c156cf4ab6623be Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 2 Mar 2020 23:56:05 -0700 Subject: [PATCH 12/21] Add ShooterHood Subsystem --- .../java/frc4388/robot/RobotContainer.java | 8 +- .../commands/shooter/CalibrateShooter.java | 20 ++-- .../robot/commands/shooter/HoldTarget.java | 9 +- .../commands/shooter/HoodPositionPID.java | 13 +-- .../commands/shooter/ShootFireGroup.java | 5 +- .../shooter/ShooterVelocityControlPID.java | 10 +- .../robot/commands/shooter/TrackTarget.java | 5 +- .../robot/subsystems/CANDigitalInput.java | 0 .../frc4388/robot/subsystems/Shooter.java | 67 +++---------- .../frc4388/robot/subsystems/ShooterAim.java | 40 +++++--- .../frc4388/robot/subsystems/ShooterHood.java | 95 +++++++++++++++++++ 11 files changed, 179 insertions(+), 93 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/CANDigitalInput.java create mode 100644 src/main/java/frc4388/robot/subsystems/ShooterHood.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 02032f9..ceea9f5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -51,6 +51,7 @@ import frc4388.robot.subsystems.LimeLight; import frc4388.robot.subsystems.Pneumatics; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; import frc4388.robot.subsystems.Storage; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -71,6 +72,7 @@ public class RobotContainer { private final Intake m_robotIntake = new Intake(); private final Shooter m_robotShooter = new Shooter(); private final ShooterAim m_robotShooterAim = new ShooterAim(); + private final ShooterHood m_robotShooterHood = new ShooterHood(); private final Climber m_robotClimber = new Climber(); private final Leveler m_robotLeveler = new Leveler(); private final Storage m_robotStorage = new Storage(); @@ -92,6 +94,10 @@ public class RobotContainer { /* Passing Drive and Pneumatics Subsystems */ m_robotPneumatics.passRequiredSubsystem(m_robotDrive); m_robotDrive.passRequiredSubsystem(m_robotPneumatics); + + m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim); + m_robotShooterHood.passRequiredSubsystem(m_robotShooter); + m_robotShooterAim.passRequiredSubsystem(m_robotShooter); configureButtonBindings(); @@ -195,7 +201,7 @@ public class RobotContainer { //Calibrates turret and hood new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) - .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim)); + .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); //Prepares storage for intaking new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.LEFT_TRIGGER) diff --git a/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java index b2a54ac..4585d04 100644 --- a/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java +++ b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java @@ -13,19 +13,23 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; public class CalibrateShooter extends CommandBase { Shooter m_shooter; ShooterAim m_shooterAim; + ShooterHood m_shooterHood; + /** * Calibrates the turret by moving the hood all the way down and moving the turret all the way right, then reseting the encoders * @param shootSub The Shooter subsystem * @param aimSub The ShooterAim subsystem */ - public CalibrateShooter(Shooter shootSub, ShooterAim aimSub) { + public CalibrateShooter(Shooter shootSub, ShooterAim aimSub, ShooterHood hoodSub) { m_shooter = shootSub; m_shooterAim = aimSub; - addRequirements(m_shooter, m_shooterAim); + m_shooterHood = hoodSub; + addRequirements(m_shooter, m_shooterHood, m_shooterAim); } // Called when the command is initially scheduled. @@ -36,10 +40,10 @@ public class CalibrateShooter extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false); - m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); - m_shooter.m_angleEncoder.setPosition(0); - m_shooter.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED); + m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false); + m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); + m_shooterHood.m_angleEncoder.setPosition(0); + m_shooterHood.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED); m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); @@ -50,8 +54,8 @@ public class CalibrateShooter extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); - m_shooter.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true); m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); diff --git a/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java b/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java index a91fb49..03b8d02 100644 --- a/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java @@ -14,15 +14,17 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; import frc4388.utility.controller.IHandController; public class HoldTarget extends CommandBase { - //Setup + // Setup NetworkTableEntry xEntry; ShooterAim m_shooterAim; Shooter m_shooter; + ShooterHood m_shooterHood; IHandController m_driverController; - //Aiming + // Aiming double turnAmount = 0; double xAngle = 0; double yAngle = 0; @@ -42,6 +44,7 @@ public class HoldTarget extends CommandBase { public HoldTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) { m_shooterAim = aimSubsystem; m_shooter = shooterSubsystem; + m_shooterHood = m_shooter.m_shooterHoodSubsystem; addRequirements(m_shooterAim); NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); } @@ -99,7 +102,7 @@ public class HoldTarget extends CommandBase { m_shooter.m_fireVel = fireVel; - m_shooter.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; + m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; } } diff --git a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java index 405530d..368f93c 100644 --- a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java @@ -10,16 +10,17 @@ package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterHood; public class HoodPositionPID extends CommandBase { - Shooter m_shooter; double firingAngle; + private ShooterHood m_shooterHood; /** * Creates a new HoodPositionPID. */ - public HoodPositionPID(Shooter subSystem) { - m_shooter = subSystem; - //addRequirements(m_shooter); + public HoodPositionPID(ShooterHood subSystem) { + m_shooterHood = subSystem; + addRequirements(m_shooterHood); } // Called when the command is initially scheduled. @@ -35,7 +36,7 @@ public class HoodPositionPID extends CommandBase { firingAngle = (-slope*m_shooter.addFireAngle())+b;*/ //SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle()); //SmartDashboard.putNumber("Fire Angle", firingAngle); - m_shooter.runAngleAdjustPID(firingAngle); + m_shooterHood.runAngleAdjustPID(firingAngle); } // Called once the command ends or is interrupted. @@ -46,7 +47,7 @@ public class HoodPositionPID extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - double encoderPos = m_shooter.m_angleAdjustMotor.getEncoder().getPosition(); + double encoderPos = m_shooterHood.m_angleAdjustMotor.getEncoder().getPosition(); if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){ return false; } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java index 2adcbda..daf6961 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import frc4388.robot.commands.storage.StorageRun; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; import frc4388.robot.subsystems.Storage; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -25,9 +26,11 @@ public class ShootFireGroup extends ParallelRaceGroup { * @param m_storage The Storage subsytem */ public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) { + ShooterHood m_shooterHood = m_shooter.m_shooterHoodSubsystem; + addCommands( new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())), - new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())), + new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle())), new HoldTarget(m_shooter, m_shooterAim), new StorageRun(m_storage) ); diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java index f806eb4..2cde87c 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java @@ -11,18 +11,22 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterHood; public class ShooterVelocityControlPID extends CommandBase { Shooter m_shooter; double m_targetVel; double m_actualVel; + private ShooterHood m_shooterHood; + /** * Runs the drum at a velocity * @param subsystem The Shooter subsytem */ - public ShooterVelocityControlPID(Shooter subsystem) { + public ShooterVelocityControlPID(Shooter subsystem, ShooterHood subHood) { m_shooter = subsystem; - addRequirements(m_shooter); + m_shooterHood = subHood; + addRequirements(m_shooter, m_shooterHood); } // Called when the command is initially scheduled. @@ -35,7 +39,7 @@ public class ShooterVelocityControlPID extends CommandBase { @Override public void execute() { m_shooter.runDrumShooterVelocityPID(/*m_shooter.addFireVel()*/13000); - m_shooter.runAngleAdjustPID(m_shooter.addFireAngle()); + m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()); //SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel()); //SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle()); } diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index 857e003..d5c638e 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; import frc4388.utility.controller.IHandController; public class TrackTarget extends CommandBase { @@ -33,6 +34,7 @@ public class TrackTarget extends CommandBase { public double m_hoodTrim; public double m_turretTrim; + private ShooterHood m_shooterHood; /** * Uses the Limelight to track the target @@ -42,6 +44,7 @@ public class TrackTarget extends CommandBase { public TrackTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) { m_shooterAim = aimSubsystem; m_shooter = shooterSubsystem; + m_shooterHood = m_shooter.m_shooterHoodSubsystem; addRequirements(m_shooterAim); NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); } @@ -97,7 +100,7 @@ public class TrackTarget extends CommandBase { m_shooter.m_fireVel = fireVel; - m_shooter.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; + m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; } } diff --git a/src/main/java/frc4388/robot/subsystems/CANDigitalInput.java b/src/main/java/frc4388/robot/subsystems/CANDigitalInput.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index e16cabe..00e54d7 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -10,15 +10,6 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.revrobotics.CANDigitalInput; -import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; -import com.revrobotics.CANEncoder; -import com.revrobotics.CANPIDController; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMax.SoftLimitDirection; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.ControlType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -31,13 +22,8 @@ import frc4388.utility.controller.IHandController; public class Shooter extends SubsystemBase { public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID); - public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); - - CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); - public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; - public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS; public static Shooter m_shooter; public static IHandController m_controller; @@ -48,10 +34,11 @@ public class Shooter extends SubsystemBase { public boolean velReached; public double m_fireVel; - public double m_fireAngle; - CANDigitalInput m_hoodUpLimit, m_hoodDownLimit; public Trims shooterTrims; + + public ShooterHood m_shooterHoodSubsystem; + public ShooterAim m_shooterAimSubsystem; /* * Creates a new Shooter subsystem, with the drum shooter and the angle adjsuter. @@ -64,7 +51,6 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.configFactoryDefault(); m_shooterFalcon.setNeutralMode(NeutralMode.Coast); m_shooterFalcon.setInverted(true); - m_angleAdjustMotor.setIdleMode(IdleMode.kBrake); m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.configClosedloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); setShooterGains(); @@ -87,17 +73,6 @@ public class Shooter extends SubsystemBase { //SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10)); //SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5)); //SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30)); - - m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_hoodUpLimit.enableLimitSwitch(true); - m_hoodDownLimit.enableLimitSwitch(true); - - m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); - m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT); - m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT); - } @Override @@ -107,7 +82,6 @@ public class Shooter extends SubsystemBase { SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity()); SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel); - SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle); //SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition()); } @@ -118,13 +92,18 @@ public class Shooter extends SubsystemBase { } } + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(ShooterHood subsystem0, ShooterAim subsystem1) { + m_shooterHoodSubsystem = subsystem0; + m_shooterAimSubsystem = subsystem1; + } + public double addFireVel() { return m_fireVel; } - public double addFireAngle() { - return m_fireAngle; - } - /** * Runs drum shooter motor. @@ -145,20 +124,6 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); } - /* Angle Adjustment PID Control */ - public void runAngleAdjustPID(double targetAngle) - { - // Set PID Coefficients - m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP); - m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI); - m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD); - m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone); - m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF); - m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput); - - m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); - } - /** * Runs drum shooter velocity PID. * @param falcon Motor to use @@ -168,12 +133,4 @@ public class Shooter extends SubsystemBase { System.out.println("dddddddddddddddddddddddd" + targetVel); m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } - - public void resetGyroAngleAdj(){ - m_angleEncoder.setPosition(0); - } - - public double getAnglePosition(){ - return m_angleEncoder.getPosition(); - } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index 07b04f5..086103c 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -22,14 +22,15 @@ import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; public class ShooterAim extends SubsystemBase { + public Shooter m_shooterSubsystem; + public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS; CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit; - // Configure PID Controllers - CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController(); - public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder(); - + // Configure PID Controllers + CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController(); + public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder(); /** * Creates a subsytem for the turret aiming @@ -49,6 +50,19 @@ public class ShooterAim extends SubsystemBase { m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_LEFT_SOFT_LIMIT); } + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(Shooter subsystem) { + m_shooterSubsystem = subsystem; + } + public void runShooterWithInput(double input) { m_shooterRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER); } @@ -71,17 +85,13 @@ public class ShooterAim extends SubsystemBase { m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition); } - public void resetGyroShooterRotate() - { - m_shooterRotateEncoder.setPosition(0); - } + public void resetGyroShooterRotate() + { + m_shooterRotateEncoder.setPosition(0); + } - public double getShooterRotatePosition(){ - return m_shooterRotateMotor.getEncoder().getPosition(); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run + public double getShooterRotatePosition() + { + return m_shooterRotateMotor.getEncoder().getPosition(); } } diff --git a/src/main/java/frc4388/robot/subsystems/ShooterHood.java b/src/main/java/frc4388/robot/subsystems/ShooterHood.java new file mode 100644 index 0000000..ae0ee66 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/ShooterHood.java @@ -0,0 +1,95 @@ +/*----------------------------------------------------------------------------*/ +/* 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.subsystems; + +import com.revrobotics.CANDigitalInput; +import com.revrobotics.CANEncoder; +import com.revrobotics.CANPIDController; +import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.Gains; +import frc4388.utility.controller.IHandController; + +public class ShooterHood extends SubsystemBase { + public Shooter m_shooterSubsystem; + + public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + + public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS; + + public CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); + public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); + + public double m_fireAngle; + public CANDigitalInput m_hoodUpLimit, m_hoodDownLimit; + + /** + * Creates a new ShooterHood. + */ + public ShooterHood() { + m_angleAdjustMotor.setIdleMode(IdleMode.kBrake); + + m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_hoodUpLimit.enableLimitSwitch(true); + m_hoodDownLimit.enableLimitSwitch(true); + + m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT); + m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle); + } + + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(Shooter subsystem) { + m_shooterSubsystem = subsystem; + } + + public double addFireAngle() { + return m_fireAngle; + } + + /* Angle Adjustment PID Control */ + public void runAngleAdjustPID(double targetAngle) + { + // Set PID Coefficients + m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP); + m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI); + m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD); + m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone); + m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF); + m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput); + + m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); + } + + public void resetGyroAngleAdj(){ + m_angleEncoder.setPosition(0); +} + + public double getAnglePosition(){ + return m_angleEncoder.getPosition(); + } +} From 1b603d4f5d1e361bb950ab687c7a78adc59ccbd3 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 3 Mar 2020 00:12:38 -0700 Subject: [PATCH 13/21] Rework Command Constructors to Make Sense Subsystems passed through the command are requirements, while subsystems gotten from other subsystems in the constructor are for reference only. --- src/main/java/frc4388/robot/RobotContainer.java | 6 +++--- .../frc4388/robot/commands/shooter/HoldTarget.java | 4 ++-- .../robot/commands/shooter/HoodPositionPID.java | 7 +++---- .../robot/commands/shooter/ShootFireGroup.java | 9 ++++----- .../robot/commands/shooter/ShootFullGroup.java | 7 ++++--- .../robot/commands/shooter/ShootPrepGroup.java | 13 ++++++++----- .../commands/shooter/ShooterVelocityControlPID.java | 8 +++----- .../frc4388/robot/commands/shooter/TrackTarget.java | 9 +++++---- 8 files changed, 32 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ceea9f5..98b0a18 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -164,13 +164,13 @@ public class RobotContainer { /* Operator Buttons */ // shoots until released new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false) + .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8))); // shoots one ball new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false) + .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8))); @@ -189,7 +189,7 @@ public class RobotContainer { // starts tracking target new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) + .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage)) .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); //.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) diff --git a/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java b/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java index 03b8d02..7ee1ff4 100644 --- a/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java @@ -41,9 +41,9 @@ public class HoldTarget extends CommandBase { * @param shooterSubsystem The Shooter subsystem * @param aimSubsystem The ShooterAim subsystem */ - public HoldTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) { + public HoldTarget(ShooterAim aimSubsystem) { m_shooterAim = aimSubsystem; - m_shooter = shooterSubsystem; + m_shooter = m_shooterAim.m_shooterSubsystem; m_shooterHood = m_shooter.m_shooterHoodSubsystem; addRequirements(m_shooterAim); NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); diff --git a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java index 368f93c..d5d807d 100644 --- a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java @@ -7,14 +7,13 @@ package frc4388.robot.commands.shooter; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterHood; public class HoodPositionPID extends CommandBase { + ShooterHood m_shooterHood; double firingAngle; - private ShooterHood m_shooterHood; + /** * Creates a new HoodPositionPID. */ @@ -49,7 +48,7 @@ public class HoodPositionPID extends CommandBase { public boolean isFinished() { double encoderPos = m_shooterHood.m_angleAdjustMotor.getEncoder().getPosition(); if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){ - return false; + return true; } return false; } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java index daf6961..b0c5af5 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java @@ -25,13 +25,12 @@ public class ShootFireGroup extends ParallelRaceGroup { * @param m_shooterAim The ShooterAim subsystem * @param m_storage The Storage subsytem */ - public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) { - ShooterHood m_shooterHood = m_shooter.m_shooterHoodSubsystem; + public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { addCommands( - new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())), - new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle())), - new HoldTarget(m_shooter, m_shooterAim), + new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()), m_shooter), + new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()), m_shooterHood), + new HoldTarget(m_shooterAim), new StorageRun(m_storage) ); } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java index e124093..8863636 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java @@ -10,6 +10,7 @@ package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; import frc4388.robot.subsystems.Storage; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -22,10 +23,10 @@ public class ShootFullGroup extends SequentialCommandGroup { * @param m_shooterAim The ShooterAim subsystem * @param m_storage The Storage subsytem */ - public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) { + public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { addCommands( - new ShootPrepGroup(m_shooter, m_shooterAim, m_storage), - new ShootFireGroup(m_shooter, m_shooterAim, m_storage) + new ShootPrepGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage), + new ShootFireGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage) ); } } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java index ef8714d..3efed56 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java @@ -9,8 +9,10 @@ package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; +import frc4388.robot.commands.storage.StoragePrepAim; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; import frc4388.robot.subsystems.Storage; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -23,12 +25,13 @@ public class ShootPrepGroup extends ParallelCommandGroup { * @param m_shooterAim The ShooterAim subsystem * @param m_storage The Storage subsytem */ - public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) { + public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { addCommands( - //new TrackTarget(m_shooter, m_shooterAim), - //new ShooterVelocityControlPID(m_shooter) - new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())) - //new StoragePrepAim(m_storage) + new TrackTarget(m_shooterAim), + new ShooterVelocityControlPID(m_shooter), + new HoodPositionPID(m_shooterHood), + //new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())), + new StoragePrepAim(m_storage) ); } } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java index 2cde87c..8634fb5 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java @@ -17,16 +17,14 @@ public class ShooterVelocityControlPID extends CommandBase { Shooter m_shooter; double m_targetVel; double m_actualVel; - private ShooterHood m_shooterHood; /** * Runs the drum at a velocity * @param subsystem The Shooter subsytem */ - public ShooterVelocityControlPID(Shooter subsystem, ShooterHood subHood) { + public ShooterVelocityControlPID(Shooter subsystem) { m_shooter = subsystem; - m_shooterHood = subHood; - addRequirements(m_shooter, m_shooterHood); + addRequirements(m_shooter); } // Called when the command is initially scheduled. @@ -39,7 +37,7 @@ public class ShooterVelocityControlPID extends CommandBase { @Override public void execute() { m_shooter.runDrumShooterVelocityPID(/*m_shooter.addFireVel()*/13000); - m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()); + //m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()); //SmartDashboard.putNumber("Fire Velocity", m_shooter.addFireVel()); //SmartDashboard.putNumber("Fire Angle", m_shooter.addFireAngle()); } diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index d5c638e..58ea976 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -19,9 +19,11 @@ import frc4388.utility.controller.IHandController; public class TrackTarget extends CommandBase { // Setup - NetworkTableEntry xEntry; ShooterAim m_shooterAim; Shooter m_shooter; + ShooterHood m_shooterHood; + + NetworkTableEntry xEntry; IHandController m_driverController; // Aiming double turnAmount = 0; @@ -34,16 +36,15 @@ public class TrackTarget extends CommandBase { public double m_hoodTrim; public double m_turretTrim; - private ShooterHood m_shooterHood; /** * Uses the Limelight to track the target * @param shooterSubsystem The Shooter subsystem * @param aimSubsystem The ShooterAim subsystem */ - public TrackTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) { + public TrackTarget(ShooterAim aimSubsystem) { m_shooterAim = aimSubsystem; - m_shooter = shooterSubsystem; + m_shooter = m_shooterAim.m_shooterSubsystem; m_shooterHood = m_shooter.m_shooterHoodSubsystem; addRequirements(m_shooterAim); NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); From 694ab7482fceafc1aef5f194d08473cfaf85455b Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Tue, 3 Mar 2020 00:42:34 -0700 Subject: [PATCH 14/21] Removed trimming, started using keenans equation for angle --- src/main/java/frc4388/robot/RobotContainer.java | 10 +++------- src/main/java/frc4388/robot/commands/HoldTarget.java | 2 +- .../java/frc4388/robot/commands/HoodPositionPID.java | 4 ++-- src/main/java/frc4388/robot/commands/TrackTarget.java | 8 +++----- src/main/java/frc4388/robot/subsystems/Shooter.java | 3 +-- 5 files changed, 10 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 08a5321..ceea2c7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -216,7 +216,7 @@ public class RobotContainer { //.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); - .whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) + .whileHeld(new TrackTarget(m_robotShooter, m_robotShooterAim)) .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30))); //Prepares storage for intaking //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) @@ -229,12 +229,8 @@ public class RobotContainer { //Run drum new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) //.whenPressed(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)); - .whenPressed(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); - //.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); - - //Trims shooter - new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS) - .whenPressed(new TrimShooter(m_robotShooter)); + .whenPressed(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))) + .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); //Calibrates turret and hood new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/HoldTarget.java b/src/main/java/frc4388/robot/commands/HoldTarget.java index fa39663..0bb327c 100644 --- a/src/main/java/frc4388/robot/commands/HoldTarget.java +++ b/src/main/java/frc4388/robot/commands/HoldTarget.java @@ -80,7 +80,7 @@ public class HoldTarget extends CommandBase { } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) { turnAmount = -VisionConstants.MOTOR_DEAD_ZONE; } - m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim); + m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim); // Finding Distance distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); diff --git a/src/main/java/frc4388/robot/commands/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/HoodPositionPID.java index b48a3c4..1ab2f43 100644 --- a/src/main/java/frc4388/robot/commands/HoodPositionPID.java +++ b/src/main/java/frc4388/robot/commands/HoodPositionPID.java @@ -31,9 +31,9 @@ public class HoodPositionPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - /*double slope = ShooterConstants.HOOD_CONVERT_SLOPE; + double slope = ShooterConstants.HOOD_CONVERT_SLOPE; double b = ShooterConstants.HOOD_CONVERT_B; - firingAngle = (-slope*m_shooter.addFireAngle())+b;*/ + firingAngle = (-slope*m_shooter.addFireAngle())+b; SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle()); SmartDashboard.putNumber("Fire Angle", firingAngle); m_shooter.runAngleAdjustPID(firingAngle); diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 197dc72..2236913 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -82,25 +82,23 @@ public class TrackTarget extends CommandBase { } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) { turnAmount = -VisionConstants.MOTOR_DEAD_ZONE; } - m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim); + m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim); // Finding Distance distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); SmartDashboard.putNumber("Distance to Target", distance); //START Equation Code - /* 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))); + //fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2))); fireAngle = Math.atan(yVel/xVel) * (180/Math.PI); - */ //END Equation Code //START CSV Code fireVel = m_shooter.m_shooterTable.getVelocity(distance); - fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different + //fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different //fireAngle = 33; //END CSV Code diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index ae10d75..63ce02d 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -164,11 +164,10 @@ public class Shooter extends SubsystemBase { /** * Runs drum shooter velocity PID. - * @param falcon Motor to use * @param targetVel Target velocity to run motor at */ public void runDrumShooterVelocityPID(double targetVel) { - System.out.println("dddddddddddddddddddddddd" + targetVel); + System.out.println("Target Velocity" + targetVel); m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } From 9288d2b934d11fa3d76a0147447f93f8698b562d Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 3 Mar 2020 01:12:07 -0700 Subject: [PATCH 15/21] Rework Command Groups to use Prep Checker --- .../java/frc4388/robot/RobotContainer.java | 1 - .../robot/commands/shooter/HoldTarget.java | 121 ------------------ .../commands/shooter/HoodPositionPID.java | 4 +- .../robot/commands/shooter/PrepChecker.java | 75 +++++++++++ .../commands/shooter/ShootFireGroup.java | 3 +- .../commands/shooter/ShootPrepGroup.java | 12 +- .../shooter/ShooterVelocityControlPID.java | 8 +- .../robot/commands/shooter/TrackTarget.java | 7 +- .../commands/storage/StoragePrepAim.java | 7 +- .../frc4388/robot/subsystems/Shooter.java | 2 +- .../frc4388/robot/subsystems/ShooterAim.java | 2 + .../frc4388/robot/subsystems/ShooterHood.java | 2 + .../frc4388/robot/subsystems/Storage.java | 1 + 13 files changed, 105 insertions(+), 140 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/shooter/HoldTarget.java create mode 100644 src/main/java/frc4388/robot/commands/shooter/PrepChecker.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 98b0a18..68dcd13 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -36,7 +36,6 @@ import frc4388.robot.commands.drive.DriveWithJoystick; import frc4388.robot.commands.drive.TurnDegrees; import frc4388.robot.commands.intake.RunIntakeWithTriggers; import frc4388.robot.commands.shooter.CalibrateShooter; -import frc4388.robot.commands.shooter.HoldTarget; import frc4388.robot.commands.shooter.ShootFullGroup; import frc4388.robot.commands.shooter.ShootPrepGroup; import frc4388.robot.commands.shooter.TrimShooter; diff --git a/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java b/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java deleted file mode 100644 index 7ee1ff4..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java +++ /dev/null @@ -1,121 +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.shooter; - -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.ShooterHood; -import frc4388.utility.controller.IHandController; - -public class HoldTarget extends CommandBase { - // Setup - NetworkTableEntry xEntry; - ShooterAim m_shooterAim; - Shooter m_shooter; - ShooterHood m_shooterHood; - IHandController m_driverController; - // Aiming - double turnAmount = 0; - double xAngle = 0; - double yAngle = 0; - double target = 0; - public double distance; - public double fireVel; - public double fireAngle; - - public double m_hoodTrim; - public double m_turretTrim; - - /** - * Uses the Limelight to track the target - * @param shooterSubsystem The Shooter subsystem - * @param aimSubsystem The ShooterAim subsystem - */ - public HoldTarget(ShooterAim aimSubsystem) { - m_shooterAim = aimSubsystem; - m_shooter = m_shooterAim.m_shooterSubsystem; - m_shooterHood = m_shooter.m_shooterHoodSubsystem; - addRequirements(m_shooterAim); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - //Vision Processing Mode - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); - } - - - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - target = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0); - xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); - yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); - - if (target == 1.0) { // If target in view - // Aiming Left/Right - turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE; - if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) { - turnAmount = 0; - } // Angle Error Zone - // Deadzones - else if (turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE) { - turnAmount = VisionConstants.MOTOR_DEAD_ZONE; - } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) { - turnAmount = -VisionConstants.MOTOR_DEAD_ZONE; - } - m_shooterAim.runShooterWithInput(-turnAmount - m_shooter.shooterTrims.m_turretTrim); - - // Finding Distance - distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); - SmartDashboard.putNumber("Distance to Target", distance); - - //START Equation Code - /* - 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) * (180/Math.PI); - */ - //END Equation Code - - //START CSV Code - fireVel = m_shooter.m_shooterTable.getVelocity(distance); - fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different - //fireAngle = 33; - //END CSV Code - - - m_shooter.m_fireVel = fireVel; - m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java index d5d807d..ae373d4 100644 --- a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java @@ -48,7 +48,9 @@ public class HoodPositionPID extends CommandBase { public boolean isFinished() { double encoderPos = m_shooterHood.m_angleAdjustMotor.getEncoder().getPosition(); if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){ - return true; + m_shooterHood.m_isHoodReady = true; + } else { + m_shooterHood.m_isHoodReady = false; } return false; } diff --git a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java new file mode 100644 index 0000000..c0ef7a6 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.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.shooter; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; +import frc4388.robot.subsystems.Storage; + +public class PrepChecker extends CommandBase { + Shooter m_shooter; + ShooterAim m_shooterAim; + ShooterHood m_shooterHood; + Storage m_storage; + + boolean m_isDrumReady = false; + boolean m_isAimReady = false; + boolean m_isHoodReady = false; + boolean m_isStorageReady = false; + + /** + * Creates a new PrepChecker. + * @param shooter used to read all shooter subsystems. Not used as a requirement so don't expect it to interrupt other commands. + * @param storage reads storage in a similar way to shooter. Not used as a requirement. + */ + public PrepChecker(Shooter shooter, Storage storage) { + m_shooter = shooter; + m_shooterAim = m_shooter.m_shooterAimSubsystem; + m_shooterHood = m_shooter.m_shooterHoodSubsystem; + m_storage = storage; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_isDrumReady = false; + m_isAimReady = false; + m_isHoodReady = false; + m_isStorageReady = false; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_isDrumReady = m_shooter.m_isDrumReady; SmartDashboard.putBoolean("ShooterVelocityPID Finished", m_isDrumReady); + m_isAimReady = m_shooterAim.m_isAimReady; SmartDashboard.putBoolean("TrackTarget Finished", m_isAimReady); + m_isHoodReady = m_shooterHood.m_isHoodReady; SmartDashboard.putBoolean("HoodPosition Finished", m_isHoodReady); + m_isStorageReady = m_storage.m_isStorageReadyToFire; SmartDashboard.putBoolean("StoragePrepAim Finished", m_isStorageReady); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_shooter.m_isDrumReady = false; + m_shooterAim.m_isAimReady = false; + m_shooterHood.m_isHoodReady = false; + m_storage.m_isStorageReadyToFire = false; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_isDrumReady && m_isAimReady && m_isHoodReady && m_isStorageReady) { + return true; + } + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java index b0c5af5..88fea47 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java @@ -26,11 +26,10 @@ public class ShootFireGroup extends ParallelRaceGroup { * @param m_storage The Storage subsytem */ public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { - addCommands( new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()), m_shooter), new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()), m_shooterHood), - new HoldTarget(m_shooterAim), + new TrackTarget(m_shooterAim), new StorageRun(m_storage) ); } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java index 3efed56..428d220 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java @@ -7,7 +7,10 @@ package frc4388.robot.commands.shooter; +import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import frc4388.robot.commands.storage.StoragePrepAim; import frc4388.robot.subsystems.Shooter; @@ -18,20 +21,21 @@ 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 ShootPrepGroup extends ParallelCommandGroup { +public class ShootPrepGroup extends ParallelDeadlineGroup { /** * Prepares the Shooter to be fired * @param m_shooter The Shooter subsytem * @param m_shooterAim The ShooterAim subsystem * @param m_storage The Storage subsytem */ - public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { - addCommands( + public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { + super( + new PrepChecker(m_shooter, m_storage), new TrackTarget(m_shooterAim), new ShooterVelocityControlPID(m_shooter), new HoodPositionPID(m_shooterHood), - //new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())), new StoragePrepAim(m_storage) + //new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())), ); } } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java index 8634fb5..8e5e6e2 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java @@ -54,12 +54,12 @@ public class ShooterVelocityControlPID extends CommandBase { double upperBound = m_targetVel + ShooterConstants.DRUM_VELOCITY_BOUND; double lowerBound = m_targetVel - ShooterConstants.DRUM_VELOCITY_BOUND; if (m_actualVel < upperBound && m_actualVel > lowerBound){ - SmartDashboard.putBoolean("ShooterVelocityPID Finished", true); - return true; + m_shooter.m_isDrumReady = true; } else{ - SmartDashboard.putBoolean("ShooterVelocityPID Finished", false); - return false; + m_shooter.m_isDrumReady = false; } + + return false; } } diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index 58ea976..828232b 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -116,10 +116,11 @@ public class TrackTarget extends CommandBase { public boolean isFinished() { if (xAngle < 1 && xAngle > -1 && target == 1) { - SmartDashboard.putBoolean("TrackTarget Finished", true); - return true; + m_shooterAim.m_isAimReady = true; + } else { + m_shooterAim.m_isAimReady = false; } - SmartDashboard.putBoolean("TrackTarget Finished", false); + return false; } } diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java b/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java index 268f39e..912b87c 100644 --- a/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java +++ b/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java @@ -50,10 +50,11 @@ public class StoragePrepAim extends CommandBase { @Override public boolean isFinished() { if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){ - SmartDashboard.putBoolean("StoragePrepAim Finished", true); + m_storage.m_isStorageReadyToFire = true; return true; + } else { + m_storage.m_isStorageReadyToFire = false; + return false; } - SmartDashboard.putBoolean("StoragePrepAim Finished", false); - return false; } } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 00e54d7..681fe35 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -32,7 +32,7 @@ public class Shooter extends SubsystemBase { public ShooterTables m_shooterTable; - public boolean velReached; + public boolean m_isDrumReady = false; public double m_fireVel; public Trims shooterTrims; diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index 086103c..aed97fa 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -28,6 +28,8 @@ public class ShooterAim extends SubsystemBase { public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS; CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit; + public boolean m_isAimReady = false; + // Configure PID Controllers CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController(); public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder(); diff --git a/src/main/java/frc4388/robot/subsystems/ShooterHood.java b/src/main/java/frc4388/robot/subsystems/ShooterHood.java index ae0ee66..d60ae3a 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterHood.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterHood.java @@ -33,6 +33,8 @@ public class ShooterHood extends SubsystemBase { public CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); + public boolean m_isHoodReady = false; + public double m_fireAngle; public CANDigitalInput m_hoodUpLimit, m_hoodDownLimit; diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 67d515d..3fe6ccd 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -31,6 +31,7 @@ public class Storage extends SubsystemBase { Intake m_intake; + public boolean m_isStorageReadyToFire = false; /** * Creates a new Storage. From faf9346f6ea19c92b6f714946b9fae7e296b2f39 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 3 Mar 2020 01:17:14 -0700 Subject: [PATCH 16/21] Touch ups --- src/main/java/frc4388/robot/commands/auto/Wait.java | 1 - .../java/frc4388/robot/commands/shooter/ShootPrepGroup.java | 4 ---- .../robot/commands/shooter/ShooterVelocityControlPID.java | 2 -- src/main/java/frc4388/robot/subsystems/CANDigitalInput.java | 0 src/main/java/frc4388/robot/subsystems/ShooterHood.java | 1 - 5 files changed, 8 deletions(-) delete mode 100644 src/main/java/frc4388/robot/subsystems/CANDigitalInput.java diff --git a/src/main/java/frc4388/robot/commands/auto/Wait.java b/src/main/java/frc4388/robot/commands/auto/Wait.java index 2b06ae3..6bbedf3 100644 --- a/src/main/java/frc4388/robot/commands/auto/Wait.java +++ b/src/main/java/frc4388/robot/commands/auto/Wait.java @@ -7,7 +7,6 @@ package frc4388.robot.commands.auto; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java index 428d220..7cc209f 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java @@ -7,11 +7,7 @@ package frc4388.robot.commands.shooter; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.RunCommand; import frc4388.robot.commands.storage.StoragePrepAim; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java index 8e5e6e2..215b6bd 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java @@ -7,11 +7,9 @@ package frc4388.robot.commands.shooter; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterHood; public class ShooterVelocityControlPID extends CommandBase { Shooter m_shooter; diff --git a/src/main/java/frc4388/robot/subsystems/CANDigitalInput.java b/src/main/java/frc4388/robot/subsystems/CANDigitalInput.java deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc4388/robot/subsystems/ShooterHood.java b/src/main/java/frc4388/robot/subsystems/ShooterHood.java index d60ae3a..c12851b 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterHood.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterHood.java @@ -21,7 +21,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; -import frc4388.utility.controller.IHandController; public class ShooterHood extends SubsystemBase { public Shooter m_shooterSubsystem; From b8a6e59fac6074f1d0c152eb56cc42e312c9b7a9 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 3 Mar 2020 01:27:12 -0700 Subject: [PATCH 17/21] Fix broke merge stuff --- .../java/frc4388/robot/commands/shooter/HoodPositionPID.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java index 93ddf62..380eeaf 100644 --- a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java @@ -8,6 +8,7 @@ package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.subsystems.ShooterHood; public class HoodPositionPID extends CommandBase { @@ -32,7 +33,7 @@ public class HoodPositionPID extends CommandBase { public void execute() { double slope = ShooterConstants.HOOD_CONVERT_SLOPE; double b = ShooterConstants.HOOD_CONVERT_B; - firingAngle = (-slope*m_shooter.addFireAngle())+b;*/ + firingAngle = (-slope*m_shooterHood.addFireAngle())+b; //SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle()); //SmartDashboard.putNumber("Fire Angle", firingAngle); m_shooterHood.runAngleAdjustPID(firingAngle); From 2f6b21930355fcddfac9ad940a5dff1855513e8d Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Tue, 3 Mar 2020 20:59:00 -0700 Subject: [PATCH 18/21] SHOOTER WORKS SEMI MANUAL --- src/main/deploy/Robot Data - Distances.csv | 11 +++-- src/main/java/frc4388/robot/Constants.java | 5 ++- .../java/frc4388/robot/RobotContainer.java | 45 +++++++++++-------- .../commands/shooter/CalibrateShooter.java | 5 --- .../commands/shooter/HoodPositionPID.java | 7 +-- .../commands/shooter/ShootFireGroup.java | 4 +- .../commands/shooter/ShootPrepGroup.java | 1 - .../robot/commands/shooter/TrackTarget.java | 7 ++- .../robot/commands/storage/StorageIntake.java | 2 +- .../commands/storage/StoragePrepAim.java | 7 +-- .../commands/storage/StoragePrepIntake.java | 2 +- .../frc4388/robot/subsystems/Shooter.java | 12 ++++- .../frc4388/robot/subsystems/ShooterAim.java | 2 + .../frc4388/robot/subsystems/ShooterHood.java | 8 ++-- 14 files changed, 71 insertions(+), 47 deletions(-) diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv index 5ff8a86..5a625da 100644 --- a/src/main/deploy/Robot Data - Distances.csv +++ b/src/main/deploy/Robot Data - Distances.csv @@ -1,4 +1,9 @@ Distance (in),Hood Ext. (u),Drum Velocity (u/ds) -21,3,10000 -262,30,15000 -492,30,15000 +74,20,8000 +144,23,10000 +162,28,10000 +208,29,10000 +296,32,12500 +418,33,16000 +430,31,16000 +450,31,16000 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2dd226d..b951b15 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -125,12 +125,15 @@ public final class Constants { public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0.0, 0.05, 0, 1.0); public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); - public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; public static final double ENCODER_TICKS_PER_REV = 2048; public static final double NEO_UNITS_PER_REV = 42; public static final double DEGREES_PER_ROT = 360; + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = + new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); + public static final int TURRET_RIGHT_SOFT_LIMIT = -2; public static final int TURRET_LEFT_SOFT_LIMIT = -55; public static final double TURRET_SPEED_MULTIPLIER = 0.3; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 621ff33..56e9545 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -36,9 +36,12 @@ import frc4388.robot.commands.drive.DriveWithJoystick; import frc4388.robot.commands.drive.TurnDegrees; import frc4388.robot.commands.intake.RunIntakeWithTriggers; import frc4388.robot.commands.shooter.CalibrateShooter; +import frc4388.robot.commands.shooter.ShootFireGroup; import frc4388.robot.commands.shooter.ShootFullGroup; import frc4388.robot.commands.shooter.ShootPrepGroup; +import frc4388.robot.commands.shooter.TrackTarget; import frc4388.robot.commands.shooter.TrimShooter; +import frc4388.robot.commands.storage.StorageIntake; import frc4388.robot.commands.storage.StoragePrepIntake; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Climber; @@ -119,6 +122,7 @@ 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 storage not + //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); } @@ -138,7 +142,6 @@ public class RobotContainer { // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) .whenPressed(new TurnDegrees(m_robotDrive, 90)); - // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .whenPressed(new Wait(m_robotDrive, 0, 0)); @@ -163,15 +166,17 @@ public class RobotContainer { /* Operator Buttons */ // shoots until released new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) - .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); - //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8))); + //.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) + //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // shoots one ball new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) - .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); - //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8))); + //.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) + //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); + .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage)) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) @@ -188,8 +193,10 @@ public class RobotContainer { // starts tracking target new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage)) - .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); + .whileHeld(new TrackTarget(m_robotShooterAim)) + .whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle()))) + //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)) + .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); //.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) //.whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30))); @@ -203,20 +210,20 @@ public class RobotContainer { .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); //Prepares storage for intaking - new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.LEFT_TRIGGER) - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8))) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + //new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.LEFT_TRIGGER) + //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8))) + //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + //.whileHeld(new StorageIntake(m_robotIntake, m_robotStorage)); //Runs storage to outtake - new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.RIGHT_TRIGGER) - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8))) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + //new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.RIGHT_TRIGGER) + //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8))) + //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); //Run drum - //new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) - //.whenPressed(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)); - //.whenPressed(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); - //.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) + .whileHeld(new ShootFireGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) + .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); } /** diff --git a/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java index 4585d04..23be5fa 100644 --- a/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java +++ b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java @@ -40,8 +40,6 @@ public class CalibrateShooter extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, false); - m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); m_shooterHood.m_angleEncoder.setPosition(0); m_shooterHood.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED); @@ -54,9 +52,6 @@ public class CalibrateShooter extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); - m_shooterHood.m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true); m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); } diff --git a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java index 380eeaf..2f5a14a 100644 --- a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java @@ -31,9 +31,10 @@ public class HoodPositionPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double slope = ShooterConstants.HOOD_CONVERT_SLOPE; - double b = ShooterConstants.HOOD_CONVERT_B; - firingAngle = (-slope*m_shooterHood.addFireAngle())+b; + //double slope = ShooterConstants.HOOD_CONVERT_SLOPE; + //double b = ShooterConstants.HOOD_CONVERT_B; + //firingAngle = (-slope*m_shooterHood.addFireAngle())+b; + firingAngle = m_shooterHood.addFireAngle(); //SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle()); //SmartDashboard.putNumber("Fire Angle", firingAngle); m_shooterHood.runAngleAdjustPID(firingAngle); diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java index 88fea47..15ee080 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java @@ -29,8 +29,8 @@ public class ShootFireGroup extends ParallelRaceGroup { addCommands( new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()), m_shooter), new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()), m_shooterHood), - new TrackTarget(m_shooterAim), - new StorageRun(m_storage) + new TrackTarget(m_shooterAim) + //new StorageRun(m_storage) ); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java index 7cc209f..14accc3 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java @@ -31,7 +31,6 @@ public class ShootPrepGroup extends ParallelDeadlineGroup { new ShooterVelocityControlPID(m_shooter), new HoodPositionPID(m_shooterHood), new StoragePrepAim(m_storage) - //new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())), ); } } diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index 6debaf5..6d35f49 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -88,15 +88,18 @@ public class TrackTarget extends CommandBase { double xVel = (distance * VisionConstants.GRAV) / (yVel); //fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2))); - fireAngle = Math.atan(yVel/xVel) * (180/Math.PI); + //fireAngle = Math.atan(yVel/xVel) * (180/Math.PI); //END Equation Code //START CSV Code fireVel = m_shooter.m_shooterTable.getVelocity(distance); - //fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different + fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different //fireAngle = 33; //END CSV Code + //fireVel = SmartDashboard.getNumber("Velocity Target", 0); + //fireAngle = SmartDashboard.getNumber("Angle Target", 3); + m_shooter.m_fireVel = fireVel; m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; diff --git a/src/main/java/frc4388/robot/commands/storage/StorageIntake.java b/src/main/java/frc4388/robot/commands/storage/StorageIntake.java index 9ead11f..47769c4 100644 --- a/src/main/java/frc4388/robot/commands/storage/StorageIntake.java +++ b/src/main/java/frc4388/robot/commands/storage/StorageIntake.java @@ -38,7 +38,7 @@ public class StorageIntake extends CommandBase { @Override public void execute() { - if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){ + if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){ m_storage.runStorage(StorageConstants.STORAGE_SPEED); intook = true; } diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java b/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java index 912b87c..6d47abf 100644 --- a/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java +++ b/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java @@ -34,7 +34,7 @@ public class StoragePrepAim extends CommandBase { @Override public void execute() { if (m_storage.getBeam(1)){ - m_storage.runStorage(StorageConstants.STORAGE_SPEED); + //m_storage.runStorage(StorageConstants.STORAGE_SPEED); } else{ m_storage.runStorage(0); @@ -49,12 +49,13 @@ public class StoragePrepAim extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){ + /*if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){ m_storage.m_isStorageReadyToFire = true; return true; } else { m_storage.m_isStorageReadyToFire = false; return false; - } + }*/ + return true; } } diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java b/src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java index c935623..b9a9cf1 100644 --- a/src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java +++ b/src/main/java/frc4388/robot/commands/storage/StoragePrepIntake.java @@ -55,7 +55,7 @@ public class StoragePrepIntake extends CommandBase { @Override public boolean isFinished() { if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){ - return true; + return false; } return false; } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 0e7571f..d8bb022 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -47,12 +47,14 @@ public class Shooter extends SubsystemBase { //Testing purposes reseting gyros //resetGyroAngleAdj(); shooterTrims = new Trims(0, 0); + //SmartDashboard.putNumber("Velocity Target", 10000); + //SmartDashboard.putNumber("Angle Target", 3); m_shooterFalcon.configFactoryDefault(); m_shooterFalcon.setNeutralMode(NeutralMode.Coast); m_shooterFalcon.setInverted(true); m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.configClosedloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.configClosedloopRamp(1.0, ShooterConstants.SHOOTER_TIMEOUT_MS); setShooterGains(); m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); @@ -64,6 +66,8 @@ public class Shooter extends SubsystemBase { m_shooterTable = new ShooterTables(); + m_shooterFalcon.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + //SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10)); //SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200)); //SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250)); @@ -83,6 +87,10 @@ public class Shooter extends SubsystemBase { SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel); + SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature()); + + SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent()); + //SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition()); } @@ -129,7 +137,7 @@ public class Shooter extends SubsystemBase { * @param targetVel Target velocity to run motor at */ public void runDrumShooterVelocityPID(double targetVel) { - System.out.println("Target Velocity" + targetVel); + //System.out.println("Target Velocity" + targetVel); m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index aed97fa..a34d59f 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -50,6 +50,8 @@ public class ShooterAim extends SubsystemBase { m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_LEFT_SOFT_LIMIT); + + m_shooterRotateMotor.setInverted(false); } @Override diff --git a/src/main/java/frc4388/robot/subsystems/ShooterHood.java b/src/main/java/frc4388/robot/subsystems/ShooterHood.java index c12851b..c177c38 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterHood.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterHood.java @@ -48,10 +48,10 @@ public class ShooterHood extends SubsystemBase { m_hoodUpLimit.enableLimitSwitch(true); m_hoodDownLimit.enableLimitSwitch(true); - m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); - m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT); - m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT); + //m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + //m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + //m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT); + //m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT); } @Override From d4804949106d31ebabaead55d12feefa0d623426 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Thu, 5 Mar 2020 18:50:58 -0700 Subject: [PATCH 19/21] update dependancies --- build.gradle | 2 +- vendordeps/Phoenix.json | 30 +++++++++++++++--------------- vendordeps/REVRobotics.json | 10 +++++----- 3 files changed, 21 insertions(+), 21 deletions(-) diff --git a/build.gradle b/build.gradle index e0194f3..2b322db 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2020.2.2" + id "edu.wpi.first.GradleRIO" version "2020.3.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index c6ec878..aa5554e 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix", - "version": "5.18.1", + "version": "5.18.2", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "http://devsite.ctr-electronics.com/maven/release/" @@ -11,19 +11,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.18.1" + "version": "5.18.2" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.18.1" + "version": "5.18.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.18.1", + "version": "5.18.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -35,7 +35,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.18.1", + "version": "5.18.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -47,7 +47,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.18.1", + "version": "5.18.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +58,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.18.1", + "version": "5.18.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -69,7 +69,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.18.1", + "version": "5.18.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -83,7 +83,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.18.1", + "version": "5.18.2", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": false, @@ -97,7 +97,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.18.1", + "version": "5.18.2", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": false, @@ -111,7 +111,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.18.1", + "version": "5.18.2", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": false, @@ -125,7 +125,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.18.1", + "version": "5.18.2", "libName": "CTRE_PhoenixDiagnostics", "headerClassifier": "headers", "sharedLibrary": false, @@ -139,7 +139,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.18.1", + "version": "5.18.2", "libName": "CTRE_PhoenixCanutils", "headerClassifier": "headers", "sharedLibrary": false, @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.18.1", + "version": "5.18.2", "libName": "CTRE_PhoenixPlatform", "headerClassifier": "headers", "sharedLibrary": false, @@ -165,7 +165,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.18.1", + "version": "5.18.2", "libName": "CTRE_PhoenixCore", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVRobotics.json b/vendordeps/REVRobotics.json index 63380b6..5eb069f 100644 --- a/vendordeps/REVRobotics.json +++ b/vendordeps/REVRobotics.json @@ -1,7 +1,7 @@ { "fileName": "REVRobotics.json", "name": "REVRobotics", - "version": "1.5.1", + "version": "1.5.2", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "http://www.revrobotics.com/content/sw/max/sdk/maven/" @@ -11,14 +11,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-java", - "version": "1.5.1" + "version": "1.5.2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-driver", - "version": "1.5.1", + "version": "1.5.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -35,7 +35,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-cpp", - "version": "1.5.1", + "version": "1.5.2", "libName": "SparkMax", "headerClassifier": "headers", "sharedLibrary": false, @@ -52,7 +52,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-driver", - "version": "1.5.1", + "version": "1.5.2", "libName": "SparkMaxDriver", "headerClassifier": "headers", "sharedLibrary": false, From 8113ce1a08aad0b5784b38ffa11e269ec1b5a206 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 6 Mar 2020 16:42:53 -0700 Subject: [PATCH 20/21] Improve Climber Code --- .../java/frc4388/robot/RobotContainer.java | 9 +++- .../commands/climber/DisengageRachet.java | 47 +++++++++++++++++++ .../climber/RunClimberWithTriggers.java | 3 ++ .../climber/RunLevelerWithJoystick.java | 2 +- .../frc4388/robot/subsystems/Climber.java | 21 ++++++++- .../frc4388/robot/subsystems/Leveler.java | 12 ++++- 6 files changed, 88 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/climber/DisengageRachet.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 56e9545..c44884a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -29,6 +29,7 @@ import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Constants.OIConstants; import frc4388.robot.commands.auto.AutoPath1FromCenter; import frc4388.robot.commands.auto.Wait; +import frc4388.robot.commands.climber.DisengageRachet; import frc4388.robot.commands.climber.RunClimberWithTriggers; import frc4388.robot.commands.climber.RunLevelerWithJoystick; import frc4388.robot.commands.drive.DriveStraightToPositionMM; @@ -101,6 +102,8 @@ public class RobotContainer { m_robotShooterHood.passRequiredSubsystem(m_robotShooter); m_robotShooterAim.passRequiredSubsystem(m_robotShooter); + m_robotLeveler.passRequiredSubsystem(m_robotClimber); + configureButtonBindings(); /* Default Commands */ @@ -118,7 +121,7 @@ public class RobotContainer { // drives climber with input from triggers on the opperator controller m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // drives the leveler with an axis input from the driver controller - m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); + m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getOperatorController())); // 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 storage not @@ -161,7 +164,9 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); - + // Disengages the rachet to allow for a climb + new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) + .whileHeld(new DisengageRachet(m_robotClimber)); /* Operator Buttons */ // shoots until released diff --git a/src/main/java/frc4388/robot/commands/climber/DisengageRachet.java b/src/main/java/frc4388/robot/commands/climber/DisengageRachet.java new file mode 100644 index 0000000..631e23e --- /dev/null +++ b/src/main/java/frc4388/robot/commands/climber/DisengageRachet.java @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* 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.climber; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Climber; + +public class DisengageRachet extends CommandBase { + Climber m_climber; + + /** + * Creates a new DisengageRachet command. + */ + public DisengageRachet(Climber subsystem) { + // Use addRequirements() here to declare subsystem dependencies. + m_climber = subsystem; + } + + // 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_climber.climberSafety) { + m_climber.shiftServo(false); + } + } + + // 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 true; + } +} diff --git a/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java b/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java index be72d49..ffa4b68 100644 --- a/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java +++ b/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java @@ -40,12 +40,15 @@ public class RunClimberWithTriggers extends CommandBase { if (rightTrigger < .5) { if(rightTrigger > leftTrigger) { output = rightTrigger; + m_climber.shiftServo(false); } if (leftTrigger > rightTrigger) { output = -leftTrigger; + m_climber.shiftServo(true); } } else { output = rightTrigger; + m_climber.shiftServo(false); } m_climber.runClimber(output); } diff --git a/src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java b/src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java index 7cf5b2f..2052ef4 100644 --- a/src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java @@ -36,7 +36,7 @@ public class RunLevelerWithJoystick extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double input = m_controller.getLeftXAxis(); + double input = m_controller.getRightXAxis(); m_leveler.runLeveler(input); } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index e9adf86..fc932d0 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -13,6 +13,8 @@ import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ClimberConstants; @@ -20,16 +22,21 @@ public class Climber extends SubsystemBase { CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless); CANDigitalInput m_climberForwardLimit, m_climberReverseLimit; + Servo m_servo; + //Spark m_spark = new Spark(4); + public boolean climberSafety = false; /** * Creates a new Climber. */ public Climber() { + m_servo = new Servo(4); + m_climberMotor.restoreFactoryDefaults(); m_climberMotor.setIdleMode(IdleMode.kBrake); - m_climberMotor.setInverted(false); + m_climberMotor.setInverted(true); m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); @@ -48,6 +55,7 @@ public class Climber extends SubsystemBase { */ public void runClimber(double input) { if(climberSafety){ + input *= 1.0; m_climberMotor.set(input); } else{ @@ -66,4 +74,15 @@ public class Climber extends SubsystemBase { { climberSafety = false; } + + /** + * @param shift true to enage rachet, false to disengage + */ + public void shiftServo(boolean shift) { + if (shift) { + m_servo.setPosition(0.5); + } else { + m_servo.setPosition(0.56); + } + } } diff --git a/src/main/java/frc4388/robot/subsystems/Leveler.java b/src/main/java/frc4388/robot/subsystems/Leveler.java index d29c748..be5e77d 100644 --- a/src/main/java/frc4388/robot/subsystems/Leveler.java +++ b/src/main/java/frc4388/robot/subsystems/Leveler.java @@ -17,7 +17,7 @@ import frc4388.robot.Constants.LevelerConstants; public class Leveler extends SubsystemBase { CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless); - private final Climber m_robotClimber = new Climber(); + Climber m_climberSubsystem; /** * Creates a new Leveler. @@ -39,11 +39,19 @@ public class Leveler extends SubsystemBase { * @param input the percent output to run motor at */ public void runLeveler(double input) { - if(m_robotClimber.climberSafety){ + if(m_climberSubsystem.climberSafety){ m_levelerMotor.set(input); } else{ m_levelerMotor.set(0); } } + + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(Climber subsystem) { + m_climberSubsystem = subsystem; + } } From 63471517c24f1872f38bf2b3219bc008d3acab64 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 6 Mar 2020 17:23:51 -0700 Subject: [PATCH 21/21] Do not disengage rachet willy nilly --- .../frc4388/robot/commands/climber/RunClimberWithTriggers.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java b/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java index ffa4b68..7685202 100644 --- a/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java +++ b/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java @@ -40,7 +40,6 @@ public class RunClimberWithTriggers extends CommandBase { if (rightTrigger < .5) { if(rightTrigger > leftTrigger) { output = rightTrigger; - m_climber.shiftServo(false); } if (leftTrigger > rightTrigger) { output = -leftTrigger; @@ -48,7 +47,6 @@ public class RunClimberWithTriggers extends CommandBase { } } else { output = rightTrigger; - m_climber.shiftServo(false); } m_climber.runClimber(output); }