diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index de35250..cea2cd0 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -1,17 +1,19 @@ package frc.robot; -/** Automatically generated file containing build version information. */ +/** + * Automatically generated file containing build version information. + */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "4829-BaseRobotCode"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 15; - public static final String GIT_SHA = "f6cf5383d95d1116029cbb69e2db020295f5c2bf"; - public static final String GIT_DATE = "2024-11-09 19:29:39 EST"; - public static final String GIT_BRANCH = "actual-unit-tests"; - public static final String BUILD_DATE = "2024-11-09 19:50:39 EST"; - public static final long BUILD_UNIX_TIME = 1731199839115L; - public static final int DIRTY = 1; + public static final int GIT_REVISION = 9; + public static final String GIT_SHA = "489425d396236751b7dfe7735b944f8b36beac20"; + public static final String GIT_DATE = "2024-11-10 12:24:24 EST"; + public static final String GIT_BRANCH = "ExamplesYay"; + public static final String BUILD_DATE = "2024-11-15 17:54:44 EST"; + public static final long BUILD_UNIX_TIME = 1731711284284L; + public static final int DIRTY = 0; - private BuildConstants() {} + private BuildConstants(){} } diff --git a/src/main/java/frc/robot/extras/util/JoystickUtil.java b/src/main/java/frc/robot/extras/util/JoystickUtil.java new file mode 100644 index 0000000..3f51dd3 --- /dev/null +++ b/src/main/java/frc/robot/extras/util/JoystickUtil.java @@ -0,0 +1,72 @@ +package frc.robot.extras.util; + +import frc.robot.Constants.HardwareConstants; +import java.util.function.DoubleSupplier; + +public class Util { + + /** + * Deadbands a value to 0 + * + * @param value Value to input + * @param deadband If the the input is within this value, the function will return 0. + * @return 0.0 if the value is within specified deadband range + */ + private static double deadband(double value, double deadband) { + if (Math.abs(value) > deadband) { + if (value > 0.0) { + return (value - deadband) / (1.0 - deadband); + } else { + return (value + deadband) / (1.0 - deadband); + } + } else { + return 0.0; + } + } + + /** + * Function to modify a singular joystick axis. + * + * @param supplierValue Supplies a double, usually by a joystick. + * @param exponent Exponent to raise the supplied value to. + * @return Returns the modified value. + */ + public static double modifyAxis(DoubleSupplier supplierValue, int exponent) { + double value = supplierValue.getAsDouble(); + + // Deadband + value = deadband(value, HardwareConstants.DEADBAND_VALUE); + + // Raise value to specified exponent + value = Math.copySign(Math.pow(value, exponent), value); + + return value; + } + + /** + * Converts the two axis coordinates to polar to get both the angle and radius, so they can work + * in a double[] list. + * + * @param xJoystick The supplied input of the xJoystick. + * @param yJoystick The supplied input of the yJoystick. + * @param exponent The exponent to raise the supplied value to. + * @return The modified axises of both joysticks in polar form. + */ + public static double[] modifyAxisPolar( + DoubleSupplier xJoystick, DoubleSupplier yJoystick, int exponent) { + double xInput = deadband(xJoystick.getAsDouble(), HardwareConstants.DEADBAND_VALUE); + double yInput = deadband(yJoystick.getAsDouble(), HardwareConstants.DEADBAND_VALUE); + if (Math.abs(xInput) > 0 && Math.abs(yInput) > 0) { + double theta = Math.atan(xInput / yInput); + double hypotenuse = Math.sqrt(xInput * xInput + yInput * yInput); + double raisedHypotenuse = Math.pow(hypotenuse, exponent); + xInput = Math.copySign(Math.sin(theta) * raisedHypotenuse, xInput); + yInput = Math.copySign(Math.cos(theta) * raisedHypotenuse, yInput); + return new double[] {xInput, yInput}; + } + return new double[] { + Math.copySign(Math.pow(xInput, exponent), xInput), + Math.copySign(Math.pow(yInput, exponent), yInput) + }; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..1936b87 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,49 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class Intake extends SubsystemBase { + private IntakeInterface io; + private IntakeInputsAutoLogged inputs = new IntakeInputsAutoLogged(); + + public Intake(IntakeInterface io) { + this.io = io; + } + + /** + * sets the angle of the pivot in degrees + * + * @param angle desired angle in degrees + */ + public void setPivotAngle(double angle) { + double angleRots = Units.degreesToRotations(angle); + io.setPivotPosition(angleRots); + Logger.recordOutput("OTBIntake/Pivot", angleRots); + } + + /** + * Sets the speed of the intake rollers + * + * @param speed intake roller speed (-1.0 to 1.0) + */ + public void setIntakeSpeed(double speed) { + io.setIntakeSpeed(speed); + Logger.recordOutput("OTBIntake/Intake", speed); + } + + public void setPivotSpeed(double speed) { + io.setPivotSpeed(speed); + } + + public void getPivotPosition() { + io.getPivotPosition(); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("OTBIntake", inputs); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeInterface.java b/src/main/java/frc/robot/subsystems/intake/IntakeInterface.java new file mode 100644 index 0000000..5fb35da --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeInterface.java @@ -0,0 +1,39 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeInterface { + @AutoLog + public static class IntakeInputs { + public boolean isConnected = true; + + // intake motor + public double intakeVelocity = 0.0; + public double intakeTemp = 0.0; + public double intakeAppliedVolts = 0.0; + public double intakeCurrentAmps = 0.0; + + // pivot + public double pivotPosition = 0.0; + public double pivotVelocity = 0.0; + public double pivotTemp = 0.0; + public double pivotAppliedVolts = 0.0; + public double pivotCurrentAmps = 0.0; + } + + default void updateInputs(IntakeInputs inputs) {} + + default void setPivotPosition(double position) {} + + default void setIntakeSpeed(double speed) {} + + default double getIntakeSpeed() { + return 0.0; + } + + default void setPivotSpeed(double speed) {} + + default double getPivotPosition() { + return 0.0; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java b/src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java new file mode 100644 index 0000000..34f0110 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java @@ -0,0 +1,119 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.Constants.HardwareConstants; + +public class PhysicalIntake implements IntakeInterface { + private final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID); + private final TalonFX leaderPivotMotor = new TalonFX(IntakeConstants.LEFT_INTAKE_PIVOT_MOTOR_ID); + private final TalonFX followerPivotMotor = + new TalonFX(IntakeConstants.RIGHT_INTAKE_PIVOT_MOTOR_ID); + + private final MotionMagicVoltage mmPositionRequest = new MotionMagicVoltage(0); + + private final StatusSignal intakeVelocity; + private final StatusSignal pivotPosition; + private final StatusSignal pivotVelocity; + + public PhysicalIntake() { + TalonFXConfiguration intakeConfig = new TalonFXConfiguration(); + + intakeConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + intakeConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + intakeConfig.MotorOutput.DutyCycleNeutralDeadband = HardwareConstants.MIN_FALCON_DEADBAND; + + intakeConfig.CurrentLimits.StatorCurrentLimit = 0.0; + intakeConfig.CurrentLimits.StatorCurrentLimitEnable = false; + intakeConfig.CurrentLimits.SupplyCurrentLimit = 0.0; + intakeConfig.CurrentLimits.SupplyCurrentLimitEnable = false; + + intakeMotor.getConfigurator().apply(intakeConfig); + + TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); + + pivotConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + pivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + pivotConfig.MotorOutput.DutyCycleNeutralDeadband = HardwareConstants.MIN_FALCON_DEADBAND; + + pivotConfig.CurrentLimits.StatorCurrentLimit = 0.0; + pivotConfig.CurrentLimits.StatorCurrentLimitEnable = false; + pivotConfig.CurrentLimits.SupplyCurrentLimit = 0.0; + pivotConfig.CurrentLimits.SupplyCurrentLimitEnable = false; + + pivotConfig.Slot0.kP = 0.0; + pivotConfig.Slot0.kI = 0.0; + pivotConfig.Slot0.kD = 0.0; + pivotConfig.Slot0.kS = 0.0; + pivotConfig.Slot0.kV = 0.0; + pivotConfig.Slot0.kA = 0.0; + pivotConfig.Slot0.kG = 0.0; + pivotConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + + pivotConfig.MotionMagic.MotionMagicCruiseVelocity = 0.0; + pivotConfig.MotionMagic.MotionMagicAcceleration = 0.0; + + // pivotConfig.Feedback.FeedbackRotorOffset = 0.0; + // pivotConfig.Feedback.RotorToSensorRatio = 0.0; + pivotConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + pivotConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 0.0; + pivotConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + pivotConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0.0; + pivotConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; + + leaderPivotMotor.setPosition(0); + followerPivotMotor.setPosition(0); + + leaderPivotMotor.getConfigurator().apply(pivotConfig); + pivotConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + followerPivotMotor.getConfigurator().apply(pivotConfig); + + intakeVelocity = intakeMotor.getVelocity(); + pivotPosition = leaderPivotMotor.getRotorPosition(); + pivotVelocity = leaderPivotMotor.getVelocity(); + } + + @Override + public void updateInputs(IntakeInterfaceInputs inputs) { + inputs.intakeVelocity = intakeVelocity.getValueAsDouble(); + inputs.pivotPosition = pivotPosition.getValueAsDouble(); + inputs.pivotVelocity = pivotVelocity.getValueAsDouble(); + } + + @Override + public void setPivotPosition(double position) { + leaderPivotMotor.setControl(mmPositionRequest.withPosition(position)); + followerPivotMotor.setControl(mmPositionRequest.withPosition(position)); + } + + @Override + public void setPivotSpeed(double speed) { + leaderPivotMotor.set(speed); + followerPivotMotor.set(speed); + } + + @Override + public double getPivotPosition() { + leaderPivotMotor.getRotorPosition().refresh(); + return leaderPivotMotor.getRotorPosition().getValueAsDouble(); + } + + @Override + public void setIntakeSpeed(double speed) { + intakeMotor.set(speed); + } + + @Override + public double getIntakeSpeed() { + intakeMotor.getVelocity().refresh(); + return intakeMotor.getVelocity().getValueAsDouble(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/SimulatedIntake.java b/src/main/java/frc/robot/subsystems/intake/SimulatedIntake.java new file mode 100644 index 0000000..212e5ba --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/SimulatedIntake.java @@ -0,0 +1,60 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; + +public class SimulatedIntake implements IntakeInterface { + DCMotorSim intakeSim = new DCMotorSim(null, DCMotor.getFalcon500(1), 0); + SingleJointedArmSim pivotSim = + new SingleJointedArmSim( + DCMotor.getFalcon500(2), IntakeConstants.INTAKE_PIVOT_GEAR_RATIO, 0, 0, 0, 0, false, 0); + private final Constraints pivotConstraints = new Constraints(0, 0); + private final ProfiledPIDController pivotController = + new ProfiledPIDController(0, 0, 0, pivotConstraints); + + private final ArmFeedforward pivotFF = new ArmFeedforward(0, 0, 0, 0); + + private double intakeAppliedVolts = 0.0; + private double pivotAppliedVolts = 0.0; + + public SimulatedIntake(IntakeInputs inputs) { + intakeSim.update(0.02); + pivotSim.update(0.02); + + inputs.intakeVelocity = intakeSim.getAngularVelocityRadPerSec() / (Math.PI * 2); + inputs.intakeCurrentAmps = intakeSim.getCurrentDrawAmps(); + inputs.intakeAppliedVolts = intakeAppliedVolts; + + inputs.pivotPosition = pivotSim.getAngleRads() / (Math.PI * 2); + inputs.pivotVelocity = pivotSim.getVelocityRadPerSec() / (Math.PI * 2); + inputs.pivotAppliedVolts = pivotAppliedVolts; + inputs.pivotCurrentAmps = pivotSim.getCurrentDrawAmps(); + } + + @Override + public void setPivotPosition(double position) { + pivotSim.setInputVoltage( + pivotController.calculate(position, pivotSim.getAngleRads() / (Math.PI * 2)) + + pivotFF.calculate( + pivotController.getSetpoint().position, pivotController.getSetpoint().velocity)); + } + + @Override + public void setIntakeSpeed(double speed) { + intakeSim.setInputVoltage(speed); + } + + @Override + public double getIntakeSpeed() { + return intakeSim.getAngularVelocityRadPerSec() / (Math.PI * 2); + } + + @Override + public double getPivotPosition() { + return pivotSim.getAngleRads() / (Math.PI * 2); + } +} \ No newline at end of file