tewneng idk

team4099 · Oct 19, 2023 · a1973e9 · a1973e9
1 parent 8f8cb5b
commit a1973e9
Show file tree

Hide file tree

Showing 6 changed files with 16 additions and 15 deletions.
diff --git 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
@@ -45,28 +45,31 @@ 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
                 else 8 - superstructure.objective.nodeColumn,
               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
@@ -109,7 +109,7 @@ object DrivetrainConstants {
     val AUTO_POS_KDX: DerivativeGain<Meter, Velocity<Meter>>
       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<Meter, Velocity<Meter>>
       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<Meter, Velocity<Meter>>
       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
@@ -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
@@ -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/.../kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt b/.../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