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

Commit

Permalink
feat: mutable zero pose
Browse files Browse the repository at this point in the history
  • Loading branch information
drive station committed Dec 14, 2024
1 parent 3267cc7 commit bb90e72
Show file tree
Hide file tree
Showing 3 changed files with 61 additions and 15 deletions.
5 changes: 5 additions & 0 deletions src/main/kotlin/com/frcteam3636/frc2024/Robot.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -198,6 +198,11 @@ object Robot : PatchedLoggedRobot() {
controller.b()
.whileTrue(Arm.coastMode().ignoringDisable(true))

controller.button(7)
.and(controller.button(8))
.and(controller.y())
.onTrue(Arm.zeroIt().ignoringDisable(true))

}

/** Add data to the driver station dashboard. */
Expand Down
21 changes: 12 additions & 9 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 @@ -7,6 +7,7 @@ import edu.wpi.first.units.Angle
import edu.wpi.first.units.Measure
import edu.wpi.first.units.Time
import edu.wpi.first.units.Units.*
import edu.wpi.first.wpilibj.DriverStation
import edu.wpi.first.wpilibj.Timer
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d
Expand Down Expand Up @@ -53,13 +54,6 @@ 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()
}
Expand All @@ -77,7 +71,7 @@ object Arm : Subsystem {
}

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

Logger.recordOutput("/Arm/IsZeroed", io.armZeroed)
}

fun moveToPosition(position: Position) =
Expand Down Expand Up @@ -116,6 +110,15 @@ object Arm : Subsystem {
})!!
}

fun zeroIt() = runOnce {
if (DriverStation.isDisabled()) {
io.zeroHere(inputs)
print("*************** Arm Zeroed !! ***************")
} else {
print("Disable the robot to zero the arm!")
}
}

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

Expand All @@ -129,6 +132,6 @@ object Arm : Subsystem {
/** Slightly picked up */
PickUp(Degrees.of(0.0)),
/** Ready to pick up */
Lower(Degrees.of(15.0))
Lower(Degrees.of(6.0))
}
}
50 changes: 44 additions & 6 deletions src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,8 @@ import com.ctre.phoenix6.signals.NeutralModeValue
import com.frcteam3636.frc2024.CTREDeviceId
import com.frcteam3636.frc2024.Robot
import com.frcteam3636.frc2024.TalonFX
import com.frcteam3636.frc2024.utils.Elastic
import com.frcteam3636.frc2024.utils.ElasticNotification
import com.frcteam3636.frc2024.utils.math.*
import edu.wpi.first.math.system.plant.DCMotor
import edu.wpi.first.units.Angle
Expand All @@ -17,6 +19,7 @@ import edu.wpi.first.units.Units.*
import edu.wpi.first.units.Voltage
import edu.wpi.first.wpilibj.DigitalInput
import edu.wpi.first.wpilibj.DutyCycleEncoder
import edu.wpi.first.wpilibj.Preferences
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim
import org.littletonrobotics.junction.Logger
import org.team9432.annotation.Logged
Expand Down Expand Up @@ -51,6 +54,11 @@ interface ArmIO {
fun updatePosition(left: Measure<Angle>, right: Measure<Angle>)

fun setCoastMode(enabled: Boolean)

fun zeroHere(inputs: ArmInputs)

val armZeroed: Boolean
get() = true
}


Expand All @@ -61,6 +69,12 @@ class ArmIOReal: ArmIO {
private val leftAbsoluteEncoder = DutyCycleEncoder(DigitalInput(0))
private val rightAbsoluteEncoder = DutyCycleEncoder(DigitalInput(1))

private var leftZero: Measure<Angle>
private var rightZero: Measure<Angle>

override var armZeroed = true
private set

private val leftConfig = TalonFXConfiguration().apply {
Slot0.apply {
pidGains = LEFT_PID_GAINS
Expand Down Expand Up @@ -105,20 +119,32 @@ class ArmIOReal: ArmIO {
}
}

private fun warnArmNotZeroed() {
Elastic.sendAlert(ElasticNotification("ARM MUST BE ZEROED!!!",
"The arm will not work unless it is zeroed before the current match."))
}

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

leftMotor.configurator.apply(leftConfig)
rightMotor.configurator.apply(rightConfig)

if (!Preferences.containsKey("LeftArmZero") || !Preferences.containsKey("RightArmZero")) {
warnArmNotZeroed()
armZeroed = false
}
leftZero = Radians.of(Preferences.getDouble("LeftArmZero", 0.0))
rightZero = Radians.of(Preferences.getDouble("RightArmZero", 0.0))
}

override fun updateInputs(inputs: ArmInputs) {
val offsetlessLeftPosition = Rotations.of(-leftAbsoluteEncoder.get() * CHAIN_GEAR_RATIO)
inputs.leftPosition = offsetlessLeftPosition + LEFT_ZERO_OFFSET
inputs.leftPosition = offsetlessLeftPosition + leftZero
Logger.recordOutput("/Arm/Required Left Offset", offsetlessLeftPosition.negate())

val offsetlessRightPosition = Rotations.of(rightAbsoluteEncoder.get() * CHAIN_GEAR_RATIO)
inputs.rightPosition = offsetlessRightPosition + RIGHT_ZERO_OFFSET
inputs.rightPosition = offsetlessRightPosition + rightZero
Logger.recordOutput("/Arm/Required Right Offset", offsetlessRightPosition.negate())

inputs.leftRelativePosition = Rotations.of(leftMotor.position.value)
Expand All @@ -143,6 +169,7 @@ class ArmIOReal: ArmIO {

private val pivotControl = MotionMagicTorqueCurrentFOC(0.0)
override fun pivotToPosition(position: Measure<Angle>) {
if (!armZeroed) return warnArmNotZeroed()
Logger.recordOutput("Arm/Position Setpoint", position)

val control = pivotControl.apply {
Expand Down Expand Up @@ -173,8 +200,17 @@ class ArmIOReal: ArmIO {
})
}

override fun zeroHere(inputs: ArmInputs) {
armZeroed = true
leftZero = (inputs.leftPosition - leftZero).negate()
rightZero = (inputs.rightPosition - rightZero).negate()
Preferences.setDouble("LeftArmZero", leftZero.`in`(Radians))
Preferences.setDouble("RightArmZero", rightZero.`in`(Radians))
}

override fun setVoltage(volts: Measure<Voltage>) {
assert(volts in Volts.of(-12.0)..Volts.of(12.0))
if (!armZeroed) return warnArmNotZeroed()
Logger.recordOutput("/Arm/Voltage", volts)
val control = VoltageOut(volts.`in`(Volts))
// leftMotor.setControl(control)
Expand All @@ -201,10 +237,6 @@ class ArmIOReal: ArmIO {
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.08)
val RIGHT_ZERO_OFFSET = Radians.of(-2.05)
}

}
Expand Down Expand Up @@ -251,6 +283,9 @@ class ArmIOSim : ArmIO {
override fun setCoastMode(enabled: Boolean) {

}

override fun zeroHere(inputs: ArmInputs) {
}
}

class ArmIOPrototype: ArmIO {
Expand All @@ -270,4 +305,7 @@ class ArmIOPrototype: ArmIO {
override fun setCoastMode(enabled: Boolean) {

}

override fun zeroHere(inputs: ArmInputs) {
}
}

0 comments on commit bb90e72

Please sign in to comment.