Merge pull request #11 from joelzoto/main

Tested out Indexer and Flywheel Commands

Author: Eddie-Edward

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();
   }
 

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()));

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<Rotation2d> leftVelocitySupplier, Supplier<Rotation2d> rightVelocitySupplier, InvertedValue kInvertedValue) {
-        return new SetVelocityAlgaeFlyWheel(leftVelocitySupplier, rightVelocitySupplier, kInvertedValue);
+    public static Command setAngularVelocity(Supplier<Rotation2d> leftVelocitySupplier, Supplier<Rotation2d> rightVelocitySupplier, InvertedValue kLeftInvertedValue, InvertedValue kRightInvertedValue) {
+        return new SetVelocityAlgaeFlyWheel(leftVelocitySupplier, rightVelocitySupplier, kLeftInvertedValue, kRightInvertedValue);
     }
 
-    public static Command setAngularVelocity(Supplier<Rotation2d> velocitySupplier, InvertedValue kInvertedValue) {
-        return setAngularVelocity(velocitySupplier, velocitySupplier, kInvertedValue);
+    public static Command setAngularVelocity(Supplier<Rotation2d> velocitySupplier, InvertedValue kLeftInvertedValue, InvertedValue kRightInvertedValue) {
+        return setAngularVelocity(velocitySupplier, velocitySupplier, kLeftInvertedValue, kRightInvertedValue);
     }
 }

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<Rotation2d> leftVelocitySupplier, rightVelocitySupplier;
-    private final InvertedValue kInvertedValue;
+    private final InvertedValue kLeftInvertedValue, kRightInvertedValue;
 
     private final Rotation2d kAllowedError = Rotation2d.fromRotations(5); // 300 RPM
 
-    SetVelocityAlgaeFlyWheel(Supplier<Rotation2d> leftVelocitySupplier, Supplier<Rotation2d> rightVelocitySupplier, InvertedValue kInvertedValue) {
+    SetVelocityAlgaeFlyWheel(Supplier<Rotation2d> leftVelocitySupplier, Supplier<Rotation2d> 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<Rotation2d> velocitySupplier, InvertedValue kInvertedValue) {
-        this(velocitySupplier, velocitySupplier, kInvertedValue);
+    SetVelocityAlgaeFlyWheel(Supplier<Rotation2d> 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);
     }
 }

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);

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

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
 
 

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);
 
   }

src/main/java/frc/robot/vision/Vision.java

@@ -134,4 +134,4 @@ private void updateEstimationStdDevs(Optional<EstimatedRobotPose> estimatedPose,
     public Matrix<N3, N1> getEstimationStdDevs() {
         return curStdDevs;
     }
-}
+}