diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 157d101..e5fbcab 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.crevolib.configs.CTREConfigs; +import frc.robot.driver.Driver; import frc.robot.driver.DriverXbox; import frc.robot.operator.OperatorXbox; @@ -46,7 +47,7 @@ public static void resetCommandsAndButtons() { // Reset Config for all gamepads and other button bindings // Driver.getInstance().resetConfig(); // Operator.getInstance().resetConfig(); - DriverXbox.getInstance().resetConfig(); + Driver.getInstance().resetConfig(); OperatorXbox.getInstance().resetConfig(); } diff --git a/src/main/java/frc/robot/algaeflywheel/AlgaeFlyWheel.java b/src/main/java/frc/robot/algaeflywheel/AlgaeFlyWheel.java index 2924dd1..3de7f33 100644 --- a/src/main/java/frc/robot/algaeflywheel/AlgaeFlyWheel.java +++ b/src/main/java/frc/robot/algaeflywheel/AlgaeFlyWheel.java @@ -18,11 +18,11 @@ /** Add your docs here. */ public class AlgaeFlyWheel extends SubsystemBase{ public static class Settings { - static final int kLeftId = 27; - static final int kRightId = 28; + static final int kLeftId = 15; + static final int kRightId = 16; - static final InvertedValue kAlgaeScoringInverted = InvertedValue.Clockwise_Positive; - static final InvertedValue kAlgaeIntakingInverted = InvertedValue.CounterClockwise_Positive; + public static final InvertedValue kAlgaeScoringInverted = InvertedValue.Clockwise_Positive; + public static final InvertedValue kAlgaeIntakingInverted = InvertedValue.CounterClockwise_Positive; static final Slot0Configs kAlgaeFlyWheelConfigs = new Slot0Configs() .withKS(0.0) @@ -44,15 +44,15 @@ public static class Settings { private AlgaeFlyWheel() { - LeftFlyWheel = new TalonFX(Settings.kLeftId, "Canivore"); - var leftTalonFXConfigurator = LeftFlyWheel.getConfigurator(); + LeftFlyWheel = new TalonFX(Settings.kLeftId); + leftTalonFXConfigurator = LeftFlyWheel.getConfigurator(); leftMotorConfigs = new MotorOutputConfigs(); leftTalonFXConfigurator.apply(Settings.kAlgaeFlyWheelConfigs); - RightFlyWheel = new TalonFX(Settings.kRightId, "Canivore"); - var rightTalonFXConfigurator = RightFlyWheel.getConfigurator(); + RightFlyWheel = new TalonFX(Settings.kRightId); + rightTalonFXConfigurator = RightFlyWheel.getConfigurator(); rightMotorConfigs = new MotorOutputConfigs(); rightTalonFXConfigurator.apply(Settings.kAlgaeFlyWheelConfigs); @@ -65,17 +65,17 @@ public static AlgaeFlyWheel getInstance() { return mInstance; } - public void setLeftFlywheelVelocity(Rotation2d velocity, InvertedValue kInvertedValue) { + public void setLeftFlywheelVelocity(Rotation2d velocity, InvertedValue kLeftInvertedValue) { // set invert to CW+ and apply config change - leftMotorConfigs.Inverted = kInvertedValue; + leftMotorConfigs.Inverted = kLeftInvertedValue; leftTalonFXConfigurator.apply(leftMotorConfigs); LeftFlyWheel.setControl(new VelocityVoltage(velocity.getRotations())); } - public void setRightFlywheelVelocity(Rotation2d velocity, InvertedValue kInvertedValue) { + public void setRightFlywheelVelocity(Rotation2d velocity, InvertedValue kRightInvertedValue) { // set invert to CW+ and apply config change - rightMotorConfigs.Inverted = kInvertedValue; + rightMotorConfigs.Inverted = kRightInvertedValue; rightTalonFXConfigurator.apply(rightMotorConfigs); RightFlyWheel.setControl(new VelocityVoltage(velocity.getRotations())); diff --git a/src/main/java/frc/robot/algaeflywheel/commands/AlgaeFlyWheelCommands.java b/src/main/java/frc/robot/algaeflywheel/commands/AlgaeFlyWheelCommands.java index 3c94347..b13f714 100644 --- a/src/main/java/frc/robot/algaeflywheel/commands/AlgaeFlyWheelCommands.java +++ b/src/main/java/frc/robot/algaeflywheel/commands/AlgaeFlyWheelCommands.java @@ -8,11 +8,11 @@ import com.ctre.phoenix6.signals.InvertedValue; public class AlgaeFlyWheelCommands { - public static Command setAngularVelocity(Supplier leftVelocitySupplier, Supplier rightVelocitySupplier, InvertedValue kInvertedValue) { - return new SetVelocityAlgaeFlyWheel(leftVelocitySupplier, rightVelocitySupplier, kInvertedValue); + public static Command setAngularVelocity(Supplier leftVelocitySupplier, Supplier rightVelocitySupplier, InvertedValue kLeftInvertedValue, InvertedValue kRightInvertedValue) { + return new SetVelocityAlgaeFlyWheel(leftVelocitySupplier, rightVelocitySupplier, kLeftInvertedValue, kRightInvertedValue); } - public static Command setAngularVelocity(Supplier velocitySupplier, InvertedValue kInvertedValue) { - return setAngularVelocity(velocitySupplier, velocitySupplier, kInvertedValue); + public static Command setAngularVelocity(Supplier velocitySupplier, InvertedValue kLeftInvertedValue, InvertedValue kRightInvertedValue) { + return setAngularVelocity(velocitySupplier, velocitySupplier, kLeftInvertedValue, kRightInvertedValue); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/algaeflywheel/commands/SetVelocityAlgaeFlyWheel.java b/src/main/java/frc/robot/algaeflywheel/commands/SetVelocityAlgaeFlyWheel.java index de85093..a0b4bed 100644 --- a/src/main/java/frc/robot/algaeflywheel/commands/SetVelocityAlgaeFlyWheel.java +++ b/src/main/java/frc/robot/algaeflywheel/commands/SetVelocityAlgaeFlyWheel.java @@ -14,27 +14,28 @@ public class SetVelocityAlgaeFlyWheel extends Command { private final AlgaeFlyWheel flywheel; private final Supplier leftVelocitySupplier, rightVelocitySupplier; - private final InvertedValue kInvertedValue; + private final InvertedValue kLeftInvertedValue, kRightInvertedValue; private final Rotation2d kAllowedError = Rotation2d.fromRotations(5); // 300 RPM - SetVelocityAlgaeFlyWheel(Supplier leftVelocitySupplier, Supplier rightVelocitySupplier, InvertedValue kInvertedValue) { + SetVelocityAlgaeFlyWheel(Supplier leftVelocitySupplier, Supplier rightVelocitySupplier, InvertedValue kLeftInvertedValue, InvertedValue kRightInvertedValue) { flywheel = AlgaeFlyWheel.getInstance(); this.leftVelocitySupplier = leftVelocitySupplier; this.rightVelocitySupplier = rightVelocitySupplier; - this.kInvertedValue = kInvertedValue; + this.kLeftInvertedValue = kLeftInvertedValue; + this.kRightInvertedValue = kRightInvertedValue; } - SetVelocityAlgaeFlyWheel(Supplier velocitySupplier, InvertedValue kInvertedValue) { - this(velocitySupplier, velocitySupplier, kInvertedValue); + SetVelocityAlgaeFlyWheel(Supplier velocitySupplier, InvertedValue kLeftInvertedValue, InvertedValue kRightInvertedValue) { + this(velocitySupplier, velocitySupplier, kLeftInvertedValue, kRightInvertedValue); } @Override public void execute() { final var leftVel = leftVelocitySupplier.get(); final var rightVel = rightVelocitySupplier.get(); - flywheel.setRightFlywheelVelocity(leftVel, kInvertedValue); - flywheel.setLeftFlywheelVelocity(rightVel, kInvertedValue); + flywheel.setRightFlywheelVelocity(leftVel, kRightInvertedValue); + flywheel.setLeftFlywheelVelocity(rightVel, kLeftInvertedValue); SmartDashboard.putBoolean("Shooter Ready (left)", (Math.abs(leftVel.getRotations()) - (Math.abs(flywheel.getLeftFlywheelVelocity().getRotations()))) < kAllowedError.getRotations()); SmartDashboard.putBoolean("Shooter Ready (right)", (Math.abs(rightVel.getRotations()) - (Math.abs(flywheel.getRightFlywheelVelocity().getRotations()))) < kAllowedError.getRotations()); @@ -55,7 +56,7 @@ public boolean isFinished() { @Override public void end(boolean isInterrupted) { - flywheel.setRightFlywheelVelocity(Rotation2d.fromDegrees(0), kInvertedValue); - flywheel.setLeftFlywheelVelocity(Rotation2d.fromDegrees(0), kInvertedValue); + flywheel.setRightFlywheelVelocity(Rotation2d.fromDegrees(0), kRightInvertedValue); + flywheel.setLeftFlywheelVelocity(Rotation2d.fromDegrees(0), kLeftInvertedValue); } } diff --git a/src/main/java/frc/robot/algaepivot/AlgaeShooterPivot.java b/src/main/java/frc/robot/algaepivot/AlgaeShooterPivot.java index 6631e19..6a77c89 100644 --- a/src/main/java/frc/robot/algaepivot/AlgaeShooterPivot.java +++ b/src/main/java/frc/robot/algaepivot/AlgaeShooterPivot.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; @@ -48,12 +49,14 @@ public class Settings{ private Constraints mConstraints; private final ProfiledPIDController mPPIDController; private final ArmFeedforward mFFController; + private MotorOutputConfigs motorConfigs; + private TalonFXConfigurator talonFXConfigurator; public AlgaeShooterPivot() { mKraken = new TalonFX(Settings.kAlgaePivotTalonID,"Canivore"); - var talonFXConfigurator = mKraken.getConfigurator(); - var motorConfigs = new MotorOutputConfigs(); + talonFXConfigurator = mKraken.getConfigurator(); + motorConfigs = new MotorOutputConfigs(); motorConfigs.Inverted = InvertedValue.Clockwise_Positive; talonFXConfigurator.apply(motorConfigs); diff --git a/src/main/java/frc/robot/driver/Driver.java b/src/main/java/frc/robot/driver/Driver.java index f713aa3..1a935fd 100644 --- a/src/main/java/frc/robot/driver/Driver.java +++ b/src/main/java/frc/robot/driver/Driver.java @@ -7,9 +7,13 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.crevolib.util.ExpCurve; import frc.crevolib.util.Gamepad; +import frc.crevolib.util.XboxGamepad; import frc.robot.Robot; +import frc.robot.algaeflywheel.AlgaeFlyWheel; +import frc.robot.algaeflywheel.commands.AlgaeFlyWheelCommands; +import frc.robot.indexer.commands.IndexerCommands; -public class Driver extends Gamepad { +public class Driver extends XboxGamepad { private static class Settings { static final int port = 0; static final String name = "driver"; @@ -38,9 +42,18 @@ public static Driver getInstance() { @Override public void setupTeleopButtons() { + controller.a().whileTrue( + AlgaeFlyWheelCommands.setAngularVelocity( + () -> AlgaeFlyWheel.Settings.kMaxAngularVelocity.times(0.8), + AlgaeFlyWheel.Settings.kAlgaeScoringInverted, + AlgaeFlyWheel.Settings.kAlgaeIntakingInverted + )); + + controller.b().whileTrue(IndexerCommands.setOutput(() ->0.10)); // Drivetrain Commands + } @Override diff --git a/src/main/java/frc/robot/driver/DriverXbox.java b/src/main/java/frc/robot/driver/DriverXbox.java index 46fad91..f7edfdc 100644 --- a/src/main/java/frc/robot/driver/DriverXbox.java +++ b/src/main/java/frc/robot/driver/DriverXbox.java @@ -14,6 +14,9 @@ import frc.crevolib.util.ExpCurve; import frc.crevolib.util.XboxGamepad; import frc.robot.Robot; +import frc.robot.algaeflywheel.AlgaeFlyWheel; +import frc.robot.algaeflywheel.commands.AlgaeFlyWheelCommands; +import frc.robot.algaeflywheel.commands.SetVelocityAlgaeFlyWheel; public class DriverXbox extends XboxGamepad { @@ -31,6 +34,7 @@ private static class Settings { private static ExpCurve rotationStickCurve; public boolean autoAim; private double reqAngularVel; + private DriverXbox() { super(DriverXbox.Settings.name, DriverXbox.Settings.port); @@ -48,6 +52,7 @@ public static DriverXbox getInstance() { @Override public void setupTeleopButtons() { + // Drivetrain Commands diff --git a/src/main/java/frc/robot/indexer/Indexer.java b/src/main/java/frc/robot/indexer/Indexer.java index 923cf59..2346931 100644 --- a/src/main/java/frc/robot/indexer/Indexer.java +++ b/src/main/java/frc/robot/indexer/Indexer.java @@ -10,7 +10,7 @@ public class Indexer extends SubsystemBase{ public static class Settings { - static int kTalonID = 10; + static int kTalonID = 17; static final double kMaxVoltage = 12.0; static final int kCurrentLimit = 200; static final int kCurrentThreshold = 100; @@ -25,7 +25,7 @@ public Indexer() { var talonFXConfigurator = mKraken.getConfigurator(); var motorConfigs = new MotorOutputConfigs(); - motorConfigs.Inverted = InvertedValue.Clockwise_Positive; + motorConfigs.Inverted = InvertedValue.CounterClockwise_Positive; talonFXConfigurator.apply(motorConfigs); } diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index c580bb7..e3c2af9 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -134,4 +134,4 @@ private void updateEstimationStdDevs(Optional estimatedPose, public Matrix getEstimationStdDevs() { return curStdDevs; } -} +} \ No newline at end of file