diff --git a/src/main/java/frc/robot/commands/manipulator/SetManipulatorSpeed.java b/src/main/java/frc/robot/commands/manipulator/SetManipulatorSpeed.java new file mode 100644 index 0000000..35cd95f --- /dev/null +++ b/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); + } +} diff --git a/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java b/src/main/java/frc/robot/subsystems/manipulator/Manipulator.java new file mode 100644 index 0000000..0108213 --- /dev/null +++ b/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); + } +} diff --git a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java new file mode 100644 index 0000000..07d39ae --- /dev/null +++ b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java @@ -0,0 +1,31 @@ +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 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. */ +} diff --git a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOSparkMax.java b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOSparkMax.java new file mode 100644 index 0000000..4f13820 --- /dev/null +++ b/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(); + } + +}