Manipulator

src/main/java/frc/robot/commands/manipulator/SetManipulatorSpeed.java

@@ -0,0 +1,21 @@
+package frc.robot.commands.manipulator;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.manipulator.Manipulator;
+
+public class SetManipulatorSpeed extends CommandBase {
+    Manipulator manipulator;
+    double speed;
+    public SetManipulatorSpeed(Manipulator manipulator, double speed){
+        this.manipulator = manipulator;
+        this.speed = speed;
+    }
+
+    private void init() {
+        addRequirements(manipulator);
+    }
+
+    public void execute() {
+        manipulator.setMotorPercent(speed);
+    }
+}

src/main/java/frc/robot/subsystems/manipulator/Manipulator.java

@@ -0,0 +1,32 @@
+package frc.robot.subsystems.manipulator;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.commands.manipulator.SetManipulatorSpeed;
+
+public class Manipulator extends SubsystemBase {
+    public ManipulatorIO io;
+    public ManipulatorIO.ManipulatorIOInputs inputs;
+
+    public Manipulator(ManipulatorIO io){
+        this.io = io;
+
+       setDefaultCommand(new SetManipulatorSpeed(this, 0.05));
+    }
+
+
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+    }
+
+    public void setMotorVoltage(double voltage) {
+        io.setVoltage(voltage);
+    }
+
+    public void setMotorPercent(double percentage) {
+        io.setPercentage(percentage);
+    }
+}

src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java

@@ -0,0 +1,33 @@
+package frc.robot.subsystems.manipulator;
+
+import frc.robot.subsystems.flywheel.FlywheelIO;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ManipulatorIO {
+    @AutoLog
+    public static class ManipulatorIOInputs {
+        public double positionRad = 0.0;
+        public double velocityRadPerSec = 0.0;
+        public double appliedVolts = 0.0;
+        public double[] currentAmps = new double[] {0.0};
+    }
+
+    /** Updates the set of loggable inputs. */
+    public default void updateInputs(ManipulatorIO.ManipulatorIOInputs inputs) {
+    }
+
+    /** Run closed loop at the specified velocity. */
+    public default void setVelocity(double velocityRadPerSec, double ffVolts) {
+    }
+
+    /** Stop in open loop. */
+    public default void stop() {
+    }
+    public default void setPercentage(double percentage) {
+    }
+
+    public default void setVoltage(double voltage) {
+    }
+
+    /** Set velocity PID constants. */
+}

src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOSparkMax.java

@@ -0,0 +1,43 @@
+package frc.robot.subsystems.manipulator;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxPIDController;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotController;
+import frc.robot.subsystems.flywheel.FlywheelIO;
+
+public class ManipulatorIOSparkMax implements ManipulatorIO {
+    public static final int deviceId = 31;
+    private final CANSparkMax motor;
+
+    public ManipulatorIOSparkMax(){
+        motor = new CANSparkMax(deviceId, CANSparkMaxLowLevel.MotorType.kBrushless);
+
+        motor.restoreFactoryDefaults();
+        motor.setIdleMode(CANSparkMax.IdleMode.kBrake);
+        motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus0, 10);
+        motor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.kStatus1, 20);
+    }
+
+    @Override
+    public void updateInputs(ManipulatorIOInputs inputs) {
+        inputs.appliedVolts = motor.getAppliedOutput() * RobotController.getBatteryVoltage();
+        inputs.currentAmps = new double[] {motor.getOutputCurrent()};
+    }
+    public void setMotorVoltage(double voltage) {
+        motor.setVoltage(voltage);
+    }
+
+    public void setPercentage(double percentage) {
+        motor.set(percentage);
+    }
+
+
+    @Override
+    public void stop() {
+        motor.stopMotor();
+    }
+
+}