Merge branch 'intake' into pivot

build.gradle

@@ -133,16 +133,17 @@ tasks.withType(JavaCompile) {
 }
 
 // Create version file
-project.compileJava.dependsOn(createVersionFile)
-gversion {
-    srcDir       = "src/main/java/"
-    classPackage = "frc.robot"
-    className    = "BuildConstants"
-    dateFormat   = "yyyy-MM-dd HH:mm:ss z"
-    timeZone     = "America/New_York"
-    indent       = "  "
-}
-
+/*
+ project.compileJava.dependsOn(createVersionFile)
+ gversion {
+ srcDir       = "src/main/java/"
+ classPackage = "frc.robot"
+ className    = "BuildConstants"
+ dateFormat   = "yyyy-MM-dd HH:mm:ss z"
+ timeZone     = "America/New_York"
+ indent       = "  "
+ }
+ */
 // Spotless formatting
 project.compileJava.dependsOn(spotlessApply)
 spotless {

gradlew

@@ -246,4 +246,4 @@ eval "set -- $(
         tr '\n' ' '
     )" '"$@"'
 
-exec "$JAVACMD" "$@"
+exec "$JAVACMD" "$@"

gradlew.bat

@@ -89,4 +89,4 @@ exit /b %EXIT_CODE%
 :mainEnd
 if "%OS%"=="Windows_NT" endlocal
 
-:omega
+:omega

src/main/java/frc/robot/Robot.java

@@ -39,11 +39,13 @@ public class Robot extends LoggedRobot {
   @Override
   public void robotInit() {
     // Record metadata
+    /*
     Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
     Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
     Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
     Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
     Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
+
     switch (BuildConstants.DIRTY) {
       case 0:
         Logger.recordMetadata("GitDirty", "All changes committed");
@@ -54,7 +56,7 @@ public void robotInit() {
       default:
         Logger.recordMetadata("GitDirty", "Unknown");
         break;
-    }
+      } */
 
     // Set up data receivers & replay source
     switch (Constants.currentMode) {

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

@@ -31,6 +31,13 @@
 import frc.robot.subsystems.drive.ModuleIOSparkMax;
 import frc.robot.subsystems.pivot.Pivot;
 import frc.robot.subsystems.pivot.PivotIOSim;
+import frc.robot.subsystems.flywheel.Flywheel;
+import frc.robot.subsystems.flywheel.FlywheelIO;
+import frc.robot.subsystems.flywheel.FlywheelIOSim;
+import frc.robot.subsystems.flywheel.FlywheelIOSparkMax;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.IntakeIOReal;
+import frc.robot.subsystems.intake.IntakeIOSim;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
@@ -67,6 +74,8 @@ public RobotContainer() {
                 new ModuleIOSparkMax(3));
         // change perchance
         pivot = Pivot.initialize(new PivotIOSim());
+        flywheel = new Flywheel(new FlywheelIOSparkMax());
+        Intake.initialize(new IntakeIOReal());
         // drive = new Drive(
         // new GyroIOPigeon2(),
         // new ModuleIOTalonFX(0),
@@ -86,6 +95,9 @@ public RobotContainer() {
                 new ModuleIOSim(),
                 new ModuleIOSim());
         pivot = Pivot.initialize(new PivotIOSim());
+        flywheel = new Flywheel(new FlywheelIOSim());
+        Intake.initialize(new IntakeIOSim());
+
         break;
 
       default:
@@ -157,7 +169,7 @@ private void configureButtonBindings() {
         .whileTrue(Commands.runEnd(() -> pivot.setGoal(Math.PI / 4), () -> pivot.setGoal(0), pivot));
   }
 
-  /**
+  /**   
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
    * @return the command to run in autonomous

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

@@ -0,0 +1,76 @@
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class Intake extends SubsystemBase {
+  private Timer noteTime = new Timer();
+  private IntakeIO io;
+  private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
+  private static Intake instance;
+  public double intakeSpeed;
+  public static double time1 = 0; // static
+  private double testamps = 0;
+  public static boolean flag = false;
+
+  private Intake(IntakeIO IO) {
+    io = IO;
+  }
+
+  public static Intake getInstance() {
+    return instance;
+  }
+
+  public static Intake initialize(IntakeIO io) {
+    if (instance == null) {
+      instance = new Intake(io);
+    }
+    return instance;
+  }
+
+  public void setIntakeSpeed(double speed) {
+    intakeSpeed = speed;
+  }
+
+  public boolean noteNotInIntake() { // ..
+    if (inputs.Amps < IntakeConstants.current) {
+      noteTime.start();
+      flag = true;
+      return inputs.Amps < IntakeConstants.current;
+    } else {
+      if (flag) {
+        time1 = noteTime.get();
+        noteTime.stop();
+        noteTime.reset();
+      }
+      return inputs.Amps < IntakeConstants.current;
+    }
+  }
+
+  public Command intake() {
+    return startEnd(() -> setIntakeSpeed(1), () -> setIntakeSpeed(0))
+        .onlyWhile(() -> noteNotInIntake());
+    // return (startEnd(() -> setIntakeSpeed(1), () -> setIntakeSpeed(0)).onlyWhile(() ->
+    // noteNotInIntake())).alongWith(startEnd(() -> setIntakeSpeed(0.1), () ->
+    // setIntakeSpeed(0)).onlyIf(() -> noteNotInIntake() == true).onlyWhile(() -> noteNotInIntake()
+    // == true));
+  }
+/*
+  public Command spike() {
+    return startEnd(() -> testamps = 150, () -> testamps = 0);
+  }
+*/
+  public Command reverse() {
+    return startEnd(() -> setIntakeSpeed(-1), () -> setIntakeSpeed(0));
+  }
+
+  @Override
+  public void periodic() {
+    Logger.recordOutput("Intake/testamps", testamps);
+    io.setMotorSpeed(intakeSpeed);
+    io.updateInputs(inputs);
+    Logger.processInputs("Intake", inputs);
+  }
+}

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

@@ -0,0 +1,7 @@
+package frc.robot.subsystems.intake;
+
+public class IntakeConstants {
+  public static final int CanID = 2; // ..
+  public static final int currentLimit = 40; // vortex
+  public static final int current = 10; // ..
+}

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

@@ -0,0 +1,18 @@
+package frc.robot.subsystems.intake;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeIO {
+  @AutoLog
+  public static class IntakeIOInputs {
+    public double angularVelocityRPM = 0.0;
+    public double angularPositionRot = 0.0;
+    public double Voltage = 0.0; // ..
+    public double Amps = 0.0;
+    public double noteTime = 0.0;
+  }
+
+  public default void updateInputs(IntakeIOInputs inputs) {}
+
+  public default void setMotorSpeed(double speed) {}
+}

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

@@ -0,0 +1,29 @@
+package frc.robot.subsystems.intake;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+
+public class IntakeIOReal implements IntakeIO {
+  private CANSparkMax IntakeMotor = new CANSparkMax(IntakeConstants.CanID, MotorType.kBrushless);
+  private final RelativeEncoder IntakEncoder = IntakeMotor.getEncoder();
+
+  public IntakeIOReal() {
+    IntakeMotor.setSmartCurrentLimit(IntakeConstants.currentLimit);
+    IntakeMotor.burnFlash();
+  }
+
+  @Override
+  public void setMotorSpeed(double speed) {
+    IntakeMotor.set(speed);
+  }
+
+  @Override
+  public void updateInputs(IntakeIOInputs inputs) {
+    inputs.angularVelocityRPM = IntakEncoder.getVelocity();
+    inputs.angularPositionRot = IntakEncoder.getPosition();
+    inputs.Voltage = IntakeMotor.getBusVoltage();
+    inputs.Amps = IntakeMotor.getOutputCurrent();
+    inputs.noteTime = Intake.time1;
+  }
+}

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

@@ -0,0 +1,23 @@
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+
+public class IntakeIOSim implements IntakeIO {
+  private final DCMotorSim motor = new DCMotorSim(DCMotor.getNeoVortex(1), 1, 1);
+
+  @Override
+  public void updateInputs(IntakeIOInputs inputs) {
+    motor.update(0.02);
+
+    inputs.angularPositionRot = motor.getAngularPositionRotations();
+    inputs.angularVelocityRPM = motor.getAngularVelocityRPM();
+    inputs.Amps = motor.getCurrentDrawAmps();
+    inputs.noteTime = Intake.time1;
+  }
+
+  @Override
+  public void setMotorSpeed(double speed) {
+    motor.setInputVoltage(speed);
+  }
+}