diff --git a/src/main/java/commands/MoveWristToAngle.java b/src/main/java/commands/MoveWristToAngle.java index 84259c7..302effb 100644 --- a/src/main/java/commands/MoveWristToAngle.java +++ b/src/main/java/commands/MoveWristToAngle.java @@ -18,7 +18,12 @@ public MoveWristToAngle(Rotation2d angle) { @Override public void execute() { - wrist.GoToPosition(angle); + wrist.goToPosition(angle); + } + + @Override + public boolean isFinished() { + return wrist.isAtAngle(angle); } } diff --git a/src/main/java/subsystems/wrist/IWrist.java b/src/main/java/subsystems/wrist/IWrist.java new file mode 100644 index 0000000..3a00d37 --- /dev/null +++ b/src/main/java/subsystems/wrist/IWrist.java @@ -0,0 +1,13 @@ +package subsystems.wrist; + +import edu.wpi.first.math.geometry.Rotation2d; + +public interface IWrist { + + public void updateInputs(WristInputsAutoLogged inputs); + + public void goToPosition(Rotation2d position); + + public void setPower(double power); + +} diff --git a/src/main/java/subsystems/wrist/Wrist.java b/src/main/java/subsystems/wrist/Wrist.java index a157aca..001a2b4 100644 --- a/src/main/java/subsystems/wrist/Wrist.java +++ b/src/main/java/subsystems/wrist/Wrist.java @@ -1,16 +1,21 @@ package subsystems.wrist; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.Logger; import utils.GBSubsystem; public class Wrist extends GBSubsystem { private static Wrist instance; - private static TalonSRX motor; + private IWrist iWrist; + private WristInputsAutoLogged inputs; + private Rotation2d targetAngle; private Wrist() { - this.motor = new TalonSRX(WristConstants.MOTOR_ID); + this.iWrist = WristFactory.create(); + this.inputs = new WristInputsAutoLogged(); + this.targetAngle = WristConstants.PresetPositions.STARTING.ANGLE; + this.iWrist.updateInputs(inputs); } public static Wrist getInstance() { @@ -20,16 +25,24 @@ public static Wrist getInstance() { return instance; } - public void GoToPosition(Rotation2d position) { - motor.set(WristConstants.PID_CONTROL_MODE, position.getRotations() % 1 * WristConstants.FULL_CIRCLE_ENCODER_TICKS); + public void goToPosition(Rotation2d position) { + this.targetAngle = position; + } + + public boolean isAtAngle(Rotation2d angle) { + return Math.abs(inputs.angle.getRadians() - angle.getRadians()) <= WristConstants.ANGLE_TOLERANCE.getRadians(); } @Override protected String getLogPath() { - return ""; + return "WRIST/"; } @Override - protected void subsystemPeriodic() {} + protected void subsystemPeriodic() { + iWrist.goToPosition(targetAngle); + iWrist.updateInputs(inputs); + Logger.processInputs("wrist", inputs); + } } diff --git a/src/main/java/subsystems/wrist/WristConstants.java b/src/main/java/subsystems/wrist/WristConstants.java index 8b82148..28589c7 100644 --- a/src/main/java/subsystems/wrist/WristConstants.java +++ b/src/main/java/subsystems/wrist/WristConstants.java @@ -1,13 +1,35 @@ package subsystems.wrist; -import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import edu.wpi.first.math.geometry.Rotation2d; public class WristConstants { - protected static final int MOTOR_ID = 11; - protected static final TalonSRXControlMode PID_CONTROL_MODE = TalonSRXControlMode.Position; + public enum PresetPositions { - protected static final int FULL_CIRCLE_ENCODER_TICKS = 168; + STARTING(Rotation2d.fromDegrees(90)), + SCORE(Rotation2d.fromDegrees(190)), + SCORE_TRAP(Rotation2d.fromDegrees(390)), // find + TRANSFER(Rotation2d.fromDegrees(185)), + SAFE(Rotation2d.fromDegrees(180)), + INTAKE(Rotation2d.fromDegrees(180)); + + public final Rotation2d ANGLE; + + PresetPositions(Rotation2d angle) { + this.ANGLE = angle; + } + + } + + public static final double LENGTH_OF_ENDEFFECTOR = 0.1; + + public static final double WRIST_MASS_KG = 0.5; + + public static final Rotation2d BACKWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(-50); + + public static final Rotation2d FORWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(200); + + public static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromRotations(0.05); } diff --git a/src/main/java/subsystems/wrist/WristFactory.java b/src/main/java/subsystems/wrist/WristFactory.java new file mode 100644 index 0000000..d21897e --- /dev/null +++ b/src/main/java/subsystems/wrist/WristFactory.java @@ -0,0 +1,17 @@ +package subsystems.wrist; + +import subsystems.wrist.simulationWrist.SimulationWrist; +import subsystems.wrist.talonWrist.TalonWrist; +import training.Robot; + +public class WristFactory { + + public static IWrist create() { + return switch (Robot.ROBOT_TYPE) { + case REAL -> new TalonWrist(); + case SIMULATION -> new SimulationWrist(); + }; + } + + +} diff --git a/src/main/java/subsystems/wrist/WristInputs.java b/src/main/java/subsystems/wrist/WristInputs.java new file mode 100644 index 0000000..7f54382 --- /dev/null +++ b/src/main/java/subsystems/wrist/WristInputs.java @@ -0,0 +1,11 @@ +package subsystems.wrist; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +@AutoLog +public class WristInputs { + + public Rotation2d angle; + +} diff --git a/src/main/java/subsystems/wrist/simulationWrist/SimulationWrist.java b/src/main/java/subsystems/wrist/simulationWrist/SimulationWrist.java new file mode 100644 index 0000000..e7260e3 --- /dev/null +++ b/src/main/java/subsystems/wrist/simulationWrist/SimulationWrist.java @@ -0,0 +1,57 @@ +package subsystems.wrist.simulationWrist; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import subsystems.GlobalConstants; +import subsystems.elbow.simulationElbow.SimulationElbowConstants; +import subsystems.wrist.IWrist; +import subsystems.wrist.WristConstants; +import subsystems.wrist.WristInputsAutoLogged; + +public class SimulationWrist implements IWrist { + + private final SingleJointedArmSim motor; + + private final PIDController controller; + + public SimulationWrist() { + motor = new SingleJointedArmSim( + DCMotor.getNEO(SimulationWristConstants.NUMBER_OF_MOTORS), + SimulationWristConstants.GEAR_RATIO, + SingleJointedArmSim.estimateMOI(WristConstants.LENGTH_OF_ENDEFFECTOR, WristConstants.WRIST_MASS_KG), + WristConstants.LENGTH_OF_ENDEFFECTOR, + WristConstants.BACKWARD_ANGLE_LIMIT.getRadians(), + WristConstants.FORWARD_ANGLE_LIMIT.getRadians(), + false, + WristConstants.PresetPositions.STARTING.ANGLE.getRadians() + ); + + controller = new PIDController(SimulationWristConstants.KP, SimulationWristConstants.KI, SimulationWristConstants.KD); + } + + + @Override + public void setPower(double power) { + setVoltage(power * GlobalConstants.SIMULATION_BATTERY_VOLTAGE); + } + + private void setVoltage(double voltage) { + double limited_voltage = Math + .min((Math.max(voltage, -GlobalConstants.SIMULATION_BATTERY_VOLTAGE)), GlobalConstants.SIMULATION_BATTERY_VOLTAGE); + motor.setInputVoltage(limited_voltage); + } + + @Override + public void updateInputs(WristInputsAutoLogged inputs) { + inputs.angle = Rotation2d.fromRadians(motor.getAngleRads()); + motor.update(SimulationElbowConstants.MOTOR_UPDATE_PERIOD); + } + + @Override + public void goToPosition(Rotation2d position) { + setVoltage(controller.calculate(motor.getAngleRads(), position.getRadians())); + } + +} diff --git a/src/main/java/subsystems/wrist/simulationWrist/SimulationWristConstants.java b/src/main/java/subsystems/wrist/simulationWrist/SimulationWristConstants.java new file mode 100644 index 0000000..e8e023d --- /dev/null +++ b/src/main/java/subsystems/wrist/simulationWrist/SimulationWristConstants.java @@ -0,0 +1,15 @@ +package subsystems.wrist.simulationWrist; + +public class SimulationWristConstants { + + public static final int NUMBER_OF_MOTORS = 1; + + public static final double GEAR_RATIO = 1; + + public static final double KP = 0.12; + + public static final double KI = 0; + + public static final double KD = 0.025; + +} diff --git a/src/main/java/subsystems/wrist/talonWrist/TalonWrist.java b/src/main/java/subsystems/wrist/talonWrist/TalonWrist.java new file mode 100644 index 0000000..f981c6b --- /dev/null +++ b/src/main/java/subsystems/wrist/talonWrist/TalonWrist.java @@ -0,0 +1,35 @@ +package subsystems.wrist.talonWrist; + +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import edu.wpi.first.math.geometry.Rotation2d; +import subsystems.wrist.IWrist; +import subsystems.wrist.WristInputsAutoLogged; + +public class TalonWrist implements IWrist { + + private static TalonSRX motor; + + public TalonWrist() { + this.motor = new TalonSRX(TalonWristConstants.MOTOR_ID); + } + + + public void goToPosition(Rotation2d position) { + motor.set( + TalonWristConstants.PID_CONTROL_MODE, + position.getRotations() % 1 * TalonWristConstants.FULL_CIRCLE_ENCODER_TICKS + ); + } + + @Override + public void setPower(double power) { + motor.set(TalonSRXControlMode.PercentOutput, power); + } + + @Override + public void updateInputs(WristInputsAutoLogged inputs) { + inputs.angle = Rotation2d.fromRotations(motor.getSelectedSensorPosition()); + } + +} diff --git a/src/main/java/subsystems/wrist/talonWrist/TalonWristConstants.java b/src/main/java/subsystems/wrist/talonWrist/TalonWristConstants.java new file mode 100644 index 0000000..aebcc4e --- /dev/null +++ b/src/main/java/subsystems/wrist/talonWrist/TalonWristConstants.java @@ -0,0 +1,13 @@ +package subsystems.wrist.talonWrist; + +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; + +public class TalonWristConstants { + + protected static final int MOTOR_ID = 11; + + protected static final TalonSRXControlMode PID_CONTROL_MODE = TalonSRXControlMode.Position; + + protected static final int FULL_CIRCLE_ENCODER_TICKS = 168; + +}