diff --git a/.run/Build & Deploy Robot for Debugging.run.xml b/.run/Build & Deploy Robot for Debugging.run.xml index 821ba32df..0fa85ce5d 100644 --- a/.run/Build & Deploy Robot for Debugging.run.xml +++ b/.run/Build & Deploy Robot for Debugging.run.xml @@ -65,4 +65,26 @@ false + + + + + + true + true + false + false + + \ No newline at end of file diff --git a/.run/Build & Deploy Robot.run.xml b/.run/Build & Deploy Robot.run.xml index 3d60b7bd7..618fba1aa 100644 --- a/.run/Build & Deploy Robot.run.xml +++ b/.run/Build & Deploy Robot.run.xml @@ -65,4 +65,26 @@ false + + + + + + true + true + false + false + + \ No newline at end of file diff --git a/.run/Build & Run Simulate Java.run.xml b/.run/Build & Run Simulate Java.run.xml new file mode 100644 index 000000000..a7218b9a4 --- /dev/null +++ b/.run/Build & Run Simulate Java.run.xml @@ -0,0 +1,24 @@ + + + + + + + true + true + false + false + + + \ No newline at end of file diff --git a/.run/Build Robot.run.xml b/.run/Build Robot.run.xml index 3899ab5c5..6b0d2ec71 100644 --- a/.run/Build Robot.run.xml +++ b/.run/Build Robot.run.xml @@ -65,4 +65,26 @@ false + + + + + + true + true + false + false + + \ No newline at end of file diff --git a/.run/Clean Build & Deploy Robot for Debugging.run.xml b/.run/Clean Build & Deploy Robot for Debugging.run.xml index 21d486c52..4e4b2d324 100644 --- a/.run/Clean Build & Deploy Robot for Debugging.run.xml +++ b/.run/Clean Build & Deploy Robot for Debugging.run.xml @@ -68,4 +68,27 @@ false + + + + + + true + true + false + false + + \ No newline at end of file diff --git a/.run/Clean Build & Deploy Robot.run.xml b/.run/Clean Build & Deploy Robot.run.xml index f1b07cd4e..d38efbee8 100644 --- a/.run/Clean Build & Deploy Robot.run.xml +++ b/.run/Clean Build & Deploy Robot.run.xml @@ -68,4 +68,27 @@ false + + + + + + true + true + false + false + + \ No newline at end of file diff --git a/.run/Clean Build & Run Simulate Java.run.xml b/.run/Clean Build & Run Simulate Java.run.xml new file mode 100644 index 000000000..d9cc9eaa4 --- /dev/null +++ b/.run/Clean Build & Run Simulate Java.run.xml @@ -0,0 +1,25 @@ + + + + + + + true + true + false + false + + + \ No newline at end of file diff --git a/.run/Clean Build Robot.run.xml b/.run/Clean Build Robot.run.xml index 7003ca64d..f11d1fd5a 100644 --- a/.run/Clean Build Robot.run.xml +++ b/.run/Clean Build Robot.run.xml @@ -68,4 +68,27 @@ false + + + + + + true + true + false + false + + \ No newline at end of file diff --git a/.run/Clean.run.xml b/.run/Clean.run.xml index c6b522475..a0b5a0241 100644 --- a/.run/Clean.run.xml +++ b/.run/Clean.run.xml @@ -65,4 +65,26 @@ false + + + + + + true + true + false + false + + \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 14b354c88..32b0f5712 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -21,4 +21,4 @@ public Command getAutonomousCommand() { private void configureBindings() { } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index e44ac51b3..981ca4120 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -4,10 +4,10 @@ public class RobotConstants { private static final String DEPLOY_PATH = Filesystem.getDeployDirectory().getPath() + "/"; + public static final RobotType ROBOT_TYPE = RobotType.SIMULATION; public static final boolean IS_REPLAY = false; public static final double PERIODIC_TIME_SECONDS = 0.02; - public static final RobotType ROBOT_TYPE = RobotType.SIMULATION; - + public enum RobotType { KABLAMA("/media/sda1/logs/"), TRIHARD(DEPLOY_PATH + "logs/"), diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java new file mode 100644 index 000000000..3d45581e8 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -0,0 +1,101 @@ +package frc.trigon.robot.subsystems.arm; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.trigon.robot.utilities.Conversions; +import org.littletonrobotics.junction.Logger; + +public class Arm extends SubsystemBase { + private final static Arm INSTANCE = new Arm(); + private final ArmIO armIO = ArmIO.generateIO(); + private final ArmInputsAutoLogged armInputs = new ArmInputsAutoLogged(); + private TrapezoidProfile + angleMotorProfile = null, + elevatorMotorProfile = null; + private double + lastAngleMotorProfileGenerationTime, + lastElevatorMotorProfileGenerationTime; + + public static Arm getInstance() { + return INSTANCE; + } + + private Arm() { + } + + @Override + public void periodic() { + armIO.updateInputs(armInputs); + Logger.processInputs("Arm", armInputs); + updateMechanism(); + } + + boolean atAngle(Rotation2d targetAngle) { + return Math.abs(armInputs.anglePositionDegrees - targetAngle.getDegrees()) <= ArmConstants.ANGLE_TOLERANCE_DEGREES; + } + + boolean atTargetElevatorPosition(double targetElevatorPositionMeters) { + return Math.abs(armInputs.elevatorPositionMeters - targetElevatorPositionMeters) <= ArmConstants.ELEVATOR_TOLERANCE_METERS; + } + + boolean isElevatorOpening(double targetElevatorPositionMeters) { + return armInputs.elevatorPositionMeters < targetElevatorPositionMeters; + } + + void generateAngleMotorProfile(Rotation2d targetAngle, double speedPercentage) { + angleMotorProfile = new TrapezoidProfile( + Conversions.scaleConstraints(ArmConstants.ANGLE_CONSTRAINTS, speedPercentage), + new TrapezoidProfile.State(targetAngle.getDegrees(), 0), + new TrapezoidProfile.State(armInputs.anglePositionDegrees, armInputs.angleVelocityDegreesPerSecond) + ); + + lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp(); + } + + void generateElevatorMotorProfile(double targetElevatorPositionMeters, double speedPercentage) { + elevatorMotorProfile = new TrapezoidProfile( + Conversions.scaleConstraints(ArmConstants.ELEVATOR_CONSTRAINTS, speedPercentage), + new TrapezoidProfile.State(targetElevatorPositionMeters, 0), + new TrapezoidProfile.State(armInputs.elevatorPositionMeters, armInputs.elevatorVelocityMetersPerSecond) + ); + + lastElevatorMotorProfileGenerationTime = Timer.getFPGATimestamp(); + } + + void setTargetAngleFromProfile() { + if (angleMotorProfile == null) { + armIO.stopAngleMotors(); + return; + } + + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + ArmConstants.TARGET_POSITION_LIGAMENT.setAngle(targetState.position); + armIO.setTargetAngleState(targetState); + } + + void setTargetElevatorPositionFromProfile() { + if (elevatorMotorProfile == null) { + armIO.stopElevatorMotors(); + return; + } + + TrapezoidProfile.State targetState = elevatorMotorProfile.calculate(getElevatorMotorProfileTime()); + ArmConstants.TARGET_POSITION_LIGAMENT.setLength(targetState.position + ArmConstants.RETRACTED_ARM_LENGTH_METERS); + armIO.setTargetElevatorState(targetState); + } + + private double getAngleMotorProfileTime() { + return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; + } + + private double getElevatorMotorProfileTime() { + return Timer.getFPGATimestamp() - lastElevatorMotorProfileGenerationTime; + } + private void updateMechanism() { + ArmConstants.ARM_LIGAMENT.setLength(armInputs.elevatorPositionMeters + ArmConstants.RETRACTED_ARM_LENGTH_METERS); + ArmConstants.ARM_LIGAMENT.setAngle(armInputs.anglePositionDegrees); + Logger.recordOutput("ArmMechanism", ArmConstants.ARM_MECHANISM); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java new file mode 100644 index 000000000..885885f59 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -0,0 +1,86 @@ +package frc.trigon.robot.subsystems.arm; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.*; +import frc.trigon.robot.utilities.Commands; + +import java.util.Set; + +public class ArmCommands { + private static final Arm ARM = Arm.getInstance(); + + public static Command getSetTargetArmStateCommand(ArmConstants.ArmState targetState) { + return getSetTargetArmStateCommand(targetState, 100, 100); + } + + public static Command getSetTargetArmPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeter) { + return getSetTargetArmPositionCommand(targetAngle, targetElevatorPositionMeter, 100, 100); + } + + /** + * Creates a command that sets the target state of the arm. + * + * @param targetState the target state of the arm + * @param angleSpeedPercentage the percentage of speed that the angle will move in + * @param elevatorSpeedPercentage the percentage of speed that the elevator will move in + * @return the command + */ + public static Command getSetTargetArmStateCommand(ArmConstants.ArmState targetState, double angleSpeedPercentage, double elevatorSpeedPercentage) { + return getSetTargetArmPositionCommand(targetState.angle, targetState.elevatorPositionMeters, angleSpeedPercentage, elevatorSpeedPercentage); + } + + /** + * Creates a command that sets the target position of the arm. + * + * @param targetAngle the target angle of the arm + * @param targetElevatorPositionMeters the target position of the arm's elevator + * @param angleSpeedPercentage the percentage of speed that the angle will move in + * @param elevatorSpeedPercentage the percentage of speed that the elevator will move in + * @return the command + */ + public static Command getSetTargetArmPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters, double angleSpeedPercentage, double elevatorSpeedPercentage) { + return new DeferredCommand( + () -> getCurrentSetTargetArmStateCommand(targetAngle, targetElevatorPositionMeters, angleSpeedPercentage, elevatorSpeedPercentage), + Set.of(ARM) + ); + } + + private static Command getCurrentSetTargetArmStateCommand(Rotation2d targetAngle, double elevatorPosition, double angleSpeedPercentage, double elevatorSpeedPercentageMeters) { + if (ARM.isElevatorOpening(elevatorPosition)) { + return new SequentialCommandGroup( + getSetTargetAngleCommand(targetAngle, angleSpeedPercentage).until(() -> ARM.atAngle(targetAngle)), + getSetTargetElevatorPositionCommand(elevatorPosition, elevatorSpeedPercentageMeters).alongWith( + Commands.removeRequirements(getSetTargetAngleCommand(targetAngle, angleSpeedPercentage)) + ) + ); + } + return new SequentialCommandGroup( + getSetTargetElevatorPositionCommand(elevatorPosition, elevatorSpeedPercentageMeters).until(() -> ARM.atTargetElevatorPosition(elevatorPosition)), + getSetTargetAngleCommand(targetAngle, angleSpeedPercentage).alongWith( + Commands.removeRequirements(getSetTargetElevatorPositionCommand(elevatorPosition, elevatorSpeedPercentageMeters)) + ) + ); + } + + private static Command getSetTargetAngleCommand(Rotation2d targetAngle, double speedPercentage) { + return new FunctionalCommand( + () -> ARM.generateAngleMotorProfile(targetAngle, speedPercentage), + ARM::setTargetAngleFromProfile, + (interrupted) -> { + }, + () -> false, + ARM + ); + } + + private static Command getSetTargetElevatorPositionCommand(double targetElevatorPositionMeters, double speedPercentage) { + return new FunctionalCommand( + () -> ARM.generateElevatorMotorProfile(targetElevatorPositionMeters, speedPercentage), + ARM::setTargetElevatorPositionFromProfile, + (interrupted) -> { + }, + () -> false, + ARM + ); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java new file mode 100644 index 000000000..a39858ee5 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -0,0 +1,60 @@ +package frc.trigon.robot.subsystems.arm; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class ArmConstants { + + public static final double RETRACTED_ARM_LENGTH_METERS = 0.64; + private static final double + MAX_ANGLE_VELOCITY = 150, + MAX_ANGLE_ACCELERATION = 190, + MAX_ELEVATOR_VELOCITY = 2, + MAX_ELEVATOR_ACCELERATION = 3; + static final TrapezoidProfile.Constraints + ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION + ), + ELEVATOR_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION + ); + + private static final double + ARM_MECHANISM_WIDTH = 4, + ARM_MECHANISM_HEIGHT = 4; + private static final double + ARM_ROOT_X = 1, + ARM_ROOT_Y = 1; + private static final double MECHANISM_LINE_WIDTH = 10; + + static final Mechanism2d ARM_MECHANISM = new Mechanism2d(ARM_MECHANISM_WIDTH, ARM_MECHANISM_HEIGHT); + private static final MechanismRoot2d + ARM_ROOT = ARM_MECHANISM.getRoot("Z_ARM_ROOT", ARM_ROOT_X, ARM_ROOT_Y), + TARGET_POSITION_ROOT = ARM_MECHANISM.getRoot("TargetPositionRoot", ARM_ROOT_X, ARM_ROOT_Y); + static final MechanismLigament2d + ARM_LIGAMENT = ARM_ROOT.append(new MechanismLigament2d("ArmLigament", 0, 0, MECHANISM_LINE_WIDTH, new Color8Bit(Color.kBlue))), + TARGET_POSITION_LIGAMENT = TARGET_POSITION_ROOT.append(new MechanismLigament2d("TargetPositionLigament", 0, 0, MECHANISM_LINE_WIDTH, new Color8Bit(Color.kGray))); + static final double + ANGLE_TOLERANCE_DEGREES = 1, + ELEVATOR_TOLERANCE_METERS = 0.01; + + public enum ArmState { + DEFAULT_STATE(Rotation2d.fromDegrees(50), 0.9), + CONE_COLLECTION(Rotation2d.fromDegrees(15), 1), + HIGH_CONE(Rotation2d.fromDegrees(85), 1.6), + LOW_CONE(Rotation2d.fromDegrees(25), 0.4); + + final Rotation2d angle; + final double elevatorPositionMeters; + + ArmState(Rotation2d angle, double elevatorPositionMeters) { + this.angle = angle; + this.elevatorPositionMeters = elevatorPositionMeters; + } + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmIO.java new file mode 100644 index 000000000..3586e4522 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmIO.java @@ -0,0 +1,45 @@ +package frc.trigon.robot.subsystems.arm; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import frc.trigon.robot.constants.RobotConstants; +import frc.trigon.robot.subsystems.arm.kablamaarm.KablamaArmIO; +import frc.trigon.robot.subsystems.arm.simulationarm.SimulationArmIO; +import org.littletonrobotics.junction.AutoLog; + +public class ArmIO { + static ArmIO generateIO() { + if (RobotConstants.IS_REPLAY) + return new ArmIO(); + if (RobotConstants.ROBOT_TYPE == RobotConstants.RobotType.KABLAMA) + return new KablamaArmIO(); + return new SimulationArmIO(); + } + + protected void updateInputs(ArmInputsAutoLogged inputs) { + } + + protected void stopAngleMotors() { + } + + protected void stopElevatorMotors() { + } + + protected void setTargetAngleState(TrapezoidProfile.State targetState) { + } + + protected void setTargetElevatorState(TrapezoidProfile.State targetState) { + } + + @AutoLog + protected static class ArmInputs { + public double angleMotorCurrent = 0; + public double angleMotorVoltage = 0; + public double anglePositionDegrees = 0; + public double angleVelocityDegreesPerSecond = 0; + + public double elevatorMotorCurrent = 0; + public double elevatorMotorVoltage = 0; + public double elevatorPositionMeters = 0; + public double elevatorVelocityMetersPerSecond = 0; + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/kablamaarm/KablamaArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/kablamaarm/KablamaArmConstants.java new file mode 100644 index 000000000..b716d6329 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/kablamaarm/KablamaArmConstants.java @@ -0,0 +1,127 @@ +package frc.trigon.robot.subsystems.arm.kablamaarm; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; +import frc.trigon.robot.utilities.Conversions; + +public class KablamaArmConstants { + private static final int + MASTER_ANGLE_MOTOR_ID = 1, + MASTER_ELEVATOR_MOTOR_ID = 2, + FOLLOWER_ANGLE_MOTOR_ID = 3, + FOLLOWER_ELEVATOR_MOTOR_ID = 4, + ANGLE_ENCODER_ID = 5, + ELEVATOR_ENCODER_ID = 6; + private static final CANSparkMax.IdleMode + ELEVATOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake, + ANGLE_IDLE_MODE = CANSparkMax.IdleMode.kBrake; + private static final int VOLTAGE_COMPENSATION_SATURATION = 12; + private static final boolean + MASTER_ANGLE_INVERTED = false, + MASTER_ELEVATOR_INVERTED = false, + FOLLOWER_ANGLE_INVERTED = false, + FOLLOWER_ELEVATOR_INVERTED = false; + private static final SensorDirectionValue ANGLE_ENCODER_DIRECTION = SensorDirectionValue.CounterClockwise_Positive; + private static final boolean ELEVATOR_ENCODER_PHASE = false; + private static final int + ELEVATOR_ENCODER_OFFSET = 0, + ANGLE_ENCODER_OFFSET = 0; + private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); + static final WPI_TalonSRX ELEVATOR_ENCODER = new WPI_TalonSRX(ELEVATOR_ENCODER_ID); + static final CANSparkMax + MASTER_ANGLE_MOTOR = new CANSparkMax(MASTER_ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), + MASTER_ELEVATOR_MOTOR = new CANSparkMax(MASTER_ELEVATOR_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), + FOLLOWER_ANGLE_MOTOR = new CANSparkMax(FOLLOWER_ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), + FOLLOWER_ELEVATOR_MOTOR = new CANSparkMax(FOLLOWER_ELEVATOR_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + + static final StatusSignal + ANGLE_MOTOR_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), + ANGLE_MOTOR_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + + private static final double + ANGLE_P = 5, + ANGLE_I = 1, + ANGLE_D = 1, + ELEVATOR_P = 5, + ELEVATOR_I = 1, + ELEVATOR_D = 1; + static final PIDController + ANGLE_PID_CONTROLLER = new PIDController(ANGLE_P, ANGLE_I, ANGLE_D), + ELEVATOR_PID_CONTROLLER = new PIDController(ELEVATOR_P, ELEVATOR_I, ELEVATOR_D); + + private static final double + ANGLE_MOTOR_KS = 0, + ANGLE_MOTOR_KV = 0, + ANGLE_MOTOR_KA = 0, + ANGLE_MOTOR_KG = 0, + ELEVATOR_MOTOR_KS = 0, + ELEVATOR_MOTOR_KV = 0, + ELEVATOR_MOTOR_KA = 0, + ELEVATOR_MOTOR_KG = 0; + static final ArmFeedforward ANGLE_FEEDFORWARD = new ArmFeedforward( + ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA + ); + static final ElevatorFeedforward ELEVATOR_FEEDFORWARD = new ElevatorFeedforward( + ELEVATOR_MOTOR_KS, ELEVATOR_MOTOR_KG, ELEVATOR_MOTOR_KV, ELEVATOR_MOTOR_KA + ); + static final double THEORETICAL_METERS_PER_REVOLUTION = 0.1256; + + static { + configureAngleEncoder(); + configureElevatorEncoder(); + configureElevatorMotors(); + configureAngleMotors(); + } + + private static void configureElevatorMotors() { + MASTER_ELEVATOR_MOTOR.restoreFactoryDefaults(); + FOLLOWER_ELEVATOR_MOTOR.restoreFactoryDefaults(); + MASTER_ELEVATOR_MOTOR.setIdleMode(ELEVATOR_IDLE_MODE); + FOLLOWER_ELEVATOR_MOTOR.setIdleMode(ELEVATOR_IDLE_MODE); + MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED); + FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ELEVATOR_INVERTED); + MASTER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + FOLLOWER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + } + + private static void configureAngleMotors() { + MASTER_ANGLE_MOTOR.restoreFactoryDefaults(); + FOLLOWER_ANGLE_MOTOR.restoreFactoryDefaults(); + MASTER_ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE); + FOLLOWER_ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE); + MASTER_ANGLE_MOTOR.setInverted(MASTER_ANGLE_INVERTED); + FOLLOWER_ANGLE_MOTOR.setInverted(FOLLOWER_ANGLE_INVERTED); + MASTER_ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + FOLLOWER_ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + } + + private static void configureAngleEncoder() { + CANcoderConfiguration angleEncoderConfig = new CANcoderConfiguration(); + angleEncoderConfig.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; + angleEncoderConfig.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_RANGE; + angleEncoderConfig.MagnetSensor.SensorDirection = ANGLE_ENCODER_DIRECTION; + ANGLE_ENCODER.getConfigurator().apply(angleEncoderConfig); + + ANGLE_MOTOR_POSITION_SIGNAL.setUpdateFrequency(100); + ANGLE_MOTOR_VELOCITY_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER.optimizeBusUtilization(); + } + + private static void configureElevatorEncoder() { + ELEVATOR_ENCODER.configFactoryDefault(); + ELEVATOR_ENCODER.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute); + ELEVATOR_ENCODER.setSensorPhase(ELEVATOR_ENCODER_PHASE); + ELEVATOR_ENCODER.setSelectedSensorPosition(Conversions.offsetRead(ELEVATOR_ENCODER.getSelectedSensorPosition(), ELEVATOR_ENCODER_OFFSET)); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/kablamaarm/KablamaArmIO.java b/src/main/java/frc/trigon/robot/subsystems/arm/kablamaarm/KablamaArmIO.java new file mode 100644 index 000000000..951b1a9ea --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/kablamaarm/KablamaArmIO.java @@ -0,0 +1,105 @@ +package frc.trigon.robot.subsystems.arm.kablamaarm; + +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import frc.trigon.robot.subsystems.arm.ArmIO; +import frc.trigon.robot.subsystems.arm.ArmInputsAutoLogged; +import frc.trigon.robot.utilities.Conversions; + +public class KablamaArmIO extends ArmIO { + private final CANSparkMax + masterAngleMotor = KablamaArmConstants.MASTER_ANGLE_MOTOR, + followerAngleMotor = KablamaArmConstants.FOLLOWER_ANGLE_MOTOR, + masterElevatorMotor = KablamaArmConstants.MASTER_ELEVATOR_MOTOR, + followerElevatorMotor = KablamaArmConstants.FOLLOWER_ELEVATOR_MOTOR; + private final TalonSRX elevatorEncoder = KablamaArmConstants.ELEVATOR_ENCODER; + + @Override + protected void updateInputs(ArmInputsAutoLogged inputs) { + inputs.angleMotorVoltage = masterAngleMotor.getBusVoltage(); + inputs.angleMotorCurrent = masterAngleMotor.getOutputCurrent(); + inputs.angleVelocityDegreesPerSecond = getAngleVelocityDegreesPerSecond(); + inputs.anglePositionDegrees = getAngleMotorPosition().getDegrees(); + + inputs.elevatorMotorVoltage = masterElevatorMotor.getBusVoltage(); + inputs.elevatorMotorCurrent = masterElevatorMotor.getOutputCurrent(); + inputs.elevatorPositionMeters = getElevatorPositionMeters(); + inputs.elevatorVelocityMetersPerSecond = getElevatorVelocityMetersPerSecond(); + } + + @Override + protected void stopAngleMotors() { + masterAngleMotor.stopMotor(); + followerAngleMotor.stopMotor(); + } + + @Override + protected void stopElevatorMotors() { + masterElevatorMotor.stopMotor(); + followerElevatorMotor.stopMotor(); + } + + @Override + protected void setTargetAngleState(TrapezoidProfile.State targetState) { + setAngleVoltage(calculateAngleOutput(targetState)); + } + + @Override + protected void setTargetElevatorState(TrapezoidProfile.State targetState) { + setElevatorVoltage(calculateElevatorOutput(targetState)); + } + + private void setAngleVoltage(double voltage) { + masterAngleMotor.setVoltage(voltage); + followerAngleMotor.setVoltage(voltage); + } + + private void setElevatorVoltage(double voltage) { + masterElevatorMotor.setVoltage(voltage); + followerElevatorMotor.setVoltage(voltage); + } + + private double calculateAngleOutput(TrapezoidProfile.State targetState) { + double pidOutput = KablamaArmConstants.ANGLE_PID_CONTROLLER.calculate( + getAngleMotorPosition().getDegrees(), + targetState.position + ); + double feedforward = KablamaArmConstants.ANGLE_FEEDFORWARD.calculate( + Units.degreesToRadians(targetState.position), + Units.degreesToRadians(targetState.velocity) + ); + return pidOutput + feedforward; + } + + private double calculateElevatorOutput(TrapezoidProfile.State targetState) { + double pidOutput = KablamaArmConstants.ELEVATOR_PID_CONTROLLER.calculate( + getElevatorPositionMeters(), + targetState.position + ); + double feedforward = KablamaArmConstants.ELEVATOR_FEEDFORWARD.calculate(targetState.velocity); + return pidOutput + feedforward; + } + + private double getElevatorPositionMeters() { + return Conversions.magTicksToRevolutions(elevatorEncoder.getSelectedSensorPosition()) / KablamaArmConstants.THEORETICAL_METERS_PER_REVOLUTION; + } + + private double getAngleVelocityDegreesPerSecond() { + double positionRevolutions = KablamaArmConstants.ANGLE_MOTOR_VELOCITY_SIGNAL.refresh().getValue(); + return Conversions.revolutionsToDegrees(positionRevolutions); + } + + private double getElevatorVelocityMetersPerSecond() { + double magTicksPerSecond = Conversions.perHundredMsToPerSecond(elevatorEncoder.getSelectedSensorVelocity()); + double elevatorPositionRevolutions = Conversions.magTicksToRevolutions(magTicksPerSecond); + return elevatorPositionRevolutions / KablamaArmConstants.THEORETICAL_METERS_PER_REVOLUTION; + } + + private Rotation2d getAngleMotorPosition() { + double positionRevolutions = KablamaArmConstants.ANGLE_MOTOR_POSITION_SIGNAL.refresh().getValue(); + return Rotation2d.fromRotations(positionRevolutions); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/simulationarm/SimulationArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/simulationarm/SimulationArmConstants.java new file mode 100644 index 000000000..b1d4f7a3d --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/simulationarm/SimulationArmConstants.java @@ -0,0 +1,78 @@ +package frc.trigon.robot.subsystems.arm.simulationarm; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ElevatorFeedforward; +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.ElevatorSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.trigon.robot.subsystems.arm.ArmConstants; + +public class SimulationArmConstants { + private static final int + ANGLE_MOTOR_AMOUNT = 2, + ELEVATOR_MOTOR_AMOUNT = 2; + private static final DCMotor + ANGLE_MOTOR_GEARBOX = DCMotor.getNEO(ANGLE_MOTOR_AMOUNT), + ELEVATOR_MOTOR_GEARBOX = DCMotor.getNEO(ELEVATOR_MOTOR_AMOUNT); + private static final double + ANGLE_GEAR_RATIO = 95.2, + ELEVATOR_GEAR_RATIO = 13.29; + private static final Rotation2d + MINIMUM_ANGLE = Rotation2d.fromDegrees(-45), + MAXIMUM_ANGLE = Rotation2d.fromDegrees(180); + private static final boolean + ANGLE_SIMULATE_GRAVITY = true, + ELEVATOR_SIMULATE_GRAVITY = true; + private static final double ARM_MASS = 12; + private static final double DRUM_RADIUS_METERS = 0.1256 / 2; + private static final double FULLY_OPENED_ARM_LENGTH_METERS = 1.7; + static final SingleJointedArmSim ANGLE_MOTOR = new SingleJointedArmSim( + ANGLE_MOTOR_GEARBOX, + ANGLE_GEAR_RATIO, + SingleJointedArmSim.estimateMOI(ArmConstants.RETRACTED_ARM_LENGTH_METERS, ARM_MASS), + ArmConstants.RETRACTED_ARM_LENGTH_METERS, + MINIMUM_ANGLE.getRadians(), + MAXIMUM_ANGLE.getRadians(), + ANGLE_SIMULATE_GRAVITY, + MINIMUM_ANGLE.getRadians() + ); + static final ElevatorSim ELEVATOR_MOTOR = new ElevatorSim( + ELEVATOR_MOTOR_GEARBOX, + ELEVATOR_GEAR_RATIO, + ARM_MASS, + DRUM_RADIUS_METERS, + ArmConstants.RETRACTED_ARM_LENGTH_METERS, + FULLY_OPENED_ARM_LENGTH_METERS, + ELEVATOR_SIMULATE_GRAVITY, + ArmConstants.RETRACTED_ARM_LENGTH_METERS + ); + + private static final double + ANGLE_P = 1.5, + ANGLE_I = 0, + ANGLE_D = 0, + ELEVATOR_P = 120, + ELEVATOR_I = 0, + ELEVATOR_D = 0; + static final PIDController + ANGLE_PID_CONTROLLER = new PIDController(ANGLE_P, ANGLE_I, ANGLE_D), + ELEVATOR_PID_CONTROLLER = new PIDController(ELEVATOR_P, ELEVATOR_I, ELEVATOR_D); + + private static final double + ANGLE_MOTOR_KS = 0, + ANGLE_MOTOR_KV = 0, + ANGLE_MOTOR_KA = 0, + ANGLE_MOTOR_KG = 0, + ELEVATOR_MOTOR_KS = 0, + ELEVATOR_MOTOR_KV = 0, + ELEVATOR_MOTOR_KA = 0, + ELEVATOR_MOTOR_KG = 0; + static final ArmFeedforward ANGLE_FEEDFORWARD = new ArmFeedforward( + ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA + ); + static final ElevatorFeedforward ELEVATOR_FEEDFORWARD = new ElevatorFeedforward( + ELEVATOR_MOTOR_KS, ELEVATOR_MOTOR_KG, ELEVATOR_MOTOR_KV, ELEVATOR_MOTOR_KA + ); +} diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/simulationarm/SimulationArmIO.java b/src/main/java/frc/trigon/robot/subsystems/arm/simulationarm/SimulationArmIO.java new file mode 100644 index 000000000..5f7a7c9bd --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/simulationarm/SimulationArmIO.java @@ -0,0 +1,89 @@ +package frc.trigon.robot.subsystems.arm.simulationarm; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.trigon.robot.constants.RobotConstants; +import frc.trigon.robot.subsystems.arm.ArmConstants; +import frc.trigon.robot.subsystems.arm.ArmIO; +import frc.trigon.robot.subsystems.arm.ArmInputsAutoLogged; + +public class SimulationArmIO extends ArmIO { + private final SingleJointedArmSim angleMotor = SimulationArmConstants.ANGLE_MOTOR; + private final ElevatorSim elevatorMotor = SimulationArmConstants.ELEVATOR_MOTOR; + private double + angleVoltage = 0, + elevatorVoltage = 0; + + @Override + protected void updateInputs(ArmInputsAutoLogged inputs) { + angleMotor.update(RobotConstants.PERIODIC_TIME_SECONDS); + elevatorMotor.update(RobotConstants.PERIODIC_TIME_SECONDS); + + inputs.angleMotorVoltage = angleVoltage; + inputs.angleMotorCurrent = angleMotor.getCurrentDrawAmps(); + inputs.anglePositionDegrees = Units.radiansToDegrees(angleMotor.getAngleRads()); + inputs.angleVelocityDegreesPerSecond = Units.radiansToDegrees(angleMotor.getVelocityRadPerSec()); + + inputs.elevatorMotorVoltage = elevatorVoltage; + inputs.elevatorMotorCurrent = elevatorMotor.getCurrentDrawAmps(); + inputs.elevatorPositionMeters = getElevatorPositionMeters(); + inputs.elevatorVelocityMetersPerSecond = elevatorMotor.getVelocityMetersPerSecond(); + } + + @Override + protected void stopAngleMotors() { + setAngleVoltage(0); + } + + @Override + protected void stopElevatorMotors() { + setElevatorVoltage(0); + } + + @Override + protected void setTargetAngleState(TrapezoidProfile.State targetState) { + setAngleVoltage(calculateAngleOutput(targetState)); + } + + @Override + protected void setTargetElevatorState(TrapezoidProfile.State targetState) { + setElevatorVoltage(calculateElevatorOutput(targetState)); + } + + private void setAngleVoltage(double voltage) { + this.angleVoltage = voltage; + angleMotor.setInputVoltage(voltage); + } + + private void setElevatorVoltage(double voltage) { + this.elevatorVoltage = voltage; + elevatorMotor.setInputVoltage(voltage); + } + + private double calculateAngleOutput(TrapezoidProfile.State targetState) { + double pidOutput = SimulationArmConstants.ANGLE_PID_CONTROLLER.calculate( + Units.radiansToDegrees(angleMotor.getAngleRads()), + targetState.position + ); + double feedforward = SimulationArmConstants.ANGLE_FEEDFORWARD.calculate( + Units.degreesToRadians(targetState.position), + Units.degreesToRadians(targetState.velocity) + ); + return pidOutput + feedforward; + } + + private double calculateElevatorOutput(TrapezoidProfile.State targetState) { + double pidOutput = SimulationArmConstants.ELEVATOR_PID_CONTROLLER.calculate( + getElevatorPositionMeters(), + targetState.position + ); + double feedforward = SimulationArmConstants.ELEVATOR_FEEDFORWARD.calculate(targetState.velocity); + return pidOutput + feedforward; + } + + private double getElevatorPositionMeters() { + return elevatorMotor.getPositionMeters() - ArmConstants.RETRACTED_ARM_LENGTH_METERS; + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/collector/Collector.java b/src/main/java/frc/trigon/robot/subsystems/collector/Collector.java index 817f120f5..8b4e17baa 100644 --- a/src/main/java/frc/trigon/robot/subsystems/collector/Collector.java +++ b/src/main/java/frc/trigon/robot/subsystems/collector/Collector.java @@ -1,15 +1,13 @@ package frc.trigon.robot.subsystems.collector; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.utilities.CurrentWatcher; +import org.littletonrobotics.junction.Logger; public class Collector extends SubsystemBase { private final static Collector INSTANCE = new Collector(); - private final TalonSRX motor = CollectorConstants.MOTOR; + private final CollectorInputsAutoLogged collectorInputs = new CollectorInputsAutoLogged(); + private final CollectorIO collectorIO = CollectorIO.generateIO(); public static Collector getInstance() { return INSTANCE; @@ -19,22 +17,20 @@ private Collector() { startCurrentWatcher(); } - public Command getSetTargetStateCommand(CollectorConstants.CollectorStates state) { - return new StartEndCommand( - () -> motor.set(ControlMode.PercentOutput, state.power), - () -> {}, - this - ); + @Override + public void periodic() { + collectorIO.updateInputs(collectorInputs); + Logger.processInputs("Collector", collectorInputs); } - private void stop() { - motor.set(ControlMode.Disabled, 0); + void setPower(double power) { + collectorIO.setPower(power); } - private void startCurrentWatcher(){ + private void startCurrentWatcher() { new CurrentWatcher( - this::stop, - motor::getSupplyCurrent, + collectorIO::stop, + () -> collectorInputs.motorCurrent, CollectorConstants.MAX_CURRENT, CollectorConstants.MAX_CURRENT_TIME ); diff --git a/src/main/java/frc/trigon/robot/subsystems/collector/CollectorCommands.java b/src/main/java/frc/trigon/robot/subsystems/collector/CollectorCommands.java new file mode 100644 index 000000000..95c466466 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/collector/CollectorCommands.java @@ -0,0 +1,17 @@ +package frc.trigon.robot.subsystems.collector; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.StartEndCommand; + +public class CollectorCommands { + private static final Collector COLLECTOR = Collector.getInstance(); + + public static Command getSetTargetStateCommand(CollectorConstants.CollectorStates targetState) { + return new StartEndCommand( + () -> COLLECTOR.setPower(targetState.power), + () -> { + }, + COLLECTOR + ); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/collector/CollectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/collector/CollectorConstants.java index a3e8acd0b..1adec2487 100644 --- a/src/main/java/frc/trigon/robot/subsystems/collector/CollectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/collector/CollectorConstants.java @@ -1,34 +1,17 @@ package frc.trigon.robot.subsystems.collector; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import java.util.function.Supplier; - public class CollectorConstants { - private static final int MOTOR_ID = 1; - private static final NeutralMode NEUTRAL_MODE_VALUE = NeutralMode.Coast; - private static final boolean INVERTED = false; - private static final int VOLTAGE_COMPENSATION_SATURATION = 12; - static final TalonSRX MOTOR = new TalonSRX(MOTOR_ID); static final int MAX_CURRENT = 10; static final double MAX_CURRENT_TIME = 0.5; - static { - MOTOR.configFactoryDefault(); - MOTOR.configVoltageCompSaturation(VOLTAGE_COMPENSATION_SATURATION); - MOTOR.enableVoltageCompensation(true); - MOTOR.setNeutralMode(NEUTRAL_MODE_VALUE); - MOTOR.setInverted(INVERTED); - } - - public enum CollectorStates{ + public enum CollectorStates { COLLECT(-1), EJECT(1), HOLD(-0.2); final double power; - CollectorStates(double power){ + CollectorStates(double power) { this.power = power; } } diff --git a/src/main/java/frc/trigon/robot/subsystems/collector/CollectorIO.java b/src/main/java/frc/trigon/robot/subsystems/collector/CollectorIO.java new file mode 100644 index 000000000..6009850ab --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/collector/CollectorIO.java @@ -0,0 +1,31 @@ +package frc.trigon.robot.subsystems.collector; + +import frc.trigon.robot.constants.RobotConstants; +import frc.trigon.robot.subsystems.collector.kablamacollector.KablamaCollectorIO; +import frc.trigon.robot.subsystems.collector.simulationcollector.SimulationCollectorIO; +import org.littletonrobotics.junction.AutoLog; + +public class CollectorIO { + static CollectorIO generateIO() { + if (RobotConstants.IS_REPLAY) + return new CollectorIO(); + if (RobotConstants.ROBOT_TYPE == RobotConstants.RobotType.KABLAMA) + return new KablamaCollectorIO(); + return new SimulationCollectorIO(); + } + + protected void updateInputs(CollectorInputsAutoLogged inputs) { + } + + protected void setPower(double power) { + } + + protected void stop() { + } + + @AutoLog + protected static class CollectorInputs { + public double motorCurrent = 0; + public double motorVoltage = 0; + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/collector/kablamacollector/KablamaCollectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/collector/kablamacollector/KablamaCollectorConstants.java new file mode 100644 index 000000000..565fc0a19 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/collector/kablamacollector/KablamaCollectorConstants.java @@ -0,0 +1,20 @@ +package frc.trigon.robot.subsystems.collector.kablamacollector; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +public class KablamaCollectorConstants { + private static final int MOTOR_ID = 1; + private static final NeutralMode NEUTRAL_MODE_VALUE = NeutralMode.Coast; + private static final boolean INVERTED = false; + private static final int VOLTAGE_COMPENSATION_SATURATION = 12; + static final WPI_TalonSRX MOTOR = new WPI_TalonSRX(MOTOR_ID); + + static { + MOTOR.configFactoryDefault(); + MOTOR.configVoltageCompSaturation(VOLTAGE_COMPENSATION_SATURATION); + MOTOR.enableVoltageCompensation(true); + MOTOR.setNeutralMode(NEUTRAL_MODE_VALUE); + MOTOR.setInverted(INVERTED); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/collector/kablamacollector/KablamaCollectorIO.java b/src/main/java/frc/trigon/robot/subsystems/collector/kablamacollector/KablamaCollectorIO.java new file mode 100644 index 000000000..a75b6443b --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/collector/kablamacollector/KablamaCollectorIO.java @@ -0,0 +1,26 @@ +package frc.trigon.robot.subsystems.collector.kablamacollector; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import frc.trigon.robot.subsystems.collector.CollectorIO; +import frc.trigon.robot.subsystems.collector.CollectorInputsAutoLogged; + +public class KablamaCollectorIO extends CollectorIO { + private final WPI_TalonSRX motor = KablamaCollectorConstants.MOTOR; + + @Override + protected void updateInputs(CollectorInputsAutoLogged inputs) { + inputs.motorVoltage = motor.getMotorOutputVoltage(); + inputs.motorCurrent = motor.getSupplyCurrent(); + } + + @Override + protected void setPower(double power) { + motor.set(ControlMode.PercentOutput, power); + } + + @Override + protected void stop() { + motor.stopMotor(); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/collector/simulationcollector/SimulationCollectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/collector/simulationcollector/SimulationCollectorConstants.java new file mode 100644 index 000000000..4356fa630 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/collector/simulationcollector/SimulationCollectorConstants.java @@ -0,0 +1,18 @@ +package frc.trigon.robot.subsystems.collector.simulationcollector; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class SimulationCollectorConstants { + private static final int MOTOR_AMOUNT = 1; + private static final DCMotor MOTOR_GEARBOX = DCMotor.getBag(MOTOR_AMOUNT); + private static final double MOMENT_OF_INERTIA = 0.003; + private static final double GEAR_RATIO = 12.8; + static final int VOLTAGE_COMPENSATION_SATURATION = 12; + + static final DCMotorSim MOTOR = new DCMotorSim( + MOTOR_GEARBOX, + GEAR_RATIO, + MOMENT_OF_INERTIA + ); +} diff --git a/src/main/java/frc/trigon/robot/subsystems/collector/simulationcollector/SimulationCollectorIO.java b/src/main/java/frc/trigon/robot/subsystems/collector/simulationcollector/SimulationCollectorIO.java new file mode 100644 index 000000000..718336094 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/collector/simulationcollector/SimulationCollectorIO.java @@ -0,0 +1,33 @@ +package frc.trigon.robot.subsystems.collector.simulationcollector; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.trigon.robot.constants.RobotConstants; +import frc.trigon.robot.subsystems.collector.CollectorIO; +import frc.trigon.robot.subsystems.collector.CollectorInputsAutoLogged; +import frc.trigon.robot.utilities.Conversions; + +public class SimulationCollectorIO extends CollectorIO { + private final DCMotorSim motor = SimulationCollectorConstants.MOTOR; + private double motorVoltage; + + @Override + protected void updateInputs(CollectorInputsAutoLogged inputs) { + motor.update(RobotConstants.PERIODIC_TIME_SECONDS); + + inputs.motorVoltage = motorVoltage; + inputs.motorCurrent = motor.getCurrentDrawAmps(); + } + + @Override + protected void setPower(double power) { + double voltage = Conversions.compensatedPowerToVoltage(power, SimulationCollectorConstants.VOLTAGE_COMPENSATION_SATURATION); + this.motorVoltage = MathUtil.clamp(voltage, -SimulationCollectorConstants.VOLTAGE_COMPENSATION_SATURATION, SimulationCollectorConstants.VOLTAGE_COMPENSATION_SATURATION); + motor.setInputVoltage(this.motorVoltage); + } + + @Override + protected void stop() { + setPower(0); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java b/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java new file mode 100644 index 000000000..f6735f8fd --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java @@ -0,0 +1,55 @@ +package frc.trigon.robot.subsystems.turret; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class Turret extends SubsystemBase { + private final static Turret INSTANCE = new Turret(); + private final TurretIO turretIO = TurretIO.generateIO(); + private final TurretInputsAutoLogged turretInputs = new TurretInputsAutoLogged(); + + public static Turret getInstance() { + return INSTANCE; + } + + private Turret() { + } + + @Override + public void periodic() { + turretIO.updateInputs(turretInputs); + Logger.processInputs("Turret", turretInputs); + updateMechanism(); + } + + void alignTurretToHub(Pose2d robotPose) { + Rotation2d targetAngle = calculateTargetAngle(robotPose); + Rotation2d targetAngleAfterLimitCheck = limitAngle(targetAngle); + TurretConstants.TARGET_ANGLE_LIGAMENT.setAngle(targetAngleAfterLimitCheck); + turretIO.setTargetAngle(targetAngleAfterLimitCheck); + } + + private Rotation2d calculateTargetAngle(Pose2d robotPose) { + Translation2d difference = TurretConstants.HUB_POSITION.minus(robotPose.getTranslation()); + double theta = Math.atan2(Math.abs(difference.getY()), Math.abs(difference.getX())); + double targetAngle = theta - Units.degreesToRadians(turretInputs.motorPositionDegrees); + return Rotation2d.fromRadians(targetAngle); + } + + private Rotation2d limitAngle(Rotation2d targetAngle) { + if (targetAngle.getDegrees() > TurretConstants.DEGREES_LIMIT) + return Rotation2d.fromDegrees(targetAngle.getDegrees() - 360); + if (targetAngle.getDegrees() < -TurretConstants.DEGREES_LIMIT) + return Rotation2d.fromDegrees(targetAngle.getDegrees() + 360); + return targetAngle; + } + + private void updateMechanism() { + TurretConstants.TURRET_LIGAMENT.setAngle(turretInputs.motorPositionDegrees); + Logger.recordOutput("TurretMechanism", TurretConstants.TURRET_MECHANISM); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/TurretCommands.java b/src/main/java/frc/trigon/robot/subsystems/turret/TurretCommands.java new file mode 100644 index 000000000..02c18d922 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/turret/TurretCommands.java @@ -0,0 +1,18 @@ +package frc.trigon.robot.subsystems.turret; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; + +import java.util.function.Supplier; + +public class TurretCommands { + private static final Turret TURRET = Turret.getInstance(); + + public static Command getAlignTurretToHubCommand(Supplier robotPose) { + return new RunCommand( + () -> TURRET.alignTurretToHub(robotPose.get()), + TURRET + ); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java new file mode 100644 index 000000000..4c42e72aa --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java @@ -0,0 +1,38 @@ +package frc.trigon.robot.subsystems.turret; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import org.littletonrobotics.junction.AutoLogOutput; + +public class TurretConstants { + static final double DEGREES_LIMIT = 200; + + private static final double + HUB_X = 8.248, + HUB_Y = 4.176; + static final Translation2d HUB_POSITION = new Translation2d(HUB_X, HUB_Y); + + private static final double + TURRET_MECHANISM_WIDTH = 4, + TURRET_MECHANISM_HEIGHT = 4; + private static final double + TURRET_ROOT_X = 2, + TURRET_ROOT_Y = 2; + private static final double + TURRET_ROOT_LENGTH = 1, + TURRET_ROOT_ANGLE = 0; + private static final double MECHANISM_LINE_WIDTH = 10; + @AutoLogOutput(key = "Turret/TurretMechanism") + static final Mechanism2d TURRET_MECHANISM = new Mechanism2d(TURRET_MECHANISM_WIDTH, TURRET_MECHANISM_HEIGHT); + static final MechanismRoot2d + TURRET_ROOT = TURRET_MECHANISM.getRoot("Z_TURRET_ROOT", TURRET_ROOT_X, TURRET_ROOT_Y), + TARGET_ANGLE_ROOT = TURRET_MECHANISM.getRoot("TARGET_ANGLE_ROOT", TURRET_ROOT_X, TURRET_ROOT_Y); + static final MechanismLigament2d + TURRET_LIGAMENT = TURRET_ROOT.append(new MechanismLigament2d("TURRET_LIGAMENT", TURRET_ROOT_LENGTH, TURRET_ROOT_ANGLE, MECHANISM_LINE_WIDTH, new Color8Bit(Color.kBlue))), + TARGET_ANGLE_LIGAMENT = TARGET_ANGLE_ROOT.append(new MechanismLigament2d("TargetAngleLigament", TURRET_ROOT_LENGTH, TURRET_ROOT_ANGLE, MECHANISM_LINE_WIDTH, new Color8Bit(Color.kGray))); + +} diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/trigon/robot/subsystems/turret/TurretIO.java new file mode 100644 index 000000000..25266b687 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/turret/TurretIO.java @@ -0,0 +1,33 @@ +package frc.trigon.robot.subsystems.turret; + +import edu.wpi.first.math.geometry.Rotation2d; +import frc.trigon.robot.constants.RobotConstants; +import frc.trigon.robot.subsystems.turret.kablamaturret.KablamaTurretIO; +import frc.trigon.robot.subsystems.turret.simulationturret.SimulationTurretIO; +import org.littletonrobotics.junction.AutoLog; + +public class TurretIO { + static TurretIO generateIO() { + if (RobotConstants.IS_REPLAY) + return new TurretIO(); + if (RobotConstants.ROBOT_TYPE == RobotConstants.RobotType.KABLAMA) + return new KablamaTurretIO(); + return new SimulationTurretIO(); + } + + protected void updateInputs(TurretInputsAutoLogged inputs) { + } + + protected void setTargetAngle(Rotation2d targetAngle) { + } + + protected void stop() { + } + + @AutoLog + protected static class TurretInputs { + public double motorPositionDegrees = 0; + public double motorVelocityDegreesPerSecond = 0; + public double motorVoltage = 0; + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/kablamaturret/KablamaTurretConstants.java b/src/main/java/frc/trigon/robot/subsystems/turret/kablamaturret/KablamaTurretConstants.java new file mode 100644 index 000000000..c16254a44 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/turret/kablamaturret/KablamaTurretConstants.java @@ -0,0 +1,83 @@ +package frc.trigon.robot.subsystems.turret.kablamaturret; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.*; + +public class KablamaTurretConstants { + static final boolean FOC_ENABLED = true; + private static final int + MOTOR_ID = 1, + ENCODER_ID = 2; + private static final InvertedValue INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; + private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; + private static final AbsoluteSensorRangeValue ENCODER_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + private static final SensorDirectionValue ENCODER_DIRECTION = SensorDirectionValue.CounterClockwise_Positive; + private static final double ENCODER_OFFSET = 0; + private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.RemoteCANcoder; + private static final double + MOTION_MAGIC_VELOCITY = 80, + MOTION_MAGIC_ACCELERATION = 160, + MOTION_MAGIC_JERK = 1600; + private static final double + P = 1, + I = 0, + D = 0, + KA = 0, + KG = 1, + KS = 0, + KV = 0; + private static final CANcoder ENCODER = new CANcoder(ENCODER_ID); + static final TalonFX MOTOR = new TalonFX(MOTOR_ID); + + static final StatusSignal + TURRET_POSITION_SIGNAL = ENCODER.getPosition(), + TURRET_VELOCITY_SIGNAL = ENCODER.getVelocity(); + + static { + configureMotor(); + configureEncoder(); + } + + private static void configureMotor() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.MotorOutput.Inverted = INVERTED_VALUE; + config.MotorOutput.NeutralMode = NEUTRAL_MODE_VALUE; + config.Audio.BeepOnConfig = false; + config.Audio.BeepOnBoot = false; + + config.Slot0.kP = P; + config.Slot0.kI = I; + config.Slot0.kD = D; + config.Slot0.kA = KA; + config.Slot0.kG = KG; + config.Slot0.kS = KS; + config.Slot0.kV = KV; + + config.Feedback.FeedbackRemoteSensorID = ENCODER_ID; + config.Feedback.FeedbackRotorOffset = ENCODER_OFFSET; + config.Feedback.FeedbackSensorSource = ENCODER_TYPE; + + config.MotionMagic.MotionMagicCruiseVelocity = MOTION_MAGIC_VELOCITY; + config.MotionMagic.MotionMagicAcceleration = MOTION_MAGIC_ACCELERATION; + config.MotionMagic.MotionMagicJerk = MOTION_MAGIC_JERK; + + MOTOR.getConfigurator().apply(config); + MOTOR.optimizeBusUtilization(); + } + + private static void configureEncoder() { + CANcoderConfiguration config = new CANcoderConfiguration(); + config.MagnetSensor.AbsoluteSensorRange = ENCODER_RANGE; + config.MagnetSensor.SensorDirection = ENCODER_DIRECTION; + config.MagnetSensor.MagnetOffset = ENCODER_OFFSET; + ENCODER.getConfigurator().apply(config); + + TURRET_POSITION_SIGNAL.setUpdateFrequency(100); + TURRET_VELOCITY_SIGNAL.setUpdateFrequency(100); + ENCODER.optimizeBusUtilization(); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/kablamaturret/KablamaTurretIO.java b/src/main/java/frc/trigon/robot/subsystems/turret/kablamaturret/KablamaTurretIO.java new file mode 100644 index 000000000..767eb25b1 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/turret/kablamaturret/KablamaTurretIO.java @@ -0,0 +1,38 @@ +package frc.trigon.robot.subsystems.turret.kablamaturret; + +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import frc.trigon.robot.subsystems.turret.TurretIO; +import frc.trigon.robot.subsystems.turret.TurretInputsAutoLogged; + +public class KablamaTurretIO extends TurretIO { + private final TalonFX motor = KablamaTurretConstants.MOTOR; + private final MotionMagicVoltage positionVoltageRequest = new MotionMagicVoltage(0).withEnableFOC(KablamaTurretConstants.FOC_ENABLED); + + @Override + protected void updateInputs(TurretInputsAutoLogged inputs) { + inputs.motorPositionDegrees = getPosition().getDegrees(); + inputs.motorVelocityDegreesPerSecond = getVelocityDegreesPerSecond(); + inputs.motorVoltage = motor.getMotorVoltage().refresh().getValue(); + } + + @Override + protected void setTargetAngle(Rotation2d targetAngle) { + motor.setControl(positionVoltageRequest.withPosition(targetAngle.getRotations())); + } + + @Override + protected void stop() { + motor.stopMotor(); + } + + private Rotation2d getPosition() { + return Rotation2d.fromRotations(KablamaTurretConstants.TURRET_POSITION_SIGNAL.refresh().getValue()); + } + + private double getVelocityDegreesPerSecond() { + return Units.rotationsToDegrees(KablamaTurretConstants.TURRET_VELOCITY_SIGNAL.refresh().getValue()); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/simulationturret/SimulationTurretConstants.java b/src/main/java/frc/trigon/robot/subsystems/turret/simulationturret/SimulationTurretConstants.java new file mode 100644 index 000000000..c9ab48ce3 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/turret/simulationturret/SimulationTurretConstants.java @@ -0,0 +1,39 @@ +package frc.trigon.robot.subsystems.turret.simulationturret; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.trigon.robot.subsystems.turret.TurretIO; + +public class SimulationTurretConstants extends TurretIO { + static final double VOLTAGE_COMPENSATION_SATURATION = 12; + private static final int MOTOR_AMOUNT = 1; + private static final DCMotor MOTOR_GEARBOX = DCMotor.getFalcon500Foc(MOTOR_AMOUNT); + private static final double MOMENT_OF_INERTIA = 0.003; + private static final double GEAR_RATIO = 100; + static final DCMotorSim MOTOR = new DCMotorSim( + MOTOR_GEARBOX, + GEAR_RATIO, + MOMENT_OF_INERTIA + ); + + private static final double + MAX_MOTOR_VELOCITY = 500, + MAX_MOTOR_ACCELERATION = 600; + private static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_MOTOR_VELOCITY, MAX_MOTOR_ACCELERATION + ); + private static final double + P = 10, + I = 0, + D = 0; + static final ProfiledPIDController PROFILED_PID_CONTROLLER = new ProfiledPIDController(P, I, D, CONSTRAINTS); + + private static final double + KS = 0, + KV = 0, + KA = 0; + static final SimpleMotorFeedforward FEEDFORWARD = new SimpleMotorFeedforward(KS, KV, KA); +} diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/simulationturret/SimulationTurretIO.java b/src/main/java/frc/trigon/robot/subsystems/turret/simulationturret/SimulationTurretIO.java new file mode 100644 index 000000000..e3bc47246 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/turret/simulationturret/SimulationTurretIO.java @@ -0,0 +1,50 @@ +package frc.trigon.robot.subsystems.turret.simulationturret; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.trigon.robot.constants.RobotConstants; +import frc.trigon.robot.subsystems.turret.TurretIO; +import frc.trigon.robot.subsystems.turret.TurretInputsAutoLogged; + +public class SimulationTurretIO extends TurretIO { + private final DCMotorSim motor = SimulationTurretConstants.MOTOR; + private double voltage = 0; + + @Override + protected void updateInputs(TurretInputsAutoLogged inputs) { + motor.update(RobotConstants.PERIODIC_TIME_SECONDS); + + inputs.motorPositionDegrees = Units.radiansToDegrees(motor.getAngularPositionRad()); + inputs.motorVelocityDegreesPerSecond = Units.radiansToDegrees(motor.getAngularVelocityRadPerSec()); + inputs.motorVoltage = voltage; + } + + @Override + protected void setTargetAngle(Rotation2d targetAngle) { + setTargetVoltage(calculateTargetAngle(targetAngle)); + } + + @Override + protected void stop() { + setTargetVoltage(0); + } + + private void setTargetVoltage(double voltage) { + double compensatedVoltage = MathUtil.clamp( + voltage, + -SimulationTurretConstants.VOLTAGE_COMPENSATION_SATURATION, + SimulationTurretConstants.VOLTAGE_COMPENSATION_SATURATION + ); + this.voltage = compensatedVoltage; + motor.setInputVoltage(compensatedVoltage); + } + + private double calculateTargetAngle(Rotation2d targetAngle) { + SimulationTurretConstants.PROFILED_PID_CONTROLLER.setGoal(targetAngle.getRotations()); + double pidOutput = SimulationTurretConstants.PROFILED_PID_CONTROLLER.calculate(targetAngle.getRotations()); + double feedforwardOutput = SimulationTurretConstants.FEEDFORWARD.calculate(targetAngle.getRotations()); + return pidOutput + feedforwardOutput; + } +}