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

Commit

Permalink
feat: working arm, intake bindings
Browse files Browse the repository at this point in the history
  • Loading branch information
@doinkythederp
doinkythederp committed Dec 13, 2024
1 parent 636d6c2 commit 2f2f34a
Show file tree
Hide file tree
Showing 8 changed files with 119 additions and 124 deletions.
105 changes: 22 additions & 83 deletions src/main/kotlin/com/frcteam3636/frc2024/Robot.kt
View file
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 @@ -14,6 +13,8 @@ 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 @@ -25,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 @@ -76,7 +75,8 @@ object Robot : PatchedLoggedRobot() {
configureBindings()
configureDashboard()

CameraServer.startAutomaticCapture();
val camera = CameraServer.startAutomaticCapture()
camera.setResolution(240, 240)
}

/** Start logging or pull replay logs from a file */
Expand Down Expand Up @@ -153,95 +153,30 @@ object Robot : PatchedLoggedRobot() {

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

// (The button with the yellow tape on it)
JoystickButton(joystickLeft, 8).onTrue(Commands.runOnce({
println("Zeroing gyro.")
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.15)
// .whileTrue(
// Indexer.indexBalloon()
// )
// controller.y()
// .debounce(0.15)
// .whileTrue(
// Indexer.outtakeBalloon()
// )
//
// controller.b()
// .debounce(0.150)
// .whileTrue(
// Indexer.outtakeBalloon()
// )
//
// controller.y()
// .debounce(0.150)
// .whileTrue(
// Indexer.indexBalloon()
// )

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

controller.rightBumper()
.whileTrue(
Commands.parallel(
Intake.intake(),
Indexer.indexBalloon()
)
)
.whileTrue(Intake.intake())

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



//SysId
// controller.leftBumper()
// .onTrue(Commands.runOnce(SignalLogger::start))
//
// controller.rightBumper()
// .onTrue(Commands.runOnce(SignalLogger::stop))
//
// controller.y()
// .whileTrue(
// Commands.sequence(
// Commands.print("Starting quasistatic"),
// Arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward)
// )
// )
//
//
// controller.a()
// .whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse))
//
// controller.x()
// .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kReverse))
//
// controller.b()
// .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kForward))

// Arm positions
controller.a()
controller.y()
.onTrue(
Arm.moveToPosition(Arm.Position.Stowed)
)
Expand All @@ -252,10 +187,14 @@ object Robot : PatchedLoggedRobot() {
)


controller.y()
controller.a()
.onTrue(
Arm.moveToPosition(Arm.Position.Lower)
)
Arm.moveToPosition(Arm.Position.Lower))


controller.b()
.whileTrue(Arm.coastMode().ignoringDisable(true))

}

/** Add data to the driver station dashboard. */
Expand Down
32 changes: 29 additions & 3 deletions src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,14 @@ 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.sin


private const val SECONDS_BETWEEN_ARM_UPDATES = 1.0
Expand Down Expand Up @@ -83,15 +86,38 @@ object Arm : Subsystem {
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
io.pivotToPosition(setpoint)
}, {
io.pivotToPosition(inputs.leftPosition)
})!!
}

fun sysIdQuasistatic(direction: Direction) =
sysID.quasistatic(direction)!!

fun sysIdDynamic(direction: Direction) =
sysID.dynamic(direction)!!

enum class Position(val angle: Measure<Angle>) {
Stowed(Radians.of(0.0)),
PickUp(Radians.of(-0.6)),
Lower(Radians.of(-0.4))
/** 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))
}
}
41 changes: 37 additions & 4 deletions src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
package com.frcteam3636.frc2024.subsystems.arm
import com.ctre.phoenix6.configs.MotorOutputConfigs
import com.ctre.phoenix6.configs.TalonFXConfiguration
import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC
import com.ctre.phoenix6.controls.VoltageOut
Expand All @@ -22,6 +23,8 @@ import org.team9432.annotation.Logged

@Logged
open class ArmInputs {
var reference = Radians.zero()!!

var rightRelativePosition = Radians.zero()!!
var leftRelativePosition = Radians.zero()!!
var leftPosition = Radians.zero()!!
Expand All @@ -46,6 +49,8 @@ interface ArmIO {
fun setVoltage(volts: Measure<Voltage>)

fun updatePosition(left: Measure<Angle>, right: Measure<Angle>)

fun setCoastMode(enabled: Boolean)
}


Expand All @@ -58,6 +63,10 @@ class ArmIOReal: ArmIO {
private val leftAbsoluteEncoder = DutyCycleEncoder(DigitalInput(0))
private val rightAbsoluteEncoder = DutyCycleEncoder(DigitalInput(1))

init {
Logger.recordOutput("/Arm/CoastMode", false)
}

override fun updateInputs(inputs: ArmInputs) {
val offsetlessLeftPosition = Rotations.of(-leftAbsoluteEncoder.get() * CHAIN_GEAR_RATIO)
inputs.leftPosition = offsetlessLeftPosition + LEFT_ZERO_OFFSET
Expand All @@ -74,10 +83,12 @@ class ArmIOReal: ArmIO {
inputs.rightAbsoluteEncoderConnected = rightAbsoluteEncoder.isConnected

inputs.leftVelocity = RotationsPerSecond.of(leftMotor.velocity.value)
inputs.rightVelocity = RotationsPerSecond.of(rightMotor.position.value)
inputs.rightVelocity = RotationsPerSecond.of(rightMotor.velocity.value)

inputs.leftCurrent = Volts.of(leftMotor.motorVoltage.value)
inputs.rightCurrent = Volts.of(rightMotor.motorVoltage.value)

inputs.reference = Rotations.of(leftMotor.closedLoopReference.value)
}

override fun updatePosition(left: Measure<Angle>, right: Measure<Angle>) {
Expand All @@ -87,7 +98,7 @@ class ArmIOReal: ArmIO {

private val pivotControl = MotionMagicTorqueCurrentFOC(0.0)
override fun pivotToPosition(position: Measure<Angle>) {
Logger.recordOutput("Shooter/Pivot/Position Setpoint", position)
Logger.recordOutput("Arm/Position Setpoint", position)

val control = pivotControl.apply {
Slot = 0
Expand All @@ -97,6 +108,19 @@ class ArmIOReal: ArmIO {
rightMotor.setControl(control)
}

override fun setCoastMode(enabled: Boolean) {
Logger.recordOutput("/Arm/CoastMode", enabled)
val config = MotorOutputConfigs().apply {
NeutralMode = if (enabled) {
NeutralModeValue.Coast
} else {
NeutralModeValue.Brake
}
}
leftMotor.configurator.apply(config)
rightMotor.configurator.apply(config)
}

override fun setVoltage(volts: Measure<Voltage>) {
assert(volts in Volts.of(-12.0)..Volts.of(12.0))
Logger.recordOutput("/Arm/Voltage", volts)
Expand Down Expand Up @@ -162,16 +186,17 @@ class ArmIOReal: ArmIO {

private const val CHAIN_GEAR_RATIO = 1.0 / 3.0
private const val GEAR_RATIO = 125.0 / CHAIN_GEAR_RATIO
val LEFT_PID_GAINS = PIDGains(64.138, 0.0, 59.411)
val LEFT_PID_GAINS = PIDGains(54.138, 0.0, 169.411)
val LEFT_FF_GAINS = MotorFFGains(0.3289, 42.817, 0.39107)
private const val LEFT_GRAVITY_GAIN = 0.17119
val RIGHT_PID_GAINS = PIDGains(63.339, 0.0, 59.134)
val RIGHT_PID_GAINS = PIDGains(53.339, 0.0, 169.134)
val RIGHT_FF_GAINS = MotorFFGains(0.29364, 43.77, 0.34751)
private const val RIGHT_GRAVITY_GAIN = 0.12181
private const val PROFILE_ACCELERATION = 1.0
private const val PROFILE_JERK = 1.0
private const val PROFILE_VELOCITY = 1.0


val LEFT_ZERO_OFFSET = Radians.of(1.09)
val RIGHT_ZERO_OFFSET = Radians.of(-0.99)
}
Expand Down Expand Up @@ -216,6 +241,10 @@ class ArmIOSim : ArmIO {
override fun updatePosition(left: Measure<Angle>, right: Measure<Angle>) {
// no drifting in sim so no need to update
}

override fun setCoastMode(enabled: Boolean) {

}
}

class ArmIOPrototype: ArmIO {
Expand All @@ -231,4 +260,8 @@ class ArmIOPrototype: ArmIO {

override fun updatePosition(left: Measure<Angle>, right: Measure<Angle>) {
}

override fun setCoastMode(enabled: Boolean) {

}
}
5 changes: 5 additions & 0 deletions src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Drivetrain.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -220,6 +220,11 @@ object Drivetrain : Subsystem, Sendable {
drive(translationJoystick.translation2d, rotationJoystick.translation2d)
}

fun driveWithOneJoystick(joystick: Joystick): Command =
run {
drive(joystick.translation2d, Translation2d(0.0, -joystick.z))
}

@Suppress("unused")
fun driveWithController(controller: CommandXboxController): Command =
run {
Expand Down
7 changes: 5 additions & 2 deletions src/main/kotlin/com/frcteam3636/frc2024/subsystems/drivetrain/Module.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,15 +133,18 @@ class DrivingTalon(id: CTREDeviceId) : DrivingMotor {

}

private val positionSignal = inner.position
private val velocitySignal = inner.velocity

init {
Robot.statusSignals[id.name] = inner.version
}

override val position: Measure<Distance>
get() = Meters.of(inner.position.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE.`in`(Meters))
get() = Meters.of(positionSignal.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE.`in`(Meters))

override var velocity: Measure<Velocity<Distance>>
get() = MetersPerSecond.of(inner.velocity.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE.`in`(Meters))
get() = MetersPerSecond.of(velocitySignal.value * DRIVING_GEAR_RATIO_TALON * WHEEL_CIRCUMFERENCE.`in`(Meters))
set(value) {
inner.setControl(VelocityTorqueCurrentFOC(value.`in`(MetersPerSecond) / DRIVING_GEAR_RATIO_TALON / WHEEL_CIRCUMFERENCE.`in`(Meters)))
}
Expand Down
Loading

0 comments on commit 2f2f34a

Please sign in to comment.