algae pivot chagnes

Author: Eddie-Edward

src/main/java/frc/robot/RobotContainer.java

@@ -5,12 +5,15 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.algaepivot.AlgaeSubsystem;
+import frc.robot.algaepivot.AlgaeSubsystem.State;
+import frc.robot.algaepivot.commands.AlgaePivotCommands;
 import frc.robot.driver.DriverXbox;
 import frc.robot.drivetrain.CommandSwerveDrivetrain;
 import frc.robot.drivetrain.TunerConstants;
 import static edu.wpi.first.units.Units.*;
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
-import frc.robot.subsystems.AlgaeSubsystem;
+import frc.robot.algaepivot.commands.*;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -30,8 +33,6 @@ public class RobotContainer {
 
     public static SendableChooser<Command> mAutonChooser;
 
-    private final AlgaeSubsystem algaeSubsystem = AlgaeSubsystem.getInstance();
-
     public RobotContainer() {
         setDefaultCommands();
     }
@@ -47,16 +48,16 @@ public Command getAutonomousCommand() {
 
     public void setDefaultCommands() {
         final var driver = DriverXbox.getInstance();
-//        CommandSwerveDrivetrain.getInstance().setDefaultCommand(
-//            CommandSwerveDrivetrain.getInstance().applyRequest(() ->
-//                drive.withVelocityX(driver.getDriveTranslation().getX() * kMaxVelocity) // Drive forward with negative Y (forward)
-//                .withVelocityY(driver.getDriveTranslation().getY() * kMaxVelocity) // Drive left with negative X (left)
-//                .withRotationalRate(driver.getDriveRotation() * kMaxAngularVelocity) // Drive counterclockwise with negative X (left)
-//            )
-//        );
-
-        //algaeSubsystem.setDefaultCommand(new AlgaeSubsystem.DefaultCommand(algaeSubsystem, () -> (driver.getRightX() + 1) / 2.0));
-        algaeSubsystem.setDefaultCommand(new AlgaeSubsystem.DefaultCommand(algaeSubsystem, () -> 1.0));
+       CommandSwerveDrivetrain.getInstance().setDefaultCommand(
+           CommandSwerveDrivetrain.getInstance().applyRequest(() ->
+               drive.withVelocityX(driver.getDriveTranslation().getX() * kMaxVelocity) // Drive forward with negative Y (forward)
+               .withVelocityY(driver.getDriveTranslation().getY() * kMaxVelocity) // Drive left with negative X (left)
+               .withRotationalRate(driver.getDriveRotation() * kMaxAngularVelocity) // Drive counterclockwise with negative X (left)
+           )
+       );
+        AlgaeSubsystem.getInstance().setDefaultCommand(
+            new AlgaeSubsystem.DefaultCommand()
+        );
     }
 }
 

src/main/java/frc/robot/algaepivot/AlgaeShooterPivot.java

@@ -1,4 +1,4 @@
-package frc.robot.subsystems;
+package frc.robot.algaepivot;
 
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.MagnetSensorConfigs;
@@ -62,6 +62,8 @@ public enum State {
     private final ArmFeedforward mFFController;
     private final ProfiledPIDController mPPIDController;
 
+    public static State kLastState;
+
     private AlgaeSubsystem() {
         mTalonPivot = new TalonFX(Settings.kTalonPivotID);
         mTalonPivot.getConfigurator().apply(new TalonFXConfiguration().withMotorOutput(new MotorOutputConfigs()
@@ -91,6 +93,7 @@ public static AlgaeSubsystem getInstance() {
     }
 
     public void setTargetState(State targetState) {
+        kLastState = targetState;
         setTargetPosition(targetState.pos);
     }
 
@@ -124,24 +127,24 @@ public void periodic() {
     }
 
     public static class DefaultCommand extends Command {
-        private final Supplier<Double> percentSupplier;
-        private final AlgaeSubsystem subsystem;
 
-        public DefaultCommand(AlgaeSubsystem subsystem, Supplier<Double> percentSupplier) {
-            this.percentSupplier = percentSupplier;
-            this.subsystem = subsystem;
-            addRequirements(subsystem);
+        public DefaultCommand() {
+
         }
 
         @Override
-        public void execute() {
-            var tp  = getTargetPosition();
-            subsystem.setTargetPosition(tp);
+        public void initialize() {
+            if (kLastState == State.kTuck) {
+                AlgaeSubsystem.getInstance().setTargetState(State.kTuck);
+            } else {
+                AlgaeSubsystem.getInstance().setTargetState(State.kStow);
+            }
         }
 
-        private Rotation2d getTargetPosition() {
-            return Rotation2d.fromRotations((Settings.kMaxPos.getRotations() - Settings.kMinPos.getRotations()) * percentSupplier.get() + Settings.kMinPos.getRotations());
+        @Override
+        public void execute() {
         }
+
     }
 
 }

…frc/robot/subsystems/AlgaeSubsystem.java …frc/robot/algaepivot/AlgaeSubsystem.javasrc/main/java/frc/robot/subsystems/AlgaeSubsystem.java renamed to src/main/java/frc/robot/algaepivot/AlgaeSubsystem.java src/main/java/frc/robot/subsystems/AlgaeSubsystem.java renamed to src/main/java/frc/robot/algaepivot/AlgaeSubsystem.java

@@ -4,7 +4,7 @@
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.AlgaeSubsystem;
+import frc.robot.algaepivot.AlgaeSubsystem;
 
 public class AlgaePivotCommands {
     public static Command setAlgaePivotAngle(AlgaeSubsystem.State state) {

src/main/java/frc/robot/algaepivot/commands/AlgaePivotCommands.java

@@ -6,40 +6,25 @@
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.algaepivot.AlgaeShooterPivot;
-import frc.robot.subsystems.AlgaeSubsystem;
+import frc.robot.algaepivot.AlgaeSubsystem;
 
 public class SetAngleAlgaePivot extends Command {
     AlgaeSubsystem mAlgaeSubsystem;
     AlgaeSubsystem.State targetState;
-
-    // public enum State {
-    //     kFloorIntake(Rotation2d.fromRotations(-0.04)),
-    //     kReefIntake(Rotation2d.fromRotations(0)),
-    //     kScore(Rotation2d.fromRotations(0.15)),
-    //     kStow(Rotation2d.fromRotations(0.23));
-
-    //     State(Rotation2d pos) {
-    //         this.pos = pos;
-    //     }
-    //     public final Rotation2d pos;
-    
-
-    //     public double getDegrees() {
-    //         return pos.getDegrees();
-    //     }
-
-    //     public Rotation2d getRotation2d() {
-    //         return pos;
-    //     }
-    // }
+    Rotation2d targetAngle;
 
     public SetAngleAlgaePivot(AlgaeSubsystem.State targetState) {
         mAlgaeSubsystem = AlgaeSubsystem.getInstance();   
         this.targetState = targetState;
         addRequirements(mAlgaeSubsystem);
     }
 
+    // public SetAngleAlgaePivot(Supplier<Rotation2d> angleSupplier) {
+    //     mAlgaeSubsystem = AlgaeSubsystem.getInstance();   
+    //     targetAngle = angle;
+    //     addRequirements(mAlgaeSubsystem);
+    // }
+
     @Override
     public void initialize() {
         mAlgaeSubsystem.setTargetPosition(targetState.pos);

src/main/java/frc/robot/algaepivot/commands/SetAngleAlgaePivot.java

@@ -18,11 +18,11 @@
 import frc.robot.algaeflywheel.AlgaeFlyWheel;
 import frc.robot.algaeflywheel.commands.AlgaeFlyWheelCommands;
 import frc.robot.algaeflywheel.commands.SetVelocityAlgaeFlyWheel;
+import frc.robot.algaepivot.AlgaeSubsystem;
 import frc.robot.algaepivot.commands.AlgaePivotCommands;
 import frc.robot.algaepivot.commands.SetAngleAlgaePivot;
 import frc.robot.drivetrain2.Drivetrain;
 import frc.robot.indexer.commands.IndexerCommands;
-import frc.robot.subsystems.AlgaeSubsystem;
 
 
 public class DriverXbox extends XboxGamepad {