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

Commit

Permalink
Nuked more stuff and other changes
Browse files Browse the repository at this point in the history
  • Loading branch information
Daniel1464 committed Jan 3, 2024
1 parent d868308 commit 3f4009d
Show file tree
Hide file tree
Showing 20 changed files with 294 additions and 519 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@ import com.batterystaple.kmeasure.quantities.*
import frc.chargers.utils.math.units.*



/**
* Represents constants for Feedforward control
* of an angular velocity targeting mechanism with no gravity.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,51 +15,58 @@ import frc.chargers.hardware.configuration.HardwareConfiguration
import frc.chargers.hardware.configuration.safeConfigure
import frc.chargers.hardware.motorcontrol.*
import frc.chargers.hardware.sensors.encoders.PositionEncoder
import frc.chargers.hardware.sensors.encoders.relative.TalonFXEncoderAdapter
import frc.chargers.hardware.sensors.encoders.ResettableEncoder
import com.ctre.phoenix6.configs.TalonFXConfiguration as CTRETalonFXConfiguration


/**
* Creates an instance of a falcon motor, controlled by a TalonFX motor controller.
* This function supports inline configuration using the configure lambda function,
* which has the context of a [TalonFXConfiguration].
*
* You do not need to manually factory default this motor, as it is factory defaulted on startup.
* This setting can be changed by setting factoryDefault = false.
* You do not need to manually factory default this motor, as it is factory defaulted on startup,
* before configuration. This setting can be changed by setting factoryDefault = false.
*/
public fun falcon(
canId: Int,
canBus: String? = null,
factoryDefault: Boolean = true
): ChargerTalonFX = when {
canBus != null -> ChargerTalonFX(canId, canBus)
else -> ChargerTalonFX(canId)
}.apply {
canBus: String = "rio",
factoryDefault: Boolean = true,
// This function block has the context of a TalonFXConfiguration,
// which means that it is basically treated as if it was called with the class itself.
// Example Usage: falcon(6){ beepOnBoot = true; neutralMode = NeutralModeValue.Brake }
configure: TalonFXConfiguration.() -> Unit = {}
): ChargerTalonFX = ChargerTalonFX(canId, canBus).apply {
// factory defaults configs on startup is factoryDefault = true
if (factoryDefault) {
configurator.apply(CTRETalonFXConfiguration(), 0.02)
}
val config = TalonFXConfiguration().apply(configure)
if (config != TalonFXConfiguration()){
configure(config)
}
}


/**
* Creates an instance of a falcon motor, controlled by a TalonFX motor controller.
* This function supports inline configuration using the configure lambda function,
* which has the context of a [TalonFXConfiguration].
*
* You do not need to manually factory default this motor, as it is factory defaulted on startup,
* before configuration. This setting can be changed by setting factoryDefault = false.
*/
public inline fun falcon(
canId: Int,
canBus: String? = null,
factoryDefault: Boolean = true,
configure: TalonFXConfiguration.() -> Unit
): ChargerTalonFX =
falcon(canId,canBus,factoryDefault).apply {
// configures the motor by "applying" a context function(configure)
// onto a configuration object, then configuring using that config object.
configure(TalonFXConfiguration().apply(configure))

public class TalonFXEncoderAdapter(
private val motorController: TalonFX
): ResettableEncoder {
private val positionSignal = motorController.position
private val velocitySignal = motorController.velocity

override fun setZero(newZero: Angle) {
val errorCode = motorController.setPosition(newZero.inUnit(rotations))
if (errorCode != StatusCode.OK){
error("When attempting to zero a talon fx, an error occurred: $errorCode")
}
}

override val angularPosition: Angle
get() = positionSignal.refresh(true).value.ofUnit(rotations)

override val angularVelocity: AngularVelocity
get() = velocitySignal.refresh(true).value.ofUnit(rotations/seconds)
}


/**
* Represents a TalonFX motor controller.
Expand All @@ -73,68 +80,72 @@ public inline fun falcon(
public open class ChargerTalonFX(deviceNumber: Int, canBus: String = "rio"):
TalonFX(deviceNumber, canBus), SmartEncoderMotorController, HardwareConfigurable<TalonFXConfiguration> {

@Suppress("LeakingThis") // Known to be safe; CTREMotorControllerEncoderAdapter ONLY uses final functions
// and does not pass around the reference to this class.
/**
* The encoder of the TalonFX.
*/
final override val encoder: TalonFXEncoderAdapter =
TalonFXEncoderAdapter(this)


private val voltageSignal = supplyVoltage
private val currentSignal = statorCurrent
private val tempSignal = deviceTemp

final override val appliedVoltage: Voltage
override val appliedVoltage: Voltage
get() = voltageSignal.refresh(true).value.ofUnit(volts)

final override val appliedCurrent: Current
override val appliedCurrent: Current
get() = currentSignal.refresh(true).value.ofUnit(amps)

final override val tempCelsius: Double
override val tempCelsius: Double
get() = tempSignal.refresh(true).value

@Suppress("LeakingThis") // Known to be safe; CTREMotorControllerEncoderAdapter ONLY uses final functions
// and does not pass around the reference to this class.
final override val encoder: TalonFXEncoderAdapter =
TalonFXEncoderAdapter(this)


private val currentSlotConfigs = Slot0Configs()
private val velocityRequest = VelocityVoltage(0.0).also{ it.Slot = 0 }
private val positionRequest = PositionVoltage(0.0).also{it.Slot = 0 }
private var currentPIDConstants = PIDConstants.None
private var currentFFConstants = AngularMotorFFConstants.None

private fun currentPIDConstants() = PIDConstants(currentSlotConfigs.kP, currentSlotConfigs.kI, currentSlotConfigs.kD)

private fun setPIDConstants(newConstants: PIDConstants){
currentSlotConfigs.kP = newConstants.kP
currentSlotConfigs.kI = newConstants.kI
currentSlotConfigs.kD = newConstants.kD
}

final override fun setAngularVelocity(

override fun setAngularVelocity(
target: AngularVelocity,
pidConstants: PIDConstants,
feedforwardConstants: AngularMotorFFConstants
) {
if (currentPIDConstants != pidConstants){
currentSlotConfigs.kP = pidConstants.kP
currentSlotConfigs.kI = pidConstants.kI
currentSlotConfigs.kD = pidConstants.kD
currentPIDConstants = pidConstants
var configHasChanged = false
if (currentPIDConstants() != pidConstants){
setPIDConstants(pidConstants)
configHasChanged = true
}
if (currentFFConstants != feedforwardConstants){
if (currentSlotConfigs.kS != feedforwardConstants.kS.inUnit(volts) || currentSlotConfigs.kV != feedforwardConstants.kV.inUnit(volts * seconds / rotations)){
currentSlotConfigs.kS = feedforwardConstants.kS.inUnit(volts)
currentSlotConfigs.kV = feedforwardConstants.kV.inUnit(volts * seconds / rotations)
currentFFConstants = feedforwardConstants
configHasChanged = true
}
if (configHasChanged){
configurator.apply(currentSlotConfigs)
}
velocityRequest.Velocity = target.inUnit(rotations/seconds)
setControl(velocityRequest)
}

private var Slot0Configs.pidConstants: PIDConstants
get() = PIDConstants(kP,kI,kD)
set(newConstants){
kP = newConstants.kP
kI = newConstants.kI
kD = newConstants.kD
}

final override fun setAngularPosition(
override fun setAngularPosition(
target: Angle,
pidConstants: PIDConstants,
absoluteEncoder: PositionEncoder?,
extraVoltage: Voltage
) {
if (currentSlotConfigs.pidConstants != pidConstants){
currentSlotConfigs.pidConstants = pidConstants
if (currentPIDConstants() != pidConstants){
setPIDConstants(pidConstants)
configurator.apply(currentSlotConfigs)
}
if (absoluteEncoder == null){
Expand All @@ -146,6 +157,8 @@ public open class ChargerTalonFX(deviceNumber: Int, canBus: String = "rio"):
setControl(positionRequest)
}



private val allConfigErrors: LinkedHashSet<StatusCode> = linkedSetOf()
private var configAppliedProperly = true
private fun StatusCode.updateConfigStatus(): StatusCode {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,15 +2,14 @@ package frc.chargers.hardware.motorcontrol.ctre

import com.batterystaple.kmeasure.quantities.*
import com.batterystaple.kmeasure.units.*
import com.ctre.phoenix.ErrorCode
import com.ctre.phoenix.motorcontrol.*
import com.ctre.phoenix.motorcontrol.can.BaseTalon
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX
import com.ctre.phoenix.sensors.CANCoder
import frc.chargers.hardware.configuration.HardwareConfigurable
import frc.chargers.hardware.configuration.HardwareConfiguration
import frc.chargers.hardware.motorcontrol.*
import frc.chargers.hardware.sensors.encoders.Encoder
import frc.chargers.hardware.sensors.encoders.relative.TalonSRXEncoderAdapter
import frc.chargers.hardware.sensors.encoders.ResettableEncoder
import kotlin.math.roundToInt

private const val TALON_SRX_ENCODER_UNITS_PER_ROTATION = 2048 // From https://docs.ctre-phoenix.com/en/latest/ch14_MCSensor.html#sensor-resolution
Expand Down Expand Up @@ -58,6 +57,36 @@ public inline fun redlineSRX(
it.configure(TalonSRXConfiguration().apply(configure))
}

public class TalonSRXEncoderAdapter(
private val ctreMotorController: IMotorController,
private val pidIndex: Int,
private val anglePerPulse: Angle
) : ResettableEncoder, IMotorController by ctreMotorController {
public constructor(
ctreMotorController: IMotorController,
pidIndex: Int,
pulsesPerRotation: Int /* Can't use Double here or both constructors will have the same JVM signature */
) : this(ctreMotorController, pidIndex, (1/pulsesPerRotation.toDouble()).ofUnit(rotations))

override fun setZero(newZero: Angle) {
val errorCode = ctreMotorController.setSelectedSensorPosition((newZero/anglePerPulse).siValue, pidIndex, DEFAULT_TIMEOUT_MS)
if (errorCode != ErrorCode.OK){
error("When setting the zero of a talon srx's encoder, an error ocurred: $errorCode")
}
}

override val angularPosition: Angle
get() = ctreMotorController.getSelectedSensorPosition(pidIndex) * anglePerPulse

override val angularVelocity: AngularVelocity
get() = ctreMotorController.getSelectedSensorVelocity(pidIndex) * anglePerPulse / timeBetweenPulses

public companion object {
private const val DEFAULT_TIMEOUT_MS = 500
private val timeBetweenPulses = 100.milli.seconds
}
}

/**
* Represents a TalonSRX motor controller.
* Includes everything in the CTRE TalonSRX class,
Expand Down Expand Up @@ -107,19 +136,6 @@ public open class ChargerTalonSRX(
configuration.selectedFeedbackCoefficients.forEach { (pidIndex, coefficient) ->
configSelectedFeedbackCoefficient(coefficient, pidIndex, TIMEOUT_MILLIS)
}
configuration.remoteFeedbackFilter?.let {
when(it) {
is BaseTalonRemoteFeedbackFilterDevice -> configRemoteFeedbackFilter(it.talon, it.remoteOrdinal,
TIMEOUT_MILLIS
)
is CANCoderRemoteFeedbackFilterDevice -> configRemoteFeedbackFilter(it.canCoder, it.remoteOrdinal,
TIMEOUT_MILLIS
)
is OtherRemoteFeedbackFilterDevice -> configRemoteFeedbackFilter(it.deviceId, it.remoteSensorSource, it.remoteOrdinal,
TIMEOUT_MILLIS
)
}
}
configuration.sensorTermFeedbackDevices.forEach { (sensorTerm, feedbackDevice) ->
configSensorTerm(sensorTerm, feedbackDevice, TIMEOUT_MILLIS)
}
Expand Down Expand Up @@ -177,7 +193,6 @@ public data class TalonSRXConfiguration(
var voltageCompensationEnabled: Boolean? = null,
val selectedFeedbackSensors: MutableMap<PIDIndex, FeedbackDevice> = mutableMapOf(),
val selectedFeedbackCoefficients: MutableMap<PIDIndex, Double> = mutableMapOf(),
var remoteFeedbackFilter: RemoteFeedbackFilterDevice? = null,
val sensorTermFeedbackDevices: MutableMap<SensorTerm, FeedbackDevice> = mutableMapOf(),
val controlFramePeriods: MutableMap<ControlFrame, Time> = mutableMapOf(),
val statusFramePeriods: MutableMap<StatusFrame, Time> = mutableMapOf(),
Expand All @@ -192,12 +207,5 @@ public data class TalonSRXConfiguration(
val customParameters: MutableMap<CustomParameterIndex, CustomParameterValue> = mutableMapOf()
): HardwareConfiguration

public sealed interface RemoteFeedbackFilterDevice
public data class CANCoderRemoteFeedbackFilterDevice(val canCoder: CANCoder, val remoteOrdinal: Int) :
RemoteFeedbackFilterDevice
public data class BaseTalonRemoteFeedbackFilterDevice(val talon: BaseTalon, val remoteOrdinal: Int) :
RemoteFeedbackFilterDevice
public data class OtherRemoteFeedbackFilterDevice(val deviceId: Int, val remoteSensorSource: RemoteSensorSource, val remoteOrdinal: Int) :
RemoteFeedbackFilterDevice

public data class LimitSwitchConfig(val type: RemoteLimitSwitchSource, val normalOpenOrClose: LimitSwitchNormal, val deviceId: Int)
Loading

0 comments on commit 3f4009d

Please sign in to comment.