70 lines
1.9 KiB
Java
70 lines
1.9 KiB
Java
package frc.robot.subsystems;
|
|
|
|
import com.revrobotics.RelativeEncoder;
|
|
import com.revrobotics.spark.ClosedLoopSlot;
|
|
import com.revrobotics.spark.SparkClosedLoopController;
|
|
import com.revrobotics.spark.SparkMax;
|
|
import com.revrobotics.spark.SparkBase.ControlType;
|
|
import com.revrobotics.spark.SparkBase.PersistMode;
|
|
import com.revrobotics.spark.SparkBase.ResetMode;
|
|
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
|
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
|
|
import static frc.robot.constants.MotorIDsConstants.*;
|
|
import static frc.robot.constants.ClosedLoopTestConstants.*;
|
|
|
|
public class ClosedLoopTest extends SubsystemBase {
|
|
SparkMax motor;
|
|
|
|
SparkClosedLoopController controller;
|
|
|
|
RelativeEncoder encoder;
|
|
|
|
public ClosedLoopTest() {
|
|
motor = new SparkMax(kMotorClosedLoopTestCANID, MotorType.kBrushless);
|
|
|
|
motor.configure(
|
|
motorConfig,
|
|
ResetMode.kResetSafeParameters,
|
|
PersistMode.kPersistParameters
|
|
);
|
|
|
|
controller = motor.getClosedLoopController();
|
|
|
|
encoder = motor.getEncoder();
|
|
encoder.setPosition(0);
|
|
}
|
|
|
|
public Command incrementPosition(double increment) {
|
|
return runOnce(() -> {
|
|
setPosition(encoder.getPosition() + increment);
|
|
});
|
|
}
|
|
|
|
public Command setPosition(double position) {
|
|
return runOnce(() -> {
|
|
controller.setReference(
|
|
position,
|
|
ControlType.kMAXMotionPositionControl,
|
|
ClosedLoopSlot.kSlot0
|
|
);
|
|
});
|
|
}
|
|
|
|
public Command setVelocity(double velocity) {
|
|
return runOnce(() -> {
|
|
controller.setReference(
|
|
velocity,
|
|
ControlType.kMAXMotionVelocityControl,
|
|
ClosedLoopSlot.kSlot0
|
|
);
|
|
});
|
|
}
|
|
|
|
public double getEncoderPosition() {
|
|
return encoder.getPosition();
|
|
}
|
|
}
|