diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index 30c2d6c0..efc8ee5c 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 776a9efd..073302e1 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() { @@ -229,11 +216,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 ff75f474..6a0a84e8 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 @@ -15,7 +13,6 @@ import com.team4099.robot2023.subsystems.gameboy.objective.isConeNode import com.team4099.robot2023.subsystems.superstructure.Superstructure import com.team4099.robot2023.util.AllianceFlipUtil import com.team4099.robot2023.util.FMSData -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 @@ -23,7 +20,6 @@ import org.team4099.lib.geometry.Pose2d 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 @@ -42,7 +38,6 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru val setupCommand = runOnce({ - Logger.getInstance().recordOutput("Auto/isAutoDriving", true) drivePose = drivetrain.odometryPose postAlignPose = @@ -57,15 +52,16 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru ) ) - finalPose = AllianceFlipUtil.apply( Pose2d( - 2.16.meters, // slightly offset in the x + 1.9.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 ) ) @@ -90,14 +86,10 @@ 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) } }) addCommands( setupCommand, @@ -116,8 +108,7 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru intermediaryWaypoints + listOf( Waypoint( - finalPose.translation.translation2d, - finalPose.rotation.inRotation2ds + finalPose.translation.translation2d, finalPose.rotation.inRotation2ds ), 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/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 a1823da7..6820ea13 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 e5a9c23d..e1452f5b 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/WaypointConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WaypointConstants.kt index b861db13..6d616b3f 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WaypointConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WaypointConstants.kt @@ -9,7 +9,7 @@ import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inRadians object WaypointConstants { - val DISTANCE_FROM_COMMUNITY = 2.5.meters + val DISTANCE_FROM_COMMUNITY = 2.1.meters enum class SubstationPoints(val waypoints: List) { CloseLeftCommunity( @@ -81,16 +81,16 @@ object WaypointConstants { CloseRightCommunity( listOf( Waypoint( - Translation2d(DISTANCE_FROM_COMMUNITY, 0.85.meters).translation2d, + Translation2d(DISTANCE_FROM_COMMUNITY, 0.76.meters).translation2d, holonomicRotation = Rotation2d(180.degrees.inRadians) ) ) ), - CloseLeftLane(listOf(Waypoint(Translation2d(3.85.meters, 4.79.meters).translation2d))), + CloseLeftLane(listOf(Waypoint(Translation2d(3.1.meters, 4.79.meters).translation2d))), CloseLoadingZoneLane(listOf(Waypoint(Translation2d(5.79.meters, 5.93.meters).translation2d))), CloseCenterLeftLane(listOf(Waypoint(Translation2d(5.79.meters, 4.79.meters).translation2d))), CloseCenterRightLane(listOf(Waypoint(Translation2d(5.79.meters, 0.85.meters).translation2d))), - CloseRightLane(listOf(Waypoint(Translation2d(3.15.meters, 0.85.meters).translation2d))), + CloseRightLane(listOf(Waypoint(Translation2d(3.31.meters, 0.76.meters).translation2d))), FarRightLane(listOf(Waypoint(Translation2d(10.16.meters, 0.85.meters).translation2d))), FarCenterLeftLane( listOf( 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 8e42f90b..21667c75 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 @@ -210,8 +210,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 b08fc941..402277ab 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, 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 e99e93c0..37c12571 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -279,20 +279,11 @@ class Superstructure( ) if (manipulator.isAtTargetedPosition) { - if (manipulator.hasCube){ - elevator.currentRequest = - Request.ElevatorRequest.TargetingPosition( - 4.inches - ) + if (manipulator.hasCube) { + elevator.currentRequest = Request.ElevatorRequest.TargetingPosition(4.inches) } else { - elevator.currentRequest = - Request.ElevatorRequest.TargetingPosition( - 3.inches - ) + elevator.currentRequest = Request.ElevatorRequest.TargetingPosition(3.inches) } - - - } } else { manipulator.currentRequest = @@ -777,7 +768,6 @@ class Superstructure( GroundIntake.TunableGroundIntakeStates.stowedDownAngle.get(), rollerVoltage ) scoringConeWithoutLoweringGroundIntake = false - } } else { groundIntake.currentRequest =