diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 62a15ef..3ee22e6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -116,7 +116,7 @@ public final class Constants { public static class HopperConstants { public static final int KICKER_MOTOR_ID = 9; - public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; + public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; public static final int SPINDEXER_MOTOR_ID = 8; public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 397d7ad..5bc5953 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -63,6 +63,14 @@ public class RobotContainer { 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.b().whileTrue(turretSubsystem.setShooter(1500)); + m_driverController.x().whileTrue(turretSubsystem.setShooter(2000)); + m_driverController.y().whileTrue(turretSubsystem.setShooter(3000)); m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); } diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index fc9a56b..d30d6fc 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -74,7 +74,7 @@ public class AutoShootCommandChassisTurret extends Command{ } }); - turretSubsystem.setLauncher(2500); + turretSubsystem.setLauncher(5500); hopperSubsystem.spindexerOn(); cameraAutoCenterController.calculate(-100); diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 87af396..1f709e1 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -20,6 +20,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; @@ -31,6 +32,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; public class TurretSubsystem extends TestableSubsystem { public enum TurretState { @@ -77,8 +79,14 @@ public class TurretSubsystem extends TestableSubsystem { launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless); 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); + config.voltageCompensation(10); + config.closedLoop.p(0.0001).i(0).d(0.0); // I = 0.0000001 + config.closedLoop.feedForward.kS(0.2).kV(0.002).kA(0.00); + // config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0); + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + config.closedLoop.positionWrappingEnabled(false); + // config.closedLoopRampRate(0.5); + // config.smartCurrentLimit(5, 50); launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); launcherControler = launcherMotor.getClosedLoopController(); @@ -90,10 +98,13 @@ public class TurretSubsystem extends TestableSubsystem { turretGoingPos = false; } - public void setLauncher(double speed) {launcherControler.setSetpoint(speed, ControlType.kVelocity);} + public void setLauncher(double speed) { + launcherControler.setSetpoint(speed, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + // launcherMotor.set(speed/1000); + } public boolean isLauncherUpToSpeed() { - return launcherControler.isAtSetpoint(); + return true; } public void turretControl(double position) @@ -113,6 +124,14 @@ public class TurretSubsystem extends TestableSubsystem { return this.runOnce(() -> setLauncher(-.5)); } + public Command setShooter(int speedRPM){ + return Commands.runEnd(() -> { + setLauncher(speedRPM); + }, () -> { + launcherMotor.stopMotor(); + }, this); + } + public void startHoming() { turretMotor.set(Constants.ShooterConstants.HOMING_SPEED); @@ -159,7 +178,8 @@ public class TurretSubsystem extends TestableSubsystem { if(LOGGING) { - Logger.recordOutput("Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); + Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); + Logger.recordOutput("TurretSubsystem-Launcher Volatge", launcherMotor.getOutputCurrent()); } }