pivotpivotpivot

simgui-ds.json

@@ -1,4 +1,9 @@
 {
+  "Keyboard 0 Settings": {
+    "window": {
+      "visible": true
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
@@ -94,7 +99,8 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "Keyboard0"
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
     }
   ]
 }

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

@@ -11,12 +11,10 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.DriveCommands;
 import frc.robot.generated.TunerConstants;
@@ -26,11 +24,10 @@
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
-import frc.robot.subsystems.intakeFeederwheel.IntakeFeederwheelIO;
-import frc.robot.subsystems.intakeFeederwheel.IntakeFeederwheelSubsystem;
-import frc.robot.subsystems.intakeFeederwheel.IntakeFeederwheelTalonFX;
-import frc.robot.subsystems.flywheel.FlywheelIO;
-import frc.robot.subsystems.flywheel.FlywheelIOTalonFX;
+import frc.robot.subsystems.pivot.PivotIO;
+import frc.robot.subsystems.pivot.PivotPos;
+import frc.robot.subsystems.pivot.PivotSubsystem;
+import frc.robot.subsystems.pivot.PivotTalonFX;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 /**
@@ -42,8 +39,7 @@
 public class RobotContainer {
     // Subsystems
     private final Drive drive;
-    private IntakeFeederwheelSubsystem intakeFeederwheel;
-    private final FlywheelIO flywheel;
+    public final PivotSubsystem pivot;
 
     // Controller
     private final CommandXboxController controller = new CommandXboxController(0);
@@ -58,6 +54,8 @@ public RobotContainer() {
                 // Real robot, instantiate hardware IO implementations
                 // ModuleIOTalonFX is intended for modules with TalonFX drive, TalonFX turn, and a
                 // CANcoder
+                pivot = new PivotSubsystem(new PivotTalonFX());
+
                 drive =
                         new Drive(
                                 new GyroIOPigeon2(),
@@ -80,40 +78,30 @@ public RobotContainer() {
                 //         new ModuleIOTalonFXS(TunerConstants.FrontRight),
                 //         new ModuleIOTalonFXS(TunerConstants.BackLeft),
                 //         new ModuleIOTalonFXS(TunerConstants.BackRight));
-
-                intakeFeederwheel = new IntakeFeederwheelSubsystem(new IntakeFeederwheelTalonFX());
-
-                flywheel = new FlywheelIOTalonFX();
                 break;
 
             case SIM:
-                // Sim robot, instantiate physics sim IO implementations
+                // Sim robot, instantiate physics sim IO implementation
+                pivot = new PivotSubsystem(new PivotTalonFX());
                 drive =
                         new Drive(
                                 new GyroIO() {},
                                 new ModuleIOSim(TunerConstants.FrontLeft),
                                 new ModuleIOSim(TunerConstants.FrontRight),
                                 new ModuleIOSim(TunerConstants.BackLeft),
                                 new ModuleIOSim(TunerConstants.BackRight));
-
-                intakeFeederwheel = new IntakeFeederwheelSubsystem(new IntakeFeederwheelTalonFX());
-
-                flywheel = new FlywheelIOTalonFX();
                 break;
 
             default:
                 // Replayed robot, disable IO implementations
+                pivot = new PivotSubsystem(new PivotIO() {});
                 drive =
                         new Drive(
                                 new GyroIO() {},
                                 new ModuleIO() {},
                                 new ModuleIO() {},
                                 new ModuleIO() {},
                                 new ModuleIO() {});
-
-                intakeFeederwheel = new IntakeFeederwheelSubsystem(new IntakeFeederwheelIO() {});
-
-                flywheel = new FlywheelIO() {};
                 break;
         }
 
@@ -146,18 +134,19 @@ public RobotContainer() {
 
     /**
      * Use this method to define your button->command mappings. Buttons can be created by
-     * instantiating a {@link GenericHID} or one of its subclasses ({@link Joystick} or {@link
-     * XboxController}), and then passing it to a {@link JoystickButton}.
+     * instantiating a {@link GenericHID} or one of its subclasses ({@link
+     * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+     * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
      */
     private void configureButtonBindings() {
+
         // Default command, normal field-relative drive
         drive.setDefaultCommand(
                 DriveCommands.joystickDrive(
                         drive,
                         () -> -controller.getLeftY(),
                         () -> -controller.getLeftX(),
                         () -> -controller.getRightX()));
-
         // Lock to 0° when A button is held
         controller
                 .a()
@@ -184,15 +173,17 @@ private void configureButtonBindings() {
                                         drive)
                                 .ignoringDisable(true));
 
-        controller.rightTrigger().onTrue(intakeFeederwheel.rollIn());
-        controller.leftTrigger().whileTrue(Commands.run(() -> flywheel.setRoller(1)));
+        controller
+                .leftBumper()
+                .onTrue(Commands.run(() -> pivot.setPosition(PivotPos.PivotDown), pivot));
+
+        /**
+         * Use this to pass the autonomous command to the main {@link Robot} class.
+         *
+         * @return the command to run in autonomous
+         */
     }
 
-    /**
-     * Use this to pass the autonomous command to the main {@link Robot} class.
-     *
-     * @return the command to run in autonomous
-     */
     public Command getAutonomousCommand() {
         return autoChooser.get();
     }

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

@@ -15,4 +15,6 @@
  */
 public final class Constants {
     public static final int PRIMARY_CONTROLLER_PORT = 0;
+
+    public static final String RIO_BUS = "rio";
 }

src/main/java/frc/robot/subsystems/flywheel/FlywheelIOTalonFX.java

@@ -2,7 +2,6 @@
 
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
-import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import frc.robot.Robot;
 import frc.robot.util.sim.PhysicsSim;

src/main/java/frc/robot/subsystems/pivot/PivotConfigs.java

@@ -0,0 +1,38 @@
+package frc.robot.subsystems.pivot;
+
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+
+public class PivotConfigs {
+    public static final int pivotMotorID = 1;
+    public static final int pivotCANcoderID = 2;
+    // values may change as tuning progresses
+    public static final CANcoderConfiguration canCoderConfig =
+            new CANcoderConfiguration()
+                    .withMagnetSensor(
+                            new com.ctre.phoenix6.configs.MagnetSensorConfigs()
+                                    .withSensorDirection(
+                                            SensorDirectionValue.CounterClockwise_Positive)
+                                    .withMagnetOffset(0.0));
+    public static TalonFXConfiguration motorConfig =
+            new TalonFXConfiguration()
+                    .withSlot0(
+                            new Slot0Configs()
+                                    .withGravityType(GravityTypeValue.Arm_Cosine)
+                                    .withKD(0.0)
+                                    .withKP(0.0)
+                                    .withKI(0.0)
+                                    .withKG(0.0)
+                                    .withKS(0.0)
+                                    .withKV(0.0))
+                    .withCurrentLimits(
+                            new CurrentLimitsConfigs()
+                                    .withStatorCurrentLimit(0.0)
+                                    .withSupplyCurrentLimit(0.0)
+                                    .withStatorCurrentLimitEnable(true)
+                                    .withSupplyCurrentLimitEnable(true));
+}

src/main/java/frc/robot/subsystems/pivot/PivotIO.java

@@ -0,0 +1,19 @@
+package frc.robot.subsystems.pivot;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface PivotIO {
+    @AutoLog
+    class PivotIOinput {
+        public double pos = 0.0;
+        public double velocity = 0.0;
+    }
+
+    public default void updateInputs(PivotIOinput inputs) {}
+
+    public default void setPosition(double position) {}
+
+    public default void applyPower(double power) {}
+
+    public default void stop() {}
+}

src/main/java/frc/robot/subsystems/pivot/PivotPos.java

@@ -0,0 +1,14 @@
+package frc.robot.subsystems.pivot;
+
+import edu.wpi.first.units.Units;
+
+public enum PivotPos {
+    PivotDown(0.0),
+    PivotUp(0.0);
+
+    public final double position;
+
+    PivotPos(double rotation) {
+        this.position = Units.Rotations.of(rotation).magnitude();
+    }
+}

src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java

@@ -0,0 +1,36 @@
+package frc.robot.subsystems.pivot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class PivotSubsystem extends SubsystemBase {
+    private final PivotIO io;
+    private final PivotIOinputAutoLogged inputs = new PivotIOinputAutoLogged();
+
+    public PivotSubsystem(PivotIO io) {
+        this.io = io;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+        Logger.processInputs("Pivot", inputs);
+    }
+
+    public void setPosition(PivotPos pivotPos) {
+        io.setPosition(pivotPos.position);
+    }
+
+    public Command stopCommand() {
+        return runOnce(io::stop);
+    }
+
+    public Command setPositionCommand(PivotPos pivotPos) {
+        return run(() -> setPosition(pivotPos)).finallyDo(io::stop);
+    }
+
+    public Command applyPowerCommand(double power) {
+        return run(() -> io.applyPower(power)).finallyDo(io::stop);
+    }
+}

src/main/java/frc/robot/subsystems/pivot/PivotTalonFX.java

@@ -0,0 +1,47 @@
+package frc.robot.subsystems.pivot;
+
+import com.ctre.phoenix6.configs.*;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.wpilibj.RobotBase;
+import frc.robot.util.PhysicsSim;
+
+public class PivotTalonFX implements PivotIO {
+    private final MotionMagicTorqueCurrentFOC motionMagicReq = new MotionMagicTorqueCurrentFOC(0);
+    private final TalonFX pivotMotor;
+    private final CANcoder pivotCan;
+
+    public PivotTalonFX() {
+
+        this.pivotMotor = new TalonFX(PivotConfigs.pivotMotorID);
+        this.pivotCan = new CANcoder(PivotConfigs.pivotCANcoderID);
+        this.pivotMotor.getConfigurator().apply(PivotConfigs.motorConfig);
+        this.pivotCan.getConfigurator().apply(PivotConfigs.canCoderConfig);
+        if (RobotBase.isSimulation()) {
+            PhysicsSim.getInstance().addTalonFX(pivotMotor, pivotCan);
+        }
+    }
+
+    @Override
+    public void updateInputs(PivotIOinput inputs) {
+        inputs.pos = (double) pivotCan.getAbsolutePosition().getValueAsDouble();
+        inputs.velocity = (double) pivotMotor.getVelocity().getValueAsDouble();
+    }
+
+    @Override
+    public void setPosition(double positionRad) {
+        pivotMotor.setControl(motionMagicReq.withPosition(positionRad));
+    }
+
+    @Override
+    public void applyPower(double power) {
+        pivotMotor.setControl(new DutyCycleOut(power));
+    }
+
+    @Override
+    public void stop() {
+        pivotMotor.stopMotor();
+    }
+}