diff --git a/build.gradle b/build.gradle index e2038ef..6ab0f53 100644 --- a/build.gradle +++ b/build.gradle @@ -53,6 +53,13 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + // Enable VisualVM connection + jvmArgs.add("-Dcom.sun.management.jmxremote=true") + jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + jvmArgs.add("-Djava.rmi.server.hostname=10.36.36.2") // Replace TE.AM with team number } // Static files artifact diff --git a/src/main/java/com/frcteam3636/frc2024/Dashboard.kt b/src/main/java/com/frcteam3636/frc2024/Dashboard.kt index 0c87fef..fd8261f 100644 --- a/src/main/java/com/frcteam3636/frc2024/Dashboard.kt +++ b/src/main/java/com/frcteam3636/frc2024/Dashboard.kt @@ -18,6 +18,7 @@ object Dashboard { SmartDashboard.putBoolean("CANivore Bus OK", (talonCanStatus.isOK)) SmartDashboard.putString("CANivore Bus Status", talonCanStatus.getName()) SmartDashboard.putBoolean("Pivot Encoder OK", Shooter.Pivot.encoderConnected) + SmartDashboard.putBoolean("Pivot Stowed", Shooter.Pivot.isStowed.asBoolean) Thread.sleep(250) } } diff --git a/src/main/java/com/frcteam3636/frc2024/Robot.kt b/src/main/java/com/frcteam3636/frc2024/Robot.kt index cddbeb0..239b0c5 100644 --- a/src/main/java/com/frcteam3636/frc2024/Robot.kt +++ b/src/main/java/com/frcteam3636/frc2024/Robot.kt @@ -3,6 +3,7 @@ package com.frcteam3636.frc2024 import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain import com.frcteam3636.frc2024.subsystems.intake.Intake import com.frcteam3636.frc2024.subsystems.shooter.Shooter +import com.frcteam3636.frc2024.subsystems.shooter.speakerTranslation import com.pathplanner.lib.auto.AutoBuilder import com.pathplanner.lib.auto.NamedCommands import edu.wpi.first.hal.FRCNetComm.tInstances @@ -56,6 +57,8 @@ object Robot : LoggedRobot() { tResourceType.kResourceType_Language, tInstances.kLanguage_Kotlin, 0, WPILibVersion.Version ) + DriverStation.silenceJoystickConnectionWarning(true) + if (isReal()) { Logger.addDataReceiver(WPILOGWriter("/U")) // Log to a USB stick Logger.addDataReceiver(NT4Publisher()) // Publish data to NetworkTables @@ -63,7 +66,6 @@ object Robot : LoggedRobot() { 1, PowerDistribution.ModuleType.kRev ) // Enables power distribution logging } else { - DriverStation.silenceJoystickConnectionWarning(true) var logPath: String? = null try { logPath = LogFileUtil.findReplayLog() // Pull the replay log from AdvantageScope (or @@ -171,17 +173,17 @@ object Robot : LoggedRobot() { // Polar driving -// Trigger(joystickLeft::getTrigger) -// .whileTrue( -// Commands.defer({ -// Drivetrain.driveWithJoystickPointingTowards( -// joystickLeft, -// DriverStation.getAlliance() -// .orElse(DriverStation.Alliance.Blue) -// .speakerTranslation.toTranslation2d() -// ) -// }, setOf(Drivetrain)) -// ) + Trigger(joystickLeft::getTrigger) + .whileTrue( + Commands.defer({ + Drivetrain.driveWithJoystickPointingTowards( + joystickLeft, + DriverStation.getAlliance() + .orElse(DriverStation.Alliance.Blue) + .speakerTranslation.toTranslation2d() + ) + }, setOf(Drivetrain)) + ) // Follow a motion profile to the selected pivot target controller.leftTrigger() @@ -236,18 +238,16 @@ object Robot : LoggedRobot() { Commands.sequence( Commands.waitUntil(Shooter.Flywheels.atDesiredVelocity), Shooter.Feeder.feed().withTimeout(0.4).beforeStarting({ - if (Note.state == Note.State.SHOOTER) { - Note.state = Note.State.NONE - } + Note.state = Note.State.NONE }), Shooter.Pivot.followMotionProfile(Shooter.Pivot.Target.STOWED), ) ) ) - Trigger(joystickLeft::getTrigger) - .whileTrue( - Shooter.Flywheels.intake() - ) +// Trigger(joystickLeft::getTrigger) +// .whileTrue( +// Shooter.Flywheels.intake() +// ) JoystickButton(joystickLeft, 8).onTrue(Commands.runOnce({ println("Zeroing gyro.") diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt index a50e353..2fda718 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt @@ -38,7 +38,6 @@ import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.Logger import org.littletonrobotics.junction.inputs.LoggableInputs import java.util.* -import kotlin.math.PI import kotlin.math.abs // A singleton object representing the drivetrain. @@ -47,7 +46,7 @@ object Drivetrain : Subsystem, TalonFXStatusProvider { /** Unit: Percent of max robot speed */ private const val TRANSLATION_SENSITIVITY = 1.0 /** Unit: Rotations per second */ - private const val ROTATION_SENSITIVITY = 0.9 + private const val ROTATION_SENSITIVITY = 1.25 private val io = when (Robot.model) { Robot.Model.SIMULATION -> DrivetrainIOSim() @@ -246,7 +245,7 @@ object Drivetrain : Subsystem, TalonFXStatusProvider { || abs(rotationJoystick.x) > JOYSTICK_DEADBAND ) { val translationInput = - Translation2d(-translationJoystick.y, -translationJoystick.x).rotateBy(DRIVER_ROTATION) + Translation2d(-translationJoystick.y, -translationJoystick.x) chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( @@ -281,16 +280,23 @@ object Drivetrain : Subsystem, TalonFXStatusProvider { enableContinuousInput(0.0, TAU) } return run { + val translationInput = if (abs(translationJoystick.x) > JOYSTICK_DEADBAND + || abs(translationJoystick.y) > JOYSTICK_DEADBAND + ) { + Translation2d(-translationJoystick.y, -translationJoystick.x) + } else { + Translation2d() + } val magnitude = rotationPIDController.calculate( target.minus(estimatedPose.translation).angle.radians - (TAU/2), estimatedPose.rotation.radians ) chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - translationJoystick.y * FREE_SPEED.baseUnitMagnitude(), - translationJoystick.x * FREE_SPEED.baseUnitMagnitude(), - magnitude, - gyroRotation.toRotation2d(), + translationInput.x * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, + translationInput.y * FREE_SPEED.baseUnitMagnitude() * TRANSLATION_SENSITIVITY, + -magnitude, + gyroRotation.toRotation2d() ) } } @@ -298,11 +304,7 @@ object Drivetrain : Subsystem, TalonFXStatusProvider { fun zeroGyro() { // When zeroing the gyro happens, the robot is facing the speaker-side. // This is a different direction on the red vs. blue sides. - gyroRotation = if (DriverStation.getAlliance() == Optional.of(DriverStation.Alliance.Red)) { - Rotation3d(0.0, 0.0, PI) - } else { - Rotation3d() - } + gyroRotation = Rotation3d() } @Suppress("unused") diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/PivotIO.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/PivotIO.kt index 871ee3f..e141f33 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/PivotIO.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/PivotIO.kt @@ -3,7 +3,6 @@ package com.frcteam3636.frc2024.subsystems.shooter import com.ctre.phoenix6.StatusSignal import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC -import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC import com.ctre.phoenix6.controls.VoltageOut import com.ctre.phoenix6.signals.GravityTypeValue @@ -200,7 +199,7 @@ class PivotIOKraken : PivotIO { override fun pivotToAndMove(position: Rotation2d, velocity: Rotation2d) { Logger.recordOutput("Shooter/Pivot/Position Setpoint", position) - val leftControl = PositionTorqueCurrentFOC(0.0).apply { + val leftControl = MotionMagicTorqueCurrentFOC(0.0).apply { Slot = 0 Position = position.rotations } @@ -264,7 +263,7 @@ class PivotIOKraken : PivotIO { const val PROFILE_JERK = 80.0 val HARDSTOP_OFFSET: Rotation2d = Rotation2d.fromDegrees(-27.0) - val ABSOLUTE_ENCODER_OFFSET: Rotation2d = Rotation2d.fromDegrees(229.5) + HARDSTOP_OFFSET + val ABSOLUTE_ENCODER_OFFSET: Rotation2d = Rotation2d.fromDegrees(343.3) + HARDSTOP_OFFSET } override val talonCANStatuses = listOf(leftMotor.version, rightMotor.version) diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/Shooter.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/Shooter.kt index 653b9fa..b6a6444 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/Shooter.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/Shooter.kt @@ -188,6 +188,8 @@ object Shooter { var target: Target = Target.AIM + var numTicks: Int = 0 + private val processedAbsoluteEncoderPosition get() = @@ -196,7 +198,9 @@ object Shooter { val encoderConnected get() = inputs.absoluteEncoderConnected override fun periodic() { - val currentZero = zeroPos.getDouble(PivotIOKraken.ABSOLUTE_ENCODER_OFFSET.radians) + numTicks++ +// println(numTicks) +// val currentZero = zeroPos.getDouble(PivotIOKraken.ABSOLUTE_ENCODER_OFFSET.radians) // io.offset = Rotation2d(currentZero) Logger.recordOutput("Shooter/Pivot/Offset Constant", PivotIOKraken.ABSOLUTE_ENCODER_OFFSET) @@ -205,7 +209,10 @@ object Shooter { Logger.processInputs("Shooter/Pivot", inputs) val encoderPos = processedAbsoluteEncoderPosition - io.setPivotPosition(encoderPos) + if (numTicks == 20) { + io.setPivotPosition(encoderPos) + numTicks = 0 + } Logger.recordOutput("Shooter/Pivot/Processed Encoder Position", encoderPos) Logger.recordOutput("Shooter/Pivot/Required Offset", -inputs.uncorrectedEncoderPosition)