Merge pull request #91 from FRC3636/storm-surge

Storm surge

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

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)
             }
         }

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,14 +57,15 @@ 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
             PowerDistribution(
                 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.")

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,28 +280,31 @@ 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()
             )
         }
     }
 
     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")

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)

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)