diff --git a/src/main/kotlin/frc/team449/ControllerBindings.kt b/src/main/kotlin/frc/team449/ControllerBindings.kt index 2a2ee73..f6e64ce 100644 --- a/src/main/kotlin/frc/team449/ControllerBindings.kt +++ b/src/main/kotlin/frc/team449/ControllerBindings.kt @@ -26,17 +26,17 @@ class ControllerBindings( /** Driver: https://docs.google.com/drawings/d/13W3qlIxzIh5MTraZGWON7IqwJvovVr8eNBvjq8_vYZI/edit * Operator: https://docs.google.com/drawings/d/1lF4Roftk6932jMCQthgKfoJVPuTVSgnGZSHs5j68uo4/edit */ - SCORE_L1() - SCORE_L2() - SCORE_L3() - SCORE_L4() + score_l1() + score_l2() + score_l3() + score_l4() - PREMOVE_L1() - PREMOVE_L2() - PREMOVE_L3() - PREMOVE_L4() + premove_l1() + premove_l2() + premove_l3() + premove_l4() - STOW() + stow() } private fun nonRobotBindings() { @@ -47,59 +47,59 @@ class ControllerBindings( resetGyro() } - private fun STOW() { + private fun stow() { mechanismController.rightBumper().onTrue( robot.superstructureManager.requestGoal(SuperstructureGoal.STOW) ) } - private fun SCORE_L1() { + private fun score_l1() { driveController.a().onTrue( robot.superstructureManager.requestGoal(SuperstructureGoal.L1) .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem)) ) } - private fun SCORE_L2() { + private fun score_l2() { driveController.x().onTrue( robot.superstructureManager.requestGoal(SuperstructureGoal.L2) .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem)) ) } - private fun SCORE_L3() { + private fun score_l3() { driveController.b().onTrue( robot.superstructureManager.requestGoal(SuperstructureGoal.L3) .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem)) ) } - private fun SCORE_L4() { + private fun score_l4() { driveController.y().onTrue( robot.superstructureManager.requestGoal(SuperstructureGoal.L4) .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem)) ) } - private fun PREMOVE_L1() { + private fun premove_l1() { mechanismController.a().onTrue( robot.superstructureManager.requestGoal(SuperstructureGoal.L1_PREMOVE) ) } - private fun PREMOVE_L2() { + private fun premove_l2() { mechanismController.x().onTrue( robot.superstructureManager.requestGoal(SuperstructureGoal.L2_PREMOVE) ) } - private fun PREMOVE_L3() { + private fun premove_l3() { mechanismController.b().onTrue( robot.superstructureManager.requestGoal(SuperstructureGoal.L3_PREMOVE) ) } - private fun PREMOVE_L4() { + private fun premove_l4() { mechanismController.y().onTrue( robot.superstructureManager.requestGoal(SuperstructureGoal.L4_PREMOVE) ) diff --git a/src/main/kotlin/frc/team449/Robot.kt b/src/main/kotlin/frc/team449/Robot.kt index 9c91143..72f7ee6 100644 --- a/src/main/kotlin/frc/team449/Robot.kt +++ b/src/main/kotlin/frc/team449/Robot.kt @@ -6,17 +6,19 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController import frc.team449.subsystems.RobotConstants import frc.team449.subsystems.drive.swerve.SwerveDrive import frc.team449.subsystems.drive.swerve.SwerveOrthogonalCommand -import frc.team449.subsystems.elevator.Elevator -import frc.team449.subsystems.elevator.Elevator.Companion.createElevator import frc.team449.subsystems.light.Light.Companion.createLight -import frc.team449.subsystems.pivot.Pivot -import frc.team449.subsystems.pivot.Pivot.Companion.createPivot import frc.team449.subsystems.superstructure.SuperstructureManager import frc.team449.subsystems.superstructure.SuperstructureManager.Companion.createSuperstructureManager +import frc.team449.subsystems.superstructure.elevator.Elevator +import frc.team449.subsystems.superstructure.elevator.Elevator.Companion.createElevator +import frc.team449.subsystems.superstructure.intake.Intake +import frc.team449.subsystems.superstructure.intake.Intake.Companion.createIntake +import frc.team449.subsystems.superstructure.pivot.Pivot +import frc.team449.subsystems.superstructure.pivot.Pivot.Companion.createPivot +import frc.team449.subsystems.superstructure.wrist.Wrist +import frc.team449.subsystems.superstructure.wrist.Wrist.Companion.createWrist import frc.team449.subsystems.vision.PoseSubsystem import frc.team449.subsystems.vision.PoseSubsystem.Companion.createPoseSubsystem -import frc.team449.subsystems.wrist.Wrist -import frc.team449.subsystems.wrist.Wrist.Companion.createWrist import frc.team449.system.AHRS @Logged @@ -47,6 +49,8 @@ class Robot : RobotBase() { val wrist: Wrist = createWrist() + val intake: Intake = createIntake() + val superstructureManager: SuperstructureManager = createSuperstructureManager(this) val light = createLight() diff --git a/src/main/kotlin/frc/team449/RobotLoop.kt b/src/main/kotlin/frc/team449/RobotLoop.kt index 666d15c..a818205 100644 --- a/src/main/kotlin/frc/team449/RobotLoop.kt +++ b/src/main/kotlin/frc/team449/RobotLoop.kt @@ -18,9 +18,9 @@ import frc.team449.commands.light.BreatheHue import frc.team449.commands.light.Rainbow import frc.team449.subsystems.FieldConstants import frc.team449.subsystems.drive.swerve.SwerveSim -import frc.team449.subsystems.elevator.ElevatorConstants -import frc.team449.subsystems.elevator.ElevatorFeedForward.Companion.createElevatorFeedForward -import frc.team449.subsystems.pivot.PivotFeedForward.Companion.createPivotFeedForward +import frc.team449.subsystems.superstructure.elevator.ElevatorConstants +import frc.team449.subsystems.superstructure.elevator.ElevatorFeedForward.Companion.createElevatorFeedForward +import frc.team449.subsystems.superstructure.pivot.PivotFeedForward.Companion.createPivotFeedForward import frc.team449.subsystems.vision.VisionConstants import org.littletonrobotics.urcl.URCL import kotlin.jvm.optionals.getOrDefault diff --git a/src/main/kotlin/frc/team449/commands/driveAlign/SimpleReefAlign.kt b/src/main/kotlin/frc/team449/commands/driveAlign/SimpleReefAlign.kt index 69f1b0d..91e60c0 100644 --- a/src/main/kotlin/frc/team449/commands/driveAlign/SimpleReefAlign.kt +++ b/src/main/kotlin/frc/team449/commands/driveAlign/SimpleReefAlign.kt @@ -24,10 +24,10 @@ import kotlin.math.min class SimpleReefAlign( private val drive: SwerveDrive, private val poseSubsystem: PoseSubsystem, - translationSpeedLim: Double = 0.8 * RobotConstants.MAX_LINEAR_SPEED, - translationAccelLim: Double = 7.0, + translationSpeedLim: Double = 0.825 * RobotConstants.MAX_LINEAR_SPEED, + translationAccelLim: Double = 5.75, headingSpeedLim: Double = PI, - headingAccelLim: Double = 5 * PI, + headingAccelLim: Double = 6 * PI, translationPID: Triple = Triple(7.5, 0.0, 0.0), headingPID: Triple = Triple(7.5, 0.0, 0.0), private val translationTolerance: Double = Units.inchesToMeters(0.75), diff --git a/src/main/kotlin/frc/team449/subsystems/intake/Intake.kt b/src/main/kotlin/frc/team449/subsystems/intake/Intake.kt deleted file mode 100644 index f370f57..0000000 --- a/src/main/kotlin/frc/team449/subsystems/intake/Intake.kt +++ /dev/null @@ -1,35 +0,0 @@ -package frc.team449.subsystems.intake - -import com.ctre.phoenix6.hardware.TalonFX -import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.PrintCommand -import edu.wpi.first.wpilibj2.command.SubsystemBase -import frc.team449.system.motor.createKraken - -// TODO(the entire class bru) -class Intake( - private val motor1: TalonFX, - private val motor2: TalonFX, - private val motor3: TalonFX, - private val motor4: TalonFX -) : SubsystemBase() { - - fun intakeCoral(): Command { - return PrintCommand("Intaking Coral") - } - - fun intakeAlgae(): Command { - return PrintCommand("Intaking Algae") - } - - companion object { - fun createIntake(): Intake { - val motor1 = createKraken(IntakeConstants.MOTOR_ONE_ID, IntakeConstants.MOTOR_ONE_INVERTED) - val motor2 = createKraken(IntakeConstants.MOTOR_TWO_ID, IntakeConstants.MOTOR_TWO_INVERTED) - val motor3 = createKraken(IntakeConstants.MOTOR_THREE_ID, IntakeConstants.MOTOR_THREE_INVERTED) - val motor4 = createKraken(IntakeConstants.MOTOR_FOUR_ID, IntakeConstants.MOTOR_FOUR_INVERTED) - - return Intake(motor1, motor2, motor3, motor4) - } - } -} diff --git a/src/main/kotlin/frc/team449/subsystems/intake/IntakeConstants.kt b/src/main/kotlin/frc/team449/subsystems/intake/IntakeConstants.kt deleted file mode 100644 index 3b8f398..0000000 --- a/src/main/kotlin/frc/team449/subsystems/intake/IntakeConstants.kt +++ /dev/null @@ -1,13 +0,0 @@ -package frc.team449.subsystems.intake - -object IntakeConstants { - const val MOTOR_ONE_ID = 10 // TODO(Change motor ID.) - const val MOTOR_TWO_ID = 11 // TODO(Change motor ID.) - const val MOTOR_THREE_ID = 12 // TODO(Change motor ID.) - const val MOTOR_FOUR_ID = 13 // TODO(Change motor ID.) - - const val MOTOR_ONE_INVERTED = false - const val MOTOR_TWO_INVERTED = false - const val MOTOR_THREE_INVERTED = false - const val MOTOR_FOUR_INVERTED = false -} diff --git a/src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureManager.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureManager.kt index d5f7864..39cf4fa 100644 --- a/src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureManager.kt +++ b/src/main/kotlin/frc/team449/subsystems/superstructure/SuperstructureManager.kt @@ -7,9 +7,9 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand import edu.wpi.first.wpilibj2.command.InstantCommand import frc.team449.Robot import frc.team449.subsystems.drive.swerve.SwerveDrive -import frc.team449.subsystems.elevator.Elevator -import frc.team449.subsystems.pivot.Pivot -import frc.team449.subsystems.wrist.Wrist +import frc.team449.subsystems.superstructure.elevator.Elevator +import frc.team449.subsystems.superstructure.pivot.Pivot +import frc.team449.subsystems.superstructure.wrist.Wrist class SuperstructureManager( private val elevator: Elevator, diff --git a/src/main/kotlin/frc/team449/subsystems/elevator/Elevator.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/elevator/Elevator.kt similarity index 98% rename from src/main/kotlin/frc/team449/subsystems/elevator/Elevator.kt rename to src/main/kotlin/frc/team449/subsystems/superstructure/elevator/Elevator.kt index 05b560f..2dc04d7 100644 --- a/src/main/kotlin/frc/team449/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/frc/team449/subsystems/superstructure/elevator/Elevator.kt @@ -1,4 +1,4 @@ -package frc.team449.subsystems.elevator +package frc.team449.subsystems.superstructure.elevator import com.ctre.phoenix6.BaseStatusSignal import com.ctre.phoenix6.configs.TalonFXConfiguration @@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import frc.team449.subsystems.superstructure.SuperstructureGoal -import frc.team449.subsystems.wrist.WristConstants +import frc.team449.subsystems.superstructure.wrist.WristConstants import frc.team449.system.motor.KrakenDogLog import java.util.function.Supplier import kotlin.math.abs diff --git a/src/main/kotlin/frc/team449/subsystems/elevator/ElevatorConstants.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/elevator/ElevatorConstants.kt similarity index 96% rename from src/main/kotlin/frc/team449/subsystems/elevator/ElevatorConstants.kt rename to src/main/kotlin/frc/team449/subsystems/superstructure/elevator/ElevatorConstants.kt index bd4f812..626718e 100644 --- a/src/main/kotlin/frc/team449/subsystems/elevator/ElevatorConstants.kt +++ b/src/main/kotlin/frc/team449/subsystems/superstructure/elevator/ElevatorConstants.kt @@ -1,4 +1,4 @@ -package frc.team449.subsystems.elevator +package frc.team449.subsystems.superstructure.elevator import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue diff --git a/src/main/kotlin/frc/team449/subsystems/elevator/ElevatorFeedForward.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/elevator/ElevatorFeedForward.kt similarity index 95% rename from src/main/kotlin/frc/team449/subsystems/elevator/ElevatorFeedForward.kt rename to src/main/kotlin/frc/team449/subsystems/superstructure/elevator/ElevatorFeedForward.kt index f7e2aeb..36be254 100644 --- a/src/main/kotlin/frc/team449/subsystems/elevator/ElevatorFeedForward.kt +++ b/src/main/kotlin/frc/team449/subsystems/superstructure/elevator/ElevatorFeedForward.kt @@ -1,7 +1,7 @@ -package frc.team449.subsystems.elevator +package frc.team449.subsystems.superstructure.elevator import edu.wpi.first.math.controller.ElevatorFeedforward -import frc.team449.subsystems.pivot.Pivot +import frc.team449.subsystems.superstructure.pivot.Pivot import kotlin.math.pow import kotlin.math.sign import kotlin.math.sin diff --git a/src/main/kotlin/frc/team449/subsystems/elevator/ElevatorSim.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/elevator/ElevatorSim.kt similarity index 95% rename from src/main/kotlin/frc/team449/subsystems/elevator/ElevatorSim.kt rename to src/main/kotlin/frc/team449/subsystems/superstructure/elevator/ElevatorSim.kt index 1eda0da..1f36504 100644 --- a/src/main/kotlin/frc/team449/subsystems/elevator/ElevatorSim.kt +++ b/src/main/kotlin/frc/team449/subsystems/superstructure/elevator/ElevatorSim.kt @@ -1,4 +1,4 @@ -package frc.team449.subsystems.elevator +package frc.team449.subsystems.superstructure.elevator import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.sim.TalonFXSimState diff --git a/src/main/kotlin/frc/team449/subsystems/elevator/TiltedElevatorSim.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/elevator/TiltedElevatorSim.kt similarity index 96% rename from src/main/kotlin/frc/team449/subsystems/elevator/TiltedElevatorSim.kt rename to src/main/kotlin/frc/team449/subsystems/superstructure/elevator/TiltedElevatorSim.kt index 5a7abe2..b4c78aa 100644 --- a/src/main/kotlin/frc/team449/subsystems/elevator/TiltedElevatorSim.kt +++ b/src/main/kotlin/frc/team449/subsystems/superstructure/elevator/TiltedElevatorSim.kt @@ -1,4 +1,4 @@ -package frc.team449.subsystems.elevator +package frc.team449.subsystems.superstructure.elevator import edu.wpi.first.math.Matrix import edu.wpi.first.math.VecBuilder diff --git a/src/main/kotlin/frc/team449/subsystems/superstructure/intake/Intake.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/intake/Intake.kt new file mode 100644 index 0000000..9ff63ea --- /dev/null +++ b/src/main/kotlin/frc/team449/subsystems/superstructure/intake/Intake.kt @@ -0,0 +1,72 @@ +package frc.team449.subsystems.superstructure.intake + +import com.ctre.phoenix6.hardware.TalonFX +import edu.wpi.first.units.Units.Seconds +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.SubsystemBase +import frc.team449.system.motor.createKraken + +// TODO(the entire class bru) +class Intake( + private val frontMotor: TalonFX, + private val backMotor: TalonFX +) : SubsystemBase() { + + fun intakeCoral(): Command { + return runOnce { + frontMotor.setVoltage(IntakeConstants.CORAL_INTAKE_VOLTAGE) + backMotor.setVoltage(-IntakeConstants.CORAL_INTAKE_VOLTAGE) + } + } + + fun outtakeCoral(): Command { + return runOnce { + frontMotor.setVoltage(IntakeConstants.CORAL_OUTTAKE_VOLTAGE) + backMotor.setVoltage(-IntakeConstants.CORAL_OUTTAKE_VOLTAGE) + } + } + + fun intakeAlgae(): Command { + return runOnce { + frontMotor.setVoltage(-IntakeConstants.ALGAE_INTAKE_VOLTAGE) + backMotor.setVoltage(IntakeConstants.ALGAE_INTAKE_VOLTAGE) + } + } + + fun outtakeAlgae(): Command { + return runOnce { + frontMotor.setVoltage(-IntakeConstants.ALGAE_OUTTAKE_VOLTAGE) + backMotor.setVoltage(IntakeConstants.ALGAE_OUTTAKE_VOLTAGE) + } + } + + fun stop(): Command { + return runOnce { + frontMotor.stopMotor() + backMotor.stopMotor() + } + } + + companion object { + fun createIntake(): Intake { + val frontMotor = createKraken( + id = IntakeConstants.FRONT_MOTOR_ID, + inverted = IntakeConstants.FRONT_MOTOR_INVERTED, + statorCurrentLimit = IntakeConstants.STATOR_LIMIT, + burstCurrentLimit = IntakeConstants.SUPPLY_LIMIT, + burstTimeLimit = Seconds.of(0.0), + updateFrequency = IntakeConstants.VALUE_UPDATE_FREQ + ) + + val backMotor = createKraken( + id = IntakeConstants.BACK_MOTOR_ID, + inverted = IntakeConstants.BACK_MOTOR_INVERTED, + statorCurrentLimit = IntakeConstants.STATOR_LIMIT, + burstCurrentLimit = IntakeConstants.SUPPLY_LIMIT, + burstTimeLimit = Seconds.of(0.0), + updateFrequency = IntakeConstants.VALUE_UPDATE_FREQ + ) + return Intake(frontMotor, backMotor) + } + } +} diff --git a/src/main/kotlin/frc/team449/subsystems/superstructure/intake/IntakeConstants.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/intake/IntakeConstants.kt new file mode 100644 index 0000000..1e84904 --- /dev/null +++ b/src/main/kotlin/frc/team449/subsystems/superstructure/intake/IntakeConstants.kt @@ -0,0 +1,22 @@ +package frc.team449.subsystems.superstructure.intake + +import edu.wpi.first.units.Units.Amps +import edu.wpi.first.units.Units.Hertz + +object IntakeConstants { + const val FRONT_MOTOR_ID = 10 // TODO(Change motor ID.) + const val BACK_MOTOR_ID = 11 // TODO(Change motor ID.) + + val STATOR_LIMIT = Amps.of(60.0) + val SUPPLY_LIMIT = Amps.of(30.0) + + val VALUE_UPDATE_FREQ = Hertz.of(10.0) + + const val FRONT_MOTOR_INVERTED = false + const val BACK_MOTOR_INVERTED = true + + const val CORAL_INTAKE_VOLTAGE = 3.0 + const val CORAL_OUTTAKE_VOLTAGE = -2.0 + const val ALGAE_INTAKE_VOLTAGE = 7.0 + const val ALGAE_OUTTAKE_VOLTAGE = -10.0 +} diff --git a/src/main/kotlin/frc/team449/subsystems/pivot/LengthPivotSim.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/pivot/LengthPivotSim.kt similarity index 98% rename from src/main/kotlin/frc/team449/subsystems/pivot/LengthPivotSim.kt rename to src/main/kotlin/frc/team449/subsystems/superstructure/pivot/LengthPivotSim.kt index 0007870..3475b13 100644 --- a/src/main/kotlin/frc/team449/subsystems/pivot/LengthPivotSim.kt +++ b/src/main/kotlin/frc/team449/subsystems/superstructure/pivot/LengthPivotSim.kt @@ -1,4 +1,4 @@ -package frc.team449.subsystems.pivot +package frc.team449.subsystems.superstructure.pivot import edu.wpi.first.math.MathUtil import edu.wpi.first.math.Matrix diff --git a/src/main/kotlin/frc/team449/subsystems/pivot/Pivot.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/pivot/Pivot.kt similarity index 99% rename from src/main/kotlin/frc/team449/subsystems/pivot/Pivot.kt rename to src/main/kotlin/frc/team449/subsystems/superstructure/pivot/Pivot.kt index f466c33..e231726 100644 --- a/src/main/kotlin/frc/team449/subsystems/pivot/Pivot.kt +++ b/src/main/kotlin/frc/team449/subsystems/superstructure/pivot/Pivot.kt @@ -1,4 +1,4 @@ -package frc.team449.subsystems.pivot +package frc.team449.subsystems.superstructure.pivot import com.ctre.phoenix6.BaseStatusSignal import com.ctre.phoenix6.configs.TalonFXConfiguration diff --git a/src/main/kotlin/frc/team449/subsystems/pivot/PivotConstants.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/pivot/PivotConstants.kt similarity index 96% rename from src/main/kotlin/frc/team449/subsystems/pivot/PivotConstants.kt rename to src/main/kotlin/frc/team449/subsystems/superstructure/pivot/PivotConstants.kt index c82810f..5c64f12 100644 --- a/src/main/kotlin/frc/team449/subsystems/pivot/PivotConstants.kt +++ b/src/main/kotlin/frc/team449/subsystems/superstructure/pivot/PivotConstants.kt @@ -1,4 +1,4 @@ -package frc.team449.subsystems.pivot +package frc.team449.subsystems.superstructure.pivot import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue diff --git a/src/main/kotlin/frc/team449/subsystems/pivot/PivotFeedForward.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/pivot/PivotFeedForward.kt similarity index 85% rename from src/main/kotlin/frc/team449/subsystems/pivot/PivotFeedForward.kt rename to src/main/kotlin/frc/team449/subsystems/superstructure/pivot/PivotFeedForward.kt index a997024..abbf9bb 100644 --- a/src/main/kotlin/frc/team449/subsystems/pivot/PivotFeedForward.kt +++ b/src/main/kotlin/frc/team449/subsystems/superstructure/pivot/PivotFeedForward.kt @@ -1,8 +1,8 @@ -package frc.team449.subsystems.pivot +package frc.team449.subsystems.superstructure.pivot import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap -import frc.team449.subsystems.elevator.Elevator -import frc.team449.subsystems.elevator.ElevatorConstants +import frc.team449.subsystems.superstructure.elevator.Elevator +import frc.team449.subsystems.superstructure.elevator.ElevatorConstants import kotlin.math.cos import kotlin.math.sign diff --git a/src/main/kotlin/frc/team449/subsystems/wrist/Wrist.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/wrist/Wrist.kt similarity index 98% rename from src/main/kotlin/frc/team449/subsystems/wrist/Wrist.kt rename to src/main/kotlin/frc/team449/subsystems/superstructure/wrist/Wrist.kt index 3e8ed6a..d36851e 100644 --- a/src/main/kotlin/frc/team449/subsystems/wrist/Wrist.kt +++ b/src/main/kotlin/frc/team449/subsystems/superstructure/wrist/Wrist.kt @@ -1,4 +1,4 @@ -package frc.team449.subsystems.wrist +package frc.team449.subsystems.superstructure.wrist import com.ctre.phoenix6.BaseStatusSignal import com.ctre.phoenix6.configs.TalonFXConfiguration @@ -13,7 +13,6 @@ import frc.team449.subsystems.superstructure.SuperstructureGoal import frc.team449.system.motor.KrakenDogLog import java.util.function.Supplier import kotlin.math.abs -import kotlin.math.log // TODO(the entire class bru) @Logged diff --git a/src/main/kotlin/frc/team449/subsystems/wrist/WristConstants.kt b/src/main/kotlin/frc/team449/subsystems/superstructure/wrist/WristConstants.kt similarity index 94% rename from src/main/kotlin/frc/team449/subsystems/wrist/WristConstants.kt rename to src/main/kotlin/frc/team449/subsystems/superstructure/wrist/WristConstants.kt index 3a836f7..9d28923 100644 --- a/src/main/kotlin/frc/team449/subsystems/wrist/WristConstants.kt +++ b/src/main/kotlin/frc/team449/subsystems/superstructure/wrist/WristConstants.kt @@ -1,4 +1,4 @@ -package frc.team449.subsystems.wrist +package frc.team449.subsystems.superstructure.wrist import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue diff --git a/src/main/kotlin/frc/team449/system/encoder/QuadCalibration.kt b/src/main/kotlin/frc/team449/system/encoder/QuadCalibration.kt index 4f77afe..f6a1285 100644 --- a/src/main/kotlin/frc/team449/system/encoder/QuadCalibration.kt +++ b/src/main/kotlin/frc/team449/system/encoder/QuadCalibration.kt @@ -2,12 +2,16 @@ package frc.team449.system.encoder import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Subsystem +import kotlin.math.pow +/** Intended use with REV throughbore encoder to calibrate on the low side of the absolute encoder reading wobble + * and will apply the offset to the relative quadrature output. */ class QuadCalibration( subsystem: Subsystem, private val absolute: AbsoluteEncoder, private val encoder: QuadEncoder, - private val numSamples: Int = 150 + private val numSamples: Int = 500, + private val name: String = "" ) : Command() { init { @@ -25,9 +29,61 @@ class QuadCalibration( } override fun end(interrupted: Boolean) { - samples.sort() - val angle = samples[(samples.size * .9).toInt()] + val angle = clusterAndComputeLowAverage(samples) encoder.resetPosition(angle) - println("***** Finished Calibrating Quadrature reading *****") + println("***** Finished Calibrating $name Quadrature reading, Low Side Avg of $angle*****") + } + + // K-Means clustering implementation + private fun kMeans(data: List, k: Int, maxIterations: Int = 1000): Map> { + require(k > 0) { "Number of clusters must be greater than 0." } + require(data.isNotEmpty()) { "Data must not be empty." } + + // Initialize centroids randomly from the data points + val centroids = data.shuffled().take(k).toMutableList() + + var clusters = mutableMapOf>() + repeat(maxIterations) { + // Clear the clusters for the current iteration + clusters = mutableMapOf() + + // Assign each data point to the nearest centroid + for (point in data) { + val closestCentroidIndex = centroids.indices.minByOrNull { i -> + (point - centroids[i]).pow(2) + } ?: 0 + clusters.computeIfAbsent(closestCentroidIndex) { mutableListOf() }.add(point) + } + + // Update centroids as the mean of the assigned points + var changed = false + for ((index, points) in clusters) { + val newCentroid = points.average() + if (newCentroid != centroids[index]) { + centroids[index] = newCentroid + changed = true + } + } + + // If centroids don't change, stop early + if (!changed) return clusters + } + + return clusters + } + + // Function to separate data into "high" and "low" clusters and compute the average of the low cluster + private fun clusterAndComputeLowAverage(data: List): Double { + // Run K-Means clustering with 2 clusters + val clusters = kMeans(data, k = 2) + + // Identify which cluster is "low" based on the average value of the clusters + val lowClusterIndex = clusters.entries.minByOrNull { (_, points) -> points.average() }?.key ?: 0 + + // Extract the low section and calculate the average + val lowSection = clusters[lowClusterIndex] ?: emptyList() + val lowAverage = lowSection.average() + + return lowAverage } }