diff --git a/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java new file mode 100644 index 0000000..a084457 --- /dev/null +++ b/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java @@ -0,0 +1,59 @@ +package frc.robot.shoulder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxPIDController; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.util.scheduling.LifecycleSubsystem; +import frc.robot.util.scheduling.SubsystemPriority; + +public class ShoulderSubsystem extends LifecycleSubsystem { + private final CANSparkMax motor; + private final CANSparkMax motorFollower; + + private RelativeEncoder encoder; + private SparkMaxPIDController pid; + private double goalAngle = 0.0; + + public ShoulderSubsystem(CANSparkMax motor, CANSparkMax motorFollower) { + super(SubsystemPriority.SHOULDER); + this.motor = motor; + this.motorFollower = motorFollower; + motorFollower.follow(motor); + encoder = motor.getEncoder(); + pid = motor.getPIDController(); + + pid.setP(5); + pid.setI(0); + pid.setD(0); + + encoder.setPosition(0); + encoder.setPositionConversionFactor(50); + } + + @Override + public void enabledPeriodic() { + pid.setReference(goalAngle / 360.0, ControlType.kSmartMotion); + } + + public void set(double angle) { + goalAngle = angle; + } + + public Command setPositionCommand(double angle) { + return run(() -> { + set(angle); + }) + .until(() -> atAngle(angle)); + } + + private boolean atAngle(double angle) { + return Math.abs(getWristAngle() - angle) < 1; + } + + private double getWristAngle() { + return encoder.getPosition() * 360.0; + } +} + diff --git a/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java b/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java index e5db634..4afddc5 100644 --- a/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java +++ b/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java @@ -13,6 +13,7 @@ public enum SubsystemPriority { SWERVE(10), IMU(10), INTAKE(10), + SHOULDER(10), // Run localization after swerve & IMU LOCALIZATION(9),