6 shooting functional

src/main/java/frc/robot/commands/states/Shooting.java

@@ -0,0 +1,54 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.states;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
+import frc.robot.constants.ConstMotion;
+import frc.robot.constants.ConstRotors;
+
+public class Shooting extends Command {
+  /** Creates a new Shooting. */
+  public Shooting() {
+
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    RobotContainer.motionInstance.setHoodPivotAngle(ConstMotion.HOOD_PIVOT_ANGLE);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+    RobotContainer.rotorsInstance.setFlywheelSpeeds(ConstRotors.FLYWHEEL_SHOOTING_SPEED);
+    if (RobotContainer.rotorsInstance.isFlyWheelAtSpeed(ConstRotors.FLYWHEEL_SHOOTING_SPEED)) {
+      RobotContainer.rotorsInstance.setTransferRollersSpeeds(ConstRotors.INTAKE_TRANSFER_SPEED);
+      RobotContainer.rotorsInstance.setSerializerRollersSpeed(ConstRotors.SERIALIZER_SHOOTING_SPEED);
+
+    } else {
+      RobotContainer.rotorsInstance.setTransferRollersSpeeds(ConstRotors.STOP_ALL);
+    }
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    RobotContainer.rotorsInstance.setFlywheelSpeeds(ConstRotors.STOP_ALL);
+    RobotContainer.rotorsInstance.setTransferRollersSpeeds(ConstRotors.STOP_ALL);
+    RobotContainer.rotorsInstance.setSerializerRollersSpeed(ConstRotors.STOP_ALL);
+
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/constants/ConstMotion.java

@@ -4,11 +4,16 @@
 
 package frc.robot.constants;
 
+import static edu.wpi.first.units.Units.Degrees;
+
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 
+import edu.wpi.first.units.measure.Angle;
+
 /** Add your docs here. */
 public class ConstMotion {
   public static final double STOP = 0;
   public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration();
   public static final TalonFXConfiguration HOOD_PIVOT_CONFIGURATION = new TalonFXConfiguration();
+  public static final Angle HOOD_PIVOT_ANGLE = Degrees.of(14);
 }

src/main/java/frc/robot/constants/ConstRotors.java

@@ -9,6 +9,9 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.fasterxml.jackson.core.filter.TokenFilter.Inclusion;
 
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.AngularVelocity;
+
 /** Add your docs here. */
 public class ConstRotors {
 
@@ -19,6 +22,11 @@ public class ConstRotors {
   public static final TalonFXConfiguration TRANSFER_ROLLERS_WEST_CONFIGURATION = new TalonFXConfiguration();
   public static final TalonFXConfiguration FLYWHEEL_EAST_CONFIGURATION = new TalonFXConfiguration();
   public static final TalonFXConfiguration FLYWHEEL_WEST_CONFIGURATION = new TalonFXConfiguration();
+  public static final AngularVelocity FLYWHEEL_SHOOTING_SPEED = Units.RPM.of(1000);
+  public static final AngularVelocity INTAKE_ROLLER_SPEED = Units.RPM.of(2000);
+  public static final AngularVelocity INTAKE_TRANSFER_SPEED = Units.RPM.of(3000);
+  public static final AngularVelocity SERIALIZER_SHOOTING_SPEED = Units.RPM.of(4000);
+  public static final AngularVelocity STOP_ALL = Units.RPM.of(0);
 
   static {
 

src/main/java/frc/robot/subsystems/Rotors.java

@@ -33,6 +33,9 @@ public class Rotors extends SubsystemBase {
   Follower flywheelWestFollower = new Follower(flywheelTopEast.getDeviceID(), true);
   Follower transferRollersEastFollower = new Follower(intakeRollersEast.getDeviceID(), true);
   Follower intakeRollerEastFollower = new Follower(intakeRollersEast.getDeviceID(), true);
+  final MotionMagicVelocityVoltage flyWheelVelocityRequest = new MotionMagicVelocityVoltage(0);
+  final MotionMagicVelocityVoltage transferRollersVelocityRequest = new MotionMagicVelocityVoltage(0);
+  final MotionMagicVelocityVoltage serializerVelocityRequest = new MotionMagicVelocityVoltage(0);
   private boolean flyWheelAtSpeed = false;
   // private boolean intakeRollersAtSpeed = false;/
 
@@ -50,7 +53,6 @@ public Rotors() {
 
   // final MotionMagicVelocityVoltage TransferVelocityRequest = new
   // MotionMagicVelocityVoltage(0);/
-  final MotionMagicVelocityVoltage flyWheelVelocityRequest = new MotionMagicVelocityVoltage(0);
 
   public AngularVelocity getFlyWheelSpeeds() {
     if (Robot.isSimulation()) {
@@ -71,17 +73,17 @@ public AngularVelocity getTransferRollersVelocity() {
     return transferRollersEast.getVelocity().getValue();
   }
 
-  public void setSerializerRollersSpeed(double speed) {
-    serializerRollers.set(speed);
+  public void setSerializerRollersSpeed(AngularVelocity speed) {
+    serializerRollers.setControl(serializerVelocityRequest.withVelocity(speed));
   }
 
   public void setIntakeRollersSpeeds(double speed) {
     intakeRollersEast.set(speed);
     intakeRollersWest.setControl(intakeRollerEastFollower);
   }
 
-  public void setTransferRollersSpeeds(double speed) {
-    transferRollersEast.set(speed);
+  public void setTransferRollersSpeeds(AngularVelocity speed) {
+    transferRollersEast.setControl(transferRollersVelocityRequest.withVelocity(speed));
     transferRollersWest.setControl(transferRollersEastFollower);
   }