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
Open

Main bot code #29

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
11b2e60
fix: correct can ids, resolve command loop overruns
doinkythederp Dec 11, 2024
8d64bbc
fix: PDH CAN ID
Dec 11, 2024
b326f2b
fix: remove negative multiplier
Dec 12, 2024
9edc483
feat: guessing and checking
Taimorioka Dec 12, 2024
41f14f5
feat: tune pid
Gavin-Niederman Dec 12, 2024
636d6c2
fix: invert right encoder direction
doinkythederp Dec 13, 2024
2f2f34a
feat: working arm, intake bindings
doinkythederp Dec 13, 2024
74dd3f1
fix: last minute changes that fix the robot
Dec 13, 2024
6a8ddc3
fix: last minute changes that fix the robot
Dec 14, 2024
2b0b1de
refactor: add period argument to followWave
Dec 14, 2024
c3b9f97
fix: use on-device logging
Dec 14, 2024
3267cc7
fix: resolve loop over-runs by using lower cam resolution
Dec 14, 2024
bb90e72
feat: mutable zero pose
Dec 14, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions build.gradle.kts
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,12 @@ deploy {
roborio.artifacts {
register<FRCJavaArtifact>("frcJava") {
jvmArgs.add("-ea") // Remove this flag during comp to disable asserts
// 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
setJarTask(tasks.jar)
dependsOn(tasks.assemble)
}
Expand Down
7 changes: 3 additions & 4 deletions src/main/kotlin/com/frcteam3636/frc2024/CAN.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,8 @@ enum class REVMotorControllerId(val num: Int) {
BackRightDrivingMotor(3),
FrontRightDrivingMotor(4),

IntakeMotor(12),
IndexerMotor(13),

IntakeMotor(21),
}

fun CANSparkMax(id: REVMotorControllerId, type: CANSparkLowLevel.MotorType) =
Expand All @@ -35,8 +34,8 @@ enum class CTREDeviceId(val num: Int, val bus: String) {
BackLeftDrivingMotor(2, "*"),
BackRightDrivingMotor(3, "*"),
FrontRightDrivingMotor(4, "*"),
RightArmMotor(10, "*"),
LeftArmMotor(11, "*"),
RightArmMotor(11, "*"),
LeftArmMotor(10, "*"),
PigeonGyro(20, "*"),
}

Expand Down
5 changes: 1 addition & 4 deletions src/main/kotlin/com/frcteam3636/frc2024/Dashboard.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
12 changes: 4 additions & 8 deletions src/main/kotlin/com/frcteam3636/frc2024/Diagnostics.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
) {
Expand All @@ -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!"))
}
Expand All @@ -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(),
)
Expand All @@ -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(),
)
Expand Down
98 changes: 31 additions & 67 deletions src/main/kotlin/com/frcteam3636/frc2024/Robot.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
package com.frcteam3636.frc2024

import com.ctre.phoenix6.SignalLogger
import com.ctre.phoenix6.StatusSignal
import com.frcteam3636.frc2024.subsystems.arm.Arm
import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain
Expand All @@ -13,6 +12,9 @@ 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.cameraserver.CameraServerSharedStore
import edu.wpi.first.cscore.UsbCamera
import edu.wpi.first.hal.FRCNetComm.tInstances
import edu.wpi.first.hal.FRCNetComm.tResourceType
import edu.wpi.first.hal.HAL
Expand All @@ -24,14 +26,12 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler
import edu.wpi.first.wpilibj2.command.Commands
import edu.wpi.first.wpilibj2.command.button.CommandXboxController
import edu.wpi.first.wpilibj2.command.button.JoystickButton
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
import org.littletonrobotics.junction.LogFileUtil
import org.littletonrobotics.junction.Logger
import org.littletonrobotics.junction.PatchedLoggedRobot
import org.littletonrobotics.junction.networktables.NT4Publisher
import org.littletonrobotics.junction.wpilog.WPILOGReader
import org.littletonrobotics.junction.wpilog.WPILOGWriter
import java.io.File
import kotlin.io.path.Path
import kotlin.io.path.exists

Expand Down Expand Up @@ -67,18 +67,16 @@ object Robot : PatchedLoggedRobot() {
)

// Joysticks are likely to be missing in simulation, which usually isn't a problem.
DriverStation.silenceJoystickConnectionWarning(!isReal())
DriverStation.silenceJoystickConnectionWarning(true) // FIXME: re-add before comp

configureAdvantageKit()
configureSubsystems()
configureAutos()
configureBindings()
configureDashboard()

Intake.register()

//configure bindings
configureBindings()
val camera = CameraServer.startAutomaticCapture()
camera.setResolution(240, 240)
}

/** Start logging or pull replay logs from a file */
Expand All @@ -104,7 +102,7 @@ object Robot : PatchedLoggedRobot() {
// Enables power distribution logging
if (model == Model.COMPETITION) {
PowerDistribution(
1, PowerDistribution.ModuleType.kRev
62, PowerDistribution.ModuleType.kRev
)
} else {
PowerDistribution(
Expand Down Expand Up @@ -155,7 +153,7 @@ object Robot : PatchedLoggedRobot() {

/** Configure which commands each joystick button triggers. */
private fun configureBindings() {
Drivetrain.defaultCommand = Drivetrain.driveWithJoysticks(joystickLeft, joystickRight)
Drivetrain.defaultCommand = Drivetrain.driveWithOneJoystick(joystickLeft)
Indexer.defaultCommand = Indexer.autoRun()

// (The button with the yellow tape on it)
Expand All @@ -164,73 +162,39 @@ object Robot : PatchedLoggedRobot() {
Drivetrain.zeroGyro()
}).ignoringDisable(true))

// //Intake
// controller.a()
// .debounce(0.150)
// .whileTrue(
// Intake.outtake()
// )
//
// controller.x()
// .debounce(0.150)
// .whileTrue(
// Intake.intake()
// )
//
// controller.b()
// .debounce(0.150)
// .whileTrue(
// Indexer.outtakeBalloon()
// )
//
// controller.y()
// .debounce(0.150)
// .whileTrue(
// Indexer.indexBalloon()
// )

// //Outtake
// controller.leftBumper()
// .whileTrue(
// Commands.parallel(
// Intake.outtake(),
// )
// )

//SysId
// Intake
controller.leftBumper()
.onTrue(Commands.runOnce(SignalLogger::start))
.whileTrue(Intake.intake())

controller.rightBumper()
.onTrue(Commands.runOnce(SignalLogger::stop))
.whileTrue(Intake.intake())

controller.rightTrigger().whileTrue(Indexer.indexBalloon())
controller.leftTrigger().whileTrue(Indexer.outtakeBalloon())




// Arm positions
controller.y()
.whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward))
.onTrue(
Arm.moveToPosition(Arm.Position.Stowed)
)

controller.x()
.onTrue(
Arm.moveToPosition(Arm.Position.PickUp)
)


controller.a()
.whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse))
.onTrue(
Arm.moveToPosition(Arm.Position.Lower))

controller.b()
.whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kForward))

controller.x()
.whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kReverse))
controller.b()
.whileTrue(Arm.coastMode().ignoringDisable(true))

//Arm positions
// controller.a()
// .onTrue(
// Arm.moveToPosition(Arm.Position.Stowed)
// )
//
// controller.x()
// .onTrue(
// Arm.moveToPosition(Arm.Position.PickUp)
// )
//
// controller.y()
// .onTrue(
// Arm.moveToPosition(Arm.Position.Lower)
// )
}

/** Add data to the driver station dashboard. */
Expand Down
58 changes: 45 additions & 13 deletions src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,28 +2,27 @@ package com.frcteam3636.frc2024.subsystems.arm

import com.ctre.phoenix6.SignalLogger
import com.frcteam3636.frc2024.Robot
import com.frcteam3636.frc2024.subsystems.intake.Intake
import com.frcteam3636.frc2024.subsystems.intake.Intake.indexerAngleLigament
import com.frcteam3636.frc2024.subsystems.intake.Intake.intakeAngleLigament
import edu.wpi.first.units.Angle
import edu.wpi.first.units.Measure
import edu.wpi.first.units.Units.Degrees
import edu.wpi.first.units.Units.Radians
import edu.wpi.first.units.Units.Volts
import edu.wpi.first.wpilibj.Timer
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d
import edu.wpi.first.wpilibj.util.Color
import edu.wpi.first.wpilibj.util.Color8Bit
import edu.wpi.first.wpilibj2.command.Command
import edu.wpi.first.wpilibj2.command.StartEndCommand
import edu.wpi.first.wpilibj2.command.Subsystem
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism
import org.littletonrobotics.junction.Logger
import kotlin.math.cos
import kotlin.math.sin


private const val SECONDS_BETWEEN_ARM_UPDATES = 0.5
private const val SECONDS_BETWEEN_ARM_UPDATES = 1.0

object Arm : Subsystem {
private var io: ArmIO = when (Robot.model) {
Expand Down Expand Up @@ -53,19 +52,29 @@ object Arm : Subsystem {
Mechanism(io::setVoltage, null, this)
)

// var inSysIdUpperRange = Trigger {
// val lowerLimit = Radians.of(3.167)
// inputs.position < lowerLimit
// && inputs.leftPosition < lowerLimit
//// && inputs.rightPosition < lowerLimit
// }

private var timer = Timer().apply {
start()
}

override fun periodic() {
io.updateInputs(inputs)
Logger.processInputs("/Arm", inputs)
armAngleLigament.angle = inputs.position.`in`(Degrees)
armWristAngleLigament.angle = 90.0 - inputs.position.`in`(Degrees)
armAngleLigament.angle = inputs.leftPosition.`in`(Degrees)
armWristAngleLigament.angle = 90.0 - inputs.leftPosition.`in`(Degrees)

if (timer.advanceIfElapsed(SECONDS_BETWEEN_ARM_UPDATES) && inputs.absoluteEncoderConnected){
io.updatePosition(inputs.absoluteEncoderPosition)
if (timer.advanceIfElapsed(SECONDS_BETWEEN_ARM_UPDATES)
&& inputs.leftAbsoluteEncoderConnected
&& inputs.rightAbsoluteEncoderConnected) {
io.updatePosition(left = inputs.leftPosition, right = inputs.rightPosition)
}

Logger.recordOutput("/Arm/Mechanism", mechanism)

}
Expand All @@ -74,8 +83,27 @@ object Arm : Subsystem {
startEnd({
io.pivotToPosition(position.angle)
}, {
io.pivotToPosition(inputs.position)
io.pivotToPosition(inputs.leftPosition)
})!!

fun coastMode() =
startEnd({
io.setCoastMode(true)
}, {
io.setCoastMode(false)
})

fun followWave(center: Measure<Angle>, magnitude: Measure<Angle>): Command {
val timer = Timer().apply {
start()
}
return runEnd({
val setpoint = magnitude * sin(timer.get() / 2.0) + center
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

WTF is this? Deserves a comment to explain how giving a timer value to a sine function is used for a set point.

io.pivotToPosition(setpoint)
}, {
io.pivotToPosition(inputs.leftPosition)
})!!
}

fun sysIdQuasistatic(direction: Direction) =
sysID.quasistatic(direction)!!
Expand All @@ -84,8 +112,12 @@ object Arm : Subsystem {
sysID.dynamic(direction)!!

enum class Position(val angle: Measure<Angle>) {
Stowed(Degrees.of(135.0)),
PickUp(Degrees.of(30.0)),
Lower(Degrees.of(-15.0))
/** All the way up */
Stowed(Degrees.of(-127.0)),
// Stowed(Degrees.of(-80.0)),
/** Slightly picked up */
PickUp(Degrees.of(0.0)),
/** Ready to pick up */
Lower(Degrees.of(15.0))
}
}
Loading
Loading