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

Commit

Permalink
More small changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@Daniel1464
Daniel1464 committed Jan 3, 2024
1 parent 3f4009d commit 9b4db3b
Show file tree
Hide file tree
Showing 4 changed files with 74 additions and 148 deletions.
6 changes: 4 additions & 2 deletions src/main/kotlin/frc/chargers/hardware/motorcontrol/ctre/ChargerTalonFX.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ public fun falcon(
// 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
// factory defaults configs on startup when factoryDefault = true
if (factoryDefault) {
configurator.apply(CTRETalonFXConfiguration(), 0.02)
}
Expand Down Expand Up @@ -133,6 +133,7 @@ public open class ChargerTalonFX(deviceNumber: Int, canBus: String = "rio"):
}
if (configHasChanged){
configurator.apply(currentSlotConfigs)
println("PID status has been updated.")
}
velocityRequest.Velocity = target.inUnit(rotations/seconds)
setControl(velocityRequest)
Expand All @@ -147,6 +148,7 @@ public open class ChargerTalonFX(deviceNumber: Int, canBus: String = "rio"):
if (currentPIDConstants() != pidConstants){
setPIDConstants(pidConstants)
configurator.apply(currentSlotConfigs)
println("PID status has been updated.")
}
if (absoluteEncoder == null){
positionRequest.Position = target.inUnit(rotations)
Expand Down Expand Up @@ -180,7 +182,7 @@ public open class ChargerTalonFX(deviceNumber: Int, canBus: String = "rio"):
val baseTalonFXConfig = CTRETalonFXConfiguration()
configurator.refresh(baseTalonFXConfig)
applyChanges(baseTalonFXConfig, configuration)
configurator.apply(baseTalonFXConfig,0.050).updateConfigStatus()
configurator.apply(baseTalonFXConfig,0.02).updateConfigStatus()

configuration.apply{
positionUpdateFrequency?.let{
Expand Down
44 changes: 20 additions & 24 deletions src/main/kotlin/frc/chargers/hardware/subsystems/swervedrive/EncoderHolonomicDrivetrain.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics
import edu.wpi.first.math.system.plant.DCMotor
import edu.wpi.first.wpilibj.RobotBase
import edu.wpi.first.wpilibj2.command.SubsystemBase
import frc.chargerlibexternal.frc4481.HeadingCorrector
import frc.chargers.advantagekitextensions.LoggableInputsProvider
import frc.chargers.constants.drivetrain.*
import frc.chargers.hardware.sensors.VisionPoseSupplier
Expand All @@ -25,17 +24,14 @@ import frc.chargers.wpilibextensions.geometry.twodimensional.asRotation2d
import frc.chargers.wpilibextensions.kinematics.*
import org.littletonrobotics.junction.Logger.*

@PublishedApi
internal val TOP_LEFT_MODULE_INPUTS: LoggableInputsProvider = LoggableInputsProvider("Drivetrain(Swerve)/TopLeftModule")

@PublishedApi
internal val TOP_RIGHT_MODULE_INPUTS: LoggableInputsProvider = LoggableInputsProvider("Drivetrain(Swerve)/TopRightModule")
private val topLeftModInputs: LoggableInputsProvider = LoggableInputsProvider("Drivetrain(Swerve)/TopLeftModule")

@PublishedApi
internal val BOTTOM_LEFT_MODULE_INPUTS: LoggableInputsProvider = LoggableInputsProvider("Drivetrain(Swerve)/BottomLeftModule")
private val topRightModInputs: LoggableInputsProvider = LoggableInputsProvider("Drivetrain(Swerve)/TopRightModule")

@PublishedApi
internal val BOTTOM_RIGHT_MODULE_INPUTS: LoggableInputsProvider = LoggableInputsProvider("Drivetrain(Swerve)/BottomRightModule")
private val bottomLeftModInputs: LoggableInputsProvider = LoggableInputsProvider("Drivetrain(Swerve)/BottomLeftModule")

private val bottomRightModInputs: LoggableInputsProvider = LoggableInputsProvider("Drivetrain(Swerve)/BottomRightModule")

/**
* A convenience function used to create an [EncoderHolonomicDrivetrain],
Expand All @@ -59,25 +55,25 @@ public fun EncoderHolonomicDrivetrain(
return EncoderHolonomicDrivetrain(
topLeft = RioPIDSwerveModule(
ModuleIOSim(
TOP_LEFT_MODULE_INPUTS,
topLeftModInputs,
turnGearbox, driveGearbox, hardwareData.turnGearRatio, hardwareData.driveGearRatio, hardwareData.turnInertiaMoment, hardwareData.driveInertiaMoment
), controlData
),
topRight = RioPIDSwerveModule(
ModuleIOSim(
TOP_RIGHT_MODULE_INPUTS,
topRightModInputs,
turnGearbox, driveGearbox, hardwareData.turnGearRatio, hardwareData.driveGearRatio, hardwareData.turnInertiaMoment, hardwareData.driveInertiaMoment
), controlData
),
bottomLeft = RioPIDSwerveModule(
ModuleIOSim(
BOTTOM_LEFT_MODULE_INPUTS,
bottomLeftModInputs,
turnGearbox, driveGearbox, hardwareData.turnGearRatio, hardwareData.driveGearRatio, hardwareData.turnInertiaMoment, hardwareData.driveInertiaMoment
), controlData
),
bottomRight = RioPIDSwerveModule(
ModuleIOSim(
BOTTOM_RIGHT_MODULE_INPUTS,
bottomRightModInputs,
turnGearbox, driveGearbox, hardwareData.turnGearRatio, hardwareData.driveGearRatio, hardwareData.turnInertiaMoment, hardwareData.driveInertiaMoment
), controlData
),
Expand Down Expand Up @@ -112,7 +108,7 @@ public fun EncoderHolonomicDrivetrain(
}

topLeft = OnboardPIDSwerveModule(
TOP_LEFT_MODULE_INPUTS,
topLeftModInputs,
controlData,
turnMotors.topLeft,
turnEncoders.topLeft,
Expand All @@ -121,7 +117,7 @@ public fun EncoderHolonomicDrivetrain(
)

topRight = OnboardPIDSwerveModule(
TOP_RIGHT_MODULE_INPUTS,
topRightModInputs,
controlData,
turnMotors.topRight,
turnEncoders.topRight,
Expand All @@ -130,7 +126,7 @@ public fun EncoderHolonomicDrivetrain(
)

bottomLeft = OnboardPIDSwerveModule(
BOTTOM_LEFT_MODULE_INPUTS,
bottomLeftModInputs,
controlData,
turnMotors.bottomLeft,
turnEncoders.bottomLeft,
Expand All @@ -139,7 +135,7 @@ public fun EncoderHolonomicDrivetrain(
)

bottomRight = OnboardPIDSwerveModule(
BOTTOM_RIGHT_MODULE_INPUTS,
bottomRightModInputs,
controlData,
turnMotors.bottomRight,
turnEncoders.bottomRight,
Expand All @@ -150,7 +146,7 @@ public fun EncoderHolonomicDrivetrain(
}else{
topLeft = RioPIDSwerveModule(
ModuleIOReal(
TOP_LEFT_MODULE_INPUTS,
topLeftModInputs,
turnMotor = turnMotors.topLeft,
turnEncoder = turnEncoders.topLeft,
driveMotor = driveMotors.topLeft,
Expand All @@ -161,7 +157,7 @@ public fun EncoderHolonomicDrivetrain(

topRight = RioPIDSwerveModule(
ModuleIOReal(
TOP_RIGHT_MODULE_INPUTS,
topRightModInputs,
turnMotor = turnMotors.topRight,
turnEncoder = turnEncoders.topRight,
driveMotor = driveMotors.topRight,
Expand All @@ -172,7 +168,7 @@ public fun EncoderHolonomicDrivetrain(

bottomLeft = RioPIDSwerveModule(
ModuleIOReal(
BOTTOM_LEFT_MODULE_INPUTS,
bottomLeftModInputs,
turnMotor = turnMotors.bottomLeft,
turnEncoder = turnEncoders.bottomLeft,
driveMotor = driveMotors.bottomLeft,
Expand All @@ -183,7 +179,7 @@ public fun EncoderHolonomicDrivetrain(

bottomRight = RioPIDSwerveModule(
ModuleIOReal(
BOTTOM_RIGHT_MODULE_INPUTS,
bottomRightModInputs,
turnMotor = turnMotors.bottomRight,
turnEncoder = turnEncoders.bottomRight,
driveMotor = driveMotors.bottomRight,
Expand Down Expand Up @@ -235,7 +231,7 @@ public class EncoderHolonomicDrivetrain(
private val distanceOffset: Distance = averageEncoderPosition() * wheelRadius
private var angleOffset = Angle(0.0)

private val headingCorrector = HeadingCorrector()
//private val headingCorrector = HeadingCorrector()
private val mostReliableHeading: Angle
get() = (gyro?.heading ?: this.heading).inputModulus(0.0.degrees..360.degrees)
/*
Expand Down Expand Up @@ -451,7 +447,7 @@ public class EncoderHolonomicDrivetrain(
// extension function defined in chargerlib
speeds = speeds.discretize(driftRate = controlData.openLoopDiscretizationRate)
// corrects heading when robot is moving at high velocities; not sure if heading should be negative or positive here
speeds = headingCorrector.correctHeading(speeds,mostReliableHeading.asRotation2d())
//speeds = headingCorrector.correctHeading(speeds,mostReliableHeading.asRotation2d())
val stateArray = kinematics.toSwerveModuleStates(speeds)

currentModuleStates = ModuleStateGroup(
Expand Down Expand Up @@ -499,7 +495,7 @@ public class EncoderHolonomicDrivetrain(
// extension function defined in chargerlib
newSpeeds = newSpeeds.discretize(driftRate = controlData.closedLoopDiscretizationRate)
// corrects heading when robot is moving at high velocities; not sure if heading should be negative or positive here
newSpeeds = headingCorrector.correctHeading(newSpeeds,-mostReliableHeading.asRotation2d())
//newSpeeds = headingCorrector.correctHeading(newSpeeds,-mostReliableHeading.asRotation2d())
val stateArray = kinematics.toSwerveModuleStates(newSpeeds)
currentModuleStates = ModuleStateGroup(
topLeftState = stateArray[0],
Expand Down
46 changes: 6 additions & 40 deletions src/main/kotlin/frc/chargers/hardware/subsystems/swervedrive/SwerveEncoders.kt
View file
Original file line number Diff line number Diff line change
@@ -1,13 +1,10 @@
package frc.chargers.hardware.subsystems.swervedrive

import com.batterystaple.kmeasure.quantities.Angle
import frc.chargers.hardware.configuration.HardwareConfigurable
import frc.chargers.hardware.configuration.HardwareConfiguration
import frc.chargers.hardware.sensors.encoders.PositionEncoder
import frc.chargers.hardware.sensors.encoders.absolute.CANcoderConfiguration
import frc.chargers.hardware.sensors.encoders.absolute.ChargerCANcoder
import frc.chargers.hardware.sensors.withOffset
import kotlin.internal.LowPriorityInOverloadResolution

/**
* Constructs an instance of [SwerveEncoders] with CTRE CANcoders.
Expand Down Expand Up @@ -38,12 +35,13 @@ public inline fun swerveCANcoders(
useAbsoluteSensor: Boolean,
configure: CANcoderConfiguration.() -> Unit = {}
): SwerveEncoders {
topLeft.configure(CANcoderConfiguration().apply(configure))
topRight.configure(CANcoderConfiguration().apply(configure))
bottomLeft.configure(CANcoderConfiguration().apply(configure))
bottomRight.configure(CANcoderConfiguration().apply(configure))
val config = CANcoderConfiguration().apply(configure)
topLeft.configure(config)
topRight.configure(config)
bottomLeft.configure(config)
bottomRight.configure(config)

return if(useAbsoluteSensor){
return if (useAbsoluteSensor){
SwerveEncoders(
topLeft.absolute,
topRight.absolute,
Expand All @@ -58,40 +56,8 @@ public inline fun swerveCANcoders(
bottomRight
)
}

}

@LowPriorityInOverloadResolution
public fun <E, C: HardwareConfiguration> SwerveEncoders(
topLeft: E,
topRight: E,
bottomLeft: E,
bottomRight: E,
configuration: C? = null
): SwerveEncoders where E: PositionEncoder, E: HardwareConfigurable<C> =
SwerveEncoders(
topLeft.apply{
if(configuration != null){
configure(configuration)
}
},
topRight.apply{
if(configuration != null){
configure(configuration)
}
},
bottomLeft.apply{
if(configuration != null){
configure(configuration)
}
},
bottomRight.apply{
if(configuration != null){
configure(configuration)
}
}
)


public data class SwerveEncoders(
val topLeft: PositionEncoder,
Expand Down
Loading

0 comments on commit 9b4db3b

Please sign in to comment.