From a1973e9dbc2637710282c26310d74b83ae2c478c Mon Sep 17 00:00:00 2001 From: AlphaPranav9102 <49816864+AlphaPranav9102@users.noreply.github.com> Date: Wed, 18 Oct 2023 21:40:59 -0400 Subject: [PATCH] tewneng idk --- .../com/team4099/robot2023/RobotContainer.kt | 2 +- .../robot2023/commands/AutoScoreCommand.kt | 17 +++++++++-------- .../config/constants/DrivetrainConstants.kt | 6 +++--- .../config/constants/FieldConstants.kt | 2 +- .../config/constants/GroundIntakeConstants.kt | 2 +- .../swervemodule/SwerveModuleIOFalcon.kt | 2 +- 6 files changed, 16 insertions(+), 15 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index c1cb1477..776a9efd 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -79,7 +79,7 @@ object RobotContainer { superstructure = Superstructure( Elevator(ElevatorIONeo), - GroundIntake(object: GroundIntakeIO {}), + GroundIntake(GroundIntakeIONeo), Manipulator(ManipulatorIONeo), Led(object : LedIO {}), GameBoy(GameboyIOServer) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt index d0ac1c65..ff75f474 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt @@ -45,10 +45,10 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru Logger.getInstance().recordOutput("Auto/isAutoDriving", true) drivePose = drivetrain.odometryPose - finalPose = + postAlignPose = AllianceFlipUtil.apply( Pose2d( - 2.16.meters, // slightly offset in the x + 1.8.meters, // slightly offset in the x FieldConstants.Grids.nodeFirstY + FieldConstants.Grids.nodeSeparationY * if (FMSData.isBlue) superstructure.objective.nodeColumn @@ -56,17 +56,20 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru 180.degrees ) ) - postAlignPose = + + + finalPose = AllianceFlipUtil.apply( Pose2d( - 1.8.meters, // slightly offset in the x + 2.16.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 ) ) + heading = drivetrain.fieldVelocity.heading // Determine the current zone and calculate the trajectory @@ -114,12 +117,10 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru listOf( Waypoint( finalPose.translation.translation2d, - finalPose.rotation.inRotation2ds ), Waypoint( postAlignPose.translation.translation2d, - postAlignPose.rotation.inRotation2ds ) ) 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 50c0d0d4..e5a9c23d 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -109,7 +109,7 @@ object DrivetrainConstants { val AUTO_POS_KDX: DerivativeGain> get() { if (RobotBase.isReal()) { - return (0.05.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 } @@ -118,7 +118,7 @@ object DrivetrainConstants { val AUTO_POS_KPY: ProportionalGain> get() { if (RobotBase.isReal()) { - return 0.0.meters.perSecond / 1.0.meters // todo:4 + return 0.25.meters.perSecond / 1.0.meters // todo:4 } else { return 7.0.meters.perSecond / 1.0.meters } @@ -135,7 +135,7 @@ object DrivetrainConstants { val AUTO_POS_KDY: DerivativeGain> get() { if (RobotBase.isReal()) { - return (0.15.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 e4642e2c..bc72113b 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt @@ -168,7 +168,7 @@ object FieldConstants { 0, Pose3d( (40.45).inches, - (108.19).inches + 99.centi.meters + 3.inches, + (108.19).inches + 114.centi.meters, (46).centi.meters, Rotation3d() ) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/GroundIntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/GroundIntakeConstants.kt index 66238fa4..b0642678 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/GroundIntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/GroundIntakeConstants.kt @@ -42,7 +42,7 @@ object GroundIntakeConstants { const val ROLLER_MOTOR_INVERTED = true const val ARM_MOTOR_INVERTED = false - val ABSOLUTE_ENCODER_OFFSET = (-196.87).degrees + val ABSOLUTE_ENCODER_OFFSET = (-159.87).degrees // From encoder to intake val ROLLER_GEAR_RATIO = (36.0.driven / 18.0.driving).gearRatio 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 a3efad7d..8e42f90b 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 @@ -112,7 +112,7 @@ class SwerveModuleIOFalcon( driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI) driveConfiguration.slot0.kD = driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD) - driveConfiguration.slot0.kF = 0.045 + driveConfiguration.slot0.kF = 0.0430 // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) driveConfiguration.supplyCurrLimit.currentLimit = DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes