fix: invert right encoder direction

FRC3636 · Dec 13, 2024 · 636d6c2 · 636d6c2
1 parent 41f14f5
commit 636d6c2
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 23 deletions.
diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Dashboard.kt b/src/main/kotlin/com/frcteam3636/frc2024/Dashboard.kt
@@ -71,12 +71,9 @@ object Dashboard {
             .withSize(6, 2)
             .withPosition(0, 2)
 
-        tab.addBoolean("NavX OK") { Diagnostics.latest.navXConnected }
+        tab.addBoolean("Gyro OK") { Diagnostics.latest.gyroConnected }
             .withPosition(10, 0)
             .withSize(2, 1)
-        tab.addBoolean("Cameras OK") { Diagnostics.latest.navXConnected }
-            .withPosition(10, 1)
-            .withSize(2, 1)
         tab.addBoolean("CAN Bus OK") { Diagnostics.latest.canStatus.transmitErrorCount == 0 && Diagnostics.latest.canStatus.receiveErrorCount == 0 }
             .withPosition(10, 2)
             .withSize(2, 1)

diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Diagnostics.kt b/src/main/kotlin/com/frcteam3636/frc2024/Diagnostics.kt
@@ -17,8 +17,7 @@ import edu.wpi.first.wpilibj.RobotController
  */
 data class Diagnostics(
     val batteryFull: Boolean,
-    val navXConnected: Boolean,
-    val camerasConnected: Boolean,
+    val gyroConnected: Boolean,
     val errorStatusCodes: Map<String, StatusCode>,
     val canStatus: CANStatus,
 ) {
@@ -29,8 +28,7 @@ data class Diagnostics(
         val messages = mutableListOf<ElasticNotification>()
 
         // Add alerts for each suspicious diagnostic condition
-        if (!navXConnected) messages.add(ElasticNotification("NavX disconnected", "The naxX gyro has disconnected!"))
-        if (!camerasConnected) messages.add(ElasticNotification("Camera disconnected", "A camera has disconnected!"))
+        if (!gyroConnected) messages.add(ElasticNotification("IMU disconnected", "The gyro has disconnected!"))
         if (canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0) {
             messages.add(ElasticNotification("roboRIO CAN Errors", "CAN bus errors detected!"))
         }
@@ -56,8 +54,7 @@ data class Diagnostics(
         /** The most recent diagnostics. */
         var latest = Diagnostics(
             batteryFull = true,
-            navXConnected = true,
-            camerasConnected = true,
+            gyroConnected = true,
             errorStatusCodes = emptyMap(),
             canStatus = CANStatus(),
         )
@@ -73,8 +70,7 @@ data class Diagnostics(
 
             return Diagnostics(
                 batteryFull = RobotController.getBatteryVoltage() >= FULL_BATTERY_VOLTAGE,
-                navXConnected = Drivetrain.inputs.gyroConnected,
-                camerasConnected = Drivetrain.allCamerasConnected,
+                gyroConnected = Drivetrain.inputs.gyroConnected,
                 errorStatusCodes = errorCodes,
                 canStatus = RobotController.getCANStatus(),
             )

diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt
@@ -13,6 +13,7 @@ import com.frcteam3636.version.BUILD_DATE
 import com.frcteam3636.version.DIRTY
 import com.frcteam3636.version.GIT_BRANCH
 import com.frcteam3636.version.GIT_SHA
+import edu.wpi.first.cameraserver.CameraServer
 import edu.wpi.first.hal.FRCNetComm.tInstances
 import edu.wpi.first.hal.FRCNetComm.tResourceType
 import edu.wpi.first.hal.HAL
@@ -74,6 +75,8 @@ object Robot : PatchedLoggedRobot() {
         configureAutos()
         configureBindings()
         configureDashboard()
+
+        CameraServer.startAutomaticCapture();
     }
 
     /** Start logging or pull replay logs from a file */
@@ -195,13 +198,22 @@ object Robot : PatchedLoggedRobot() {
 //                Indexer.indexBalloon()
 //            )
 
-//        //Outtake
-//        controller.leftBumper()
-//            .whileTrue(
-//                Commands.parallel(
-//                    Intake.outtake(),
-//                )
-//            )
+        //Outtake
+        controller.leftBumper()
+            .whileTrue(
+                Commands.parallel(
+                    Intake.outtake(),
+                    Indexer.outtakeBalloon()
+                )
+            )
+
+        controller.rightBumper()
+            .whileTrue(
+                Commands.parallel(
+                    Intake.intake(),
+                    Indexer.indexBalloon()
+                )
+            )
 
         //SysId
 //        controller.leftBumper()

diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt
@@ -63,8 +63,8 @@ class ArmIOReal: ArmIO {
         inputs.leftPosition = offsetlessLeftPosition + LEFT_ZERO_OFFSET
         Logger.recordOutput("/Arm/Required Left Offset", offsetlessLeftPosition.negate())
 
-        val offsetlessRightPosition = Rotations.of(-rightAbsoluteEncoder.get() * CHAIN_GEAR_RATIO)
-        inputs.rightPosition = offsetlessLeftPosition + RIGHT_ZERO_OFFSET
+        val offsetlessRightPosition = Rotations.of(rightAbsoluteEncoder.get() * CHAIN_GEAR_RATIO)
+        inputs.rightPosition = offsetlessRightPosition + RIGHT_ZERO_OFFSET
         Logger.recordOutput("/Arm/Required Right Offset", offsetlessRightPosition.negate())
 
         inputs.leftRelativePosition = Rotations.of(leftMotor.position.value)
@@ -172,8 +172,8 @@ class ArmIOReal: ArmIO {
         private const val PROFILE_JERK = 1.0
         private const val PROFILE_VELOCITY = 1.0
 
-        val LEFT_ZERO_OFFSET = Radians.of(1.05)
-        val RIGHT_ZERO_OFFSET = Radians.of(1.05)
+        val LEFT_ZERO_OFFSET = Radians.of(1.09)
+        val RIGHT_ZERO_OFFSET = Radians.of(-0.99)
     }
 
 }