feat: long night

build.gradle

@@ -65,12 +65,6 @@ configurations.all {
     exclude group: "edu.wpi.first.wpilibj"
 }
 
-//task(checkAkitInstall, dependsOn: "classes", type: JavaExec) {
-//    mainClass = "org.littletonrobotics.junction.CheckInstall"
-//    classpath = sourceSets.main.runtimeClasspath
-//}
-//compileJava.finalizedBy checkAkitInstall
-
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 4.
 dependencies {

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

@@ -62,4 +62,10 @@ public static class Drivetrain{
 
 
   }
+
+  public static class Intake {
+    public static int rollerID = 18;
+    public static int leadPivotID = 17;
+    public static int followerPivotID = 16;
+  }
 }

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

@@ -25,8 +25,11 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.FeedForwardCharacterization;
+import frc.robot.commands.IntakeCommands;
 import frc.robot.commands.ShooterCommands;
 import frc.robot.subsystems.drive.*;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.IntakeIOReal;
 import frc.robot.subsystems.limelight.Limelight;
 import frc.robot.subsystems.limelight.LimelightIOReal;
 import frc.robot.subsystems.shooter.*;
@@ -45,6 +48,7 @@ public class RobotContainer {
   private final Shooter shooter;
 
   private final Limelight limelight;
+  private final Intake intake;
 
   // Controller
   private final CommandXboxController controller = new CommandXboxController(0);
@@ -69,8 +73,10 @@ public RobotContainer() {
                 new Shooter(
                         new FlywheelIOTalonFX(),
                         new FlywheelIOSparkMaxSingle(1, 13, false, 20),
-                        new FlywheelIOSparkMaxSingle(10, 14, true, 40));
+                        new FlywheelIOSparkMaxSingle(1.0/10.0, 14, true, 40));
         limelight = new Limelight(new LimelightIOReal());
+//        limelight = null;
+        intake = new Intake(new IntakeIOReal());
         break;
       }
 
@@ -87,6 +93,7 @@ public RobotContainer() {
                         new ModuleIOSim());
         shooter = null;
         limelight = null;
+        intake = null;
         break;
       }
 
@@ -106,6 +113,7 @@ public RobotContainer() {
                         });
         shooter = null;
         limelight = null;
+        intake = null;
         break;
       }
     }
@@ -134,6 +142,7 @@ private void configureButtonBindings() {
 
 //    shooter.setDefaultCommand(new ShooterCommands.ShooterIdle(shooter));
       shooter.setDefaultCommand(new ShooterCommands.ShooterTesting(shooter));
+      intake.setDefaultCommand(new IntakeCommands.IntakeTesting(intake));
     controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
     controller.y().onTrue(new InstantCommand(() -> drive.resetPose()));
 //    controller
@@ -146,7 +155,10 @@ private void configureButtonBindings() {
 //                    drive)
 //                .ignoringDisable(true));
 
-    controller.a()
+    controller.a().onTrue(new IntakeCommands.IntakeDown(intake));
+    controller.b().onTrue(new IntakeCommands.IntakeUp(intake));
+
+    controller.rightTrigger(0.7)
             .whileTrue(DriveCommands.LimelightRotDrive(
               drive,
               () -> -controller.getLeftX(),

src/main/java/frc/robot/commands/DriveCommands.java

@@ -28,7 +28,7 @@
 
 public class DriveCommands {
   private static final double DEADBAND = 0.1;
-  private static PIDController pid = new PIDController(0,0,0);
+  private static PIDController pid = new PIDController(1,0,0);
 
   private DriveCommands() {}
 
@@ -88,7 +88,7 @@ public static Command LimelightRotDrive(
                     double omega = MathUtil.applyDeadband(limelight.getTx() * limelight.getHorizontalFov()/(2*100), 0.01);
                     // Square values
                     linearMagnitude = linearMagnitude * linearMagnitude;
-                    omega = Math.copySign(omega * omega, omega);
+//                    omega = Math.copySign(omega * omega, omega);
 
                     // Calcaulate new linear velocity
                     Translation2d linearVelocity =

src/main/java/frc/robot/commands/IntakeCommands.java

@@ -0,0 +1,58 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.intake.Intake;
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+
+public class IntakeCommands {
+    public static class IntakeTesting extends CommandBase{
+        LoggedDashboardNumber intakeAngle = new LoggedDashboardNumber("IntakeAngle", 0);
+        LoggedDashboardNumber rollerVolts = new LoggedDashboardNumber("RollerVolts", 0);
+        Intake intake;
+        public IntakeTesting(Intake intake){
+            this.intake = intake;
+            addRequirements(intake);
+        }
+
+        @Override
+        public void execute() {
+            intake.setPivotAngle(Rotation2d.fromRotations(intakeAngle.get()));
+            intake.rollerVoltage(rollerVolts.get());
+        }
+
+    }
+
+    public static class IntakeUp extends CommandBase{
+        Intake intake;
+        public IntakeUp(Intake intake){
+            this.intake = intake;
+            addRequirements(intake);
+        }
+
+        @Override
+        public void initialize() {
+            intake.setPivotAngle(Rotation2d.fromRotations(5));
+            intake.rollerVoltage(0);
+        }
+    }
+
+    public static class IntakeDown extends CommandBase{
+        Intake intake;
+        public IntakeDown(Intake intake){
+            this.intake = intake;
+            addRequirements(intake);
+        }
+
+        @Override
+        public void initialize() {
+            intake.setPivotAngle(Rotation2d.fromRotations(24));
+            intake.rollerVoltage(8);
+        }
+    }
+
+
+
+
+
+}

src/main/java/frc/robot/commands/ShooterCommands.java

@@ -99,6 +99,7 @@ public ShooterTesting(Shooter shooter) {
 
     @Override
     public void execute() {
+//      shooter.setMainVelocity(mainRPM.get());
       shooter.setMainVelocity(mainRPM.get());
       shooter.setSecondaryVelocity(secondaryRPM.get());
       shooter.setLoadingVoltage(loadingVoltage.get());

src/main/java/frc/robot/subsystems/intake/Intake.java

@@ -0,0 +1,64 @@
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.util.LoggedTunableNumber;
+import org.littletonrobotics.junction.Logger;
+
+
+public class Intake extends SubsystemBase {
+  private static final boolean TUNING_MODE = true;
+  private final IntakeIO io;
+
+  private final IntakeInputsAutoLogged inputs = new IntakeInputsAutoLogged();
+
+  private final LoggedTunableNumber kPLead = new LoggedTunableNumber("Intake/kPLead", 0.058);
+  private final LoggedTunableNumber kILead = new LoggedTunableNumber("Intake/kILead", 0);
+  private final LoggedTunableNumber kDLead = new LoggedTunableNumber("Intake/kDLead", 0.7);
+
+  private final LoggedTunableNumber kPFollow = new LoggedTunableNumber("Intake/kPFollow", 0.045);
+  private final LoggedTunableNumber kIFollow = new LoggedTunableNumber("Intake/kIFollow", 0);
+  private final LoggedTunableNumber kDFollow = new LoggedTunableNumber("Intake/kDFollow", 3);
+
+//  private final ArmFeedforward ff = new ArmFeedforward()
+
+  private Rotation2d setpoint = new Rotation2d();
+
+  public Intake(IntakeIO io) {
+    this.io = io;
+    io.setPivotBreakMode(true);
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.getInstance().processInputs("Intake", inputs);
+
+    io.configureLeadPID(0.05, 0, .7);
+    io.configureSecondPID(0.045, 0, 4);
+
+
+    if (TUNING_MODE && (kPLead.hasChanged() || kILead.hasChanged() || kDLead.hasChanged())) {
+      io.configureLeadPID(kPLead.get(), kILead.get(), kDLead.get());
+    }
+    if (TUNING_MODE && (kPFollow.hasChanged() || kIFollow.hasChanged() || kDFollow.hasChanged())) {
+    }
+
+    Logger.getInstance().recordOutput("Intake/setpoint", setpoint.getRotations());
+  }
+
+
+  public void rollerVoltage(double volts){
+    io.setRollerVoltage(volts);
+  }
+
+  public void setPivotAngle(Rotation2d angle){
+    System.out.println("Setting Angle Subsystem");
+    setpoint = angle;
+    io.setPivotPosition(angle);
+  }
+
+
+
+}

src/main/java/frc/robot/subsystems/intake/IntakeIO.java

@@ -0,0 +1,29 @@
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeIO {
+  @AutoLog
+  class IntakeInputs {
+    public double pivotVoltage = 0.0;
+    public double[] pivotCurrent = new double[] {};
+    public double[] pivotPosition = new double[]{};
+    public double pivotVelocity = 0.0;
+    public double rollerVoltage = 0.0;
+    public double rollerCurrent = 0.0;
+  }
+
+  public default void updateInputs(IntakeInputs inputs) {}
+
+  public default void setPivotVoltage(double volts) {}
+
+  public default void setRollerVoltage(double volts) {}
+
+  public default void setPivotBreakMode(boolean breakMode) {}
+
+  public default void setPivotPosition(Rotation2d position){}
+
+  public default void configureLeadPID(double kP, double kI, double kD){}
+  public default void configureSecondPID(double kP, double kI, double kD){}
+}

src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java

@@ -0,0 +1,114 @@
+package frc.robot.subsystems.intake;
+
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel;
+import com.revrobotics.SparkMaxPIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.Constants;
+
+public class IntakeIOReal implements IntakeIO {
+  private final TalonFX roller = new TalonFX(Constants.Intake.rollerID);
+  private final CANSparkMax leadSpark =
+      new CANSparkMax(Constants.Intake.leadPivotID, CANSparkMaxLowLevel.MotorType.kBrushless);
+  private final CANSparkMax followSpark =
+      new CANSparkMax(Constants.Intake.followerPivotID, CANSparkMaxLowLevel.MotorType.kBrushless);
+  private final SparkMaxPIDController leadSparkPID = leadSpark.getPIDController();
+  private final SparkMaxPIDController followSparkPID = followSpark.getPIDController();
+
+  private final StatusSignal<Double> rollerDutyCycle = roller.getDutyCycle();
+  private final StatusSignal<Double> rollerSupplyVoltage =  roller.getSupplyVoltage();
+  private final StatusSignal<Double> rollerCurrent = roller.getStatorCurrent();
+
+  public IntakeIOReal() {
+    //        roller.
+
+    leadSpark.restoreFactoryDefaults();
+    followSpark.restoreFactoryDefaults();
+
+    leadSpark.getEncoder().setPosition(0);
+    followSpark.getEncoder().setPosition(0);
+    leadSpark.setInverted(false);
+    followSpark.setInverted(true);
+
+    leadSpark.enableVoltageCompensation(12.0);
+    leadSpark.setSmartCurrentLimit(30);
+    followSpark.enableVoltageCompensation(12.0);
+    followSpark.setSmartCurrentLimit(30);
+
+    leadSpark.burnFlash();
+    followSpark.burnFlash();
+
+    var config = new TalonFXConfiguration();
+    config.CurrentLimits.StatorCurrentLimit = 30.0;
+    config.CurrentLimits.StatorCurrentLimitEnable = true;
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    roller.getConfigurator().apply(config);
+    roller.setInverted(true);
+
+    rollerDutyCycle.setUpdateFrequency(50.0);
+    rollerSupplyVoltage.setUpdateFrequency(50.0);
+    rollerCurrent.setUpdateFrequency(50.0);
+  }
+
+  @Override
+  public void updateInputs(IntakeInputs inputs) {
+    inputs.pivotCurrent =
+        new double[] {leadSpark.getOutputCurrent(), followSpark.getOutputCurrent()};
+    inputs.pivotVoltage = leadSpark.getBusVoltage() * leadSpark.getAppliedOutput();
+    inputs.pivotPosition = new double[]{leadSpark.getEncoder().getPosition(), followSpark.getEncoder().getPosition()};
+    inputs.pivotVelocity = leadSpark.getEncoder().getVelocity();
+
+    rollerDutyCycle.refresh();
+    rollerSupplyVoltage.refresh();
+    rollerCurrent.refresh();
+
+    inputs.rollerVoltage = rollerDutyCycle.getValue() * rollerSupplyVoltage.getValue();
+    inputs.rollerCurrent = rollerCurrent.getValue();
+  }
+
+  @Override
+  public void setPivotVoltage(double volts) {
+    leadSpark.setVoltage(volts);
+  }
+
+  @Override
+  public void setRollerVoltage(double volts) {
+    roller.setControl(new VoltageOut(volts));
+    //        roller.setVoltage(volts);
+  }
+
+  @Override
+  public void setPivotBreakMode(boolean breakMode) {
+    CANSparkMax.IdleMode idleMode = CANSparkMax.IdleMode.fromId(breakMode ? 1 : 0);
+    leadSpark.setIdleMode(idleMode);
+    followSpark.setIdleMode(idleMode);
+  }
+
+  @Override
+  public void configureLeadPID(double kP, double kI, double kD) {
+    leadSparkPID.setP(kP, 0);
+    leadSparkPID.setI(kI, 0);
+    leadSparkPID.setD(kD, 0);
+    leadSparkPID.setFF(0, 0);
+  }
+
+  @Override
+  public void configureSecondPID(double kP, double kI, double kD){
+    followSparkPID.setP(kP, 0);
+    followSparkPID.setI(kI, 0);
+    followSparkPID.setD(kD, 0);
+    followSparkPID.setFF(0, 0);
+  }
+
+  @Override
+  public void setPivotPosition(Rotation2d position) {
+    System.out.println("Set Pivot Position IO");
+    leadSparkPID.setReference(position.getRotations(), CANSparkMax.ControlType.kPosition);
+    followSparkPID.setReference(position.getRotations(), CANSparkMax.ControlType.kPosition);;
+  }
+}