diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a68376e..1e6cd5b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -62,7 +62,7 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.commands.RunMiddleSwitch; +import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.AimToCenter; import frc4388.robot.commands.ShooterCommands.Shoot; @@ -316,7 +316,7 @@ public class RobotContainer { .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret)); + // .whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)); //B > Shoot with Lime new JoystickButton(getOperatorController(), XboxController.Button.kB.value) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 91b1854..065ec83 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -188,16 +188,16 @@ public class RobotMap { SwerveDriveConstants.SWERVE_TIMEOUT_MS); } -// // Shooter Config -// /* Boom Boom Subsystem */ + // Shooter Config + /* Boom Boom Subsystem */ public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); -// // turret subsystem + // turret subsystem public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); // hood subsystem - public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); // Create motor CANSparkMax void configureShooterMotorControllers() { @@ -219,9 +219,9 @@ public class RobotMap { ShooterConstants.SHOOTER_TIMEOUT_MS); // RIGHT FALCON - shooterFalconRight.setInverted(false); - shooterFalconRight.setNeutralMode(NeutralMode.Coast); shooterFalconRight.configFactoryDefault(); + shooterFalconRight.setNeutralMode(NeutralMode.Coast); + shooterFalconRight.setInverted(false); shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); // m_shooterFalconRight.configPeakOutputForward(0, @@ -236,24 +236,18 @@ public class RobotMap { ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.follow(shooterFalconLeft); -// } -// // /* Turret Subsytem */ - // shooterFalconRight.configStatorCurrentLimit(new - // StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers - // out of our ass anymore - // shooterFalconLeft.configSupplyCurrentLimit(new - // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull -// // numbers out of our ass anymore + // turret + shooterTurret.restoreFactoryDefaults(); + shooterTurret.setIdleMode(IdleMode.kBrake); + shooterTurret.setInverted(true); -// // hood subsystem + // hood subsystem angleAdjusterMotor.restoreFactoryDefaults(); angleAdjusterMotor.setIdleMode(IdleMode.kBrake); angleAdjusterMotor.setInverted(true); } - - - + /* Serializer Subsystem */ public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); @@ -277,14 +271,12 @@ public class RobotMap { extenderMotor.setIdleMode(IdleMode.kBrake); } - void configureSerializerMotors() { - serializerBelt.restoreFactoryDefaults(); - } + void configureSerializerMotors() { + serializerBelt.restoreFactoryDefaults(); + } - /* Storage Subsystem */ + /* Storage Subsystem */ public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); -// public final DigitalInput beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); -// public final DigitalInput beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); void configureStorageMotors() { storageMotor.restoreFactoryDefaults(); diff --git a/src/main/java/frc4388/robot/commands/RunMiddleSwitch.java b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java similarity index 96% rename from src/main/java/frc4388/robot/commands/RunMiddleSwitch.java rename to src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java index 3a61d55..095cc9f 100644 --- a/src/main/java/frc4388/robot/commands/RunMiddleSwitch.java +++ b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands; +package frc4388.robot.commands.ButtonBoxCommands; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java index 7eb3572..02ce30a 100644 --- a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java @@ -13,13 +13,14 @@ import frc4388.robot.subsystems.Intake; // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class ExtenderIntakeGroup extends ParallelRaceGroup { - public static int direction = 1; // assume extender starts retracted completely + public static int direction; /** Creates a new RunExtenderAndIntake. */ public ExtenderIntakeGroup(Intake intake, Extender extender) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - ExtenderIntakeGroup.direction = 1; // Does this make sense? It kind of defeats the purpose of making it static, does it work without? + + ExtenderIntakeGroup.direction = 1; // assume extender starts retracted completely addCommands(new RunIntakeConditionally(intake), new RunExtender(extender)); } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/SpitOutWrongColor.java b/src/main/java/frc4388/robot/commands/ShooterCommands/SpitOutWrongColor.java new file mode 100644 index 0000000..7be2329 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/SpitOutWrongColor.java @@ -0,0 +1,73 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.ShooterCommands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Storage; +import frc4388.robot.subsystems.Turret; +import frc4388.utility.RobotTime; + +public class SpitOutWrongColor extends CommandBase { + + // subsystems + private BoomBoom drum; + private Turret turret; + private Storage storage; + + // time (in milliseconds) + private long initialTime; + private long elapsedTime; + private long threshold; + + private double initialTurret; + private int spitVelocity; + + /** Creates a new SpitOutWrongColor. */ + public SpitOutWrongColor(BoomBoom drum, Turret turret, Storage storage) { + // Use addRequirements() here to declare subsystem dependencies. + + this.drum = drum; + this.turret = turret; + this.storage = storage; + + initialTime = System.currentTimeMillis(); + elapsedTime = 0; + threshold = 2000; + + initialTurret = this.turret.getEncoderPosition(); + spitVelocity = 2000; + + addRequirements(this.drum, this.turret, this.storage); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + elapsedTime = System.currentTimeMillis() - initialTime; + + this.storage.runStorage(0.9); + this.turret.gotoMidpoint(); + this.drum.runDrumShooterVelocityPID(spitVelocity); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + this.storage.runStorage(0.0); + this.turret.runShooterRotatePID(initialTurret * ShooterConstants.TURRET_DEGREES_PER_ROT); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return (elapsedTime >= threshold); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 5f60cdb..ee63473 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -65,7 +65,7 @@ public class SwerveDrive extends SubsystemBase { m_rightBack = rightBack; m_gyro = gyro; - modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack}; + modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack}; m_poseEstimator = new SwerveDrivePoseEstimator( m_gyro.getRotation2d(), diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index d6aae1a..cb07d7f 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -30,37 +30,42 @@ public class Turret extends SubsystemBase { public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, // MotorType.kBrushless); public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; - // SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit; - public Gyro m_turretGyro; - public double m_targetDistance = 0; + SparkMaxPIDController m_boomBoomRotatePIDController; + public RelativeEncoder m_boomBoomRotateEncoder; - public boolean m_isAimReady = false; + SparkMaxPIDController m_boomBoomRotatePIDController; + public RelativeEncoder m_boomBoomRotateEncoder; - SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController(); - public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); SparkMaxLimitSwitch m_boomBoomLeftLimit; SparkMaxLimitSwitch m_boomBoomRightLimit; - // Variables - public Turret(CANSparkMax boomBoomRotateMotor) { // Take in rotate motor as an argument + public Turret(CANSparkMax boomBoomRotateMotor) { m_boomBoomRotateMotor = boomBoomRotateMotor; m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); - m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_boomBoomRightLimit.enableLimitSwitch(true); m_boomBoomLeftLimit.enableLimitSwitch(true); + // setTurretLimitSwitches(false); + // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); + // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_LIMIT); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT); setTurretSoftLimits(true); - m_boomBoomRotateMotor.setInverted(true); - + setTurretPIDGains(); + } + + /** + * Set gains for turret PIDs. + */ + public void setTurretPIDGains() { m_boomBoomRotatePIDController.setP(m_shooterTGains.kP); m_boomBoomRotatePIDController.setI(m_shooterTGains.kI); m_boomBoomRotatePIDController.setD(m_shooterTGains.kD); @@ -87,6 +92,15 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); } + /** + * Set status of turret limit switches. + * @param set Boolean to set limit switches to. + */ + public void setTurretLimitSwitches(boolean set) { + m_boomBoomRightLimit.enableLimitSwitch(set); + m_boomBoomLeftLimit.enableLimitSwitch(set); + } + public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) { m_boomBoomSubsystem = subsystem0; m_sDriveSubsystem = subsystem1; @@ -115,14 +129,21 @@ public class Turret extends SubsystemBase { runShooterRotatePID(0); } - public double getboomBoomRotatePosition() { + /** + * Run a PID to go to the midpoint position, between the two soft limits. + */ + public void gotoMidpoint() { + runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT); + } + + public double getEncoderPosition() { return m_boomBoomRotateEncoder.getPosition(); } public double getBoomBoomAngleDegrees() { return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360 / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; - } + } // TODO: does this method work? public double getCurrent(){ return m_boomBoomRotateMotor.getOutputCurrent(); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 748bcd5..08d3d5e 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -86,10 +86,10 @@ public void track(){ public void checkFinished(){ if (xAngle < 0.5 && xAngle > -0.5 && target == 1){ - m_turret.m_isAimReady = true; + // m_turret.m_isAimReady = true; } else{ - m_turret.m_isAimReady = false; + // m_turret.m_isAimReady = false; } }