Midnight day 3, going to master

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

@@ -63,20 +63,36 @@ public static class CoralConstants {
   }
 
   public static class ShooterConstants {
-    // 2 Neos, 4in shooter wheels, 4:1 gearbox reduction
+    // 2 Neos, 4in shooter wheels
     public static final int kLeaderMotorId = 15;
     public static final int kFollowerMotorId = 16;
   }
 
   public static class TurretConstants {
-    // 2 Neos, 12in diameter, 25:1 gearbox, 10:1 pivot gearing, non-continuous (270
-    // FOV)
-    public static final int kLeaderMotorId = 17;
-    public static final int kFollowerMotorId = 18;
+    // 1 Neo, 6.875 in diameter, 4:1 gearbox, 10:1 pivot gearing, non-continuous
+    // 360 deg
+    public static final int kMotorId = 17;
   }
 
   public static class HoodConstants {
     // 1 Neo, 0-90 degree variability, 50:1 reduction
     public static final int kMotorId = 19;
   }
+
+  // Intake subsystem CAN IDs start at 30
+  public static class IntakeConstants {
+    // SparkFlex controlling the intake flywheel
+    public static final int kPivotMotorId = 30;
+    public static final int kRollerMotorId = 31;
+  }
+
+  // Hopper subsystem CAN IDs start at 40
+  public static class HopperConstants {
+    public static final int kHopperMotorId = 40;
+  }
+
+  // Kicker subsystem CAN IDs start at 50
+  public static class KickerConstants {
+    public static final int kKickerMotorId = 50;
+  }
 }

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

@@ -17,12 +17,24 @@
 import frc.robot.controls.DriverControls;
 import frc.robot.controls.OperatorControls;
 import frc.robot.controls.PoseControls;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.KickerSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.Superstructure;
 import frc.robot.subsystems.SwerveSubsystem;
 import swervelib.SwerveDrive;
 
 public class RobotContainer {
   private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve"));
 
+  private final IntakeSubsystem intake = new IntakeSubsystem();
+  private final HopperSubsystem hopper = new HopperSubsystem();
+  private final KickerSubsystem kicker = new KickerSubsystem();
+  private final ShooterSubsystem shooter = new ShooterSubsystem();
+
+  private final Superstructure superstructure = new Superstructure(shooter, null, null, intake, hopper, kicker);
+
   private final SendableChooser<Command> autoChooser;
 
   /**
@@ -53,8 +65,8 @@ public RobotContainer() {
 
   private void configureBindings() {
     // Set up controllers
-    DriverControls.configure(ControllerConstants.kDriverControllerPort, drivebase, null);
-    OperatorControls.configure(ControllerConstants.kOperatorControllerPort, drivebase, null);
+    DriverControls.configure(ControllerConstants.kDriverControllerPort, drivebase, superstructure);
+    OperatorControls.configure(ControllerConstants.kOperatorControllerPort, drivebase, superstructure);
     PoseControls.configure(ControllerConstants.kPoseControllerPort, drivebase);
   }
 

src/main/java/frc/robot/controls/DriverControls.java

@@ -88,6 +88,7 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur
     } else if (Robot.isSimulation()) {
       controller.back().whileTrue(fireAlgae(drivetrain));
     } else {
+      controller.y().onTrue((Commands.runOnce(drivetrain::zeroGyro)));
       controller.leftBumper().whileTrue(Commands.runOnce(drivetrain::lock, drivetrain).repeatedly());
     }
   }

src/main/java/frc/robot/controls/OperatorControls.java

@@ -1,5 +1,7 @@
 package frc.robot.controls;
 
+import static edu.wpi.first.units.Units.Degrees;
+
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.subsystems.Superstructure;
 import frc.robot.subsystems.SwerveSubsystem;
@@ -8,5 +10,30 @@ public class OperatorControls {
 
   public static void configure(int port, SwerveSubsystem drivetrain, Superstructure superstructure) {
     CommandXboxController controller = new CommandXboxController(port);
+
+    // Intake controls - A to intake, B to eject
+    controller.a().whileTrue(superstructure.intakeCommand());
+    controller.b().whileTrue(superstructure.ejectCommand());
+
+    // Hopper controls - X to run hopper forward, Y to run backward
+    controller.x().whileTrue(superstructure.hopperFeedCommand());
+    controller.y().whileTrue(superstructure.hopperReverseCommand());
+
+    // Shooter controls - Right bumper to shoot
+    controller.rightBumper().whileTrue(superstructure.shootCommand());
+    controller.leftBumper().whileTrue(superstructure.stopShootingCommand());
+
+    // Kicker controls
+    controller.back().whileTrue(superstructure.kickerFeedCommand());
+    controller.start().whileTrue(superstructure.kickerStopCommand());
+
+    // Intake pivot controls. Setpoints need to be tested and finalized.
+
+    // 0 for default
+    // -45 for collection
+    // +25 just because. We can add more setpoints if necessary.
+    // controller.povUp().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(25)));
+    // controller.povRight().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(0)));
+    // controller.povDown().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(-45)));
   }
 }

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

@@ -0,0 +1,84 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.thethriftybot.ThriftyNova;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.Pounds;
+import static edu.wpi.first.units.Units.RPM;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import yams.gearing.GearBox;
+import yams.gearing.MechanismGearing;
+import yams.mechanisms.config.FlyWheelConfig;
+import yams.mechanisms.velocity.FlyWheel;
+import yams.motorcontrollers.SmartMotorController;
+import yams.motorcontrollers.SmartMotorControllerConfig;
+import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode;
+import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode;
+import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity;
+import yams.motorcontrollers.local.NovaWrapper;
+
+public class HopperSubsystem extends SubsystemBase {
+
+  private static final double HOPPER_SPEED = 1.0;
+
+  // Nova motor controller with NEO motor
+  private ThriftyNova hopperNova = new ThriftyNova(Constants.HopperConstants.kHopperMotorId);
+
+  private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this)
+      .withControlMode(ControlMode.OPEN_LOOP)
+      .withTelemetry("HopperMotor", TelemetryVerbosity.HIGH)
+      .withGearing(new MechanismGearing(GearBox.fromReductionStages(4))) // 4:1 gear reduction
+      .withMotorInverted(false)
+      .withIdleMode(MotorMode.BRAKE)
+      .withStatorCurrentLimit(Amps.of(40));
+
+  private SmartMotorController smc = new NovaWrapper(hopperNova, DCMotor.getNEO(1), smcConfig);
+
+  private final FlyWheelConfig hopperConfig = new FlyWheelConfig(smc)
+      .withDiameter(Inches.of(4))
+      .withMass(Pounds.of(0.5))
+      .withUpperSoftLimit(RPM.of(6000))
+      .withLowerSoftLimit(RPM.of(-6000))
+      .withTelemetry("Hopper", TelemetryVerbosity.HIGH);
+
+  private FlyWheel hopper = new FlyWheel(hopperConfig);
+
+  public HopperSubsystem() {
+  }
+
+  /**
+   * Command to run the hopper forward while held.
+   */
+  public Command feedCommand() {
+    return hopper.set(HOPPER_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Hopper.Feed");
+  }
+
+  /**
+   * Command to run the hopper in reverse while held.
+   */
+  public Command reverseCommand() {
+    return hopper.set(-HOPPER_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Hopper.Reverse");
+  }
+
+  /**
+   * Command to stop the hopper.
+   */
+  public Command stopCommand() {
+    return hopper.set(0).withName("Hopper.Stop");
+  }
+
+  @Override
+  public void periodic() {
+    hopper.updateTelemetry();
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    hopper.simIterate();
+  }
+}

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

@@ -0,0 +1,126 @@
+package frc.robot.subsystems;
+
+import com.thethriftybot.ThriftyNova;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.measure.Angle;
+
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+import static edu.wpi.first.units.Units.DegreesPerSecondPerSecond;
+import static edu.wpi.first.units.Units.Feet;
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.Pounds;
+import static edu.wpi.first.units.Units.RPM;
+import static edu.wpi.first.units.Units.Seconds;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import yams.gearing.GearBox;
+import yams.gearing.MechanismGearing;
+import yams.mechanisms.config.ArmConfig;
+import yams.mechanisms.config.FlyWheelConfig;
+import yams.mechanisms.positional.Arm;
+import yams.mechanisms.velocity.FlyWheel;
+import yams.motorcontrollers.SmartMotorController;
+import yams.motorcontrollers.SmartMotorControllerConfig;
+import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode;
+import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode;
+import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity;
+import yams.motorcontrollers.local.NovaWrapper;
+
+public class IntakeSubsystem extends SubsystemBase {
+
+  private static final double INTAKE_SPEED = 0.1;
+
+  // ThriftyNova controlling the intake roller
+  private ThriftyNova rollerNova = new ThriftyNova(Constants.IntakeConstants.kRollerMotorId);
+
+  private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this)
+      .withControlMode(ControlMode.OPEN_LOOP)
+      .withTelemetry("IntakeMotor", TelemetryVerbosity.HIGH)
+      .withGearing(new MechanismGearing(GearBox.fromReductionStages(1))) // Direct drive, adjust if geared
+      .withMotorInverted(false)
+      .withIdleMode(MotorMode.COAST)
+      .withStatorCurrentLimit(Amps.of(40));
+
+  private SmartMotorController smc = new NovaWrapper(rollerNova, DCMotor.getNeoVortex(1), smcConfig);
+
+  private final FlyWheelConfig intakeConfig = new FlyWheelConfig(smc)
+      .withDiameter(Inches.of(4))
+      .withMass(Pounds.of(0.5))
+      .withUpperSoftLimit(RPM.of(6000))
+      .withLowerSoftLimit(RPM.of(-6000))
+      .withTelemetry("Intake", TelemetryVerbosity.HIGH);
+
+  private FlyWheel intake = new FlyWheel(intakeConfig);
+
+  // 5:1, 5:1, 60/18 reduction
+  private SmartMotorControllerConfig intakePivotSmartMotorConfig = new SmartMotorControllerConfig(this)
+      .withControlMode(ControlMode.CLOSED_LOOP)
+      .withClosedLoopController(200, 0, 0, DegreesPerSecond.of(360), DegreesPerSecondPerSecond.of(360))
+      .withFeedforward(new ArmFeedforward(0, 0, 0.1))
+      .withTelemetry("IntakePivotMotor", TelemetryVerbosity.HIGH)
+      .withGearing(new MechanismGearing(GearBox.fromReductionStages(5, 5, 60.0 / 18)))
+      .withMotorInverted(true)
+      .withIdleMode(MotorMode.BRAKE)
+      .withStatorCurrentLimit(Amps.of(10))
+      .withClosedLoopRampRate(Seconds.of(0.1));
+
+  private ThriftyNova pivotMotor = new ThriftyNova(Constants.IntakeConstants.kPivotMotorId);
+
+  private SmartMotorController intakePivotController = new NovaWrapper(pivotMotor, DCMotor.getNeoVortex(1),
+      intakePivotSmartMotorConfig);
+
+  private final ArmConfig intakePivotConfig = new ArmConfig(intakePivotController)
+      .withSoftLimits(Degrees.of(-95), Degrees.of(45)) // TODO: Find and set proper limits once setpoints and range is
+                                                       // known
+      .withHardLimit(Degrees.of(-100), Degrees.of(50))
+      .withStartingPosition(Degrees.of(-90))
+      .withLength(Feet.of(1))
+      .withMass(Pounds.of(2)) // Reis says: 2 pounds, not a lot
+      .withTelemetry("IntakePivot", TelemetryVerbosity.HIGH);
+
+  private Arm intakePivot = new Arm(intakePivotConfig);
+
+  public IntakeSubsystem() {
+  }
+
+  /**
+   * Command to run the intake while held.
+   */
+  public Command intakeCommand() {
+    return intake.set(INTAKE_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Intake.Run");
+  }
+
+  /**
+   * Command to eject while held.
+   */
+  public Command ejectCommand() {
+    return intake.set(-INTAKE_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Intake.Eject");
+  }
+
+  public Command setPivotAngle(Angle angle) {
+    return intakePivot.setAngle(angle).withName("IntakePivot.SetAngle");
+  }
+
+  public Command rezero() {
+    return Commands.runOnce(() -> pivotMotor.setEncoderPosition(0), this).withName("IntakePivot.Rezero");
+  }
+
+  @Override
+  public void periodic() {
+    intake.updateTelemetry();
+    intakePivot.updateTelemetry();
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    intake.simIterate();
+    intakePivot.simIterate();
+  }
+}