diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..e0f15db --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic" +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.kt b/src/main/java/frc/robot/RobotContainer.kt index 1154a12..5c088c4 100755 --- a/src/main/java/frc/robot/RobotContainer.kt +++ b/src/main/java/frc/robot/RobotContainer.kt @@ -17,7 +17,9 @@ import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.button.CommandXboxController import frc.robot.generated.TunerConstants +import frc.robot.subsystems.ArmSubsystem import frc.robot.subsystems.CommandSwerveDrivetrain +import frc.robot.subsystems.vision.VisionSubsystem import java.util.function.Supplier class RobotContainer { @@ -35,6 +37,8 @@ class RobotContainer { /* Setting up bindings for necessary control of the swerve drive platform */ private val joystick = CommandXboxController(0) var drivetrain: CommandSwerveDrivetrain = TunerConstants.DriveTrain // drivetrain + val camera: VisionSubsystem = VisionSubsystem() + val armSubsystem: ArmSubsystem = ArmSubsystem // Slew Rate Limiters to limit acceleration of joystick inputs private val xLimiter = SlewRateLimiter(2.0) @@ -68,23 +72,25 @@ class RobotContainer { .withVelocityY(-joystick.leftX * MaxSpeed) // Drive left with negative X (left) .withRotationalRate(-joystick.rightX * MaxAngularRate) } // Drive counterclockwise with negative X (left) - - + armSubsystem.defaultCommand = armSubsystem.getCommand { joystick.rightTriggerAxis - joystick.leftTriggerAxis } joystick.a().whileTrue(drivetrain.applyRequest { brake }) - joystick.b().whileTrue(drivetrain - .applyRequest { point.withModuleDirection(Rotation2d(-joystick.leftY, -joystick.leftX)) }) + joystick.b().whileTrue( + drivetrain + .applyRequest { point.withModuleDirection(Rotation2d(-joystick.leftY, -joystick.leftX)) }, + ) // reset the field-centric heading on left bumper press joystick.leftBumper().onTrue(drivetrain.runOnce { drivetrain.seedFieldRelative() }) + // face robot toward the closest note + joystick.rightBumper().onTrue(drivetrain.applyRequest { point.withModuleDirection(camera.getNearestRotation()) }) + if (Utils.isSimulation()) { drivetrain.seedFieldRelative(Pose2d(Translation2d(), Rotation2d.fromDegrees(90.0))) } drivetrain.registerTelemetry { state: SwerveDrivetrain.SwerveDriveState -> logger.telemeterize(state) } } - - init { configureBindings() } diff --git a/src/main/java/frc/robot/generated/TunerConstants.kt b/src/main/java/frc/robot/generated/TunerConstants.kt index b33793d..111ad1a 100755 --- a/src/main/java/frc/robot/generated/TunerConstants.kt +++ b/src/main/java/frc/robot/generated/TunerConstants.kt @@ -35,7 +35,7 @@ object TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private const val kSlipCurrentA = 150.0 + private const val kSlipCurrentA = 20.0 // TODO originally 150 // Initial configs for the drive and steer motors and the CANcoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. @@ -44,8 +44,8 @@ object TunerConstants { .withCurrentLimits( CurrentLimitsConfigs() // Swerve azimuth does not require much torque output, so we can set a relatively low // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(60.0) - .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(60.0) // TODO originally 60 + .withStatorCurrentLimitEnable(true), ) private val cancoderInitialConfigs = CANcoderConfiguration() @@ -54,7 +54,7 @@ object TunerConstants { // Theoretical free speed (m/s) at 12v applied output; // This needs to be tuned to your individual robot - const val kSpeedAt12VoltsMps: Double = 9.46 + const val kSpeedAt12VoltsMps: Double = 1.0 // TODO originally 9.46 // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot @@ -70,7 +70,6 @@ object TunerConstants { private const val kCANbusName = "rio" private const val kPigeonId = 30 - // These are only used for simulation private const val kSteerInertia = 0.00001 private const val kDriveInertia = 0.001 @@ -104,7 +103,6 @@ object TunerConstants { .withSteerMotorInitialConfigs(steerInitialConfigs) .withCANcoderInitialConfigs(cancoderInitialConfigs) - // Front Left private const val kFrontLeftDriveMotorId = 6 private const val kFrontLeftSteerMotorId = 7 @@ -145,17 +143,16 @@ object TunerConstants { private const val kBackRightXPosInches = -12.125 private const val kBackRightYPosInches = -12.125 - private val FrontLeft: SwerveModuleConstants = ConstantCreator.createModuleConstants( kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, Units.inchesToMeters( - kFrontLeftXPosInches + kFrontLeftXPosInches, ), Units.inchesToMeters(kFrontLeftYPosInches), - kInvertLeftSide + kInvertLeftSide, ) .withSteerMotorInverted(kFrontLeftSteerInvert) private val FrontRight: SwerveModuleConstants = ConstantCreator.createModuleConstants( @@ -164,16 +161,22 @@ object TunerConstants { kFrontRightEncoderId, kFrontRightEncoderOffset, Units.inchesToMeters( - kFrontRightXPosInches + kFrontRightXPosInches, ), Units.inchesToMeters(kFrontRightYPosInches), - kInvertRightSide + kInvertRightSide, ) .withSteerMotorInverted(kFrontRightSteerInvert) private val BackLeft: SwerveModuleConstants = ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, Units.inchesToMeters( - kBackLeftXPosInches - ), Units.inchesToMeters(kBackLeftYPosInches), kInvertLeftSide + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + Units.inchesToMeters( + kBackLeftXPosInches, + ), + Units.inchesToMeters(kBackLeftYPosInches), + kInvertLeftSide, ) .withSteerMotorInverted(kBackLeftSteerInvert) private val BackRight: SwerveModuleConstants = ConstantCreator.createModuleConstants( @@ -182,15 +185,18 @@ object TunerConstants { kBackRightEncoderId, kBackRightEncoderOffset, Units.inchesToMeters( - kBackRightXPosInches + kBackRightXPosInches, ), Units.inchesToMeters(kBackRightYPosInches), - kInvertRightSide + kInvertRightSide, ) .withSteerMotorInverted(kBackRightSteerInvert) val DriveTrain: CommandSwerveDrivetrain = CommandSwerveDrivetrain( - DrivetrainConstants, FrontLeft, - FrontRight, BackLeft, BackRight + DrivetrainConstants, + FrontLeft, + FrontRight, + BackLeft, + BackRight, ) } diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.kt b/src/main/java/frc/robot/subsystems/ArmSubsystem.kt new file mode 100644 index 0000000..fab72ea --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.kt @@ -0,0 +1,37 @@ +package frc.robot.subsystems + +import com.ctre.phoenix6.hardware.TalonFX +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Subsystem + +object ArmSubsystem : Subsystem { + var motor: TalonFX + override fun periodic() { + // This method will be called once per scheduler run + } + + init { + // Initialize the arm subsystem + motor = TalonFX(31) + } + + fun extend(i: Double) { + motor.set(-0.4 * i) + } + + fun retract(i: Double) { + motor.set(0.4 * i) + } // i love programming + + fun getCommand(joystickTriggerValue: () -> Double): Command { + return run { + if (joystickTriggerValue.invoke() > 0.1) { + extend(joystickTriggerValue.invoke()) + } else if (joystickTriggerValue.invoke() < -0.1) { + retract( + -joystickTriggerValue.invoke(), + ) + } + } + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.kt b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.kt new file mode 100644 index 0000000..e22e8a5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.kt @@ -0,0 +1,34 @@ +package frc.robot.subsystems.vision + +import edu.wpi.first.math.geometry.Rotation2d +import edu.wpi.first.wpilibj2.command.SubsystemBase +import org.photonvision.PhotonCamera +import org.photonvision.targeting.PhotonTrackedTarget + +class VisionSubsystem : SubsystemBase() { + private var camera: PhotonCamera = PhotonCamera("ShitCam") + private lateinit var rot: Rotation2d + private var countWithoutMeasurement: Int = 0 + + override fun periodic() { + // This method will be called once per scheduler run + var result = camera.getLatestResult() + if (result.hasTargets()) { + var target: PhotonTrackedTarget = result.getBestTarget() + rot = Rotation2d.fromDegrees(target.yaw) + countWithoutMeasurement = 0 + } else { + countWithoutMeasurement++ + } + + if (countWithoutMeasurement > 25) { // method called every 20 ms, + // so 25 * 20 = 500 ms without a target before setting rotation to 0 + rot = Rotation2d.fromDegrees(0.0) + } + } + + // returns angle to nearest note relative to the front of the robot + fun getNearestRotation(): Rotation2d { + return rot + } +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..0e80a16 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,57 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2024.3.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2024.3.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2024.3.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2024.3.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2024.3.1" + } + ] +} \ No newline at end of file