diff --git a/src/main/java/org/team1540/advantagekitdemo/BuildConstants.java b/src/main/java/org/team1540/advantagekitdemo/BuildConstants.java index 2bfbbbf..783e97b 100644 --- a/src/main/java/org/team1540/advantagekitdemo/BuildConstants.java +++ b/src/main/java/org/team1540/advantagekitdemo/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "advantageKitDemo"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = -1; - public static final String GIT_SHA = "UNKNOWN"; - public static final String GIT_DATE = "UNKNOWN"; - public static final String GIT_BRANCH = "UNKNOWN"; - public static final String BUILD_DATE = "2024-01-03 22:28:19 PST"; - public static final long BUILD_UNIX_TIME = 1704349699908L; - public static final int DIRTY = 129; + public static final int GIT_REVISION = 2; + public static final String GIT_SHA = "2a08f6b65c19ec91dfce3fb905ed68b2ab4608ec"; + public static final String GIT_DATE = "2024-01-04 09:13:53 PST"; + public static final String GIT_BRANCH = "main"; + public static final String BUILD_DATE = "2024-01-04 16:52:55 PST"; + public static final long BUILD_UNIX_TIME = 1704415975607L; + public static final int DIRTY = 1; private BuildConstants(){} } diff --git a/src/main/java/org/team1540/advantagekitdemo/Constants.java b/src/main/java/org/team1540/advantagekitdemo/Constants.java index c865f38..c875203 100644 --- a/src/main/java/org/team1540/advantagekitdemo/Constants.java +++ b/src/main/java/org/team1540/advantagekitdemo/Constants.java @@ -41,6 +41,8 @@ public enum SimulationMode { REPLAY } + public static final double DEADZONE_RADIUS = 0.1; + public static final class DrivetrainConstants { public static final int FL_ID = 1; public static final int BL_ID = 2; @@ -53,7 +55,7 @@ public static final class DrivetrainConstants { public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(4.0); public static final double TRACK_WIDTH_METERS = 26.5; - public static final double ROBOT_WEIGHT_KG = Units.lbsToKilograms(118); // omg its tem 118 teh robnots + public static final double ROBOT_MASS_KG = Units.lbsToKilograms(118); // omg its tem 118 teh robnots public static final double ROBOT_MOI = 2.54; // omg its tem 254 teh chezy pofs public static final double VELOCITY_KP = 3.2925; @@ -79,10 +81,28 @@ public static final class ElevatorConstants { public static final double KP = 1; public static final double KI = 0; public static final double KD = 0; - public static final TrapezoidProfile.Constraints MOTION_CONSTRAINTS = new TrapezoidProfile.Constraints(2.5, 12.5); + public static final TrapezoidProfile.Constraints MOTION_CONSTRAINTS = + new TrapezoidProfile.Constraints(2.5, 12.5); public static final double KS = 0; public static final double KG = 0.38; public static final double KV = 0.19; } + + public static final class IntakeConstants { + public static final double WRIST_GEARING = 50; + public static final double INTAKE_LENGTH_METERS = Units.inchesToMeters(17); + public static final double INTAKE_MASS_KG = Units.lbsToKilograms(8); + public static final double INTAKE_MOI = INTAKE_MASS_KG * INTAKE_LENGTH_METERS * INTAKE_LENGTH_METERS / 3; + + public static final double WRIST_KP = 1; + public static final double WRIST_KI = 0; + public static final double WRIST_KD = 0; + public static final TrapezoidProfile.Constraints MOTION_CONSTRAINTS = + new TrapezoidProfile.Constraints(1.8, 7.2); + + public static final double WRIST_KS = 0; + public static final double WRIST_KG = 0.87; + public static final double WRIST_KV = 5.64; + } } diff --git a/src/main/java/org/team1540/advantagekitdemo/Robot.java b/src/main/java/org/team1540/advantagekitdemo/Robot.java index 0591314..1fc00aa 100644 --- a/src/main/java/org/team1540/advantagekitdemo/Robot.java +++ b/src/main/java/org/team1540/advantagekitdemo/Robot.java @@ -21,6 +21,7 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import org.team1540.advantagekitdemo.util.SuperstructureVisualizer; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -99,6 +100,7 @@ public void robotPeriodic() { // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); + SuperstructureVisualizer.periodic(); } /** diff --git a/src/main/java/org/team1540/advantagekitdemo/RobotContainer.java b/src/main/java/org/team1540/advantagekitdemo/RobotContainer.java index 1bb619f..4905e15 100644 --- a/src/main/java/org/team1540/advantagekitdemo/RobotContainer.java +++ b/src/main/java/org/team1540/advantagekitdemo/RobotContainer.java @@ -18,9 +18,18 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.team1540.advantagekitdemo.commands.ArcadeDriveCommand; +import org.team1540.advantagekitdemo.commands.ElevatorManualCommand; +import org.team1540.advantagekitdemo.commands.WristManualCommand; import org.team1540.advantagekitdemo.subsystems.drivetrain.Drivetrain; import org.team1540.advantagekitdemo.subsystems.drivetrain.DrivetrainIOReal; import org.team1540.advantagekitdemo.subsystems.drivetrain.DrivetrainIOSim; +import org.team1540.advantagekitdemo.subsystems.elevator.Elevator; +import org.team1540.advantagekitdemo.subsystems.elevator.ElevatorIO; +import org.team1540.advantagekitdemo.subsystems.elevator.ElevatorIOSim; +import org.team1540.advantagekitdemo.subsystems.intake.Intake; +import org.team1540.advantagekitdemo.subsystems.intake.IntakeIO; +import org.team1540.advantagekitdemo.subsystems.intake.IntakeIOSim; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -30,9 +39,12 @@ */ public class RobotContainer { // Controller - private final CommandXboxController controller = new CommandXboxController(0); + private final CommandXboxController driver = new CommandXboxController(0); + private final CommandXboxController copilot = new CommandXboxController(1); final Drivetrain drivetrain; + final Elevator elevator; + final Intake intake; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -40,8 +52,12 @@ public class RobotContainer { public RobotContainer() { if (Robot.isReal()) { drivetrain = new Drivetrain(new DrivetrainIOReal()); + elevator = new Elevator(new ElevatorIO() {}); + intake = new Intake(new IntakeIO() {}); } else { drivetrain = new Drivetrain(new DrivetrainIOSim()); + elevator = new Elevator(new ElevatorIOSim()); + intake = new Intake(new IntakeIOSim()); } configureButtonBindings(); } @@ -53,6 +69,9 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { + drivetrain.setDefaultCommand(new ArcadeDriveCommand(drivetrain, driver)); + elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); + intake.setDefaultCommand(new WristManualCommand(intake, copilot)); } /** diff --git a/src/main/java/org/team1540/advantagekitdemo/commands/ArcadeDriveCommand.java b/src/main/java/org/team1540/advantagekitdemo/commands/ArcadeDriveCommand.java new file mode 100644 index 0000000..9ef3894 --- /dev/null +++ b/src/main/java/org/team1540/advantagekitdemo/commands/ArcadeDriveCommand.java @@ -0,0 +1,46 @@ +package org.team1540.advantagekitdemo.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.team1540.advantagekitdemo.Constants; +import org.team1540.advantagekitdemo.subsystems.drivetrain.Drivetrain; + +public class ArcadeDriveCommand extends Command { + private final Drivetrain drivetrain; + + private final CommandXboxController xBoxController; + + private final SlewRateLimiter leftRateLimiter = new SlewRateLimiter(5); + private final SlewRateLimiter rightRateLimiter = new SlewRateLimiter(5); + + public ArcadeDriveCommand(Drivetrain drivetrain, CommandXboxController xBoxController) { + this.drivetrain = drivetrain; + this.xBoxController = xBoxController; + addRequirements(drivetrain); + } + + public void execute() { + double throttle = MathUtil.applyDeadband(-xBoxController.getLeftY(), Constants.DEADZONE_RADIUS); + double turn = MathUtil.applyDeadband(xBoxController.getRightX(), Constants.DEADZONE_RADIUS); + double left = leftRateLimiter.calculate( + MathUtil.clamp(throttle + turn,-1, 1) + ); + double right = rightRateLimiter.calculate( + MathUtil.clamp(throttle - turn, -1, 1) + ); + + drivetrain.drivePercent(left, right); + } + + @Override + public void end(boolean interrupted) { + drivetrain.stop(); + } + + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/org/team1540/advantagekitdemo/commands/ElevatorManualCommand.java b/src/main/java/org/team1540/advantagekitdemo/commands/ElevatorManualCommand.java new file mode 100644 index 0000000..e7542ef --- /dev/null +++ b/src/main/java/org/team1540/advantagekitdemo/commands/ElevatorManualCommand.java @@ -0,0 +1,26 @@ +package org.team1540.advantagekitdemo.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.team1540.advantagekitdemo.subsystems.elevator.Elevator; + +public class ElevatorManualCommand extends Command { + private final Elevator elevator; + private final CommandXboxController controller; + + public ElevatorManualCommand(Elevator elevator, CommandXboxController controller) { + this.elevator = elevator; + this.controller = controller; + addRequirements(elevator); + } + + @Override + public void execute() { + elevator.setPercent(controller.getRightTriggerAxis() - controller.getLeftTriggerAxis()); + } + + @Override + public void end(boolean interrupted) { + elevator.stop(); + } +} diff --git a/src/main/java/org/team1540/advantagekitdemo/commands/WristManualCommand.java b/src/main/java/org/team1540/advantagekitdemo/commands/WristManualCommand.java new file mode 100644 index 0000000..91c242f --- /dev/null +++ b/src/main/java/org/team1540/advantagekitdemo/commands/WristManualCommand.java @@ -0,0 +1,26 @@ +package org.team1540.advantagekitdemo.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.team1540.advantagekitdemo.subsystems.intake.Intake; + +public class WristManualCommand extends Command { + private final Intake intake; + private final CommandXboxController controller; + + public WristManualCommand(Intake intake, CommandXboxController controller) { + this.intake = intake; + this.controller = controller; + addRequirements(intake); + } + + @Override + public void execute() { + intake.setWristPercent(controller.getRightX()); + } + + @Override + public void end(boolean interrupted) { + intake.stopWrist(); + } +} diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/drivetrain/DrivetrainIOSim.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/drivetrain/DrivetrainIOSim.java index 2dd6d77..24c3d0d 100644 --- a/src/main/java/org/team1540/advantagekitdemo/subsystems/drivetrain/DrivetrainIOSim.java +++ b/src/main/java/org/team1540/advantagekitdemo/subsystems/drivetrain/DrivetrainIOSim.java @@ -12,7 +12,7 @@ public class DrivetrainIOSim implements DrivetrainIO { DCMotor.getFalcon500(2), GEAR_RATIO, ROBOT_MOI, - ROBOT_WEIGHT_KG, + ROBOT_MASS_KG, WHEEL_DIAMETER_METERS / 2, TRACK_WIDTH_METERS, null diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/Elevator.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/Elevator.java index fdc50a0..527a432 100644 --- a/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/Elevator.java +++ b/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/Elevator.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; import org.littletonrobotics.junction.Logger; +import org.team1540.advantagekitdemo.util.SuperstructureVisualizer; import static org.team1540.advantagekitdemo.Constants.ElevatorConstants.*; @@ -25,6 +26,8 @@ public void periodic() { super.periodic(); io.updateInputs(inputs); Logger.processInputs("Elevator", inputs); + + SuperstructureVisualizer.setElevatorPosition(getPositionMeters()); } public void setPercent(double percentOutput) { @@ -42,8 +45,7 @@ public void setPosition(double positionMeters) { } public void stop() { - if (RobotState.isDisabled()) setVoltage(0); - else setPosition(getPositionMeters()); + setPercent(0); } public double getPositionMeters() { diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIO.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIO.java index 7c10f78..c52a794 100644 --- a/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIO.java @@ -3,8 +3,6 @@ import org.littletonrobotics.junction.AutoLog; public interface ElevatorIO { - - @AutoLog class ElevatorIOInputs { public double positionMeters = 0; diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/intake/Intake.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/intake/Intake.java new file mode 100644 index 0000000..1379080 --- /dev/null +++ b/src/main/java/org/team1540/advantagekitdemo/subsystems/intake/Intake.java @@ -0,0 +1,73 @@ +package org.team1540.advantagekitdemo.subsystems.intake; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; +import org.littletonrobotics.junction.Logger; +import org.team1540.advantagekitdemo.util.SuperstructureVisualizer; + +import static org.team1540.advantagekitdemo.Constants.IntakeConstants.*; + +public class Intake extends ProfiledPIDSubsystem { + private final IntakeIO io; + private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + + private final ArmFeedforward wristFeedforward = new ArmFeedforward(WRIST_KS, WRIST_KG, WRIST_KV); + + public Intake(IntakeIO io) { + super(new ProfiledPIDController(WRIST_KP, WRIST_KI, WRIST_KD, MOTION_CONSTRAINTS)); + this.io = io; + } + + @Override + public void periodic() { + super.periodic(); + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); + + SuperstructureVisualizer.setWristPosition(getWristPosition()); + } + + public void setIntakePercent(double percentOutput) { + io.setIntakeVoltage(12 * percentOutput); + } + + public void setWristPercent(double percentOutput) { + disable(); + setWristVoltage(12 * percentOutput); + } + + public void setWristPosition(Rotation2d position) { + enable(); + setGoal(position.getRotations()); + } + + private void setWristVoltage(double volts) { + io.setWristVoltage(volts); + } + + public void stopIntake() { + setIntakePercent(0); + } + + public void stopWrist() { + setWristPercent(0); + } + + public Rotation2d getWristPosition() { + return inputs.wristPosition; + } + + @Override + protected void useOutput(double output, TrapezoidProfile.State setpoint) { + setWristVoltage(output + wristFeedforward.calculate(getWristPosition().getRadians(), setpoint.velocity)); + } + + @Override + protected double getMeasurement() { + return inputs.wristPosition.getRotations(); + } +} diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/intake/IntakeIO.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..9aa9ed6 --- /dev/null +++ b/src/main/java/org/team1540/advantagekitdemo/subsystems/intake/IntakeIO.java @@ -0,0 +1,22 @@ +package org.team1540.advantagekitdemo.subsystems.intake; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + @AutoLog + class IntakeIOInputs { + public Rotation2d wristPosition = new Rotation2d(); + public double wristAppliedVolts = 0; + public double wristCurrentAmps = 0; + + public double intakeAppliedVolts = 0; + public double intakeCurrentAmps = 0; + } + + default void updateInputs(IntakeIOInputs inputs) {} + + default void setWristVoltage(double volts) {} + + default void setIntakeVoltage(double volts) {} +} diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/intake/IntakeIOSim.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/intake/IntakeIOSim.java new file mode 100644 index 0000000..ed4ceb3 --- /dev/null +++ b/src/main/java/org/team1540/advantagekitdemo/subsystems/intake/IntakeIOSim.java @@ -0,0 +1,44 @@ +package org.team1540.advantagekitdemo.subsystems.intake; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; + +import static org.team1540.advantagekitdemo.Constants.IntakeConstants.*; + +public class IntakeIOSim implements IntakeIO { + private final SingleJointedArmSim wristSim = + new SingleJointedArmSim( + DCMotor.getFalcon500(1), + WRIST_GEARING, + INTAKE_MOI, + INTAKE_LENGTH_METERS, + Math.toRadians(0), + Math.toRadians(180), + true, + Math.toRadians(0) + ); + + private double intakeVoltage; + private double wristVoltage; + + @Override + public void updateInputs(IntakeIOInputs inputs) { + wristSim.update(0.02); + inputs.wristPosition = Rotation2d.fromDegrees(wristSim.getAngleRads()); + inputs.wristAppliedVolts = wristVoltage; + inputs.wristCurrentAmps = wristSim.getCurrentDrawAmps(); + inputs.intakeAppliedVolts = intakeVoltage; + } + + @Override + public void setWristVoltage(double volts) { + this.wristVoltage = volts; + wristSim.setInputVoltage(volts); + } + + @Override + public void setIntakeVoltage(double volts) { + intakeVoltage = volts; + } +} diff --git a/src/main/java/org/team1540/advantagekitdemo/util/SuperstructureVisualizer.java b/src/main/java/org/team1540/advantagekitdemo/util/SuperstructureVisualizer.java new file mode 100644 index 0000000..1da47ad --- /dev/null +++ b/src/main/java/org/team1540/advantagekitdemo/util/SuperstructureVisualizer.java @@ -0,0 +1,45 @@ +package org.team1540.advantagekitdemo.util; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import org.littletonrobotics.junction.Logger; +import org.team1540.advantagekitdemo.Constants.*; + +public class SuperstructureVisualizer { + private static Pose3d elevatorStage1 = new Pose3d(); + private static Pose3d elevatorCarriage = new Pose3d(); + private static Pose3d wrist = new Pose3d(); + + private static double elevatorPosition = 0; + + public static void periodic() { + Logger.recordOutput("Mechanism3d", elevatorStage1, elevatorCarriage, wrist); + } + + public static void setElevatorPosition(double positionMeters) { + if (positionMeters <= ElevatorConstants.STAGE_1_HEIGHT_METERS) { + elevatorStage1 = new Pose3d(); + elevatorCarriage = new Pose3d(0.0, positionMeters, 0.0, new Rotation3d()); + } else { + elevatorCarriage = new Pose3d(0.0, ElevatorConstants.STAGE_1_HEIGHT_METERS, 0.0, new Rotation3d()); + elevatorStage1 = + new Pose3d( + 0.0, + positionMeters - ElevatorConstants.STAGE_1_HEIGHT_METERS, + 0.0, + new Rotation3d() + ); + } + elevatorPosition = positionMeters; + } + + public static void setWristPosition(Rotation2d wristPosition) { + wrist = new Pose3d( + 0.0, + elevatorPosition, + 0.0, + new Rotation3d(0, wristPosition.getDegrees(), 0) + ); + } +}