cleaned up a lot of unclear variable names and i understand it now

blair-robot-project · Feb 7, 2025 · 3cf7a48 · 3cf7a48
1 parent 3afe28f
commit 3cf7a48
Show file tree

Hide file tree

Showing 4 changed files with 56 additions and 61 deletions.
diff --git a/src/main/kotlin/frc/team449/ControllerBindings.kt b/src/main/kotlin/frc/team449/ControllerBindings.kt
@@ -71,8 +71,8 @@ class ControllerBindings(
 //    robot.driveController.x().onTrue(robot.pathfinder.path(AutoScoreCommandConstants.reef1PoseBlue))
 //    println(robot.pathfinder.pathpoints(AutoScoreCommandConstants.reef1PoseBlue))
 
-    robot.driveController.x().onTrue(PrintCommand("invoke path").andThen(robot.pathfinder.path(AutoScoreCommandConstants.reef1PoseBlue)))
-    robot.driveController.a().onTrue(PrintCommand("invoke other path").andThen(robot.pathfinder.path(AutoScoreCommandConstants.reef1PoseRed)))
+    robot.driveController.x().onTrue(PrintCommand("blue path").andThen(robot.pathfinder.path(AutoScoreCommandConstants.reef1PoseBlue)))
+    robot.driveController.a().onTrue(PrintCommand("red path").andThen(robot.pathfinder.path(AutoScoreCommandConstants.reef1PoseRed)))
 //    var reefPose = AutoScoreCommandConstants.testPose
 //    val constraints = PathConstraints(
 //      3.0,

diff --git a/src/main/kotlin/frc/team449/Robot.kt b/src/main/kotlin/frc/team449/Robot.kt
@@ -2,7 +2,7 @@ package frc.team449
 
 import edu.wpi.first.wpilibj.PowerDistribution
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController
-import frc.team449.commands.autoscoreCommands.pathfinder
+import frc.team449.commands.autoscoreCommands.AutoScorePathfinder
 import frc.team449.subsystems.RobotConstants
 import frc.team449.subsystems.drive.swerve.SwerveDrive
 import frc.team449.subsystems.drive.swerve.SwerveOrthogonalCommand
@@ -58,5 +58,5 @@ class Robot : RobotBase(), Logged {
 
   val light = createLight()
 
-  val pathfinder = pathfinder(this)
+  val pathfinder = AutoScorePathfinder(this)
 }
diff --git a/src/main/kotlin/frc/team449/RobotLoop.kt b/src/main/kotlin/frc/team449/RobotLoop.kt
@@ -2,20 +2,12 @@ package frc.team449
 
 import com.ctre.phoenix6.SignalLogger
 import com.pathplanner.lib.auto.AutoBuilder
-import com.pathplanner.lib.config.ModuleConfig
 import com.pathplanner.lib.config.PIDConstants
 import com.pathplanner.lib.config.RobotConfig
 import com.pathplanner.lib.controllers.PPHolonomicDriveController
-import com.pathplanner.lib.path.PathConstraints
-import com.pathplanner.lib.pathfinding.LocalADStar
 import com.pathplanner.lib.pathfinding.Pathfinding
 import edu.wpi.first.hal.FRCNetComm
 import edu.wpi.first.hal.HAL
-import edu.wpi.first.math.geometry.Pose2d
-import edu.wpi.first.math.geometry.Rotation2d
-import edu.wpi.first.math.geometry.Translation2d
-import edu.wpi.first.math.system.plant.DCMotor
-import edu.wpi.first.math.util.Units
 import edu.wpi.first.wpilibj.*
 import edu.wpi.first.wpilibj.DriverStation.Alliance
 import edu.wpi.first.wpilibj.RobotBase
@@ -25,11 +17,9 @@ import frc.team449.auto.RoutineChooser
 import frc.team449.commands.autoscoreCommands.AutoScoreCommandConstants
 import frc.team449.commands.autoscoreCommands.AutoScoreCommands
 import frc.team449.commands.autoscoreCommands.WebConnection
-import frc.team449.commands.autoscoreCommands.pathfinder
 import frc.team449.commands.light.BlairChasing
 import frc.team449.commands.light.BreatheHue
 import frc.team449.commands.light.Rainbow
-import frc.team449.subsystems.drive.swerve.SwerveConstants
 import frc.team449.subsystems.drive.swerve.SwerveSim
 import frc.team449.subsystems.elevator.ElevatorFeedForward.Companion.createElevatorFeedForward
 import frc.team449.subsystems.pivot.PivotFeedForward.Companion.createPivotFeedForward
@@ -111,7 +101,7 @@ class RobotLoop : TimedRobot(), Logged {
     Monologue.setupMonologue(this, "/Monologuing", false, false)
 
     URCL.start()
-    Pathfinding.setPathfinder(robot.pathfinder.adstar)
+    Pathfinding.setPathfinder(robot.pathfinder.ADStar)
   }
 
   override fun driverStationConnected() {
@@ -178,7 +168,7 @@ class RobotLoop : TimedRobot(), Logged {
   override fun testPeriodic() {}
 
   override fun simulationInit() {
-    Pathfinding.setPathfinder(robot.pathfinder.adstar)
+    Pathfinding.setPathfinder(robot.pathfinder.ADStar)
   }
 
   override fun simulationPeriodic() {

diff --git a/.../commands/autoscoreCommands/pathfinder.kt → .../autoscoreCommands/AutoScorePathfinder.kt b/.../commands/autoscoreCommands/pathfinder.kt → .../autoscoreCommands/AutoScorePathfinder.kt
@@ -8,73 +8,80 @@ import edu.wpi.first.math.geometry.Pose2d
 import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.math.geometry.Translation2d
 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.InstantCommand
 import kotlin.math.floor
 
-class pathfinder(val robot: Robot) : SubsystemBase() {
-  var adstar = LocalADStar()
-  var pathPub: StructArrayPublisher<Pose2d>? = null
-  var velsPub: DoubleArrayPublisher? = null
-  var pathSub: StructArraySubscriber<Pose2d>? = null
-  var velsSub: DoubleArraySubscriber? = null
-  var path: PathPlannerPath? = null
-  var points: DoubleArray = doubleArrayOf(0.0, 0.0)
+class AutoScorePathfinder(val robot: Robot) : SubsystemBase() {
+  var ADStar = LocalADStar()
+  //lateinit is necessary here
+  private lateinit var pathPub: StructArrayPublisher<Pose2d>
+  private lateinit var velsPub: DoubleArrayPublisher
+  private lateinit var pathSub: StructArraySubscriber<Pose2d>
+  private lateinit var velsSub: DoubleArraySubscriber
+  private lateinit var path: PathPlannerPath
+  private var velocities: DoubleArray = doubleArrayOf()
   private val timer = Timer()
-  var expectedtime = 0.0
-  var pathstart = 0.0
-  var poseafter: Pose2d = Pose2d(Translation2d(0.0, 0.0), Rotation2d(0.0))
-  var indexinpath = 0
+  private var ET = 0.0 //expected time
+  private var startingTime = 0.0
+  private val zeroPose = Pose2d(Translation2d(0.0, 0.0), Rotation2d(0.0))
+  private var poseEnd = zeroPose
+  private var pathIndex = 0
+  private var lastTimeCalled = 0.0
 
   init {
     timer.restart()
     pathPub = NetworkTableInstance.getDefault().getStructArrayTopic("/activePath", Pose2d.struct).publish(*arrayOf<PubSubOption>())
-    velsPub = NetworkTableInstance.getDefault().getDoubleArrayTopic("/pathvels").publish(*arrayOf<PubSubOption>())
+    velsPub = NetworkTableInstance.getDefault().getDoubleArrayTopic("/pathVelocities").publish(*arrayOf<PubSubOption>())
 
     pathSub =
       NetworkTableInstance.getDefault().getStructArrayTopic("/activePath", Pose2d.struct)
-        .subscribe(arrayOf(Pose2d(Translation2d(0.0, 0.0), Rotation2d(0.0)), Pose2d(Translation2d(0.0, 0.0), Rotation2d(0.0))))
-    velsSub = NetworkTableInstance.getDefault().getDoubleArrayTopic("/pathvels").subscribe(doubleArrayOf(0.0, 0.0))
+        .subscribe(arrayOf(zeroPose, zeroPose))
+    velsSub = NetworkTableInstance.getDefault().getDoubleArrayTopic("/pathVelocities").subscribe(doubleArrayOf())
+    PathPlannerPath.clearCache()
+  }
+
+  private fun resetVelocities() {
+    velocities = doubleArrayOf()
   }
 
   override fun periodic() {
-    //println("first pose: ${pathSub?.get()?.get(0)}")
-    //println()
-    if ((pathSub?.get()?.get(0) ?: Pose2d()) != Pose2d(Translation2d(0.0, 0.0), Rotation2d(0.0))) {
-      expectedtime = (pathSub?.get()!!.size - 1) * 0.02 //num sections*time/section
-      println("time now in periodic: ${timer.get()}")
-      println("pathstart: $pathstart")
-      println("expectedtime: $expectedtime")
-      var alongpath = (timer.get() - pathstart) / expectedtime
-      println("alongpath: $alongpath")
-      println("num sections: ${pathSub?.get()!!.size - 1}")
-      indexinpath = floor(alongpath / (1 / (pathSub?.get()!!.size - 1))).toInt() + 1
-      println("index thing: $indexinpath")
-      println()
+    val currentTime = timer.get()
+    val dt = currentTime - lastTimeCalled
+    lastTimeCalled = currentTime
+    if (dt > 0.2) /*command called a new time*/ {
+      startingTime = timer.get()
     }
-    if (adstar.isNewPathAvailable) {
-      path = adstar.getCurrentPath(
+    val sections = pathSub.get().size - 1
+    if ((pathSub.get()[0]) != zeroPose) {
+      ET = (pathSub.get().size - 1) * 0.02 //num sections*time/section
+      println("ct: $currentTime")
+      println("st: $startingTime, et: $ET")
+      // pathCompletion ranging from 0 to 1
+      val pathCompletion = (timer.get() - startingTime) / ET
+      pathIndex = floor(pathCompletion * sections).toInt()
+      println("index thing: $pathIndex")
+    }
+    if (ADStar.isNewPathAvailable) {
+      path = ADStar.getCurrentPath(
         PathConstraints(
           RobotConstants.MAX_LINEAR_SPEED,
           RobotConstants.MAX_ACCEL,
           RobotConstants.MAX_ROT_SPEED,
           RobotConstants.ROT_RATE_LIMIT
         ), GoalEndState(0.0, robot.poseSubsystem.pose.rotation)
       )
-      if (path != null) {
-//        expectedtime = (pathSub?.get()!!.size - 1) * 0.02 //num sections*time/section
-        for (point in path!!.allPathPoints) {
-          points += (point.maxV)
-        }
-        pathPub?.set(path!!.pathPoses.toTypedArray<Pose2d>())
-        velsPub?.set(points)
-
+      resetVelocities()
+      for (point in path.allPathPoints) {
+        velocities += (point.maxV)
       }
+      pathPub.set(path.pathPoses.toTypedArray<Pose2d>())
+      velsPub.set(velocities)
+
     }
 //    if (pathSub?.get()!=null) {
 //      println("num poses: ${pathSub?.get()!!.size}")
@@ -87,7 +94,7 @@ class pathfinder(val robot: Robot) : SubsystemBase() {
 //      println()
 //      println()
 //    }
-    if (velsSub?.get() != null) {
+    if (velsSub.get() != null) {
 //      for (vel in velsSub?.get()!!) {
 //        print("vel: $vel")
 //      }
@@ -103,10 +110,8 @@ class pathfinder(val robot: Robot) : SubsystemBase() {
     //println("time now in path command: ${timer.get()}")
     //println("index thing: $indexinpath")
     return runOnce({
-      pathstart = timer.get()+0.04
-      //println(robot.poseSubsystem.pose.translation)
-      adstar.setStartPosition(robot.poseSubsystem.pose.translation)
-      adstar.setGoalPosition(goalPosition.translation)
+      ADStar.setStartPosition(robot.poseSubsystem.pose.translation)
+      ADStar.setGoalPosition(goalPosition.translation)
     }).andThen(PrintCommand("goal updated"))
     //.andThen(PrintCommand("pathstart: $pathstart")).andThen(PrintCommand(" "))
   } }