diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0737efd..e702b94 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,7 +27,6 @@ public final class Constants { public static class IDFilters { public static final int[] RED_HUB_SHOOTING_IDS = new int[]{ 8, 9, 10, 11 }; public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{ 27, 26, 25, 24 }; - // public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{ 18, 19, 20, 21 }; } public static final String BACK_CAMERA_NAME = "limelight-back"; @@ -97,7 +96,7 @@ public final class Constants { public static final int TURRET_MOTOR_ID = 4; public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; public static final int TURRET_LIMIT_CHANNEL = 0; - public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(265); + public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165); public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5); public static final double HOMING_SPEED = .35; public static final double TURRET_GEAR_RATIO = 1; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1a2646d..397d7ad 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -42,7 +42,6 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - // Configure the trigger bindings configureBindings(); } @@ -58,20 +57,13 @@ public class RobotContainer { */ private void configureBindings() { - // turretSubsystem.setDefaultCommand(new ManualShootCommand(turretSubsystem, m_driverController.getHID())); - // hopperSubsystem.setDefaultCommand(new ManualHopperCommand(hopperSubsystem, m_driverController.getHID())); - // intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID())); - + turretSubsystem.startHoming(); m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)); m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1)); m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2)); - // m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand()); - // m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand()); - m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); - } /** diff --git a/src/main/java/frc/robot/commands/ManualShootCommand.java b/src/main/java/frc/robot/commands/ManualShootCommand.java index 3225c01..dbe750c 100644 --- a/src/main/java/frc/robot/commands/ManualShootCommand.java +++ b/src/main/java/frc/robot/commands/ManualShootCommand.java @@ -24,24 +24,19 @@ public class ManualShootCommand extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(m_controller.getRightTriggerAxis()>.8) + if(m_controller.getRightTriggerAxis()>.5 && m_controller.getLeftTriggerAxis()==0) { - m_subsystem.setLauncher(5000); - m_controller.setRumble(RumbleType.kBothRumble, 1); + m_subsystem.startRotate(-.5); + m_controller.setRumble(RumbleType.kRightRumble, 1); } - else if(m_controller.getRightTriggerAxis()>.5) + else if(m_controller.getLeftTriggerAxis()>.5 && m_controller.getRightTriggerAxis()==0) { - m_subsystem.setLauncher(3000); - m_controller.setRumble(RumbleType.kBothRumble, .67); - } - else if(m_controller.getRightTriggerAxis()>.2) - { - m_subsystem.setLauncher(2000); - m_controller.setRumble(RumbleType.kBothRumble, .33); + m_subsystem.startRotate(.5); + m_controller.setRumble(RumbleType.kLeftRumble, 1); } else { - m_subsystem.setLauncher(0); + m_subsystem.startRotate(0); m_controller.setRumble(RumbleType.kBothRumble, 0); } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 6198ada..87af396 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -27,8 +27,6 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkMaxConfig; import frc.robot.Constants; -import edu.wpi.first.units.AngleUnit; -import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; @@ -42,7 +40,6 @@ public class TurretSubsystem extends TestableSubsystem { } private final boolean LOGGING = true; - private TurretState currentState; private DigitalInput turretLimitSwitch; private TalonFX turretMotor; @@ -78,8 +75,7 @@ public class TurretSubsystem extends TestableSubsystem { // Launcher configuration launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless); - SparkMaxConfig config = new SparkMaxConfig() - .apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO); + SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO); config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED); config.closedLoop.p(1).i(0).d(0.1); config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0); @@ -119,13 +115,13 @@ public class TurretSubsystem extends TestableSubsystem { public void startHoming() { - //turretMotor.set(Constants.ShooterConstants.HOMING_SPEED); - //currentState = TurretState.HOMING; + turretMotor.set(Constants.ShooterConstants.HOMING_SPEED); + currentState = TurretState.HOMING; } public void startRotate(double speed) { - /*turretGoingPos=speed>0; + turretGoingPos=speed>0; if(!passedLimit()) { @@ -134,7 +130,7 @@ public class TurretSubsystem extends TestableSubsystem { else { turretMotor.set(0); - }*/ + } } public boolean passedLimit(){ @@ -148,7 +144,6 @@ public class TurretSubsystem extends TestableSubsystem { @Override public void periodic() { - // This method will be called once per scheduler run switch (currentState) { case HOMING: @@ -174,19 +169,36 @@ public class TurretSubsystem extends TestableSubsystem { if(turretLimitSwitch.get()) { currentState = TurretState.MANUAL; - //turretMotor.set(0); - //turretMotor.setPosition(0); + turretMotor.set(0); + turretMotor.setPosition(0); + goTo(limitPos); + } + } + + private void goTo(Angle pos) + { + if(pos.gt(limitNeg) && pos.lt(limitPos)) + { + while(getTurretAngle()!=pos) + { + if(getTurretAngle().gt(pos)) + { + turretMotor.set(.5); + } + else + { + turretMotor.set(-.5); + } + } } } private void periodicManual() - { - //If the turret move, it will be a uh-oh, so DONT MOVE IT - - /*if(passedLimit()) + { + if(passedLimit()) { - //turretMotor.set(0); - }*/ + turretMotor.set(0); + } } @PostTest()