5-intaking-functional

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

@@ -19,8 +19,11 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.DeferredCommand;
+import edu.wpi.first.wpilibj2.command.Subsystem;
 import frc.robot.DeviceIDs.controllerIDs;
 import frc.robot.commands.AddVisionMeasurement;
+import frc.robot.commands.states.Intaking;
+import frc.robot.commands.states.RetractIntake;
 import frc.robot.constants.ConstSystem.constControllers;
 import frc.robot.subsystems.DriverStateMachine;
 import frc.robot.subsystems.DriverStateMachine.DriverState;
@@ -55,7 +58,8 @@ public class RobotContainer {
   private final RobotPoses loggedRobotPose = robotPose;
   public static final Vision subVision = new Vision();
   private final Vision loggedSubVision = subVision;
-
+  public static final Intaking intakingInstance = new Intaking();
+  public static final RetractIntake RetractingInstance = new RetractIntake();
   Command TRY_NONE = Commands.deferredProxy(
       () -> subStateMachine.tryState(RobotState.NONE));
 
@@ -95,7 +99,10 @@ private void configDriverBindings() {
     // subDrivetrain.resetModulesToAbsolute()));
     conDriver.btn_Back
         .onTrue(Commands.runOnce(() -> subDrivetrain.resetPose(new Pose2d(0, 0, new Rotation2d()))));
-
+    conDriver.btn_RightTrigger
+        .whileTrue(intakingInstance);
+    conDriver.btn_LeftTrigger
+        .whileTrue(RetractingInstance);
     // Example Pose Drive
     conDriver.btn_X
         .whileTrue(EXAMPLE_POSE_DRIVE)

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

@@ -0,0 +1,42 @@
+// 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 Intaking extends Command {
+  /** Creates a new Intaking. */
+  public Intaking() {
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.INTAKE_PIVOT_DEPLOY);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    RobotContainer.rotorsInstance.setIntakeRollersSpeeds(ConstRotors.INTAKE_ROLLERS_SPEED);
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    RobotContainer.rotorsInstance.setIntakeRollersSpeeds(ConstRotors.STOP);
+
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,38 @@
+// 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;
+
+public class RetractIntake extends Command {
+  /** Creates a new RetractIntake. */
+  public RetractIntake() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.INTAKE_PIVOT_RETRACT);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -5,15 +5,56 @@
 package frc.robot.constants;
 
 import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Rotations;
 
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
+
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Angle;
 
 import edu.wpi.first.units.measure.Angle;
 
 /** Add your docs here. */
 public class ConstMotion {
-  public static final double STOP = 0;
+  // 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 INTAKE_PIVOT_DEPLOY = Degrees.of(125);
+  public static final Angle INTAKE_PIVOT_RETRACT = Degrees.of(0);
   public static final Angle HOOD_PIVOT_ANGLE = Degrees.of(14);
+
+  static {
+
+    INTAKE_PIVOT_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    INTAKE_PIVOT_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+    INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
+    INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
+    INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ForwardSoftLimitThreshold = INTAKE_PIVOT_DEPLOY.in(Rotations);
+    INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitThreshold = INTAKE_PIVOT_RETRACT.in(Rotations);
+    INTAKE_PIVOT_CONFIGURATION.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
+    INTAKE_PIVOT_CONFIGURATION.Slot0.kS = 0.3;
+    INTAKE_PIVOT_CONFIGURATION.Slot0.kP = 40;
+    INTAKE_PIVOT_CONFIGURATION.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign;
+    INTAKE_PIVOT_CONFIGURATION.Feedback.SensorToMechanismRatio = 1.0 / ((16.0 / 42.0) * (18.0 / 60.0) * (12.0 / 60.0));
+
+    HOOD_PIVOT_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    HOOD_PIVOT_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+    HOOD_PIVOT_CONFIGURATION.Feedback.SensorToMechanismRatio = 1.0 / ((10.0 / 42.0) * (10.0 / 210.0));
+    HOOD_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
+    HOOD_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
+    HOOD_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Degrees.of(46).in(Rotations);
+    // Do not change, it's not at zero because the hood is not perfectly at 0 when
+    // the encoder reads 0
+    HOOD_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Degrees.of(1.6).in(Rotations);
+    HOOD_PIVOT_CONFIGURATION.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
+    HOOD_PIVOT_CONFIGURATION.Slot0.kS = 0.2;
+    HOOD_PIVOT_CONFIGURATION.Slot0.kP = 300;
+    HOOD_PIVOT_CONFIGURATION.Slot0.kG = 0.1;
+    HOOD_PIVOT_CONFIGURATION.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign;
+  }
+
 }

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

@@ -7,7 +7,8 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.signals.InvertedValue;
 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;
 
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.AngularVelocity;
@@ -22,6 +23,7 @@ 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 INTAKE_ROLLERS_SPEED = Units.RPM.of(3);
   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);
@@ -34,22 +36,34 @@ public class ConstRotors {
     // NeutralModeValue.Coast;/
 
     FLYWHEEL_WEST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast;
-    FLYWHEEL_WEST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+    FLYWHEEL_WEST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+    FLYWHEEL_WEST_CONFIGURATION.Slot0.kS = 0.14;
+    FLYWHEEL_WEST_CONFIGURATION.Slot0.kV = 0.11167;
+    FLYWHEEL_WEST_CONFIGURATION.Slot0.kA = 0;
+    FLYWHEEL_WEST_CONFIGURATION.Slot0.kP = 0.5;
+    FLYWHEEL_WEST_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity = 0;
+    FLYWHEEL_WEST_CONFIGURATION.MotionMagic.MotionMagicAcceleration = 9999;
+    FLYWHEEL_WEST_CONFIGURATION.MotionMagic.MotionMagicJerk = 0;
     FLYWHEEL_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true;
-    FLYWHEEL_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35;
+    FLYWHEEL_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 70;
 
     FLYWHEEL_EAST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast;
     FLYWHEEL_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+    FLYWHEEL_EAST_CONFIGURATION.Slot0.kS = 0.14;
+    FLYWHEEL_EAST_CONFIGURATION.Slot0.kV = 0.11167;
+    FLYWHEEL_EAST_CONFIGURATION.Slot0.kA = 0;
+    FLYWHEEL_EAST_CONFIGURATION.Slot0.kP = 0.5;
+    FLYWHEEL_EAST_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity = 0;
+    FLYWHEEL_EAST_CONFIGURATION.MotionMagic.MotionMagicAcceleration = 9999;
+    FLYWHEEL_EAST_CONFIGURATION.MotionMagic.MotionMagicJerk = 0;
     FLYWHEEL_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true;
-    FLYWHEEL_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35;
+    FLYWHEEL_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 70;
 
     SERIALIZER_ROLLERS_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast;
     SERIALIZER_ROLLERS_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-    SERIALIZER_ROLLERS_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true;
-    SERIALIZER_ROLLERS_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35;
 
     INTAKE_ROLLERS_EAST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast;
-    INTAKE_ROLLERS_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+    INTAKE_ROLLERS_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
     INTAKE_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true;
     INTAKE_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35;
 
@@ -59,17 +73,20 @@ public class ConstRotors {
     INTAKE_ROLLERS_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35;
 
     TRANSFER_ROLLERS_EAST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast;
-    TRANSFER_ROLLERS_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-    TRANSFER_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true;
-    TRANSFER_ROLLERS_EAST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35;
+    TRANSFER_ROLLERS_EAST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+    TRANSFER_ROLLERS_EAST_CONFIGURATION.Slot0.kP = 0.7;
+    TRANSFER_ROLLERS_EAST_CONFIGURATION.Slot0.kS = 0.15;
+    TRANSFER_ROLLERS_EAST_CONFIGURATION.Slot0.kV = 0.12;
+    TRANSFER_ROLLERS_EAST_CONFIGURATION.Slot0.kA = 0;
+    TRANSFER_ROLLERS_EAST_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity = 0;
+    TRANSFER_ROLLERS_EAST_CONFIGURATION.MotionMagic.MotionMagicAcceleration = 9999;
+    TRANSFER_ROLLERS_EAST_CONFIGURATION.MotionMagic.MotionMagicJerk = 0;
 
     TRANSFER_ROLLERS_WEST_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast;
     TRANSFER_ROLLERS_WEST_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-    TRANSFER_ROLLERS_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true;
-    TRANSFER_ROLLERS_WEST_CONFIGURATION.CurrentLimits.SupplyCurrentLowerLimit = 35;
 
   }
 
-  public static final double STOP = 0;
+  public static final AngularVelocity STOP = Units.RPM.of(0);
 
 }

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

@@ -4,6 +4,7 @@
 
 package frc.robot.subsystems;
 
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
@@ -37,6 +38,8 @@ public class Rotors extends SubsystemBase {
   final MotionMagicVelocityVoltage transferRollersVelocityRequest = new MotionMagicVelocityVoltage(0);
   final MotionMagicVelocityVoltage serializerVelocityRequest = new MotionMagicVelocityVoltage(0);
   private boolean flyWheelAtSpeed = false;
+
+  final MotionMagicVelocityVoltage intakeRollersVelocityRequest = new MotionMagicVelocityVoltage(0);
   // private boolean intakeRollersAtSpeed = false;/
 
   public Rotors() {
@@ -77,8 +80,8 @@ public void setSerializerRollersSpeed(AngularVelocity speed) {
     serializerRollers.setControl(serializerVelocityRequest.withVelocity(speed));
   }
 
-  public void setIntakeRollersSpeeds(double speed) {
-    intakeRollersEast.set(speed);
+  public void setIntakeRollersSpeeds(AngularVelocity speed) {
+    intakeRollersEast.setControl(intakeRollersVelocityRequest.withVelocity(speed));
     intakeRollersWest.setControl(intakeRollerEastFollower);
   }