diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 553db50c..e6532664 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -76,10 +76,6 @@ public static class Indexer { } - public static class Elevator { - public static final double ELEVATOR_MAX_HEIGHT = Units.inchesToMeters(21.0); - } - public static class Shooter { public static class Flywheels { // TODO: determine ids @@ -132,4 +128,47 @@ public static class Pivot { public static final Rotation2d ERROR_TOLERANCE = Rotation2d.fromDegrees(0.2); } } + + public static class Elevator { + public static final double CHAIN_HEIGHT_METERS = Units.inchesToMeters(28.25); + public static final double ELEVATOR_MAX_HEIGHT = Units.inchesToMeters(48.0); + public static final double ELEVATOR_MINIMUM_HEIGHT = Units.inchesToMeters(27.0); + public static final double CLIMBING_HOOKS_MINIMUM_HEIGHT = Units.inchesToMeters(12.0); + public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + ELEVATOR_MAX_HEIGHT - ELEVATOR_MINIMUM_HEIGHT; + + public enum ElevatorState { + /** + * At max height :D + */ + TOP(ELEVATOR_MAX_HEIGHT), + /** + * At minimum height :D + */ + BOTTOM(ELEVATOR_MINIMUM_HEIGHT), + /** + * At height for top of initial climb :D + */ + CLIMB(254.0), //TODO: Find these values :D + /** + * At height for trap doing :D + */ + TRAP(254.0), //TODO: Find these values :D + /** + * At height for top of initial climb :D + */ + AMP(254.0); //TODO: Find these values :D + + public final double heightMeters; + ElevatorState(double heightMeters) { + this.heightMeters = heightMeters; + } + } + } + + public static class Tramp { + public static final double GEAR_RATIO = 3.0 / 1.0; + public static final double TRAP_SCORING_TIME_SECONDS = 1.114; //TODO: Find these values :D + public static final int TRAMP_MOTOR_ID = -1; //TODO: Configure this later + public static final int TRAMP_BEAM_BREAK_CHANNEL = -1; //TODO: Configure this later + } } diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 0d65987d..a11f6ccd 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -14,6 +14,10 @@ import org.team1540.robot2024.commands.SwerveDriveCommand; import org.team1540.robot2024.commands.indexer.IntakeCommand; import org.team1540.robot2024.subsystems.drive.*; +import org.team1540.robot2024.subsystems.tramp.Tramp; +import org.team1540.robot2024.subsystems.tramp.TrampIO; +import org.team1540.robot2024.subsystems.tramp.TrampIOSim; +import org.team1540.robot2024.subsystems.tramp.TrampIOSparkMax; import org.team1540.robot2024.subsystems.shooter.*; import org.team1540.robot2024.subsystems.indexer.Indexer; import org.team1540.robot2024.subsystems.indexer.IndexerIO; @@ -33,6 +37,7 @@ public class RobotContainer { // Subsystems public final Drivetrain drivetrain; + public final Tramp tramp; public final Shooter shooter; public final Indexer indexer; @@ -62,14 +67,13 @@ public RobotContainer() { new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT)), new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)), new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT))); + tramp = new Tramp(new TrampIOSparkMax()); shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX()); - indexer = new Indexer( new IndexerIOSparkMax() ); break; - case SIM: // Sim robot, instantiate physics sim IO implementations drivetrain = @@ -79,6 +83,7 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); + tramp = new Tramp(new TrampIOSim()); shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim()); indexer = new Indexer( @@ -96,10 +101,14 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {}); + indexer = new Indexer( new IndexerIO() {} ); + + tramp = new Tramp(new TrampIO() {}); + break; } diff --git a/src/main/java/org/team1540/robot2024/commands/TrampCommand.java b/src/main/java/org/team1540/robot2024/commands/TrampCommand.java new file mode 100644 index 00000000..a9cd6307 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/TrampCommand.java @@ -0,0 +1,33 @@ +package org.team1540.robot2024.commands; + +import org.team1540.robot2024.subsystems.tramp.Tramp; + +import edu.wpi.first.wpilibj2.command.Command; + +public class TrampCommand extends Command { + + private final Tramp tramp; + + public TrampCommand(Tramp tramp) { + this.tramp = tramp; + addRequirements(tramp); + } + + @Override + public void initialize() { + tramp.setPercent(0.5); + } + + @Override + public void execute() {} + + @Override + public boolean isFinished() { +return tramp.isBeamBreakBlocked(); + } + + @Override + public void end(boolean interrupted) { + tramp.stopTramp(); + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java index 73794b20..2bb2d4b7 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java @@ -50,6 +50,7 @@ public ShooterPivotIOTalonFX() { motorConfig.Slot0.kS = KS; motorConfig.Slot0.kG = KG; motorConfig.Slot0.kV = KV; + motorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; motorConfig.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY_RPS; motorConfig.MotionMagic.MotionMagicAcceleration = MAX_ACCEL_RPS2; diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java new file mode 100644 index 00000000..683b2503 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java @@ -0,0 +1,33 @@ +package org.team1540.robot2024.subsystems.tramp; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class Tramp extends SubsystemBase { + private final TrampIO io; + private final TrampIOInputsAutoLogged inputs = new TrampIOInputsAutoLogged(); + + public Tramp(TrampIO io) { + this.io = io; + } + + public void setPercent(double percentage) { + io.setVoltage(12.0 * percentage); + } + + public boolean isBeamBreakBlocked() { + return inputs.breamBreakTripped; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Tramp", inputs); + } + + public void stopTramp() { + io.setVoltage(0); + } + +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIO.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIO.java new file mode 100644 index 00000000..7e77c45a --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIO.java @@ -0,0 +1,16 @@ +package org.team1540.robot2024.subsystems.tramp; + +import org.littletonrobotics.junction.AutoLog; + + +public interface TrampIO { + @AutoLog + class TrampIOInputs { + public boolean breamBreakTripped = false; + public double velocityRPM = 0.0; + public double appliedVolts = 0.0; + public double currentAmps = 0.0; + } + default void setVoltage(double volts) {} + default void updateInputs(TrampIOInputs inputs) {} +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSim.java new file mode 100644 index 00000000..e0b4fdd6 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSim.java @@ -0,0 +1,27 @@ +package org.team1540.robot2024.subsystems.tramp; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import org.team1540.robot2024.Constants; + +import static org.team1540.robot2024.Constants.Tramp.GEAR_RATIO; + +public class TrampIOSim implements TrampIO{ + private final DCMotorSim sim = new DCMotorSim(DCMotor.getNEO(1), GEAR_RATIO,0.025); //TODO: Fix MOI its probably wrong :D + + private double appliedVolts = 0.0; + @Override + public void updateInputs(TrampIOInputs inputs) { + sim.update(Constants.LOOP_PERIOD_SECS); + inputs.velocityRPM = sim.getAngularVelocityRPM(); + inputs.appliedVolts = appliedVolts; + inputs.currentAmps = sim.getCurrentDrawAmps(); + } + + @Override + public void setVoltage(double volts) { + appliedVolts = MathUtil.clamp(volts, -12, 12); + sim.setInputVoltage(appliedVolts); + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java new file mode 100644 index 00000000..dfef09ff --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java @@ -0,0 +1,31 @@ +package org.team1540.robot2024.subsystems.tramp; + +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.wpilibj.DigitalInput; +import org.team1540.robot2024.Constants.Tramp; + +public class TrampIOSparkMax implements TrampIO { + private final DigitalInput beamBreak = new DigitalInput(0); + //TODO: Potentially change name :D + private final CANSparkMax motor = new CANSparkMax(Tramp.TRAMP_MOTOR_ID, MotorType.kBrushless); + private final RelativeEncoder motorEncoder = motor.getEncoder(); + public TrampIOSparkMax() { + motor.setIdleMode(CANSparkMax.IdleMode.kBrake); + //Potentially invert motor + motor.setSmartCurrentLimit(40); + motor.enableVoltageCompensation(12); + } + @Override + public void setVoltage(double volts) { + motor.setVoltage(volts); + } + + public void updateInputs(TrampIOInputs inputs) { + inputs.breamBreakTripped = !(beamBreak.get()); //I think returns false when broken... Returns true when broken now. + inputs.velocityRPM = motorEncoder.getVelocity(); + inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage(); + inputs.currentAmps = motor.getOutputCurrent(); + } +}