feat: Intake & Indexer IO and subsystem

flamingchickens1540 · Jan 27, 2024 · 61e7def · 61e7def
1 parent a491418
commit 61e7def
Show file tree

Hide file tree

Showing 6 changed files with 142 additions and 43 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -57,6 +57,13 @@ public static class Drivetrain {
     }
     public static class Indexer {
         public static final int INTAKE_ID = 0;
+        public static final int FEEDER_ID = 0;
+        public static final double FEEDER_KP = 0.0;
+        public static final double FEEDER_KI = 0.0;
+        public static final double FEEDER_KD = 0.0;
+        public static final double FEEDER_KS = 0.0;
+        public static final double FEEDER_KV = 0.0;
+        public static final double GEAR_RATIO = 0.0;
 
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java b/src/main/java/org/team1540/robot2024/commands/indexer/IntakeCommand.java
@@ -0,0 +1,30 @@
+package org.team1540.robot2024.commands.indexer;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import org.team1540.robot2024.subsystems.indexer.Indexer;
+
+public class IntakeCommand extends Command {
+
+    private final Indexer indexer;
+
+    public IntakeCommand(Indexer indexer) {
+        this.indexer = indexer;
+        addRequirements(indexer);
+    }
+
+    @Override
+    public void initialize() {
+        indexer.setIntakePercent(0.4);
+    }
+
+
+    @Override
+    public boolean isFinished() {
+        return indexer.noteInIndexer();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        indexer.setIntakePercent(0);
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
@@ -1,5 +1,8 @@
 package org.team1540.robot2024.subsystems.indexer;
 
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
 
@@ -17,14 +20,29 @@ public void periodic() {
         Logger.processInputs("Indexer", indexerInputs);
     }
 
-    public void setIntakeSpeed(double speed) {
-        indexerIO.setIntakeSpeed(speed);
+    public void setIntakePercent(double percent) {
+        indexerIO.setIntakeVoltage(percent * 12.0);
     }
 
-    public void setIntakeVoltage(double volts) {
-        indexerIO.setIntakeVoltage(volts);
+    public boolean noteInIndexer() {
+        return indexerInputs.noteInIntake;
     }
 
+    public Command feedToAmp() {
+        return Commands.sequence(
+                Commands.runOnce(() -> indexerIO.setFeederVelocity(-600), this),
+                Commands.waitSeconds(5),
+                Commands.runOnce(() -> indexerIO.setFeederVelocity(0), this)
+        );
+    }
+
+    public Command feedToShooter() {
+        return Commands.sequence(
+                Commands.runOnce(() -> indexerIO.setFeederVelocity(1200), this),
+                Commands.waitSeconds(1),
+                Commands.runOnce(() -> indexerIO.setFeederVelocity(0), this)
+        );
+    }
 
 
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java
@@ -6,9 +6,13 @@ public interface IndexerIO {
 
     @AutoLog
     class IndexerIOInputs {
-        public double intakeVoltage = 0.0;
-        public double intakeCurrent = 0.0;
-        public double intakeVelocityRPM = 0.0;
+        public double intakeVoltage;
+        public double intakeCurrent;
+        public double intakeVelocityRPM;
+        public double feederVoltage;
+        public double feederCurrentAmps;
+        public double feederVelocityRPM;
+        public boolean noteInIntake;
     }
 
     /**
@@ -18,8 +22,8 @@ default void updateInputs(IndexerIOInputs inputs) {}
 
     default void setIntakeVoltage(double volts) {}
 
-    default void setIntakeSpeed(double speed) {}
-
+    default void setFeederVoltage(double volts) {}
 
+    default void setFeederVelocity(double velocity) {}
 
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOReal.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOReal.java
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java
@@ -0,0 +1,74 @@
+package org.team1540.robot2024.subsystems.indexer;
+
+import com.revrobotics.*;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DigitalInput;
+
+import static org.team1540.robot2024.Constants.Indexer.*;
+
+
+public class IndexerIOSparkMax implements IndexerIO {
+    private final CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless);
+    private final CANSparkMax feederMotor = new CANSparkMax(FEEDER_ID, CANSparkLowLevel.MotorType.kBrushless);
+    DigitalInput indexerBeamBreak = new DigitalInput(0);
+    private final SparkPIDController feederPID;
+    private final SimpleMotorFeedforward feederFF = new SimpleMotorFeedforward(FEEDER_KS, FEEDER_KV);
+
+
+    public IndexerIOSparkMax() {
+        intakeMotor.setIdleMode(CANSparkBase.IdleMode.kBrake);
+        intakeMotor.enableVoltageCompensation(12.0);
+        intakeMotor.setSmartCurrentLimit(30);
+
+        feederMotor.setIdleMode(CANSparkBase.IdleMode.kBrake);
+        feederMotor.enableVoltageCompensation(12.0);
+        feederMotor.setSmartCurrentLimit(30);
+
+        feederPID = feederMotor.getPIDController();
+        feederPID.setP(FEEDER_KP, 0);
+        feederPID.setI(FEEDER_KI, 0);
+        feederPID.setD(FEEDER_KD, 0);
+
+
+
+    }
+    // 2 jobs,
+    // 1. be able to route to tramp
+    // 2. feeding the shooter (by default, be still, spin up fast 4 for shooter.
+    // make cmd factory spin
+    // slower in other direction for tramp
+
+    @Override
+    public void updateInputs(IndexerIOInputs inputs) {
+        inputs.intakeCurrent = intakeMotor.getOutputCurrent();
+        inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput();
+        inputs.intakeVelocityRPM = intakeMotor.getEncoder().getVelocity();
+        inputs.feederVoltage = feederMotor.getOutputCurrent();
+        inputs.feederCurrentAmps = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput();
+        inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity();
+        inputs.noteInIntake = indexerBeamBreak.get();
+    }
+
+    @Override
+    public void setIntakeVoltage(double volts) {
+        intakeMotor.setVoltage(volts);
+    }
+
+    @Override
+    public void setFeederVoltage(double volts) {
+        feederMotor.setVoltage(volts);
+    }
+
+    @Override
+    public void setFeederVelocity(double velocity) {
+        feederPID.setReference(
+                Units.radiansPerSecondToRotationsPerMinute(velocity) * GEAR_RATIO,
+                CANSparkBase.ControlType.kVelocity,
+                0,
+                feederFF.calculate(velocity),
+                SparkPIDController.ArbFFUnits.kVoltage
+        );
+    }
+}
+// for elevator sim, app. voltage