Skip to content
This repository has been archived by the owner on Dec 19, 2024. It is now read-only.

Main bot code #29

Open
wants to merge 13 commits into
base: main
Choose a base branch
from
Prev Previous commit
Next Next commit
fix: invert right encoder direction
  • Loading branch information
doinkythederp committed Dec 13, 2024

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
commit 636d6c2a5263faebf450468979a784693f0b9401
5 changes: 1 addition & 4 deletions src/main/kotlin/com/frcteam3636/frc2024/Dashboard.kt
Original file line number Diff line number Diff line change
@@ -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)
12 changes: 4 additions & 8 deletions src/main/kotlin/com/frcteam3636/frc2024/Diagnostics.kt
Original file line number Diff line number Diff line change
@@ -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(),
)
26 changes: 19 additions & 7 deletions src/main/kotlin/com/frcteam3636/frc2024/Robot.kt
Original file line number Diff line number Diff line change
@@ -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()
Original file line number Diff line number Diff line change
@@ -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)
}

}
Loading