fixed some adstar things, tried ktlint

src/main/kotlin/frc/team449/ControllerBindings.kt

@@ -9,7 +9,6 @@ import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.DriverStation.Alliance
 import edu.wpi.first.wpilibj.RobotBase
-import edu.wpi.first.wpilibj2.command.Commands.runOnce
 import edu.wpi.first.wpilibj2.command.ConditionalCommand
 import edu.wpi.first.wpilibj2.command.InstantCommand
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController

src/main/kotlin/frc/team449/Robot.kt

@@ -2,7 +2,6 @@ package frc.team449
 
 import edu.wpi.first.wpilibj.PowerDistribution
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController
-import frc.team449.auto.choreo.MagnetizePIDPoseAlign
 import frc.team449.commands.autoscoreCommands.pathfinder
 import frc.team449.subsystems.RobotConstants
 import frc.team449.subsystems.drive.swerve.SwerveDrive

src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommandConstants.kt

@@ -31,9 +31,6 @@ class AutoScoreCommandConstants() {
     L4
   }
 
-
-
-
   companion object {
     /**
      * syntactic sugar method
@@ -264,7 +261,7 @@ class AutoScoreCommandConstants() {
     val reef12Rotation2dRed = Rotation2d(radians(-60))
     val reef12PoseRed = Pose2d(reef12Translation2dRed, reef12Rotation2dRed)
 
-    const val ROT_MAX_ACCEL = 2*PI
+    const val ROT_MAX_ACCEL = 2 * PI
     const val MAX_ACCEL = 10.0
     const val MAX_ROT_SPEED = 5 * PI / 4 // r ad/s
     val MAX_LINEAR_SPEED = SwerveConstants.MAX_ATTAINABLE_MK4I_SPEED // m/s

src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommands.kt

@@ -4,13 +4,11 @@ import com.pathplanner.lib.auto.AutoBuilder
 import com.pathplanner.lib.path.PathConstraints
 import edu.wpi.first.math.geometry.Pose2d
 import edu.wpi.first.math.geometry.Translation2d
-import edu.wpi.first.math.util.Units
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.DriverStation.Alliance
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.InstantCommand
 import frc.team449.Robot
-import frc.team449.subsystems.RobotConstants
 import frc.team449.subsystems.superstructure.SuperstructureGoal
 import frc.team449.subsystems.vision.PoseSubsystem
 

src/main/kotlin/frc/team449/commands/autoscoreCommands/MagnetizePIDPoseAlign.kt

@@ -96,7 +96,7 @@ class MagnetizePIDPoseAlign(
   fun calculate(currPose: Pose2d, desState: Pose2d): ChassisSpeeds {
     val xPID = xController.calculate(currPose.x, desState.x)
     val yPID = yController.calculate(currPose.y, desState.y)
-    val angPID = thetaController.calculate(currPose.rotation.radians, desState.rotation.radians)//
+    val angPID = thetaController.calculate(currPose.rotation.radians, desState.rotation.radians) //
 
     return ChassisSpeeds.fromFieldRelativeSpeeds(xPID, yPID, angPID, currPose.rotation)
   }

src/main/kotlin/frc/team449/commands/autoscoreCommands/RemoteADStar.kt

@@ -41,7 +41,9 @@ class RemoteADStar : Pathfinder {
     pathPointsSub =
       nt.getDoubleArrayTopic("/PPLibCoprocessor/RemoteADStar/pathPoints")
         .subscribe(
-          DoubleArray(0), PubSubOption.keepDuplicates(true), PubSubOption.sendAll(true)
+          DoubleArray(0),
+          PubSubOption.keepDuplicates(true),
+          PubSubOption.sendAll(true)
         )
 
     nt.addListener(
@@ -77,7 +79,8 @@ class RemoteADStar : Pathfinder {
         }
       } catch (e: Exception) {
         DriverStation.reportError(
-          "RemoteADStar failed to load navgrid. Pathfinding will not be functional.", false
+          "RemoteADStar failed to load navgrid. Pathfinding will not be functional.",
+          false
         )
       }
     }
@@ -140,7 +143,8 @@ class RemoteADStar : Pathfinder {
    * position of the path to properly avoid obstacles
    */
   override fun setDynamicObstacles(
-    obs: List<Pair<Translation2d, Translation2d>>, currentRobotPos: Translation2d
+    obs: List<Pair<Translation2d, Translation2d>>,
+    currentRobotPos: Translation2d
   ) {
     val obsArr = DoubleArray(((obs.size * 2) + 1) * 2)
 
@@ -160,4 +164,4 @@ class RemoteADStar : Pathfinder {
 
     dynamicObsPub.set(obsArr)
   }
-}
+}

src/main/kotlin/frc/team449/commands/autoscoreCommands/pathfinder.kt

@@ -1,7 +1,5 @@
 package frc.team449.commands.autoscoreCommands
 
-import com.pathplanner.lib.auto.AutoBuilder
-import com.pathplanner.lib.commands.FollowPathCommand
 import com.pathplanner.lib.config.RobotConfig
 import com.pathplanner.lib.path.GoalEndState
 import com.pathplanner.lib.path.PathConstraints
@@ -12,14 +10,13 @@ import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.math.geometry.Translation2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds
 import edu.wpi.first.networktables.*
+import edu.wpi.first.wpilibj.Timer
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.PrintCommand
 import edu.wpi.first.wpilibj2.command.SubsystemBase
 import frc.team449.Robot
-import frc.team449.subsystems.RobotConstants
-import edu.wpi.first.wpilibj.Timer
-import edu.wpi.first.wpilibj2.command.WaitCommand
 import frc.team449.auto.choreo.MagnetizePIDPoseAlign
+import frc.team449.subsystems.RobotConstants
 import kotlin.math.floor
 
 class pathfinder(val robot: Robot) : SubsystemBase() {
@@ -31,16 +28,16 @@ class pathfinder(val robot: Robot) : SubsystemBase() {
   var path: PathPlannerPath? = null
   var points: DoubleArray = doubleArrayOf(0.0, 0.0)
   private val timer = Timer()
-  var expectedtime = 0.0
+  var expectedtime: Double? = 0.0
   var pathstart = 0.0
   var poseafter: Pose2d = Pose2d(Translation2d(0.0, 0.0), Rotation2d(0.0))
   var indexinpath = 0
-  var speednow: ChassisSpeeds = ChassisSpeeds(0.0,0.0,0.0)
-  var otherspeednow: ChassisSpeeds = ChassisSpeeds(0.0,0.0,0.0)
+  var speednow: ChassisSpeeds = ChassisSpeeds(0.0, 0.0, 0.0)
+  var otherspeednow: ChassisSpeeds = ChassisSpeeds(0.0, 0.0, 0.0)
   var pathrunning = false
   var pathvalid = false
   var prevpose: Pose2d? = robot.poseSubsystem.pose
-  var nextpose: Pose2d? = Pose2d(Translation2d(0.0,0.0), Rotation2d(0.0))
+  var nextpose: Pose2d? = Pose2d(Translation2d(0.0, 0.0), Rotation2d(0.0))
   var alongpath = 0.0
 
   init {
@@ -55,36 +52,41 @@ class pathfinder(val robot: Robot) : SubsystemBase() {
   }
 
   override fun periodic() { // periodic is 2 loops ahead of path init
-    //if (indexinpath+2 <= pathSub?.get()!!.size){
+    // if (indexinpath+2 <= pathSub?.get()!!.size){
     pathrunning = (alongpath < 1.0)
-    pathvalid =  ((pathSub?.get()?.get(0) ?: Pose2d()) != Pose2d(Translation2d(0.0, 0.0), Rotation2d(0.0)))
-//    println("pathrunning $pathrunning")
-//    println("pathvalid $pathvalid")
+    pathvalid = ((pathSub?.get()?.get(0) ?: Pose2d()) != Pose2d(Translation2d(0.0, 0.0), Rotation2d(0.0)))
+    println("pathrunning $pathrunning")
+    println("pathvalid $pathvalid") //
     if (pathrunning && pathvalid) {
-      expectedtime = path?.generateTrajectory(robot.drive.currentSpeeds, robot.poseSubsystem.pose.rotation, RobotConfig.fromGUISettings())?.totalTimeSeconds!!
-      println("time now in periodic: ${timer.get()}")
-      println("pathstart: $pathstart")
-      println("expectedtime: $expectedtime")
-      alongpath = (timer.get() - pathstart) / expectedtime
-      println("alongpath: $alongpath")
-      var numsections = pathSub?.get()!!.size - 1
-      println("num sections: $numsections")
-      println("index thing: ${floor(alongpath * numsections) + 1}")
-      indexinpath = (floor(alongpath * numsections)).toInt()+1
-      if (alongpath < 1.0) {
-        prevpose = pathSub?.get()?.get(indexinpath-1)
-        nextpose = pathSub?.get()?.get(indexinpath)//
-        if (prevpose != null) {
-          if (nextpose != null) {
-            speednow = MagnetizePIDPoseAlign(robot.drive, robot.poseSubsystem, robot.poseSubsystem.pose, robot.driveController).calculate(robot.poseSubsystem.pose, nextpose!!)
-            println("speed now: ${speednow*1.0}")
-            robot.drive.set(speednow*1.0)
+      try {
+        expectedtime = path?.generateTrajectory(robot.drive.currentSpeeds, robot.poseSubsystem.pose.rotation, RobotConfig.fromGUISettings())?.totalTimeSeconds!!
+        println("time now in periodic: ${timer.get()}")
+        println("pathstart: $pathstart")
+        println("expectedtime: $expectedtime")
+        alongpath = (timer.get() - pathstart) / expectedtime!!
+        println("alongpath: $alongpath")
+        var numsections = pathSub?.get()!!.size - 1
+        println("num sections: $numsections")
+        println("index thing: ${floor(alongpath * numsections) + 1}")
+        indexinpath = (floor(alongpath * numsections)).toInt() + 1
+        if (alongpath < 1.0) {
+          prevpose = pathSub?.get()?.get(indexinpath - 1)
+          nextpose = pathSub?.get()?.get(indexinpath) //
+          if (prevpose != null) {
+            if (nextpose != null) {
+              speednow = MagnetizePIDPoseAlign(robot.drive, robot.poseSubsystem, robot.poseSubsystem.pose, robot.driveController).calculate(robot.poseSubsystem.pose, nextpose!!)
+              println("speed now: ${speednow * 1.0}")
+              robot.drive.set(speednow * 1.0)
+            }
           }
+          println()
         }
-        println()
-      }
-      if (indexinpath == pathSub?.get()!!.size) {
-        println("done")
+        if (indexinpath == pathSub?.get()!!.size) {
+          println("done")
+          alongpath = 0.0
+        }
+      } catch (e: Exception) {
+        println("path done probably")
         alongpath = 0.0
       }
     }
@@ -96,10 +98,11 @@ class pathfinder(val robot: Robot) : SubsystemBase() {
           RobotConstants.MAX_ACCEL,
           RobotConstants.MAX_ROT_SPEED,
           RobotConstants.ROT_RATE_LIMIT
-        ), GoalEndState(0.0, robot.poseSubsystem.pose.rotation)
+        ),
+        GoalEndState(0.0, robot.poseSubsystem.pose.rotation)
       )
       if (path != null) {
-        alongpath=0.0
+        alongpath = 0.0
         for (point in path!!.allPathPoints) {
           points += (point.maxV)
         }
@@ -112,15 +115,15 @@ class pathfinder(val robot: Robot) : SubsystemBase() {
   }
 
   fun path(goalPosition: Pose2d): Command {
-    //println("time now in path command: ${timer.get()}")
-    //println("index thing: $indexinpath")
+    // println("time now in path command: ${timer.get()}")
+    // println("index thing: $indexinpath")
     return runOnce({
       alongpath = 0.0
-      pathstart = timer.get()//
-      //println(robot.poseSubsystem.pose.translation)
+      pathstart = timer.get() //
+      // println(robot.poseSubsystem.pose.translation)
       adstar.setStartPosition(robot.poseSubsystem.pose.translation)
       adstar.setGoalPosition(goalPosition.translation)
-    }).andThen(PrintCommand("                                                                                                              goal updated"))//
-    //.andThen(PrintCommand("pathstart: $pathstart")).andThen(PrintCommand(" "))
+    }).andThen(PrintCommand("                                                                                                              goal updated")) //
+    // .andThen(PrintCommand("pathstart: $pathstart")).andThen(PrintCommand(" "))
   }
-}
+}