diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index de35250..52b537e 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -6,11 +6,11 @@ public final class BuildConstants { 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 String GIT_SHA = "9b2d52efc94f51550c86781dc4d51112afc44b13"; + public static final String GIT_DATE = "2024-12-16 17:53:23 EST"; + public static final String GIT_BRANCH = "ExamplesYay"; + public static final String BUILD_DATE = "2024-12-18 17:41:48 EST"; + public static final long BUILD_UNIX_TIME = 1734561708416L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/commands/intake/Outtake.java b/src/main/java/frc/robot/commands/intake/Outtake.java new file mode 100644 index 0000000..79c6cd0 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/Outtake.java @@ -0,0 +1,35 @@ +// This command ejects the note from the intake by reversing the rollors + +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.intake.Intake; + +public class Outtake extends Command { + private final Intake intakeSubsystem; + + public Outtake(Intake intake) { + this.intakeSubsystem = intake; + addRequirements(this.intakeSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + intakeSubsystem.setIntakeSpeed(-1); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/intake/RegularIntake.java b/src/main/java/frc/robot/commands/intake/RegularIntake.java new file mode 100644 index 0000000..07eab50 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/RegularIntake.java @@ -0,0 +1,35 @@ +// This command runs the rollers from the intake and grabs the note from the floor + +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.intake.Intake; + +public class RegularIntake extends Command { + private final Intake intakeSubsystem; + + public RegularIntake(Intake intake) { + this.intakeSubsystem = intake; + addRequirements(this.intakeSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + intakeSubsystem.setIntakeSpeed(1); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/extras/util/JoystickUtil.java b/src/main/java/frc/robot/extras/util/JoystickUtil.java index 1ffc4bb..444024e 100644 --- a/src/main/java/frc/robot/extras/util/JoystickUtil.java +++ b/src/main/java/frc/robot/extras/util/JoystickUtil.java @@ -43,18 +43,6 @@ public static double modifyAxis(DoubleSupplier supplierValue, int exponent) { return value; } - public static double modifyAxisCubed(DoubleSupplier supplierValue) { - double value = supplierValue.getAsDouble(); - - // Deadband - value = JoystickUtil.deadband(value, JoystickConstants.DEADBAND_VALUE); - - // Cube the axis - value = Math.copySign(value * value * value, 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. @@ -81,22 +69,4 @@ public static double[] modifyAxisPolar( Math.copySign(Math.pow(yInput, exponent), yInput) }; } - - public static double[] modifyAxisCubedPolar(DoubleSupplier xJoystick, DoubleSupplier yJoystick) { - double xInput = - JoystickUtil.deadband(xJoystick.getAsDouble(), JoystickConstants.DEADBAND_VALUE); - double yInput = deadband(yJoystick.getAsDouble(), JoystickConstants.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 cubedHypotenuse = Math.pow(hypotenuse, 3); - xInput = Math.copySign(Math.sin(theta) * cubedHypotenuse, xInput); - yInput = Math.copySign(Math.cos(theta) * cubedHypotenuse, yInput); - return new double[] {xInput, yInput}; - } - return new double[] { - Math.copySign(xInput * xInput * xInput, xInput), - Math.copySign(yInput * yInput * yInput, yInput) - }; - } } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java new file mode 100644 index 0000000..701b932 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -0,0 +1,39 @@ +/** This subsystem is an elevator that uses PID for its position. */ +package frc.robot.subsystems.elevator; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class Elevator extends SubsystemBase { + /** Creates a new Elevator. */ + private ElevatorInterface elevatorInterface; + + private ElevatorInputsAutoLogged inputs = new ElevatorInputsAutoLogged(); + + public Elevator(ElevatorInterface elevatorInterface) { + this.elevatorInterface = elevatorInterface; + } + + public double getElevatorPosition() { + return elevatorInterface.getElevatorPosition(); + } + + public double getVolts() { + return elevatorInterface.getVolts(); + } + + public void setElevatorPosition(double position) { + elevatorInterface.setElevatorPosition(position); + } + + public void setVolts(double volts) { + elevatorInterface.setVolts(volts); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + elevatorInterface.updateInputs(inputs); + Logger.processInputs("Elevator/", inputs); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java new file mode 100644 index 0000000..e02141b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.elevator; + +public class ElevatorConstants { + public static final int ELEVATOR_LEADER_MOTOR_ID = 0 - 9; + public static final int ELEVATOR_FOLLOWER_MOTOR_ID = 0 - 9; + + public static final double ELEVATOR_P = 0 - 9; + public static final double ELEVATOR_I = 0 - 9; + public static final double ELEVATOR_D = 0 - 9; + + public static final double DRUM_RADIUS = 0 - 9; + public static final double ELEVATOR_GEAR_RATIO = 2; + public static final double ELEVATOR_CARRIAGE_MASS = 0 - 9; + public static final double ENCODER_CONVERSION_FACTOR = 71.81; + public static final double MIN_HEIGHT = 0; + public static final double MAX_HEIGHT = 0 - 9; + + public static final double INTAKE_POSITION = MIN_HEIGHT; + public static final double SHOOT_AMP_POSITION = 0 - 9; + public static final double SHOOT_SPEAKER_POSITION = 0 - 9; + public static final double SHOOT_PASS_POSITION = 0 - 9; + public static final double ELEVATOR_OVER_DEFENSE = MAX_HEIGHT; + + public static final double VELOCITY_METERS_PER_SECOND = 0 - 9; +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java new file mode 100644 index 0000000..8ed4759 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java @@ -0,0 +1,37 @@ +package frc.robot.subsystems.elevator; + +import org.littletonrobotics.junction.AutoLog; + +public interface ElevatorInterface { + + @AutoLog + public static class ElevatorInputs { + public double leaderMotorPosition = 0.0; + + public double followerMotorPosition = 0.0; + } + + /** + * Updates inputs for elevator for AdvantageKit to log + * + * @param inputs values related to the elevator + */ + public default void updateInputs(ElevatorInputs inputs) {} + + /** + * Gets the current position of the elevator + * + * @return + */ + public default double getElevatorPosition() { + return 0.0; + } + + public default void setElevatorPosition(double position) {} + + public default void setVolts(double volts) {} + + public default double getVolts() { + return 0.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java b/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java new file mode 100644 index 0000000..83f09d7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java @@ -0,0 +1,61 @@ +package frc.robot.subsystems.elevator; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.subsystems.elevator.ElevatorInterface.ElevatorInputs; + +/** Add your docs here. */ +public class PhysicalElevator { + private TalonFX leaderMotor = new TalonFX(ElevatorConstants.ELEVATOR_LEADER_MOTOR_ID); + private TalonFX followerMotor = new TalonFX(ElevatorConstants.ELEVATOR_FOLLOWER_MOTOR_ID); + + StatusSignal leaderPosition; + StatusSignal followerPosition; + + StatusSignal leaderAppliedVoltage; + StatusSignal followerAppliedVoltage; + + public PhysicalElevator() { + leaderPosition = leaderMotor.getRotorPosition(); + followerPosition = followerMotor.getRotorPosition(); + + leaderAppliedVoltage = leaderMotor.getMotorVoltage(); + followerAppliedVoltage = followerMotor.getMotorVoltage(); + + TalonFXConfiguration elevatorConfig = new TalonFXConfiguration(); + + elevatorConfig.Slot0.kP = ElevatorConstants.ELEVATOR_P; + elevatorConfig.Slot0.kI = ElevatorConstants.ELEVATOR_I; + elevatorConfig.Slot0.kD = ElevatorConstants.ELEVATOR_D; + + leaderMotor.getConfigurator().apply(elevatorConfig); + followerMotor.getConfigurator().apply(elevatorConfig); + } + + public void updateInputs(ElevatorInputs inputs) { + inputs.leaderMotorPosition = leaderPosition.getValueAsDouble(); + inputs.followerMotorPosition = followerPosition.getValueAsDouble(); + } + + public double getElevatorPosition() { + return leaderPosition.getValueAsDouble(); + } + + public void setElevatorPosition(double position) { + leaderMotor.setPosition(position); + followerMotor.setPosition(position); + } + + public void setVolts(double volts) { + leaderMotor.setVoltage(volts); + followerMotor.setVoltage(volts); + ; + } + + public double getVolts() { + return leaderMotor.getMotorVoltage().getValueAsDouble(); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java new file mode 100644 index 0000000..6d6d099 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java @@ -0,0 +1,51 @@ +package frc.robot.subsystems.elevator; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; + +/** Add your docs here. */ +public class SimulatedElevator implements ElevatorInterface { + private ElevatorSim elevatorSim = + new ElevatorSim( + DCMotor.getFalcon500(2), + ElevatorConstants.ELEVATOR_GEAR_RATIO, + ElevatorConstants.ELEVATOR_CARRIAGE_MASS, + ElevatorConstants.DRUM_RADIUS, + ElevatorConstants.MIN_HEIGHT, + ElevatorConstants.MAX_HEIGHT, + true, + 0.0); + private PIDController simPID; + private double currentVolts; + + public SimulatedElevator() { + simPID = + new PIDController( + ElevatorConstants.ELEVATOR_P, + ElevatorConstants.ELEVATOR_I, + ElevatorConstants.ELEVATOR_D); + } + + public void updateInputs(ElevatorInputs inputs) { + inputs.leaderMotorPosition = getElevatorPosition(); + inputs.followerMotorPosition = getElevatorPosition(); + } + + public void setElevatorPosition(double position) { + setVolts(simPID.calculate(getElevatorPosition(), position)); + } + + public double getElevatorPosition() { + return elevatorSim.getPositionMeters(); + } + + public void setVolts(double volts) { + currentVolts = simPID.calculate(volts); + elevatorSim.setInputVoltage(currentVolts); + } + + public double getVolts() { + return currentVolts; + } +} 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..675d610 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,29 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class Intake extends SubsystemBase { + private IntakeInterface intakeInterface; + private IntakeInputsAutoLogged inputs = new IntakeInputsAutoLogged(); + + public Intake(IntakeInterface intakeInterface) { + this.intakeInterface = intakeInterface; + } + + /** + * Sets the speed of the intake rollers + * + * @param speed intake roller speed (-1.0 to 1.0) + */ + public void setIntakeSpeed(double speed) { + intakeInterface.setIntakeSpeed(speed); + Logger.recordOutput("OTBIntake/IntakeSpeed", speed); + } + + @Override + public void periodic() { + intakeInterface.updateInputs(inputs); + Logger.processInputs("OTBIntake/", inputs); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java new file mode 100644 index 0000000..a12910d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems.intake; + +// This cool file contains constants for our intake +public final class IntakeConstants { + public static final int INTAKE_MOTOR_ID = 0; + public static final int LEFT_INTAKE_PIVOT_MOTOR_ID = 62; + public static final int RIGHT_INTAKE_PIVOT_MOTOR_ID = 0 - 9; + + public static final int NOTE_SENSOR_ID = 0 - 9; + + public static final double INTAKE_PIVOT_GEAR_RATIO = 8.0; + + public static final double INTAKE_PIVOT_OUT = 0 - 9; + public static final double INTAKE_PIVOT_IN = 0 - 9; + + public static final double INTAKE_PIVOT_NEUTRAL_SPEED = 0.0; + + public static final double OUTTAKE_SPEED = 0 - 9; + public static final double INTAKE_SPEED = 0.8; + public static final double INTAKE_NEUTRAL_SPEED = 0.0; + public static final double FLAPPER_SPEED = 1.0; + + public static final double INTAKE_STATOR_LIMIT = 60; + public static final double INTAKE_SUPPLY_LIMIT = 40; + public static final boolean INTAKE_STATOR_ENABLE = true; + public static final boolean INTAKE_SUPPLY_ENABLE = true; +} 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..0856c0e --- /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; + } + + /** + * Updates intake values + * + * @param inputs + */ + default void updateInputs(IntakeInputs inputs) {} + + /** + * Sets the intake speed + * + * @param speed The intake speed value from -1 to 1 (negative being reversed) + */ + default void setIntakeSpeed(double speed) {} + + /** + * Gets the current speed of the intake + * + * @return Current velocity of the intake + */ + default double getIntakeSpeed() { + return 0.0; + } +} 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..47a918b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java @@ -0,0 +1,48 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +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 StatusSignal intakeVelocity; + + 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); + + intakeVelocity = intakeMotor.getVelocity(); + } + + @Override + public void updateInputs(IntakeInputs inputs) { + inputs.intakeVelocity = intakeVelocity.getValueAsDouble(); + } + + @Override + public void setIntakeSpeed(double speed) { + intakeMotor.set(speed); + } + + @Override + public double getIntakeSpeed() { + intakeMotor.getVelocity().refresh(); + return intakeMotor.getVelocity().getValueAsDouble(); + } +} 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..171c20e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/SimulatedIntake.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class SimulatedIntake implements IntakeInterface { + DCMotorSim intakeSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getFalcon500(1), 0.01, 1), + DCMotor.getFalcon500(1), + 0); + private double intakeAppliedVolts = 0.0; + + public SimulatedIntake() {} + + @Override + public void updateInputs(IntakeInputs inputs) { + intakeSim.update(0.02); + + inputs.intakeVelocity = + RadiansPerSecond.of(intakeSim.getAngularVelocityRadPerSec()).in(RotationsPerSecond); + inputs.intakeCurrentAmps = intakeSim.getCurrentDrawAmps(); + inputs.intakeAppliedVolts = intakeAppliedVolts; + } + + @Override + public void setIntakeSpeed(double speed) { + intakeSim.setInputVoltage(speed); + } + + @Override + public double getIntakeSpeed() { + return RadiansPerSecond.of(intakeSim.getAngularVelocityRadPerSec()).in(RotationsPerSecond); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java index ebc9216..e6ab282 100644 --- a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java +++ b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java @@ -55,16 +55,17 @@ static OdometryThread createInstance(DeviceCANBus canBus) { registeredInputs.toArray(new OdometryDoubleInput[0]), registeredStatusSignals.toArray(new BaseStatusSignal[0])); case SIM -> new OdometryThreadSim(); - case REPLAY -> inputs -> {}; + // case REPLAY -> inputs -> {}; + default -> throw new IllegalArgumentException("Unexpected value: " + Constants.CURRENT_MODE); }; } @AutoLog - class OdometryThreadInputs { + public class OdometryThreadInputs { public double[] measurementTimeStamps = new double[0]; } - void updateInputs(OdometryThreadInputs inputs); + default void updateInputs(OdometryThreadInputs inputs) {} default void start() {}