From 4995decb84186228e88059ef120ecec5ae792331 Mon Sep 17 00:00:00 2001 From: James Ding Date: Tue, 12 Mar 2024 12:02:25 -0700 Subject: [PATCH] Add changes from 3/11/24 --- .wpilib/wpilib_preferences.json | 10 +- src/main/java/org/rambots/BuildConstants.java | 10 +- src/main/java/org/rambots/Robot.kt | 2 +- src/main/java/org/rambots/RobotContainer.kt | 60 ++++++--- .../rambots/commands/ArmPositionCommand.kt | 9 +- .../org/rambots/commands/AutoShootCommand.kt | 5 + .../org/rambots/commands/DriveCommands.java | 32 ++--- .../commands/ElevatorPositionCommand.kt | 9 +- .../rambots/commands/MultiDistanceShot.java | 98 --------------- .../rambots/commands/PathFinderAndFollow.java | 1 + .../org/rambots/commands/ShooterCommand.kt | 27 ++++ .../rambots/commands/WristPositionCommand.kt | 26 ++++ .../java/org/rambots/config/ArmConstants.kt | 3 +- .../org/rambots/config/ShooterConstants.kt | 22 ++++ .../java/org/rambots/config/WristConstants.kt | 13 ++ .../org/rambots/subsystems/ArmSubsystem.kt | 8 +- .../rambots/subsystems/ElevatorSubsystem.kt | 10 +- .../rambots/subsystems/ShooterSubsystem.kt | 115 ++++++++++++++++++ .../org/rambots/subsystems/SuperStructure.kt | 45 +++++++ .../org/rambots/subsystems/WristSubsystem.kt | 43 +++++++ .../drive/PhoenixOdometryThread.java | 1 + src/main/java/org/rambots/util/Alert.java | 3 +- .../java/org/rambots/util/CommandUtils.kt | 7 -- .../org/rambots/util/LimelightHelpers.java | 3 - .../org/rambots/util/LoggedTunableNumber.java | 2 +- 25 files changed, 387 insertions(+), 177 deletions(-) create mode 100644 src/main/java/org/rambots/commands/AutoShootCommand.kt delete mode 100644 src/main/java/org/rambots/commands/MultiDistanceShot.java create mode 100644 src/main/java/org/rambots/commands/ShooterCommand.kt create mode 100644 src/main/java/org/rambots/commands/WristPositionCommand.kt create mode 100644 src/main/java/org/rambots/config/ShooterConstants.kt create mode 100644 src/main/java/org/rambots/config/WristConstants.kt create mode 100644 src/main/java/org/rambots/subsystems/ShooterSubsystem.kt create mode 100644 src/main/java/org/rambots/subsystems/SuperStructure.kt create mode 100644 src/main/java/org/rambots/subsystems/WristSubsystem.kt delete mode 100644 src/main/java/org/rambots/util/CommandUtils.kt diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index a72b2c8..9af7112 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2024", - "teamNumber": 2204 -} + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2024", + "teamNumber": 2204 +} \ No newline at end of file diff --git a/src/main/java/org/rambots/BuildConstants.java b/src/main/java/org/rambots/BuildConstants.java index d1efb49..12937ab 100644 --- a/src/main/java/org/rambots/BuildConstants.java +++ b/src/main/java/org/rambots/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "frc-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 10; - public static final String GIT_SHA = "4af6a0d87fab6e63d1cb30c4959e65c908757abf"; - public static final String GIT_DATE = "2024-03-06 15:29:43 PST"; + public static final int GIT_REVISION = 11; + public static final String GIT_SHA = "f998f6b7e2633dad7ed07b42397f1271f6200e79"; + public static final String GIT_DATE = "2024-03-10 13:54:54 PDT"; public static final String GIT_BRANCH = "james/swerve"; - public static final String BUILD_DATE = "2024-03-09 19:35:02 PST"; - public static final long BUILD_UNIX_TIME = 1710041702135L; + public static final String BUILD_DATE = "2024-03-11 21:28:28 PDT"; + public static final long BUILD_UNIX_TIME = 1710217708081L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/org/rambots/Robot.kt b/src/main/java/org/rambots/Robot.kt index 76539f0..c0b3d31 100644 --- a/src/main/java/org/rambots/Robot.kt +++ b/src/main/java/org/rambots/Robot.kt @@ -52,7 +52,7 @@ object Robot : LoggedRobot() { when (Constants.getMode()) { Constants.Mode.REAL -> { -// Logger.addDataReceiver(WPILOGWriter()) + Logger.addDataReceiver(WPILOGWriter()) Logger.addDataReceiver(NT4Publisher()) } diff --git a/src/main/java/org/rambots/RobotContainer.kt b/src/main/java/org/rambots/RobotContainer.kt index 9fe9f69..1edbf10 100644 --- a/src/main/java/org/rambots/RobotContainer.kt +++ b/src/main/java/org/rambots/RobotContainer.kt @@ -14,16 +14,15 @@ package org.rambots import com.pathplanner.lib.auto.AutoBuilder import edu.wpi.first.math.geometry.* -import edu.wpi.first.wpilibj.PS5Controller import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.Subsystem import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller -import edu.wpi.first.wpilibj2.command.button.CommandXboxController import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine import org.littletonrobotics.junction.networktables.LoggedDashboardChooser import org.rambots.commands.* -import org.rambots.subsystems.ArmSubsystem -import org.rambots.subsystems.ElevatorSubsystem +import org.rambots.subsystems.* import org.rambots.subsystems.drive.* import org.rambots.subsystems.drive.DriveConstants.moduleConfigs import org.rambots.subsystems.vision.AprilTagVision @@ -31,8 +30,6 @@ import org.rambots.subsystems.vision.AprilTagVisionIO import org.rambots.subsystems.vision.AprilTagVisionIOLimelight import org.rambots.subsystems.vision.AprilTagVisionIOPhotonVisionSIM import org.rambots.util.VisionHelpers.TimestampedVisionUpdate -import java.util.function.Consumer -import kotlin.math.pow /** @@ -166,21 +163,56 @@ object RobotContainer { drive.defaultCommand = DriveCommands.joystickDrive( drive, driveController, - { (-controller.leftY).pow(3.0) }, - { (-controller.leftX).pow(3.0) }, + { -controller.leftY }, + { -controller.leftX }, { -controller.rightX } ) ArmSubsystem.defaultCommand = ArmPositionCommand { ArmSubsystem.desiredPosition } ElevatorSubsystem.defaultCommand = ElevatorPositionCommand { ElevatorSubsystem.desiredPosition } + WristSubsystem.defaultCommand = WristPositionCommand { WristSubsystem.desiredPosition } - controller.L1().whileTrue(AmpScoring()) - controller.R2().whileTrue(SourceIntake()) - controller.square().whileTrue(ArmPositionCommand { -70.0 }) - controller.circle().whileTrue(ArmPositionCommand { 0.0 }) +// controller.L1().whileTrue(AmpScoring()) +// controller.R2().whileTrue(SourceIntake()) +// +// controller.square().whileTrue(ArmPositionCommand { -70.0 }) +// controller.circle().whileTrue(ArmPositionCommand { 0.0 }) + +// controller.povUp().whileTrue(ElevatorPositionCommand { 0.0 }) +// controller.povDown().whileTrue(ElevatorPositionCommand { -48.0 }) +// +// controller.povLeft().whileTrue(WristPositionCommand { -125.0 }) +// controller.povRight().whileTrue(WristPositionCommand { 0.0 }) + +// controller.cross().whileTrue(Commands.startEnd({ ShooterSubsystem.intake() }, { ShooterSubsystem.stopIntake()}, ShooterSubsystem )) +// controller.circle().whileTrue(Commands.startEnd({ShooterSubsystem.shoot(3000.0, 3000.0)}, {ShooterSubsystem.stopShooter()}, ShooterSubsystem)) + + controller.L2().onTrue(SuperStructure.intakeCommand) + controller.L2().onFalse(SuperStructure.homeCommandFromIntake) - controller.povUp().whileTrue(ElevatorPositionCommand { 0.0 }) - controller.povDown().whileTrue(ElevatorPositionCommand { -70.0 }) + controller.circle().onTrue(Commands.runOnce({ ShooterSubsystem.shoot(6000.0, 6000.0) }, ShooterSubsystem)) + controller.circle().onFalse(Commands.runOnce({ ShooterSubsystem.stopShooter() }, ShooterSubsystem)) + + controller.square().onTrue(Commands.runOnce({ ShooterSubsystem.intake() }, ShooterSubsystem)) + controller.square().onFalse(Commands.runOnce({ ShooterSubsystem.stopIntake() }, ShooterSubsystem)) + + controller.povUp().whileTrue(Commands.runOnce({ WristSubsystem.desiredPosition++})) + controller.povDown().whileTrue(Commands.runOnce({ WristSubsystem.desiredPosition--})) + + +// controller.R2().whileTrue( +// Commands.startEnd( +// { +// ShooterSubsystem.shoot(7000.0, 7000.0) +// ShooterSubsystem.intake() +// }, +// { +// ShooterSubsystem.stopShooter() +// ShooterSubsystem.stopIntake() +// }, +// ShooterSubsystem +// ) +// ) // controller.a().whileTrue(PathFinderAndFollow(driveController.driveModeType)) diff --git a/src/main/java/org/rambots/commands/ArmPositionCommand.kt b/src/main/java/org/rambots/commands/ArmPositionCommand.kt index a20588b..732946a 100644 --- a/src/main/java/org/rambots/commands/ArmPositionCommand.kt +++ b/src/main/java/org/rambots/commands/ArmPositionCommand.kt @@ -1,14 +1,11 @@ package org.rambots.commands import edu.wpi.first.wpilibj2.command.Command -import org.rambots.config.ArmConstants.ARM_ACCEPTABLE_ERROR import org.rambots.subsystems.ArmSubsystem -import org.rambots.util.CommandUtils.generateErrorRange -class ArmPositionCommand(private val angle: () -> Double, private val finishState: () -> Double) : Command() { +class ArmPositionCommand(private val angle: () -> Double, private val finishCondition: (position: Double) -> Boolean) : Command() { - constructor(angle: () -> Double) : this(angle, angle) - constructor(angle: Double) : this ({angle}) + constructor(angle: () -> Double) : this(angle, { true }) init { addRequirements(ArmSubsystem) @@ -23,7 +20,7 @@ class ArmPositionCommand(private val angle: () -> Double, private val finishStat } override fun isFinished(): Boolean { - return generateErrorRange(ArmSubsystem.position, ARM_ACCEPTABLE_ERROR).contains(finishState.invoke()) + return finishCondition.invoke(ArmSubsystem.position) } } \ No newline at end of file diff --git a/src/main/java/org/rambots/commands/AutoShootCommand.kt b/src/main/java/org/rambots/commands/AutoShootCommand.kt new file mode 100644 index 0000000..3580d86 --- /dev/null +++ b/src/main/java/org/rambots/commands/AutoShootCommand.kt @@ -0,0 +1,5 @@ +package org.rambots.commands + +class AutoShootCommand { + +} \ No newline at end of file diff --git a/src/main/java/org/rambots/commands/DriveCommands.java b/src/main/java/org/rambots/commands/DriveCommands.java index 11b0914..207861d 100644 --- a/src/main/java/org/rambots/commands/DriveCommands.java +++ b/src/main/java/org/rambots/commands/DriveCommands.java @@ -39,27 +39,16 @@ private DriveCommands() { /** * Field relative drive command using two joysticks (controlling linear and angular velocities). */ - public static Command joystickDrive( - Drive drive, - DriveController driveMode, - DoubleSupplier xSupplier, - DoubleSupplier ySupplier, - DoubleSupplier omegaSupplier) { + public static Command joystickDrive(Drive drive, DriveController driveMode, DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier) { return Commands.runOnce( () -> { // Apply deadband - double linearMagnitude = - MathUtil.applyDeadband( - Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND); - Rotation2d linearDirection = - new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double linearMagnitude = MathUtil.applyDeadband(Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND); + Rotation2d linearDirection = new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); if (driveMode.isHeadingControlled()) { final var targetAngle = driveMode.getHeadingAngle(); - omega = - Drive.getThetaController() - .calculate( - drive.getPose().getRotation().getRadians(), targetAngle.get().getRadians()); + omega = Drive.getThetaController().calculate(drive.getPose().getRotation().getRadians(), targetAngle.get().getRadians()); if (Drive.getThetaController().atGoal()) { omega = 0; } @@ -71,19 +60,12 @@ public static Command joystickDrive( omega = Math.copySign(omega * omega, omega); // Calcaulate new linear velocity - Translation2d linearVelocity = - new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) - .getTranslation(); + Translation2d linearVelocity = new Pose2d(new Translation2d(), linearDirection).transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation(); // Convert to field relative speeds & send command - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; + boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * drivetrainConfig.maxLinearVelocity(), - linearVelocity.getY() * drivetrainConfig.maxLinearVelocity(), + ChassisSpeeds.fromFieldRelativeSpeeds(linearVelocity.getX() * drivetrainConfig.maxLinearVelocity(), linearVelocity.getY() * drivetrainConfig.maxLinearVelocity(), omega * drivetrainConfig.maxAngularVelocity(), isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) diff --git a/src/main/java/org/rambots/commands/ElevatorPositionCommand.kt b/src/main/java/org/rambots/commands/ElevatorPositionCommand.kt index a0d646f..2f405c1 100644 --- a/src/main/java/org/rambots/commands/ElevatorPositionCommand.kt +++ b/src/main/java/org/rambots/commands/ElevatorPositionCommand.kt @@ -1,14 +1,11 @@ package org.rambots.commands import edu.wpi.first.wpilibj2.command.Command -import org.rambots.config.ArmConstants.ARM_ACCEPTABLE_ERROR import org.rambots.subsystems.ElevatorSubsystem -import org.rambots.util.CommandUtils.generateErrorRange -class ElevatorPositionCommand(private val position: () -> Double, private val finishState: () -> Double) : Command() { +class ElevatorPositionCommand(private val position: () -> Double, private val finishCondition: (position: Double) -> Boolean) : Command() { - constructor(position: () -> Double) : this(position, position) - constructor(position: Double) : this ({position}) + constructor(position: () -> Double) : this(position, { true }) init { addRequirements(ElevatorSubsystem) @@ -23,7 +20,7 @@ class ElevatorPositionCommand(private val position: () -> Double, private val fi } override fun isFinished(): Boolean { - return generateErrorRange(ElevatorSubsystem.position, ARM_ACCEPTABLE_ERROR).contains(finishState.invoke()) + return finishCondition.invoke(ElevatorSubsystem.position) } } \ No newline at end of file diff --git a/src/main/java/org/rambots/commands/MultiDistanceShot.java b/src/main/java/org/rambots/commands/MultiDistanceShot.java deleted file mode 100644 index bf2ab96..0000000 --- a/src/main/java/org/rambots/commands/MultiDistanceShot.java +++ /dev/null @@ -1,98 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.rambots.commands; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.wpilibj2.command.Command; -import org.littletonrobotics.junction.AutoLogOutput; -import org.rambots.subsystems.flywheel.Flywheel; -import org.rambots.util.AllianceFlipUtil; - -import java.util.function.Supplier; - -/** - * A command that shoots game piece from multi-distance position from the target. - */ -public class MultiDistanceShot extends Command { - Supplier poseSupplier; - Pose2d targetPose; - Flywheel flywheel; - InterpolatingDoubleTreeMap distanceMap = new InterpolatingDoubleTreeMap(); - - double distance; - double speed; - - /** - * Creates a new MultiDistanceShot command. - * - * @param poseSupplier The supplier for the robot's current pose. - * @param targetPose The target pose to shoot at. - * @param flywheel The flywheel subsystem. - */ - public MultiDistanceShot(Supplier poseSupplier, Pose2d targetPose, Flywheel flywheel) { - this.poseSupplier = poseSupplier; - this.targetPose = targetPose; - this.flywheel = flywheel; - - // Populate the distance map with distance-speed pairs - distanceMap.put(1.0, 10.0); - distanceMap.put(2.3, 15.7); - distanceMap.put(3.6, 21.9); - distanceMap.put(4.9, 27.8); - distanceMap.put(6.2, 33.6); - distanceMap.put(7.5, 39.4); - } - - @Override - public void initialize() { - // Apply any necessary transformations to the target pose - targetPose = AllianceFlipUtil.apply(targetPose); - } - - @Override - public void execute() { - // Calculate the distance from the current pose to the target pose - distance = poseSupplier.get().getTranslation().getDistance(targetPose.getTranslation()); - - // Get the corresponding speed from the distance-speed map - speed = distanceMap.get(distance); - - // Run the flywheel at the calculated speed - flywheel.runVelocity(speed); - } - - @Override - public void end(boolean interrupted) { - // Stop the flywheel when the command ends - flywheel.stop(); - } - - @Override - public boolean isFinished() { - // The command never finishes on its own - return false; - } - - /** - * Gets the distance from the current pose to the target pose. - * - * @return The distance in units. - */ - @AutoLogOutput(key = "Shooter/DistanceToTarget") - public double getDistance() { - return distance; - } - - /** - * Gets the speed of the flywheel. - * - * @return The speed in units per second. - */ - @AutoLogOutput(key = "Shooter/Speed") - public double getSpeed() { - return speed; - } -} diff --git a/src/main/java/org/rambots/commands/PathFinderAndFollow.java b/src/main/java/org/rambots/commands/PathFinderAndFollow.java index 99c0518..7e2365b 100644 --- a/src/main/java/org/rambots/commands/PathFinderAndFollow.java +++ b/src/main/java/org/rambots/commands/PathFinderAndFollow.java @@ -18,6 +18,7 @@ * A command that runs pathfindThenFollowPath based on the current drive mode. */ public class PathFinderAndFollow extends Command { + private final Supplier driveModeSupplier; private Command scoreCommand; private Command pathRun; diff --git a/src/main/java/org/rambots/commands/ShooterCommand.kt b/src/main/java/org/rambots/commands/ShooterCommand.kt new file mode 100644 index 0000000..bd1ad7e --- /dev/null +++ b/src/main/java/org/rambots/commands/ShooterCommand.kt @@ -0,0 +1,27 @@ +package org.rambots.commands + +import edu.wpi.first.wpilibj2.command.Command +import org.rambots.subsystems.ShooterSubsystem + +class ShooterCommand(private val topVelocity: () -> Double, private val bottomVelocity: () -> Double , private val finishCondition: (topVelocity: Double, bottomVelocity: Double) -> Boolean): Command() { + + constructor(topVelocity: () -> Double, bottomVelocity: () -> Double): this(topVelocity, bottomVelocity, { _,_ -> true }) + + init { + addRequirements(ShooterSubsystem) + } + + override fun initialize() { + ShooterSubsystem.topDesiredVelocity = topVelocity.invoke() + ShooterSubsystem.bottomDesiredVelocity = bottomVelocity.invoke() + } + + override fun execute() { + ShooterSubsystem.shoot(topVelocity.invoke(), bottomVelocity.invoke()) + } + + override fun isFinished(): Boolean { + return finishCondition.invoke(topVelocity.invoke(), bottomVelocity.invoke()) + } + +} \ No newline at end of file diff --git a/src/main/java/org/rambots/commands/WristPositionCommand.kt b/src/main/java/org/rambots/commands/WristPositionCommand.kt new file mode 100644 index 0000000..499aec4 --- /dev/null +++ b/src/main/java/org/rambots/commands/WristPositionCommand.kt @@ -0,0 +1,26 @@ +package org.rambots.commands + +import edu.wpi.first.wpilibj2.command.Command +import org.rambots.subsystems.WristSubsystem + +class WristPositionCommand(private val angle: () -> Double, private val finishCondition: (position: Double) -> Boolean) : Command() { + + constructor(angle: () -> Double) : this(angle, { true }) + + init { + addRequirements(WristSubsystem) + } + + override fun initialize() { + WristSubsystem.desiredPosition = angle.invoke() + } + + override fun execute() { + WristSubsystem.position = angle.invoke() + } + + override fun isFinished(): Boolean { + return finishCondition.invoke(WristSubsystem.position) + } + +} \ No newline at end of file diff --git a/src/main/java/org/rambots/config/ArmConstants.kt b/src/main/java/org/rambots/config/ArmConstants.kt index 9f66a8d..9656a70 100644 --- a/src/main/java/org/rambots/config/ArmConstants.kt +++ b/src/main/java/org/rambots/config/ArmConstants.kt @@ -7,9 +7,10 @@ object ArmConstants { const val ARM_NEO_LEADER_ID = 13 const val ARM_NEO_FOLLOWER_ID = 14 + const val ARM_ACCEPTABLE_ERROR = 0.5 + val ARM_PID = PIDConstants(0.02, 0.0, 0.0) - val ARM_ACCEPTABLE_ERROR = 0.5 diff --git a/src/main/java/org/rambots/config/ShooterConstants.kt b/src/main/java/org/rambots/config/ShooterConstants.kt new file mode 100644 index 0000000..2caf319 --- /dev/null +++ b/src/main/java/org/rambots/config/ShooterConstants.kt @@ -0,0 +1,22 @@ +package org.rambots.config + +import com.pathplanner.lib.util.PIDConstants + +object ShooterConstants { + + const val SHOOTER_TOP_ID = 19 + const val SHOOTER_BOTTOM_ID = 20 + + val SHOOTER_PID = PIDConstants(0.00085, 0.0000011, 0.1) + + const val SHOOTER_DEFAULT_VELOCITY = 500.0 + + const val INTAKE_BOTTOM_ID = 17 + const val INTAKE_TOP_ID = 18 + + const val INTAKE_OUTPUT = 0.45 + + const val INTAKE_STALL_CURRENT = 30 + + const val VELOCITY_ERROR = 100 +} \ No newline at end of file diff --git a/src/main/java/org/rambots/config/WristConstants.kt b/src/main/java/org/rambots/config/WristConstants.kt new file mode 100644 index 0000000..58aadfb --- /dev/null +++ b/src/main/java/org/rambots/config/WristConstants.kt @@ -0,0 +1,13 @@ +package org.rambots.config + +import com.pathplanner.lib.util.PIDConstants + +object WristConstants { + + const val WRIST_ID = 21 + + val WRIST_PID = PIDConstants(0.04, 0.0, 0.0) + + const val WRIST_INTAKE_POSITION = -116.0 + +} \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/ArmSubsystem.kt b/src/main/java/org/rambots/subsystems/ArmSubsystem.kt index b3f74b0..d56a7ff 100644 --- a/src/main/java/org/rambots/subsystems/ArmSubsystem.kt +++ b/src/main/java/org/rambots/subsystems/ArmSubsystem.kt @@ -32,10 +32,14 @@ object ArmSubsystem : SubsystemBase() { setSmartCurrentLimit(40) } - private val follower = CANSparkMax(ARM_NEO_FOLLOWER_ID, CANSparkLowLevel.MotorType.kBrushless) + private val follower = CANSparkMax(ARM_NEO_FOLLOWER_ID, CANSparkLowLevel.MotorType.kBrushless).apply { + restoreFactoryDefaults() + + idleMode = CANSparkBase.IdleMode.kBrake + setSmartCurrentLimit(40) + } init { - follower.restoreFactoryDefaults() follower.follow(leader, true) } diff --git a/src/main/java/org/rambots/subsystems/ElevatorSubsystem.kt b/src/main/java/org/rambots/subsystems/ElevatorSubsystem.kt index 0a6f2fb..5220938 100644 --- a/src/main/java/org/rambots/subsystems/ElevatorSubsystem.kt +++ b/src/main/java/org/rambots/subsystems/ElevatorSubsystem.kt @@ -35,13 +35,19 @@ object ElevatorSubsystem : SubsystemBase() { setSmartCurrentLimit(40) } - private val follower = CANSparkMax(ELEVATOR_FOLLOWER_ID, CANSparkLowLevel.MotorType.kBrushless) + private val follower = CANSparkMax(ELEVATOR_FOLLOWER_ID, CANSparkLowLevel.MotorType.kBrushless).apply { + restoreFactoryDefaults() + + idleMode = CANSparkBase.IdleMode.kBrake + setSmartCurrentLimit(40) + } + init { - follower.restoreFactoryDefaults() follower.follow(leader, true) } + override fun periodic() { Logger.recordOutput("Elevator/DesiredPosition", desiredPosition) Logger.recordOutput("Elevator/Offset", offset) diff --git a/src/main/java/org/rambots/subsystems/ShooterSubsystem.kt b/src/main/java/org/rambots/subsystems/ShooterSubsystem.kt new file mode 100644 index 0000000..aa4a7da --- /dev/null +++ b/src/main/java/org/rambots/subsystems/ShooterSubsystem.kt @@ -0,0 +1,115 @@ +package org.rambots.subsystems + +import com.revrobotics.CANSparkBase +import com.revrobotics.CANSparkLowLevel +import com.revrobotics.CANSparkMax +import com.revrobotics.SparkMaxPIDController +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger +import org.rambots.config.ShooterConstants.INTAKE_BOTTOM_ID +import org.rambots.config.ShooterConstants.INTAKE_OUTPUT +import org.rambots.config.ShooterConstants.INTAKE_TOP_ID +import org.rambots.config.ShooterConstants.SHOOTER_BOTTOM_ID +import org.rambots.config.ShooterConstants.SHOOTER_DEFAULT_VELOCITY +import org.rambots.config.ShooterConstants.SHOOTER_PID +import org.rambots.config.ShooterConstants.SHOOTER_TOP_ID +import org.rambots.config.ShooterConstants.INTAKE_STALL_CURRENT + +object ShooterSubsystem : SubsystemBase() { + + var topDesiredVelocity = 0.0 + var bottomDesiredVelocity = 0.0 + + val topVelocity get() = shooterTop.encoder.velocity + val bottomVelocity get() = shooterBottom.encoder.velocity + + val shooterTop = CANSparkMax(SHOOTER_TOP_ID, CANSparkLowLevel.MotorType.kBrushless).apply { + restoreFactoryDefaults() + + idleMode = CANSparkBase.IdleMode.kCoast + + pidController.apply { + setSmartMotionMaxVelocity(5700.0, 0) + setSmartMotionMaxAccel(7000.0, 0) + + p = SHOOTER_PID.kP + i = SHOOTER_PID.kI + d = SHOOTER_PID.kD + } + } + val shooterBottom = CANSparkMax(SHOOTER_BOTTOM_ID, CANSparkLowLevel.MotorType.kBrushless).apply { + restoreFactoryDefaults() + + idleMode = CANSparkBase.IdleMode.kCoast + + pidController.apply { + setSmartMotionMaxVelocity(5700.0, 0) + setSmartMotionMaxAccel(7000.0, 0) + + p = SHOOTER_PID.kP + i = SHOOTER_PID.kI + d = SHOOTER_PID.kD + } + } + + private val intakeTopLead = CANSparkMax(INTAKE_TOP_ID, CANSparkLowLevel.MotorType.kBrushless).apply { + restoreFactoryDefaults() + idleMode = CANSparkBase.IdleMode.kBrake + } + private val intakeBottomFollower = CANSparkMax(INTAKE_BOTTOM_ID, CANSparkLowLevel.MotorType.kBrushless).apply { + restoreFactoryDefaults() + idleMode = CANSparkBase.IdleMode.kBrake + } + + val intakeCurrent get() = intakeTopLead.outputCurrent + + val intakeStalling get() = intakeCurrent > INTAKE_STALL_CURRENT + + init { + intakeBottomFollower.follow(intakeTopLead, false) + } + + fun intake() { + intakeTopLead.set(INTAKE_OUTPUT) + } + + fun reverseIntake() { + intakeTopLead.set(-INTAKE_OUTPUT) + } + + fun stopIntake() { + intakeTopLead.stopMotor() + } + + fun shoot(topVelocity: Double, bottomVelocity: Double) { + shooterTop.set(-1.0) + shooterBottom.set(-1.0) + } + + fun stopShooter() { + shooterTop.set(0.0) + shooterBottom.set(0.0) + } + + override fun periodic() { + Logger.recordOutput("Shooter/Intake/Top/Output", intakeTopLead.appliedOutput) + Logger.recordOutput("Shooter/Intake/Top/Velocity", intakeTopLead.encoder.velocity) + Logger.recordOutput("Shooter/Intake/Top/Current", intakeTopLead.outputCurrent) + Logger.recordOutput("Shooter/Intake/Top/Temperature", intakeTopLead.motorTemperature) + + Logger.recordOutput("Shooter/Intake/Bottom/Output", intakeBottomFollower.appliedOutput) + Logger.recordOutput("Shooter/Intake/Bottom/Velocity", intakeBottomFollower.encoder.velocity) + Logger.recordOutput("Shooter/Intake/Bottom/Current", intakeBottomFollower.outputCurrent) + Logger.recordOutput("Shooter/Intake/Bottom/Temperature", intakeBottomFollower.motorTemperature) + + Logger.recordOutput("Shooter/Top/Output", shooterTop.appliedOutput) + Logger.recordOutput("Shooter/Top/Velocity", shooterTop.encoder.velocity) + Logger.recordOutput("Shooter/Top/Current", shooterTop.outputCurrent ) + Logger.recordOutput("Shooter/Top/Temperature", shooterTop.motorTemperature) + + Logger.recordOutput("Shooter/Bottom/Output", shooterBottom.appliedOutput) + Logger.recordOutput("Shooter/Bottom/Velocity", shooterBottom.encoder.velocity) + Logger.recordOutput("Shooter/Bottom/Current", shooterBottom.outputCurrent) + Logger.recordOutput("Shooter/Bottom/Temperature", shooterBottom.motorTemperature) + } +} \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/SuperStructure.kt b/src/main/java/org/rambots/subsystems/SuperStructure.kt new file mode 100644 index 0000000..e601718 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/SuperStructure.kt @@ -0,0 +1,45 @@ +package org.rambots.subsystems + +import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.Commands.waitUntil +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import edu.wpi.first.wpilibj2.command.WaitCommand +import org.rambots.commands.ArmPositionCommand +import org.rambots.commands.ElevatorPositionCommand +import org.rambots.commands.ShooterCommand +import org.rambots.commands.WristPositionCommand + +object SuperStructure { + + private val homeCommand get() = SequentialCommandGroup( + ArmPositionCommand ({ 0.0 }, { it < 5.0 }), + WristPositionCommand( { 0.0 }, { it < 5.0 }), + ElevatorPositionCommand ({ 0.0 }, { true }) + ) + + val homeCommandFromIntake get() = SequentialCommandGroup( + Commands.runOnce({ ShooterSubsystem.stopIntake()}, ShooterSubsystem), + WristPositionCommand({ 0.0 }, { it > -55.0 }), + ElevatorPositionCommand({ 0.0 }, { it < 5.0 }) + ) + + val intakeCommand get() = SequentialCommandGroup( + ArmPositionCommand ({ 0.0 }, { it < 5.0 }), + ElevatorPositionCommand ({ -48.0 }, { it < -30.0 }), + WristPositionCommand ({ -125.0 }, { true }), + Commands.runOnce ({ ShooterSubsystem.intake()}, ShooterSubsystem), + WaitCommand(0.5), + waitUntil { ShooterSubsystem.intakeStalling }, + homeCommandFromIntake + ) + + val manualShoot get() = SequentialCommandGroup( + ArmPositionCommand ({ 0.0 }, { it < 5.0 }), + WristPositionCommand({ -25.0 }, { it < -20.0 }), + ShooterCommand({2000.0}, {1500.0}, {topVelocity, bottomVelocity -> ShooterSubsystem.topVelocity >= topVelocity && ShooterSubsystem.bottomVelocity >= bottomVelocity}), + Commands.runOnce({ ShooterSubsystem.intake() }, ShooterSubsystem), + Commands.runOnce({ ShooterSubsystem.stopShooter() }, ShooterSubsystem), + Commands.runOnce({ ShooterSubsystem.stopIntake() }, ShooterSubsystem), + homeCommand + ) +} \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/WristSubsystem.kt b/src/main/java/org/rambots/subsystems/WristSubsystem.kt new file mode 100644 index 0000000..5f967d4 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/WristSubsystem.kt @@ -0,0 +1,43 @@ +package org.rambots.subsystems + +import com.revrobotics.CANSparkBase +import com.revrobotics.CANSparkLowLevel +import com.revrobotics.CANSparkMax +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger +import org.rambots.config.ArmConstants.ARM_PID +import org.rambots.config.WristConstants.WRIST_ID +import org.rambots.config.WristConstants.WRIST_PID + +object WristSubsystem : SubsystemBase() { + + var offset = 0.0 + var desiredPosition = 0.0 + offset + var position + get() = motor.encoder.position + set(value) { + motor.pidController.setReference(value, CANSparkBase.ControlType.kPosition) + } + + private val motor = CANSparkMax(WRIST_ID, CANSparkLowLevel.MotorType.kBrushless).apply { + restoreFactoryDefaults() + + idleMode = CANSparkBase.IdleMode.kBrake + pidController.apply { + p = WRIST_PID.kP + i = WRIST_PID.kI + d = WRIST_PID.kD + } + + setSmartCurrentLimit(40) + } + + override fun periodic() { + Logger.recordOutput("Wrist/DesiredPosition", desiredPosition) + Logger.recordOutput("Wrist/Offset", offset) + Logger.recordOutput("Wrist/Position", motor.encoder.position) + Logger.recordOutput("Wrist/Velocity", motor.encoder.velocity) + Logger.recordOutput("Wrist/Current", motor.outputCurrent) + } + +} \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/org/rambots/subsystems/drive/PhoenixOdometryThread.java index 600b009..bb6a6cc 100644 --- a/src/main/java/org/rambots/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/org/rambots/subsystems/drive/PhoenixOdometryThread.java @@ -19,6 +19,7 @@ import com.ctre.phoenix6.hardware.ParentDevice; import org.littletonrobotics.junction.Logger; +import java.sql.Array; import java.util.ArrayList; import java.util.List; import java.util.OptionalDouble; diff --git a/src/main/java/org/rambots/util/Alert.java b/src/main/java/org/rambots/util/Alert.java index ec32511..1d2fd5a 100644 --- a/src/main/java/org/rambots/util/Alert.java +++ b/src/main/java/org/rambots/util/Alert.java @@ -20,7 +20,8 @@ * Class for managing persistent alerts to be sent over NetworkTables. */ public class Alert { - private static Map groups = new HashMap(); + + private static final Map groups = new HashMap(); private final AlertType type; private boolean active = false; diff --git a/src/main/java/org/rambots/util/CommandUtils.kt b/src/main/java/org/rambots/util/CommandUtils.kt deleted file mode 100644 index 4fae595..0000000 --- a/src/main/java/org/rambots/util/CommandUtils.kt +++ /dev/null @@ -1,7 +0,0 @@ -package org.rambots.util - -object CommandUtils { - - fun generateErrorRange(value: Double, error: Double): ClosedRange = (value - error)..(value + error) - -} \ No newline at end of file diff --git a/src/main/java/org/rambots/util/LimelightHelpers.java b/src/main/java/org/rambots/util/LimelightHelpers.java index a1ce1e1..74e58e8 100644 --- a/src/main/java/org/rambots/util/LimelightHelpers.java +++ b/src/main/java/org/rambots/util/LimelightHelpers.java @@ -126,9 +126,6 @@ public static String getJSONDump(String limelightName) { return getLimelightNTString(limelightName, "json"); } - ///// - ///// - /** * Switch to getBotPose * diff --git a/src/main/java/org/rambots/util/LoggedTunableNumber.java b/src/main/java/org/rambots/util/LoggedTunableNumber.java index 6e3e10d..26f5c46 100644 --- a/src/main/java/org/rambots/util/LoggedTunableNumber.java +++ b/src/main/java/org/rambots/util/LoggedTunableNumber.java @@ -24,7 +24,7 @@ public class LoggedTunableNumber { private boolean hasDefault = false; private double defaultValue; private LoggedDashboardNumber dashboardNumber; - private Map lastHasChangedValues = new HashMap<>(); + private final Map lastHasChangedValues = new HashMap<>(); /** * Create a new LoggedTunableNumber