making it work(ish) again

src/main/deploy/paths/goToAmpFile.json

@@ -1,21 +1,6 @@
 {"commands" : [
   [
     "gotoAmp"
-  ],
-  [
-    "gotoSpeakerLeft"
-  ],
-  [
-    "gotoAmp"
-  ],
-  [
-    "gotoSpeakerLeft"
-  ],
-  [
-    "gotoAmp"
-  ],
-  [
-    "gotoSpeakerLeft"
   ]
 ]
 }

src/main/kotlin/frc/lib/FieldPositions.kt

@@ -22,7 +22,8 @@ object FieldPositions {
 
     }
 
-    val amp = FieldLocation(Pose2d(14.929358, 8.2042, 270.0.rotation2dFromDeg()),
+    val amp = FieldLocation(
+            Pose2d(14.929358, 8.2042, 270.0.rotation2dFromDeg()),
             Pose2d(1.84404, 8.2042, 270.0.rotation2dFromDeg()))
 
     val sourceBaseline = FieldLocation(
@@ -38,7 +39,7 @@ object FieldPositions {
 
     //obstacles do need to be iterated through, but they do not need to have names
     val obstacles:Array<Obstacle> = arrayOf(
-        Obstacle(Pose2d(1.0, 1.0, 0.0.rotation2dFromDeg()), 1.0),
+        Obstacle(Pose2d(4.6, 4.2, 0.0.rotation2dFromDeg()), 1.5)
         //list obstacles here...
     )
 

src/main/kotlin/frc/lib/swerve/SwerveDriveBase.kt

@@ -20,7 +20,7 @@ import kotlin.math.IEEErem
 import kotlin.math.PI
 
 class SwerveDriveBase(startingPose: Pose2d) : SubsystemBase() {
-    private val field = Field2d()
+    val field = Field2d()
     private val shuffleboardTab = Shuffleboard.getTab("Drivetrain")
     private val gyroInputs = GyroIOInputsAutoLogged()
     private var poseEstimator: SwerveDrivePoseEstimator
@@ -63,7 +63,7 @@ class SwerveDriveBase(startingPose: Pose2d) : SubsystemBase() {
 
     private val camera: Camera
     init {
-        field.getObject("Obstacle").pose = Pose2d(1.0,2.0,0.0.rotation2dFromDeg())
+        field.getObject("Obstacle").pose = Pose2d(4.6,4.2,0.0.rotation2dFromDeg())
         SwerveDriveConstants.DrivetrainConsts.thetaPIDController.enableContinuousInput(
                 180.0, -180.0
         )

src/main/kotlin/frc/robot/RobotContainer.kt

@@ -70,8 +70,6 @@ class RobotContainer {
                 goal.red
             else
                 goal.blue
-        var midpoint = Pose2d()
-
         return gotoPose(to)
     }
 
@@ -85,46 +83,11 @@ class RobotContainer {
 
      */
     fun gotoPose(to: Pose2d):Command {
-        val midpoint = AtomicReference(Pose2d(0.0, 0.0, 0.0.rotation2dFromDeg()))
-        val points = AtomicReference(mutableListOf<Pose2d>())
         val toRun = AtomicReference(SequentialCommandGroup())
          return runOnce({
-            val pose = driveSubsystem.getPose()
-             thingy.setDouble(pose.x)
-            val m: Double = (to.y - pose.y) / (to.x - pose.x)
-            val b: Double = -m*to.x + to.y
-
-            for (obstacle in obstacles) {
-                val ntwo = b - obstacle.location.y
-                val obx = obstacle.location.x
-                val btwo = -obx * 2 + ntwo * 2 * m
-                val a = m.pow(2) + 1
-                val c = ntwo.pow(2) + obx.pow(2) - obstacle.radiusMeters.pow(2)
-                val det = btwo.pow(2) - 4 * a * c
-                if (det >= 0) {
-                    val intersection: Pose2d = if (pose.x > obstacle.location.x) Pose2d(
-                        ((-btwo + sqrt(det)) / (2 * a)), (m * ((-btwo + sqrt(det)) / (2 * a)) + b), pose.rotation
-                    ) else Pose2d(
-                        ((-btwo - sqrt(det)) / (2 * a)), (m * ((-btwo + sqrt(det)) / (2 * a)) + b), pose.rotation
-                    )
-                    midpoint.set(Pose2d(
-                        //FIX THIS SO IT DOESNt COLLIDE WITH WALLS
-                        intersection.x + SwerveDriveConstants.DrivetrainConsts.TRACK_WIDTH_METERS+obstacle.radiusMeters,
-                        -1 / m * (intersection.x + SwerveDriveConstants.DrivetrainConsts.TRACK_WIDTH_METERS + obstacle.radiusMeters - midpoint.get().x) + midpoint.get().y,
-                        pose.rotation
-                    ))
-                    //REWRITE!!!
-                    val pts = points.get()
-                    pts.add(midpoint.get())
-                    points.set(pts)
-                }
-            }
-             val pts = points.get()
-             pts.add(to)
-             points.set(pts)
-
+            val points = pathfind(driveSubsystem.getPose(), to)
              val sqi = toRun.get()
-             for (point in points.get()){
+             for (point in points){
                  sqi.addCommands(SwerveAutoDrive(
                      {point},
                      Pose2d(0.1,0.1,10.0.rotation2dFromDeg()),
@@ -140,6 +103,40 @@ class RobotContainer {
         )
     }
 
+    fun pathfind(from:Pose2d, to:Pose2d):List<Pose2d> {
+        val pose = from
+        val m: Double = (to.y - pose.y) / (to.x - pose.x)
+        val b: Double = -m*to.x + to.y
+        for (obstacle in obstacles) {
+            val ntwo = b - obstacle.location.y
+            val obx = obstacle.location.x
+            val btwo = -obx * 2 + ntwo * 2 * m
+            val a = m.pow(2) + 1
+            val c = ntwo.pow(2) + obx.pow(2) - obstacle.radiusMeters.pow(2)
+            val det = btwo.pow(2) - 4 * a * c
+            if (det >= 0) {
+                val intersection: Pose2d = if (pose.x > obstacle.location.x) Pose2d(
+                    ((-btwo + sqrt(det)) / (2 * a)), (m * ((-btwo + sqrt(det)) / (2 * a)) + b), pose.rotation
+                ) else Pose2d(
+                    ((-btwo - sqrt(det)) / (2 * a)), (m * ((-btwo + sqrt(det)) / (2 * a)) + b), pose.rotation
+                )
+                if(intersection.x in pose.x..to.x || intersection.x in to.x..pose.x) {
+                    val midpoint =
+                        Pose2d(
+                            //FIX THIS SO IT DOESNt COLLIDE WITH WALLS
+                            intersection.x + SwerveDriveConstants.DrivetrainConsts.TRACK_WIDTH_METERS + obstacle.radiusMeters,
+                            -1 / m * (intersection.x + SwerveDriveConstants.DrivetrainConsts.TRACK_WIDTH_METERS + obstacle.radiusMeters),
+                            pose.rotation
+                        )
+                    driveSubsystem.field.getObject(midpoint.x.toString()).pose = intersection
+                    return listOf(pathfind(from, midpoint), pathfind(midpoint, to)).flatten()
+
+                }
+            }
+        }
+        return listOf(to)
+    }
+
 
     fun goToSpeakerCloser(): Command {
         var startingPose = Pose2d()