diff --git a/src/main/java/org/rambots/BuildConstants.java b/src/main/java/org/rambots/BuildConstants.java index 041ca90..d1efb49 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 = 9; - public static final String GIT_SHA = "e235e0327a4c8b1b2be006c6080e1000250747ba"; - public static final String GIT_DATE = "2024-03-05 18:20:38 PST"; + 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 String GIT_BRANCH = "james/swerve"; - public static final String BUILD_DATE = "2024-03-06 15:22:24 PST"; - public static final long BUILD_UNIX_TIME = 1709767344100L; + public static final String BUILD_DATE = "2024-03-09 19:35:02 PST"; + public static final long BUILD_UNIX_TIME = 1710041702135L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/org/rambots/RobotContainer.kt b/src/main/java/org/rambots/RobotContainer.kt index c7fd0f2..9fe9f69 100644 --- a/src/main/java/org/rambots/RobotContainer.kt +++ b/src/main/java/org/rambots/RobotContainer.kt @@ -22,6 +22,8 @@ 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.drive.* import org.rambots.subsystems.drive.DriveConstants.moduleConfigs import org.rambots.subsystems.vision.AprilTagVision @@ -30,6 +32,7 @@ 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 /** @@ -74,7 +77,7 @@ object RobotContainer { // new ModuleIOTalonFX(2), // new ModuleIOTalonFX(3)); // flywheel = Flywheel(FlywheelIOTalonFX()) - aprilTagVision = AprilTagVision(AprilTagVisionIOLimelight("limelight")) + aprilTagVision = AprilTagVision(AprilTagVisionIOLimelight("limelight-three")) } Constants.Mode.SIM -> { @@ -160,11 +163,25 @@ object RobotContainer { * instantiating a [GenericHID] or one of its subclasses ([ ] or [XboxController]), and then passing it to a [ ]. */ private fun configureButtonBindings() { - drive.defaultCommand = DriveCommands.joystickDrive(drive, driveController, { -controller.leftY }, { -controller.leftX }, { -controller.rightX }) + drive.defaultCommand = DriveCommands.joystickDrive( + drive, + driveController, + { (-controller.leftY).pow(3.0) }, + { (-controller.leftX).pow(3.0) }, + { -controller.rightX } + ) + ArmSubsystem.defaultCommand = ArmPositionCommand { ArmSubsystem.desiredPosition } + ElevatorSubsystem.defaultCommand = ElevatorPositionCommand { ElevatorSubsystem.desiredPosition } 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 { -70.0 }) + // controller.a().whileTrue(PathFinderAndFollow(driveController.driveModeType)) // controller diff --git a/src/main/java/org/rambots/commands/ArmPositionCommand.kt b/src/main/java/org/rambots/commands/ArmPositionCommand.kt new file mode 100644 index 0000000..a20588b --- /dev/null +++ b/src/main/java/org/rambots/commands/ArmPositionCommand.kt @@ -0,0 +1,29 @@ +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() { + + constructor(angle: () -> Double) : this(angle, angle) + constructor(angle: Double) : this ({angle}) + + init { + addRequirements(ArmSubsystem) + } + + override fun initialize() { + ArmSubsystem.desiredPosition = angle.invoke() + } + + override fun execute() { + ArmSubsystem.position = angle.invoke() + } + + override fun isFinished(): Boolean { + return generateErrorRange(ArmSubsystem.position, ARM_ACCEPTABLE_ERROR).contains(finishState.invoke()) + } + +} \ 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 a74fd1c..11b0914 100644 --- a/src/main/java/org/rambots/commands/DriveCommands.java +++ b/src/main/java/org/rambots/commands/DriveCommands.java @@ -45,7 +45,7 @@ public static Command joystickDrive( DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier) { - return Commands.run( + return Commands.runOnce( () -> { // Apply deadband double linearMagnitude = diff --git a/src/main/java/org/rambots/commands/ElevatorPositionCommand.kt b/src/main/java/org/rambots/commands/ElevatorPositionCommand.kt new file mode 100644 index 0000000..a0d646f --- /dev/null +++ b/src/main/java/org/rambots/commands/ElevatorPositionCommand.kt @@ -0,0 +1,29 @@ +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() { + + constructor(position: () -> Double) : this(position, position) + constructor(position: Double) : this ({position}) + + init { + addRequirements(ElevatorSubsystem) + } + + override fun initialize() { + ElevatorSubsystem.desiredPosition = position.invoke() + } + + override fun execute() { + ElevatorSubsystem.position = position.invoke() + } + + override fun isFinished(): Boolean { + return generateErrorRange(ElevatorSubsystem.position, ARM_ACCEPTABLE_ERROR).contains(finishState.invoke()) + } + +} \ 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 new file mode 100644 index 0000000..9f66a8d --- /dev/null +++ b/src/main/java/org/rambots/config/ArmConstants.kt @@ -0,0 +1,63 @@ +package org.rambots.config + +import com.pathplanner.lib.util.PIDConstants + +object ArmConstants { + /** Arm Configuration */ + const val ARM_NEO_LEADER_ID = 13 + const val ARM_NEO_FOLLOWER_ID = 14 + + val ARM_PID = PIDConstants(0.02, 0.0, 0.0) + + val ARM_ACCEPTABLE_ERROR = 0.5 + + + + /** Elevator PID */ + const val ElevatorKP = 0.2 + const val ElevatorKI = 0.0 + const val ElevatorKD = 0.0 + + /** Intake Configuration */ + const val IntakeMotorOneCANID = 17 + const val IntakeMotorTwoCANID = 18 + const val IntakePower = -0.2 + const val IntakeCurrent = 20 + + /** Shooter Configuration */ + const val TopShooterCANID = 19 + const val BottomShooterCANID = 20 + + /** Shooter PID */ + const val ShooterKP = 0.00035 + const val ShooterKD = 0.15 + + /** Wrist Configuration */ + const val WristCANID = 21 + + /** Wrist PID */ + const val WristKP = 0.2 + const val WristKI = 0.0 + const val WristKD = 0.0 + + /** Intake Positions */ + const val ArmIntakePosition = 0.0 + const val WristIntakePosition = -63.0 + const val ElevatorIntakePosition = -50.0 + + /** Climb Positions */ + const val ArmClimbPosition = -70.0 + const val ElevatorClimbPosition = -70.0 + const val ElevatorRetractPosition = 0.0 + + /** Manual Arm Controls */ + // place holders + const val MaximumAngle = 90 + const val MinimumAngle = 30 + const val ArmIdlePosition = -45.0 + + /** Limelight */ + const val LimelightMountAngleDegrees = 25.0 + const val LimelightLensHeightInches = 20.0 + const val SpeakerGoalHeightInches = 50.0 +} \ No newline at end of file diff --git a/src/main/java/org/rambots/config/ElevatorConstants.kt b/src/main/java/org/rambots/config/ElevatorConstants.kt new file mode 100644 index 0000000..fcbac03 --- /dev/null +++ b/src/main/java/org/rambots/config/ElevatorConstants.kt @@ -0,0 +1,12 @@ +package org.rambots.config + +import com.pathplanner.lib.util.PIDConstants + +object ElevatorConstants { + + const val ELEVATOR_LEADER_ID = 15 + const val ELEVATOR_FOLLOWER_ID = 16 + + val ELEVATOR_PID = PIDConstants(0.2, 0.0, 0.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 new file mode 100644 index 0000000..b3f74b0 --- /dev/null +++ b/src/main/java/org/rambots/subsystems/ArmSubsystem.kt @@ -0,0 +1,50 @@ +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_NEO_FOLLOWER_ID +import org.rambots.config.ArmConstants.ARM_NEO_LEADER_ID +import org.rambots.config.ArmConstants.ARM_PID + +object ArmSubsystem : SubsystemBase() { + + var offset = 0.0 + var desiredPosition = 0.0 + offset + var position + get() = leader.encoder.position + set(value) { + leader.pidController.setReference(value, CANSparkBase.ControlType.kPosition) + } + + private val leader = CANSparkMax(ARM_NEO_LEADER_ID, CANSparkLowLevel.MotorType.kBrushless).apply { + restoreFactoryDefaults() + + idleMode = CANSparkBase.IdleMode.kBrake + pidController.apply { + p = ARM_PID.kP + i = ARM_PID.kI + d = ARM_PID.kD + } + + setSmartCurrentLimit(40) + } + + private val follower = CANSparkMax(ARM_NEO_FOLLOWER_ID, CANSparkLowLevel.MotorType.kBrushless) + + init { + follower.restoreFactoryDefaults() + follower.follow(leader, true) + } + + override fun periodic() { + Logger.recordOutput("Arm/DesiredPosition", desiredPosition) + Logger.recordOutput("Arm/Offset", offset) + Logger.recordOutput("Arm/Position", leader.encoder.position) + Logger.recordOutput("Arm/Velocity", leader.encoder.velocity) + Logger.recordOutput("Arm/Current", leader.outputCurrent) + } + +} \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/ElevatorSubsystem.kt b/src/main/java/org/rambots/subsystems/ElevatorSubsystem.kt new file mode 100644 index 0000000..0a6f2fb --- /dev/null +++ b/src/main/java/org/rambots/subsystems/ElevatorSubsystem.kt @@ -0,0 +1,53 @@ +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_NEO_FOLLOWER_ID +import org.rambots.config.ArmConstants.ARM_NEO_LEADER_ID +import org.rambots.config.ArmConstants.ARM_PID +import org.rambots.config.ElevatorConstants.ELEVATOR_FOLLOWER_ID +import org.rambots.config.ElevatorConstants.ELEVATOR_LEADER_ID +import org.rambots.config.ElevatorConstants.ELEVATOR_PID + +object ElevatorSubsystem : SubsystemBase() { + + var offset = 0.0 + var desiredPosition = 0.0 + offset + var position + get() = leader.encoder.position + set(value) { + leader.pidController.setReference(value, CANSparkBase.ControlType.kPosition) + } + + private val leader = CANSparkMax(ELEVATOR_LEADER_ID, CANSparkLowLevel.MotorType.kBrushless).apply { + restoreFactoryDefaults() + + idleMode = CANSparkBase.IdleMode.kBrake + pidController.apply { + p = ELEVATOR_PID.kP + i = ELEVATOR_PID.kI + d = ELEVATOR_PID.kD + } + + setSmartCurrentLimit(40) + } + + private val follower = CANSparkMax(ELEVATOR_FOLLOWER_ID, CANSparkLowLevel.MotorType.kBrushless) + + init { + follower.restoreFactoryDefaults() + follower.follow(leader, true) + } + + override fun periodic() { + Logger.recordOutput("Elevator/DesiredPosition", desiredPosition) + Logger.recordOutput("Elevator/Offset", offset) + Logger.recordOutput("Elevator/Position", leader.encoder.position) + Logger.recordOutput("Elevator/Velocity", leader.encoder.velocity) + Logger.recordOutput("Elevator/Current", leader.outputCurrent) + } + +} \ No newline at end of file diff --git a/src/main/java/org/rambots/util/CommandUtils.kt b/src/main/java/org/rambots/util/CommandUtils.kt new file mode 100644 index 0000000..4fae595 --- /dev/null +++ b/src/main/java/org/rambots/util/CommandUtils.kt @@ -0,0 +1,7 @@ +package org.rambots.util + +object CommandUtils { + + fun generateErrorRange(value: Double, error: Double): ClosedRange = (value - error)..(value + error) + +} \ No newline at end of file