diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 0255f40..e89f8d9 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -137,6 +137,7 @@ public class Robot extends TimedRobot { m_robotContainer.m_robotClimber.getCurrent() + // m_robotContainer.m_robotHood.getCurrent() + m_robotContainer.m_robotIntake.getCurrent() + + m_robotContainer.m_robotExtender.getCurrent() + m_robotContainer.m_robotSerializer.getCurrent() + m_robotContainer.m_robotStorage.getCurrent() + m_robotContainer.m_robotSwerveDrive.getCurrent(); @@ -151,10 +152,10 @@ public class Robot extends TimedRobot { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - VelocityCorrection vc = new VelocityCorrection(m_robotContainer.m_robotSwerveDrive, m_robotContainer.m_robotBoomBoom); - System.out.println("Position: " + vc.position); - System.out.println("Velocity: " + vc.cartesianVelocity); - System.out.println("Target: " + vc.target.toString()); + // VelocityCorrection vc = new VelocityCorrection(m_robotContainer.m_robotSwerveDrive, m_robotContainer.m_robotBoomBoom); + // System.out.println("Position: " + vc.position); + // System.out.println("Velocity: " + vc.cartesianVelocity); + // System.out.println("Target: " + vc.target.toString()); //SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition()); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 45095c2..fc00270 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -67,6 +67,7 @@ import frc4388.robot.commands.Shoot; import frc4388.robot.commands.TrackTarget; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.Extender; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -98,7 +99,9 @@ public class RobotContainer { // Subsystems public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); - public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); + public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotSerializer); + public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor); + public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); // private final LED m_robotLED = new LED(m_robotMap.LEDController); // public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); @@ -258,11 +261,11 @@ public class RobotContainer { .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(1.0), m_robotIntake)) - .whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0.0), m_robotIntake)); + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-1.0), m_robotIntake)) - .whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0.0), m_robotIntake)); + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); // Right Bumper > Storage In // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java new file mode 100644 index 0000000..f0749a5 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Extender.java @@ -0,0 +1,66 @@ +// 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.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMaxLimitSwitch; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Extender extends SubsystemBase { + + CANSparkMax m_extenderMotor; + + private SparkMaxLimitSwitch m_inLimit; + private SparkMaxLimitSwitch m_outLimit; + + public boolean toggle; + + /** Creates a new Extender. */ + public Extender(CANSparkMax extenderMotor) { + + m_extenderMotor = extenderMotor; + + m_extenderMotor.restoreFactoryDefaults(); + m_extenderMotor.setInverted(true); + + m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_inLimit.enableLimitSwitch(true); + m_outLimit.enableLimitSwitch(true); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Runs The Extender- + * @param extended Wether the Extender Is Extended + */ + // public void runExtender(boolean extended) { + // if (!m_serializer.getBeam() && !extended) return; + // double extenderMotorSpeed = extended ? 0.25d : -0.25d; + // m_extenderMotor.set(extenderMotorSpeed); + // } + + public void runExtender(double input) { + // if (!m_serializer.getBeam() && input < 0.) return; + m_extenderMotor.set(input); + } + + public double getCurrent() { + return m_extenderMotor.getOutputCurrent(); + } + + /** + * Toggles The Extender + */ + // public void toggleExtender() { + // toggle = !toggle; + // runExtender(toggle); + // } +} diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 2754c6d..4c086b2 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -17,29 +17,15 @@ import com.revrobotics.CANSparkMax; public class Intake extends SubsystemBase { private WPI_TalonFX m_intakeMotor; - private CANSparkMax m_extenderMotor; private Serializer m_serializer; - private SparkMaxLimitSwitch m_inLimit; - private SparkMaxLimitSwitch m_outLimit; - - public boolean toggle; /** Creates a new Intake. */ - public Intake(WPI_TalonFX intakeMotor, CANSparkMax extenderMotor, Serializer serializer) { + public Intake(WPI_TalonFX intakeMotor, Serializer serializer) { m_intakeMotor = intakeMotor; - m_extenderMotor = extenderMotor; m_serializer = serializer; - m_extenderMotor.restoreFactoryDefaults(); - m_intakeMotor.setNeutralMode(NeutralMode.Brake); m_intakeMotor.setInverted(false); - m_extenderMotor.setInverted(true); - - m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_inLimit.enableLimitSwitch(true); - m_outLimit.enableLimitSwitch(true); } @Override @@ -56,32 +42,8 @@ public class Intake extends SubsystemBase { SmartDashboard.putNumber("Intake Current Supply", m_intakeMotor.getSupplyCurrent()); SmartDashboard.putNumber("Intake Current Stator", m_intakeMotor.getStatorCurrent()); } - /** - * Runs The Extender- - * @param extended Wether the Extender Is Extended - */ - public void runExtender(boolean extended) { - if (!m_serializer.getBeam() && !extended) return; - double extenderMotorSpeed = extended ? 0.25d : -0.25d; - m_extenderMotor.set(extenderMotorSpeed); - } - /** - * Moves the extender motor to pull the intake in or out - * @param input A value from -1.0 to 1.0, positive is in - */ - public void runExtender(double input) { - if (!m_serializer.getBeam() && input < 0.) return; - m_extenderMotor.set(input); - } - /** - * Toggles The Extender - */ - public void toggleExtender() { - toggle = !toggle; - runExtender(toggle); - } - - public double getCurrent(){ - return m_intakeMotor.getSupplyCurrent() + m_extenderMotor.getOutputCurrent(); + + public double getCurrent() { + return m_intakeMotor.getSupplyCurrent(); } } \ No newline at end of file