diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index 35c833aa..5f8ec0d9 100644 --- a/src/main/kotlin/com/team4099/robot2023/Robot.kt +++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt @@ -11,11 +11,9 @@ import com.team4099.robot2023.util.Alert.AlertType import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.NTSafePublisher import edu.wpi.first.hal.AllianceStationID -import edu.wpi.first.wpilibj.AnalogInput import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.PowerDistribution import edu.wpi.first.wpilibj.RobotBase -import edu.wpi.first.wpilibj.RobotController import edu.wpi.first.wpilibj.livewindow.LiveWindow import edu.wpi.first.wpilibj.simulation.DriverStationSim import edu.wpi.first.wpilibj2.command.Command @@ -135,8 +133,6 @@ object Robot : LoggedRobot() { CommandScheduler.getInstance().onCommandInterrupt { command: Command -> Logger.getInstance().recordOutput("/ActiveCommands/${command.name}", false) } - - } override fun disabledExit() { diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 493cbfa1..0c8c4783 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -1,33 +1,24 @@ package com.team4099.robot2023 -import com.ctre.phoenix.motorcontrol.can.TalonFX import com.team4099.robot2023.auto.AutonomousSelector import com.team4099.robot2023.commands.AutoIntakeCommand import com.team4099.robot2023.commands.AutoScoreCommand import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand -import com.team4099.robot2023.commands.drivetrain.SwerveModuleTuningCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIO import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2 -import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModule -import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIO -import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIOFalcon import com.team4099.robot2023.subsystems.elevator.Elevator -import com.team4099.robot2023.subsystems.elevator.ElevatorIO import com.team4099.robot2023.subsystems.elevator.ElevatorIONeo import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim import com.team4099.robot2023.subsystems.gameboy.GameBoy import com.team4099.robot2023.subsystems.gameboy.GameboyIOServer import com.team4099.robot2023.subsystems.gameboy.objective.isConeNode import com.team4099.robot2023.subsystems.groundintake.GroundIntake -import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIO import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIONeo import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIOSim import com.team4099.robot2023.subsystems.led.Led @@ -36,7 +27,6 @@ import com.team4099.robot2023.subsystems.led.LedIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO import com.team4099.robot2023.subsystems.manipulator.Manipulator -import com.team4099.robot2023.subsystems.manipulator.ManipulatorIO import com.team4099.robot2023.subsystems.manipulator.ManipulatorIONeo import com.team4099.robot2023.subsystems.manipulator.ManipulatorIOSim import com.team4099.robot2023.subsystems.superstructure.Request @@ -44,7 +34,6 @@ import com.team4099.robot2023.subsystems.superstructure.Superstructure import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar import com.team4099.robot2023.util.driver.Ryan -import edu.wpi.first.wpilibj.AnalogInput import edu.wpi.first.wpilibj.RobotBase import org.littletonrobotics.junction.Logger import org.team4099.lib.smoothDeadband @@ -64,7 +53,7 @@ object RobotContainer { init { if (RobotBase.isReal()) { // Real Hardware Implementations - //drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {} + // drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {} drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal) vision = Vision( @@ -124,16 +113,14 @@ object RobotContainer { drivetrain ) + /* - /* - - drivetrain.defaultCommand = - SwerveModuleTuningCommand( - drivetrain, - { (ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) * 180).degrees }, - ) - */ - + drivetrain.defaultCommand = + SwerveModuleTuningCommand( + drivetrain, + { (ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) * 180).degrees }, + ) + */ } fun requestSuperstructureIdle() { @@ -233,11 +220,15 @@ object RobotContainer { ) ControlBoard.groundIntakeCone.whileTrue(superstructure.groundIntakeConeCommand()) - ControlBoard.autoScore.whileTrue(AutoScoreCommand(drivetrain, superstructure) - .finallyDo { Logger.getInstance().recordOutput("Auto/isAutoDriving", false) } + ControlBoard.autoScore.whileTrue( + AutoScoreCommand(drivetrain, superstructure).finallyDo { + Logger.getInstance().recordOutput("Auto/isAutoDriving", false) + } ) - ControlBoard.autoIntake.whileTrue(AutoIntakeCommand(drivetrain, superstructure) - .finallyDo { Logger.getInstance().recordOutput("Auto/isAutoDriving", false) } + ControlBoard.autoIntake.whileTrue( + AutoIntakeCommand(drivetrain, superstructure).finallyDo { + Logger.getInstance().recordOutput("Auto/isAutoDriving", false) + } ) ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand()) diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index fae01386..2820247d 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -90,8 +90,6 @@ object AutonomousSelector { autonomousModeChooser.addOption("Score Preload Cone", AutonomousMode.PRELOAD_SCORE_AUTO) - - autoTab.add("Mode", autonomousModeChooser.sendableChooser).withSize(4, 2).withPosition(2, 0) autoEngageWidgit = diff --git a/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt index aa9171d2..765b7f78 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt @@ -88,5 +88,4 @@ class AutoIntakeCommand(val drivetrain: Drivetrain, val superstructure: Superstr superstructure.groundIntakeConeCommand() ) } - } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt index 17499fe7..ae96e96f 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt @@ -2,8 +2,6 @@ package com.team4099.robot2023.commands import com.team4099.lib.math.Zone2d import com.team4099.lib.trajectory.Waypoint -import com.team4099.robot2023.RobotContainer -import com.team4099.robot2023.commands.drivetrain.DriveBrakeModeCommand import com.team4099.robot2023.commands.drivetrain.DrivePathCommand import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.FieldConstants @@ -16,17 +14,14 @@ import com.team4099.robot2023.subsystems.superstructure.Superstructure import com.team4099.robot2023.util.AllianceFlipUtil import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.Velocity2d -import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.wpilibj2.command.Commands.runOnce import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Pose2d -import org.team4099.lib.geometry.Pose3d import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.inRotation2ds import org.team4099.lib.units.perSecond @@ -46,7 +41,6 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru val setupCommand = runOnce({ - Logger.getInstance().recordOutput("Auto/isAutoDriving", true) drivePose = drivetrain.odometryPose postAlignPose = @@ -61,15 +55,16 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru ) ) - finalPose = AllianceFlipUtil.apply( Pose2d( 2.00.meters, // slightly offset in the x FieldConstants.Grids.nodeFirstY + FieldConstants.Grids.nodeSeparationY * - (if (FMSData.isBlue) superstructure.objective.nodeColumn - else 8 - superstructure.objective.nodeColumn), + ( + if (FMSData.isBlue) superstructure.objective.nodeColumn + else 8 - superstructure.objective.nodeColumn + ), 180.degrees ) ) @@ -94,18 +89,12 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru }) val breakCommand = - runOnce({ - drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) } - }) + runOnce({ drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) } }) val coastCommand = - runOnce({ - drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(false) } - }) + runOnce({ drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(false) } }) - val updatedDrivePose = runOnce({ - finalDrivePose = drivetrain.odometryPose - }) + val updatedDrivePose = runOnce({ finalDrivePose = drivetrain.odometryPose }) addCommands( setupCommand, @@ -121,7 +110,7 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru drivetrain.odometryPose.rotation.inRotation2ds ) ) + - //intermediaryWaypoints + + // intermediaryWaypoints + listOf( Waypoint( finalPose.translation.translation2d, @@ -132,9 +121,9 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru }, keepTrapping = false, flipForAlliances = false, - endVelocity= Velocity2d(0.meters.perSecond, 0.0.meters.perSecond), + endVelocity = Velocity2d(0.meters.perSecond, 0.0.meters.perSecond), tolerance = Pose2d(5.inches, 3.inches, 3.degrees), - forceRobotVelocityCheck=true + forceRobotVelocityCheck = true ), updatedDrivePose, DrivePathCommand( @@ -147,7 +136,7 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru finalDrivePose.rotation.inRotation2ds ) ) + - //intermediaryWaypoints + + // intermediaryWaypoints + listOf( Waypoint( postAlignPose.translation.translation2d, diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt index 4038a558..5a97036c 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt @@ -6,7 +6,8 @@ import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.radians import org.team4099.lib.units.perSecond -class DriveBrakeModeCommand(val drivetrain: Drivetrain, val isBreakMode: Boolean = true) : CommandBase() { +class DriveBrakeModeCommand(val drivetrain: Drivetrain, val isBreakMode: Boolean = true) : + CommandBase() { init { addRequirements(drivetrain) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index 632cf45e..07d110eb 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -333,9 +333,8 @@ class DrivePathCommand( trajCurTime = Clock.fpgaTime - trajStartTime return endPathOnceAtReference && (!keepTrapping || swerveDriveController.atReference()) && - trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds && ( - drivetrain.fieldVelocity.magnitude < 0.1.meters.perSecond || !forceRobotVelocityCheck - ) + trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds && + (drivetrain.fieldVelocity.magnitude < 0.1.meters.perSecond || !forceRobotVelocityCheck) } override fun end(interrupted: Boolean) { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt index c1c95cf9..de4d688a 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt @@ -4,8 +4,6 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj2.command.CommandBase import org.littletonrobotics.junction.Logger -import java.util.function.DoubleSupplier -import java.util.function.Supplier class TeleopDriveCommand( val driver: DriverProfile, diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index 3dc82a9c..ab4566ea 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -1,6 +1,5 @@ package com.team4099.robot2023.config.constants -import edu.wpi.first.wpilibj.AnalogInput import org.team4099.lib.units.base.seconds import org.team4099.lib.units.milli @@ -25,9 +24,6 @@ object Constants { val LOOP_PERIOD_TIME = 20.milli.seconds val POWER_DISTRIBUTION_HUB_ID = 1 - - - enum class GamePiece { CUBE, CONE, diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index f278b73d..9d11e466 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -77,10 +77,10 @@ object DrivetrainConstants { val DRIVE_STATOR_THRESHOLD_CURRENT_LIMIT = 80.0.amps val DRIVE_STATOR_TRIGGER_THRESHOLD_TIME = 1.0.seconds - val FRONT_LEFT_MODULE_ZERO = 1.3.radians + 180.degrees + 180.degrees - val FRONT_RIGHT_MODULE_ZERO = 4.49.radians + 180.degrees + 180.degrees - val BACK_LEFT_MODULE_ZERO = 3.22.radians + 180.degrees + 180.degrees - val BACK_RIGHT_MODULE_ZERO = 0.99.radians - 180.degrees + 180.degrees + val FRONT_LEFT_MODULE_ZERO = 1.3.radians + 180.degrees + 180.degrees + val FRONT_RIGHT_MODULE_ZERO = 4.49.radians + 180.degrees + 180.degrees + val BACK_LEFT_MODULE_ZERO = 3.22.radians + 180.degrees + 180.degrees + val BACK_RIGHT_MODULE_ZERO = 0.99.radians - 180.degrees + 180.degrees val STEERING_COMPENSATION_VOLTAGE = 10.volts val DRIVE_COMPENSATION_VOLTAGE = 12.volts @@ -109,7 +109,8 @@ object DrivetrainConstants { val AUTO_POS_KDX: DerivativeGain> get() { if (RobotBase.isReal()) { - return (0.1.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond // todo: 0.25 + return (0.1.meters.perSecond / (1.0.meters.perSecond)) + .metersPerSecondPerMetersPerSecond // todo: 0.25 } else { return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond } @@ -135,7 +136,8 @@ object DrivetrainConstants { val AUTO_POS_KDY: DerivativeGain> get() { if (RobotBase.isReal()) { - return (0.025.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond // todo: 0.25 + return (0.025.meters.perSecond / (1.0.meters.perSecond)) + .metersPerSecondPerMetersPerSecond // todo: 0.25 } else { return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt index bc72113b..30730ae1 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt @@ -38,64 +38,64 @@ object FieldConstants { // Pranav's Home (Active) -// val homeAprilTags: List = -// listOf( -// AprilTag( -// 1, -// Pose3d( -// 40.inches, -// (104.125).inches, -// (18.22).inches, -// Rotation3d(0.0.radians, 0.0.radians, 0.0.radians) -// ) -// ), -// AprilTag(5, Pose3d((1).centi.meters, (87.5).inches, (89.3).centi.meters, Rotation3d())), -// AprilTag( -// 3, -// Pose3d( -// (42.875).inches, -// (113.25).inches, -// (18.22).inches, -// Rotation3d(0.0.radians, 0.0.radians, 0.0.radians) -// ) -// ), -// AprilTag( -// 4, -// Pose3d( -// (409.5).inches, -// (85.5).inches, -// (27.833).inches, -// Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians) -// ) -// ), -// AprilTag( -// 6, -// Pose3d( -// (40.45).inches, -// (174.19).inches, // FIRST's diagram has a typo (it says 147.19) -// (18.22).inches, -// Rotation3d() -// ) -// ), -// AprilTag( -// 0, -// Pose3d( -// (1).centi.meters, -// (87.5).inches + 274.8.centi.meters, -// (91.5).centi.meters, -// Rotation3d() -// ) -// ), -// AprilTag( -// 8, -// Pose3d( -// (0).inches, -// (0).inches, -// (0).inches, -// Rotation3d() -// ) -// ) -// ) + // val homeAprilTags: List = + // listOf( + // AprilTag( + // 1, + // Pose3d( + // 40.inches, + // (104.125).inches, + // (18.22).inches, + // Rotation3d(0.0.radians, 0.0.radians, 0.0.radians) + // ) + // ), + // AprilTag(5, Pose3d((1).centi.meters, (87.5).inches, (89.3).centi.meters, Rotation3d())), + // AprilTag( + // 3, + // Pose3d( + // (42.875).inches, + // (113.25).inches, + // (18.22).inches, + // Rotation3d(0.0.radians, 0.0.radians, 0.0.radians) + // ) + // ), + // AprilTag( + // 4, + // Pose3d( + // (409.5).inches, + // (85.5).inches, + // (27.833).inches, + // Rotation3d(0.0.radians, 0.0.radians, Math.PI.radians) + // ) + // ), + // AprilTag( + // 6, + // Pose3d( + // (40.45).inches, + // (174.19).inches, // FIRST's diagram has a typo (it says 147.19) + // (18.22).inches, + // Rotation3d() + // ) + // ), + // AprilTag( + // 0, + // Pose3d( + // (1).centi.meters, + // (87.5).inches + 274.8.centi.meters, + // (91.5).centi.meters, + // Rotation3d() + // ) + // ), + // AprilTag( + // 8, + // Pose3d( + // (0).inches, + // (0).inches, + // (0).inches, + // Rotation3d() + // ) + // ) + // ) // Church (Inactive) @@ -160,7 +160,7 @@ object FieldConstants { // Rotation3d(0.0.degrees, 0.0.degrees, 180.degrees))) // ) - //church tags + // church tags val homeAprilTags: List = listOf( AprilTag(1, Pose3d((40.45).inches, (108.19).inches, (45).centi.meters, Rotation3d())), @@ -177,7 +177,6 @@ object FieldConstants { // AprilTag locations (do not flip for red alliance) - val aprilTags: List = listOf( AprilTag( @@ -233,8 +232,6 @@ object FieldConstants { AprilTag(8, Pose3d((40.45).inches, (42.19).inches, (18.22).inches, Rotation3d())) ) - - val wpilibAprilTags = if (Constants.Universal.REAL_FIELD) aprilTags.map { it.apriltagWpilib } else homeAprilTags.map { it.apriltagWpilib } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt index bfce7c8c..441fee36 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt @@ -64,6 +64,9 @@ object VisionConstants { val VERITCAL_FOV = 45.7.degrees val HIGH_TAPE_HEIGHT = 43.875.inches + 1.inches val MID_TAPE_HEIGHT = 23.905.inches + 1.inches + val CONE_HEIGHT = 12.8125.inches + val CUBE_HEIGHT = 9.625.inches + val LL_TRANSFORM = Transform3d( Translation3d(1.1438.inches, 10.3966.inches, 12.9284.inches), diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt index e4a4cf4e..dd92bcd1 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt @@ -35,7 +35,6 @@ import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.inMetersPerSecondPerSecond import org.team4099.lib.units.perSecond import kotlin.math.IEEErem -import kotlin.math.abs import kotlin.math.withSign class SwerveModule(val io: SwerveModuleIO) { @@ -132,24 +131,24 @@ class SwerveModule(val io: SwerveModuleIO) { } Logger.getInstance().processInputs(io.label, inputs) - Logger.getInstance() - .recordOutput( - "${io.label}/driveSpeedSetpointMetersPerSecond", - if (!shouldInvert) speedSetPoint.inMetersPerSecond - else -speedSetPoint.inMetersPerSecond - ) - Logger.getInstance() - .recordOutput( - "${io.label}/driveAccelSetpointMetersPerSecondSq", - accelerationSetPoint.inMetersPerSecondPerSecond - ) - Logger.getInstance() - .recordOutput("${io.label}/steeringSetpointRadians", steeringSetPoint.inRadians) - Logger.getInstance() - .recordOutput( - "${io.label}/steeringValueDegreesWithMod", - inputs.steeringPosition.inDegrees.IEEErem(360.0) - ) + Logger.getInstance() + .recordOutput( + "${io.label}/driveSpeedSetpointMetersPerSecond", + if (!shouldInvert) speedSetPoint.inMetersPerSecond + else -speedSetPoint.inMetersPerSecond + ) + Logger.getInstance() + .recordOutput( + "${io.label}/driveAccelSetpointMetersPerSecondSq", + accelerationSetPoint.inMetersPerSecondPerSecond + ) + Logger.getInstance() + .recordOutput("${io.label}/steeringSetpointRadians", steeringSetPoint.inRadians) + Logger.getInstance() + .recordOutput( + "${io.label}/steeringValueDegreesWithMod", + inputs.steeringPosition.inDegrees.IEEErem(360.0) + ) Logger.getInstance().recordOutput("SwerveModule/SpeedSetPoint", speedSetPoint.inMetersPerSecond) Logger.getInstance().recordOutput("SwerveModule/SteeringSetPoint", steeringSetPoint.inRadians) @@ -240,13 +239,19 @@ class SwerveModule(val io: SwerveModuleIO) { optimizedState .speedMetersPerSecond .meters - .perSecond/* * Math.cos(abs((optimizedState.angle.angle - inputs.steeringPosition).inRadians) ) */ // consider desaturating wheel speeds here if it doesn't work + .perSecond /* * Math.cos(abs((optimizedState.angle.angle - inputs.steeringPosition).inRadians) ) */ // consider desaturating wheel speeds here if it doesn't work // from drivetrain ) Logger.getInstance() .recordOutput("${io.label}/steeringSetpointOptimized", optimizedState.angle.degrees) } else { - io.setOpenLoop(desiredState.angle.angle, desiredState.speedMetersPerSecond.meters.perSecond /* * Math.cos(abs((desiredState.angle.angle - inputs.steeringPosition).inRadians)) */) + io.setOpenLoop( + desiredState.angle.angle, + desiredState + .speedMetersPerSecond + .meters + .perSecond /* * Math.cos(abs((desiredState.angle.angle - inputs.steeringPosition).inRadians)) */ + ) Logger.getInstance() .recordOutput("${io.label}/steeringSetpointNonOptimized", desiredState.angle.degrees) } @@ -268,7 +273,7 @@ class SwerveModule(val io: SwerveModuleIO) { ) { if (optimize) { val optimizedVelState = - SwerveModuleState.optimize(desiredVelState, inputs.steeringPosition.inRotation2ds) + SwerveModuleState.optimize(desiredVelState, inputs.steeringPosition.inRotation2ds) val optimizedAccelState = SwerveModuleState.optimize(desiredAccelState, inputs.steeringPosition.inRotation2ds) @@ -276,21 +281,13 @@ class SwerveModule(val io: SwerveModuleIO) { speedSetPoint = optimizedVelState.speedMetersPerSecond.meters.perSecond accelerationSetPoint = optimizedAccelState.speedMetersPerSecond.meters.perSecond.perSecond - io.setClosedLoop( - steeringSetPoint, - speedSetPoint, - accelerationSetPoint - ) + io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) } else { steeringSetPoint = desiredVelState.angle.angle speedSetPoint = desiredVelState.speedMetersPerSecond.meters.perSecond accelerationSetPoint = desiredAccelState.speedMetersPerSecond.meters.perSecond.perSecond - io.setClosedLoop( - steeringSetPoint, - speedSetPoint, - accelerationSetPoint - ) + io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt index 0db0417a..80b97b44 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt @@ -7,9 +7,6 @@ import com.ctre.phoenix.motorcontrol.can.TalonFX import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants -import com.team4099.robot2023.subsystems.falconspin.Falcon500 -import com.team4099.robot2023.subsystems.falconspin.MotorChecker -import com.team4099.robot2023.subsystems.falconspin.MotorCollection import edu.wpi.first.wpilibj.AnalogInput import edu.wpi.first.wpilibj.RobotController import org.littletonrobotics.junction.Logger @@ -163,8 +160,8 @@ class SwerveModuleIOFalcon( } override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) { - inputs.driveAppliedVoltage = driveFalcon.motorOutputVoltage.volts //no - inputs.steeringAppliedVoltage = steeringFalcon.motorOutputVoltage.volts //no + inputs.driveAppliedVoltage = driveFalcon.motorOutputVoltage.volts // no + inputs.steeringAppliedVoltage = steeringFalcon.motorOutputVoltage.volts // no inputs.driveStatorCurrent = driveFalcon.statorCurrent.amps inputs.driveSupplyCurrent = driveFalcon.supplyCurrent.amps @@ -177,14 +174,13 @@ class SwerveModuleIOFalcon( inputs.driveVelocity = driveSensor.velocity inputs.steeringVelocity = steeringSensor.velocity - inputs.driveTemp = driveFalcon.temperature.celsius //no - inputs.steeringTemp = steeringFalcon.temperature.celsius //no + inputs.driveTemp = driveFalcon.temperature.celsius // no + inputs.steeringTemp = steeringFalcon.temperature.celsius // no inputs.potentiometerOutputRaw = potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI inputs.potentiometerOutputRadians = potentiometerOutput.radians - Logger.getInstance() .recordOutput( "$label/potentiometerRadiansWithOffset", @@ -213,8 +209,7 @@ class SwerveModuleIOFalcon( feedforward.inVolts / 12.0 ) setSteeringSetpoint(steering) - Logger.getInstance() - .recordOutput("$label/setpointVelocity", speed.inMetersPerSecond) + Logger.getInstance().recordOutput("$label/setpointVelocity", speed.inMetersPerSecond) } /** diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index fe0aa183..24bfb5ff 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -78,7 +78,7 @@ class Elevator(val io: ElevatorIO) { val enableElevator = LoggedTunableNumber("Elevator/enableMovementElevator", ElevatorConstants.ENABLE_ELEVATOR) - val minPosition = + val minPosition = LoggedTunableValue( "Elevator/minPosition", ElevatorConstants.ELEVATOR_IDLE_HEIGHT, @@ -414,7 +414,9 @@ class Elevator(val io: ElevatorIO) { .recordOutput("Elevator/currentRequest", currentRequest.javaClass.simpleName) Logger.getInstance() - .recordOutput("Elevator/lastHomingStatorCurrentTripTime", lastHomingStatorCurrentTripTime.inSeconds) + .recordOutput( + "Elevator/lastHomingStatorCurrentTripTime", lastHomingStatorCurrentTripTime.inSeconds + ) if (Constants.Tuning.DEBUGING_MODE) { Logger.getInstance() diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt index c8e416c5..5ad3201b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONeo.kt @@ -6,9 +6,6 @@ import com.revrobotics.SparkMaxPIDController import com.team4099.lib.math.clamp import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.ElevatorConstants -import com.team4099.robot2023.subsystems.falconspin.MotorChecker -import com.team4099.robot2023.subsystems.falconspin.MotorCollection -import com.team4099.robot2023.subsystems.falconspin.Neo import org.littletonrobotics.junction.Logger import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.Meter diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt index 1db5d08b..70bbe625 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt @@ -3,15 +3,9 @@ package com.team4099.robot2023.subsystems.limelight import com.team4099.lib.hal.Clock import com.team4099.lib.logging.TunableNumber import com.team4099.lib.vision.TargetCorner -import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.VisionConstants -import com.team4099.robot2023.subsystems.gameboy.objective.Objective -import com.team4099.robot2023.subsystems.gameboy.objective.isConeNode -import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.LimelightReading import com.team4099.robot2023.util.PoseEstimator -import com.team4099.robot2023.util.findClosestPose import com.team4099.robot2023.util.rotateBy import com.team4099.robot2023.util.toPose3d import com.team4099.robot2023.util.toTransform3d @@ -33,7 +27,6 @@ import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inRadians -import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.tan import java.util.function.Consumer import kotlin.math.hypot @@ -46,10 +39,6 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { var poseSupplier: () -> Pose2d = { Pose2d() } var visionConsumer: Consumer> = Consumer {} - var nodeToLookFor: () -> Objective = { Objective() } - - val midConeNodePoses = mutableListOf() - val highConeNodePoses = mutableListOf() // i think we need this for camera project to irl coordinates val vpw = (2.0 * (VisionConstants.Limelight.HORIZONTAL_FOV / 2).tan) @@ -58,27 +47,6 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { private val xyStdDevCoefficient = TunableNumber("LimelightVision/xystdev", 0.05) private val thetaStdDev = TunableNumber("LimelightVision/thetaStdDev", 0.75) - init { - for (nodeIndex in listOf(0, 2, 3, 5, 6, 8)) { - midConeNodePoses.add( - Pose3d( - FieldConstants.Grids.midX, - FieldConstants.Grids.nodeFirstY + FieldConstants.Grids.nodeSeparationY * nodeIndex, - VisionConstants.Limelight.MID_TAPE_HEIGHT, - Rotation3d() - ) - ) - highConeNodePoses.add( - Pose3d( - FieldConstants.Grids.highX, - FieldConstants.Grids.nodeFirstY + FieldConstants.Grids.nodeSeparationY * nodeIndex, - VisionConstants.Limelight.HIGH_TAPE_HEIGHT, - Rotation3d() - ) - ) - } - } - override fun periodic() { val startTime = Clock.realTimestamp @@ -87,88 +55,60 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { var currentPose: Pose2d = poseSupplier.invoke() - val visibleNodes = mutableListOf() + val visibleGamePieces = mutableListOf() - // mathematically figure out which nodes we're looking at based on limelight fov + // mathematically figure out which game pieces we're looking at based on limelight neural + // network detector // process transform outputs from LL and get the target corners - // calculating true node position - val nodeX = - when (nodeToLookFor.invoke().nodeTier) { - Constants.Universal.NodeTier.HYBRID -> FieldConstants.Grids.lowX - Constants.Universal.NodeTier.MID -> FieldConstants.Grids.midX - Constants.Universal.NodeTier.HIGH -> FieldConstants.Grids.highX - else -> -1337.meters - } - - val nodeColumn = - if (FMSData.isBlue) nodeToLookFor.invoke().nodeColumn - else 8 - nodeToLookFor.invoke().nodeColumn - val nodeY = FieldConstants.Grids.nodeFirstY + FieldConstants.Grids.nodeSeparationY * nodeColumn - - val nodeZ = - when (nodeToLookFor.invoke().nodeTier) { - Constants.Universal.NodeTier.HYBRID -> 0.0.meters - Constants.Universal.NodeTier.MID -> VisionConstants.Limelight.MID_TAPE_HEIGHT - Constants.Universal.NodeTier.HIGH -> VisionConstants.Limelight.HIGH_TAPE_HEIGHT - else -> -1337.meters - } - - val nodeRotation = - Rotation3d(0.0.radians, 0.0.radians, if (FMSData.isBlue) 0.0.degrees else 180.degrees) - - val nodePose = Pose3d(nodeX, nodeY, nodeZ, nodeRotation) - + // calculating true game piece position val robotPoses = mutableListOf() if (inputs.validReading) { - for (target in inputs.retroTargets) { - visibleNodes.add(solveTargetPoseFromAngle(currentPose, target, nodeZ)) + for (target in inputs.gamePieceTargets) { + visibleGamePieces.add( + solveTargetPoseFromAngle( + currentPose, + target, + ( + if (target.className == "cone") VisionConstants.Limelight.CONE_HEIGHT / 2 + else VisionConstants.Limelight.CUBE_HEIGHT / 2 + ) + ) + ) } // this is adding where we think they are,, not where they actually are } val timestampedVisionUpdates = mutableListOf() - val trueVisibleNodes = mutableListOf() - - for (node in visibleNodes) { - val searchList = - if (nodeZ == VisionConstants.Limelight.MID_TAPE_HEIGHT) midConeNodePoses - else highConeNodePoses - - val closestPose = node.findClosestPose(*searchList.toTypedArray()) - - trueVisibleNodes.add(closestPose) - + for (gamePiecePose in visibleGamePieces) { // find inverse translation from the detected pose to robot val targetToCamera = - closestPose + gamePiecePose .relativeTo( currentPose.toPose3d().transformBy(VisionConstants.Limelight.LL_TRANSFORM) ) .toTransform3d() .inverse() - val trueNodePoseToRobot = closestPose.transformBy(targetToCamera) + val trueNodePoseToRobot = gamePiecePose.transformBy(targetToCamera) // Add to vision updates val xyStdDev = xyStdDevCoefficient.get() * targetToCamera.translation.norm.inMeters.pow(2) val thetaStdDev = thetaStdDev.get() * targetToCamera.translation.norm.inMeters.pow(2) - if (nodeToLookFor.invoke().isConeNode()) { - robotPoses.add(trueNodePoseToRobot.toPose2d()) + robotPoses.add(trueNodePoseToRobot.toPose2d()) - timestampedVisionUpdates.add( - PoseEstimator.TimestampedVisionUpdate( - inputs.timestamp, - trueNodePoseToRobot.toPose2d(), - VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev) - ) + timestampedVisionUpdates.add( + PoseEstimator.TimestampedVisionUpdate( + inputs.timestamp, + trueNodePoseToRobot.toPose2d(), + VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev) ) - } + ) } Logger.getInstance() @@ -178,11 +118,8 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { Logger.getInstance() .recordOutput( - "LimelightVision/robotVisibleNodes", *visibleNodes.map { it.pose3d }.toTypedArray() - ) - Logger.getInstance() - .recordOutput( - "LimelightVision/trueVisibleNodes", *trueVisibleNodes.map { it.pose3d }.toTypedArray() + "LimelightVision/robotVisiblePieces", + *visibleGamePieces.map { it.pose3d }.toTypedArray() ) Logger.getInstance() diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIO.kt index 04738c45..a12bf576 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIO.kt @@ -3,7 +3,9 @@ package com.team4099.robot2023.subsystems.limelight import com.team4099.robot2023.util.LimelightReading import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs +import org.team4099.lib.units.base.Decimal import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.percent import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees @@ -15,50 +17,70 @@ interface LimelightVisionIO { var timestamp = 0.0.seconds var fps = 0.0 var validReading = false - var angle = 0.degrees - var retroTargets = listOf() + var gamePieceTargets = listOf() + var xAngle = 0.degrees + var yAngle = 0.degrees + var targetSize = 0.percent override fun fromLog(table: LogTable?) { table?.getDouble("timestampSeconds", timestamp.inSeconds)?.let { timestamp = it.seconds } table?.getDouble("fps", fps)?.let { fps = it } table?.getBoolean("validReading", validReading)?.let { validReading = it } - table?.getDouble("simpleAngleDegrees", angle.inDegrees)?.let { angle = it.degrees } + table?.getDouble("xAngleDegrees", xAngle.inDegrees)?.let { xAngle = it.degrees } + table?.getDouble("yAngleDegrees", yAngle.inDegrees)?.let { yAngle = it.degrees } + table?.getDouble("targetSizePercent", targetSize.value)?.let { targetSize = it.percent } val numOfTargets = table?.getInteger("numOfTargets", 0) ?: 0 val retrievedTargets = mutableListOf() for (targetIndex in 0 until numOfTargets) { + val className: String? = table?.getString("Detection/$targetIndex/class", "") + val confidence: Decimal? = table?.getDouble("Detection/$targetIndex/conf", 0.0)?.percent val targetTx: Angle? = table?.getDouble("Detection/$targetIndex/tx", 0.0)?.degrees - val targetTy: Angle? = table?.getDouble("Detection/$targetIndex/tx", 0.0)?.degrees - val targetTxPixels: Double? = table?.getDouble("Detection/$targetIndex/txPixels", 0.0) - val targetTyPixels: Double? = table?.getDouble("Detection/$targetIndex/tyPixels", 0.0) - val targetTs: Angle? = table?.getDouble("Detection/$targetIndex/tyPixels", 0.0)?.degrees - if (targetTx != null && + val targetTy: Angle? = table?.getDouble("Detection/$targetIndex/ty", 0.0)?.degrees + val targetTxPixels: Double? = table?.getDouble("Detection/$targetIndex/txp", 0.0) + val targetTyPixels: Double? = table?.getDouble("Detection/$targetIndex/typ", 0.0) + val targetTa: Decimal? = table?.getDouble("Detection/$targetIndex/ta", 0.0)?.percent + if ((className == "cone" || className == "cube") && + confidence != null && + targetTx != null && targetTy != null && targetTxPixels != null && targetTyPixels != null && - targetTs != null + targetTa != null ) { retrievedTargets.add( - LimelightReading(targetTx, targetTy, targetTxPixels, targetTyPixels, targetTs) + LimelightReading( + className, + confidence, + targetTx, + targetTy, + targetTxPixels, + targetTyPixels, + targetTa + ) ) } } - retroTargets = retrievedTargets.toList() + gamePieceTargets = retrievedTargets.toList() } override fun toLog(table: LogTable?) { table?.put("timestampSeconds", timestamp.inSeconds) table?.put("fps", fps) table?.put("validReading", validReading) - table?.put("simpleAngleDegrees", angle.inDegrees) - table?.put("numOfTargets", retroTargets.size.toLong()) - table?.put("cornersX", retroTargets.map { it.txPixel }.toDoubleArray()) - table?.put("cornersY", retroTargets.map { it.tyPixel }.toDoubleArray()) - for (i in retroTargets.indices) { - table?.put("Detection/$i/txDegrees", retroTargets[i].tx.inDegrees) - table?.put("Detection/$i/tyDegrees", retroTargets[i].ty.inDegrees) - table?.put("Detection/$i/tyPixels", retroTargets[i].tyPixel) - table?.put("Detection/$i/txPixels", retroTargets[i].txPixel) - table?.put("Detection/$i/tsDegrees", retroTargets[i].ts.inDegrees) + table?.getDouble("xAngleDegrees", xAngle.inDegrees) + table?.getDouble("yAngleDegrees", yAngle.inDegrees) + table?.getDouble("targetSizePercent", targetSize.value) + table?.put("numOfTargets", gamePieceTargets.size.toLong()) + table?.put("cornersX", gamePieceTargets.map { it.txPixel }.toDoubleArray()) + table?.put("cornersY", gamePieceTargets.map { it.tyPixel }.toDoubleArray()) + for (i in gamePieceTargets.indices) { + table?.put("Detection/$i/class", gamePieceTargets[i].className) + table?.put("Detection/$i/conf", gamePieceTargets[i].confidence.value) + table?.put("Detection/$i/tx", gamePieceTargets[i].tx.inDegrees) + table?.put("Detection/$i/ty", gamePieceTargets[i].ty.inDegrees) + table?.put("Detection/$i/typ", gamePieceTargets[i].tyPixel) + table?.put("Detection/$i/txp", gamePieceTargets[i].txPixel) + table?.put("Detection/$i/ta", gamePieceTargets[i].ta.value) } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOReal.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOReal.kt index 4df9cb42..3b9fa842 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOReal.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOReal.kt @@ -7,6 +7,7 @@ import com.team4099.utils.LimelightHelpers import edu.wpi.first.networktables.NetworkTableEntry import edu.wpi.first.networktables.NetworkTableInstance import org.team4099.lib.units.base.inMilliseconds +import org.team4099.lib.units.base.percent import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.milli @@ -25,8 +26,12 @@ object LimelightVisionIOReal : LimelightVisionIO { NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("cl") private val dataEntry: NetworkTableEntry = NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("tcornxy") - private val angleEntry: NetworkTableEntry = + private val xAngleEntry: NetworkTableEntry = NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("tx") + private val yAngleEntry: NetworkTableEntry = + NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("ty") + private val targetSizeEntry: NetworkTableEntry = + NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("ta") override fun updateInputs(inputs: LimelightVisionIO.LimelightVisionIOInputs) { val totalLatency = @@ -36,12 +41,14 @@ object LimelightVisionIOReal : LimelightVisionIO { ) inputs.timestamp = Clock.realTimestamp - totalLatency - inputs.angle = angleEntry.getDouble(0.0).degrees + inputs.xAngle = xAngleEntry.getDouble(0.0).degrees + inputs.yAngle = yAngleEntry.getDouble(0.0).degrees + inputs.targetSize = (targetSizeEntry.getDouble(0.0) * 100).percent inputs.fps = 1000 / totalLatency.inMilliseconds inputs.validReading = true - inputs.retroTargets = - LimelightHelpers.getLatestResults(LIMELIGHT_NAME).targetingResults.targets_Retro.map { + inputs.gamePieceTargets = + LimelightHelpers.getLatestResults(LIMELIGHT_NAME).targetingResults.targets_Detector.map { LimelightReading(it) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOSim.kt index 94d5a123..a7dc7117 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOSim.kt @@ -3,6 +3,7 @@ package com.team4099.robot2023.subsystems.limelight import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.util.LimelightReading +import org.team4099.lib.units.base.percent import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.radians @@ -22,10 +23,16 @@ object LimelightVisionIOSim : LimelightVisionIO { override fun updateInputs(inputs: LimelightVisionIO.LimelightVisionIOInputs) { inputs.timestamp = Clock.realTimestamp - inputs.angle = 0.0.radians + inputs.xAngle = 0.0.radians + inputs.yAngle = 0.0.radians + inputs.targetSize = 0.0.percent inputs.fps = 90.0 inputs.validReading = true - inputs.retroTargets = - listOf(LimelightReading(tunableTx.get(), tunableTy.get(), 0.0, 0.0, 10.degrees)) + inputs.gamePieceTargets = + listOf( + LimelightReading( + "", 0.0.percent, tunableTx.get(), tunableTy.get(), 0.0, 0.0, 0.0.percent + ) + ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt index 70d5ff80..1c9ad090 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -233,11 +233,8 @@ class Superstructure( } } - if (elevator.currentRequest is Request.ElevatorRequest.Home){ - elevator.currentRequest = - Request.ElevatorRequest.TargetingPosition( - 3.inches - ) + if (elevator.currentRequest is Request.ElevatorRequest.Home) { + elevator.currentRequest = Request.ElevatorRequest.TargetingPosition(3.inches) } // Outputs @@ -286,20 +283,11 @@ class Superstructure( ) if (manipulator.isAtTargetedPosition) { - if (manipulator.hasCube){ - elevator.currentRequest = - Request.ElevatorRequest.TargetingPosition( - 6.inches - ) + if (manipulator.hasCube) { + elevator.currentRequest = Request.ElevatorRequest.TargetingPosition(6.inches) } else { - elevator.currentRequest = - Request.ElevatorRequest.TargetingPosition( - 3.inches - ) + elevator.currentRequest = Request.ElevatorRequest.TargetingPosition(3.inches) } - - - } } else { manipulator.currentRequest = @@ -307,9 +295,7 @@ class Superstructure( manipulator.armPositionTarget, rollerVoltage ) elevator.currentRequest = - Request.ElevatorRequest.TargetingPosition( - elevator.elevatorPositionTarget - ) + Request.ElevatorRequest.TargetingPosition(elevator.elevatorPositionTarget) } // Transition @@ -788,7 +774,6 @@ class Superstructure( GroundIntake.TunableGroundIntakeStates.stowedDownAngle.get(), rollerVoltage ) scoringConeWithoutLoweringGroundIntake = false - } } else { groundIntake.currentRequest = diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index 20f7c22b..2753072f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -111,9 +111,10 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { // Logger.getInstance().recordOutput("Vision/${VisionConstants.CAMERA_NAMES[instance]}_transform", cameraPose.relativeTo(tuningPosition.toPose3d()).pose3d) robotPose = cameraPose.transformBy(cameraPoses[instance].inverse()).toPose2d() -// println( -// "CameraPoseX: ${cameraPose.x}, transformX: ${cameraPoses[instance].x}, robotPoseX: ${robotPose.x}" -// ) + // println( + // "CameraPoseX: ${cameraPose.x}, transformX: ${cameraPoses[instance].x}, + // robotPoseX: ${robotPose.x}" + // ) } 2.0 -> { val error0 = values[1] diff --git a/src/main/kotlin/com/team4099/robot2023/util/LimelightReading.kt b/src/main/kotlin/com/team4099/robot2023/util/LimelightReading.kt index 388dc94e..3851db03 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/LimelightReading.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/LimelightReading.kt @@ -1,24 +1,30 @@ package com.team4099.robot2023.util -import com.team4099.utils.LimelightHelpers.LimelightTarget_Retro +import com.team4099.utils.LimelightHelpers.LimelightTarget_Detector +import org.team4099.lib.units.base.Decimal +import org.team4099.lib.units.base.percent import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees data class LimelightReading( + val className: String, + val confidence: Decimal, val tx: Angle, val ty: Angle, val txPixel: Double, val tyPixel: Double, - val ts: Angle + val ta: Decimal ) { constructor( - limelightReading: LimelightTarget_Retro + limelightReading: LimelightTarget_Detector ) : this( + limelightReading.className, + limelightReading.confidence.percent, limelightReading.tx.degrees, limelightReading.ty.degrees, limelightReading.tx_pixels, limelightReading.ty_pixels, - limelightReading.ts.degrees - ) {} + limelightReading.ta.percent + ) }