end of offseason :sob

AsianKoala · Sep 18, 2021 · 926cddb · 926cddb
1 parent 9d8e3b4
commit 926cddb
Show file tree

Hide file tree

Showing 25 changed files with 77 additions and 93 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/localization/Speedometer.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/localization/Speedometer.kt
@@ -11,7 +11,7 @@ class Speedometer {
     private var bestIndex = 0
 
     fun update(position: Pose): Pose {
-        return if(pastPositions.size == 0) {
+        return if (pastPositions.size == 0) {
             pastPositions.add(TimePose(position))
             Pose(Point.ORIGIN, Angle.EAST)
         } else {
@@ -28,6 +28,5 @@ class Speedometer {
 
             Pose(speeds, angvel)
         }
-
     }
-}
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/localization/SuperOdo.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/localization/SuperOdo.kt
@@ -18,7 +18,6 @@ class SuperOdo(private val startPose: Pose, private val startL: Int, private val
         @JvmField var K = 1.0
     }
 
-
     val kalmanFilter = KalmanFilter(startPose.h.angle, Q, R, P, K)
 
     private var currentPosition: Pose = startPose.copy
@@ -32,7 +31,6 @@ class SuperOdo(private val startPose: Pose, private val startL: Int, private val
     var lastIMUAngle = 0.0
     var imuBias = Angle.EAST
 
-
     fun update(azuTelemetry: AzusaTelemetry, currLeftEncoder: Int, currRightEncoder: Int, currPerpEncoder: Int, imuHeading: Double): Pose {
 
         val actualCurrLeft = -(currLeftEncoder - startL)
@@ -51,8 +49,7 @@ class SuperOdo(private val startPose: Pose, private val startL: Int, private val
         val angleIncrement = (lWheelDelta - rWheelDelta) / turnScalar
         val odocalcangle = -Angle(((leftTotal - rightTotal) / turnScalar), AngleUnit.RAD) + startPose.h + imuBias
 
-
-        if(System.currentTimeMillis() - mintime > lastIMURead) {
+        if (System.currentTimeMillis() - mintime > lastIMURead) {
             lastIMUAngle = imuHeading
             lastIMURead = System.currentTimeMillis()
 
@@ -64,7 +61,6 @@ class SuperOdo(private val startPose: Pose, private val startL: Int, private val
         val finalAngle = kalmanFilter.update(odocalcangle.angle, lastIMUAngle)
         azuTelemetry.addData("finalAngle: ", finalAngle)
 
-
         val perpPredict = angleIncrement * perpScalar
         val dx = aWheelDelta - perpPredict
 
@@ -79,4 +75,4 @@ class SuperOdo(private val startPose: Pose, private val startL: Int, private val
 
         return currentPosition
     }
-}
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/path/builders/PathBuilder.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/path/builders/PathBuilder.kt
@@ -5,4 +5,4 @@ import org.firstinspires.ftc.teamcode.control.path.Path
 abstract class PathBuilder {
 
     abstract fun build(): Path
-}
+}
diff --git a/...Code/src/main/java/org/firstinspires/ftc/teamcode/control/path/funcs/InterruptFunction.kt b/...Code/src/main/java/org/firstinspires/ftc/teamcode/control/path/funcs/InterruptFunction.kt
@@ -5,4 +5,4 @@ import org.firstinspires.ftc.teamcode.hardware.Azusa
 
 fun interface InterruptFunction : Func {
     fun run(azusa: Azusa, path: Path): Boolean
-}
+}
diff --git a/...Code/src/main/java/org/firstinspires/ftc/teamcode/control/path/funcs/LoopUntilFunction.kt b/...Code/src/main/java/org/firstinspires/ftc/teamcode/control/path/funcs/LoopUntilFunction.kt
@@ -5,4 +5,4 @@ import org.firstinspires.ftc.teamcode.hardware.Azusa
 
 fun interface LoopUntilFunction : Func {
     fun run(azusa: Azusa, path: Path): Boolean
-}
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/path/funcs/RepeatFunction.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/path/funcs/RepeatFunction.kt
@@ -5,4 +5,4 @@ import org.firstinspires.ftc.teamcode.hardware.Azusa
 
 fun interface RepeatFunction : Func {
     fun run(azusa: Azusa, path: Path)
-}
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/path/funcs/SimpleFunction.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/path/funcs/SimpleFunction.kt
@@ -5,4 +5,4 @@ import org.firstinspires.ftc.teamcode.hardware.Azusa
 
 fun interface SimpleFunction : Func {
     fun run(azusa: Azusa, path: Path)
-}
+}
diff --git a/.../src/main/java/org/firstinspires/ftc/teamcode/control/path/purepursuit/PurePursuitPath.kt b/.../src/main/java/org/firstinspires/ftc/teamcode/control/path/purepursuit/PurePursuitPath.kt
@@ -11,7 +11,6 @@ import org.firstinspires.ftc.teamcode.control.path.waypoints.Waypoint
 import org.firstinspires.ftc.teamcode.hardware.Azusa
 import org.firstinspires.ftc.teamcode.util.math.*
 import org.firstinspires.ftc.teamcode.util.math.MathUtil.toRadians
-import kotlin.math.absoluteValue
 
 class PurePursuitPath(waypoints: ArrayList<Waypoint>) : Path(waypoints) {
     override fun update(azusa: Azusa) {
@@ -43,7 +42,7 @@ class PurePursuitPath(waypoints: ArrayList<Waypoint>) : Path(waypoints) {
                 }
             }
         } while (skip && currWaypoint < waypoints.size - 1)
-        if(isFinished) return
+        if (isFinished) return
 
         val start = waypoints[currWaypoint]
         val end = waypoints[currWaypoint + 1]
@@ -57,16 +56,16 @@ class PurePursuitPath(waypoints: ArrayList<Waypoint>) : Path(waypoints) {
         azusa.azuTelemetry.addData("followpoint", target.p)
         azusa.azuTelemetry.addData("target", end)
         azusa.azuTelemetry.fieldOverlay()
-                .setStroke("white")
-                .strokeLine(azusa.currPose.p.dbNormalize.x, azusa.currPose.p.dbNormalize.y, target.p.dbNormalize.x, target.p.dbNormalize.y)
+            .setStroke("white")
+            .strokeLine(azusa.currPose.p.dbNormalize.x, azusa.currPose.p.dbNormalize.y, target.p.dbNormalize.x, target.p.dbNormalize.y)
 
         if ((end is StopWaypoint && azusa.currPose.distance(end) < end.followDistance) || end is PointTurnWaypoint) {
             PurePursuitController.goToPosition(azusa, end)
         } else {
             val (nx, ny) = target.p.dbNormalize
             azusa.azuTelemetry.fieldOverlay()
-                    .setStroke("purple")
-                    .strokeCircle(nx, ny, 1.0)
+                .setStroke("purple")
+                .strokeCircle(nx, ny, 1.0)
 
             val relTarget = PurePursuitController.relVals(azusa.currPose, target.p)
 
@@ -75,11 +74,9 @@ class PurePursuitPath(waypoints: ArrayList<Waypoint>) : Path(waypoints) {
             val deltaH = PurePursuitController.getDeltaH(azusa.currPose, target)
             val turnPower = deltaH / 90.0.toRadians
 
-
-
             // get perpendicular intersetion for x component of the robot
             val dClip = currPose.p.distance(clip)
-            if(dClip > 4) {
+            if (dClip > 4) {
                 val relClip = PurePursuitController.relVals(currPose, target.p)
                 val relMovement = relClip / 4.0
                 val finalMovement = (relMovement + movementPowers) / 2.0
@@ -89,4 +86,4 @@ class PurePursuitPath(waypoints: ArrayList<Waypoint>) : Path(waypoints) {
             }
         }
     }
-}
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/system/BaseAuto.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/system/BaseAuto.kt
@@ -2,8 +2,6 @@ package org.firstinspires.ftc.teamcode.control.system
 
 import com.qualcomm.robotcore.eventloop.opmode.Disabled
 import org.firstinspires.ftc.teamcode.control.path.Path
-import org.firstinspires.ftc.teamcode.control.path.PurePursuitController
-import org.firstinspires.ftc.teamcode.control.path.purepursuit.PurePursuitPath
 import org.firstinspires.ftc.teamcode.util.opmode.Globals
 import java.util.*
 

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/system/BaseOpMode.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/system/BaseOpMode.kt
@@ -40,12 +40,10 @@ abstract class BaseOpMode : TunableLinearOpMode() {
 
         azusaTelemetry = AzusaTelemetry(this)
 
-
         val packet = OpModePacket(startPose, debugging, hardwareMap, azusaTelemetry, gamepad1, gamepad2)
         if ((internalOpModeServices as OpModeManagerImpl).activeOpModeName.startsWith("Azusa", true)) {
             azusa = Azusa(packet)
             robot = azusa
-
         } else {
             firefly = Firefly(packet)
             robot = firefly

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/system/StateMachine.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/system/StateMachine.kt
@@ -10,17 +10,17 @@ open class StateMachine(private val states: ArrayList<State>) {
     fun run() {
         if (!killCond()) {
             var done = 0
-           activeStates.forEach {
-               it.value.update()
-               if(deadStates.containsKey(it.key)) {
-                   it.value.onKill()
-                   activeStates.remove(it.key)
-               }
+            activeStates.forEach {
+                it.value.update()
+                if (deadStates.containsKey(it.key)) {
+                    it.value.onKill()
+                    activeStates.remove(it.key)
+                }
 
-               if(it.value.killed) {
-                   done++
-               }
-           }
+                if (it.value.killed) {
+                    done++
+                }
+            }
             defaultKillCond = done < activeStates.size
         }
     }
@@ -30,7 +30,7 @@ open class StateMachine(private val states: ArrayList<State>) {
     }
 
     fun forceKillState(name: String) {
-        if(activeStates.containsKey(name)) {
+        if (activeStates.containsKey(name)) {
             deadStates[name] = activeStates[name]!!
         }
     }

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/test/KalmanFilter.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/test/KalmanFilter.kt
@@ -4,7 +4,7 @@ class KalmanFilter(var x: Double, val Q: Double, val R: Double, var P: Double, v
 
     private var xprev = x
     private var pprev = P
-    private  var u = 0.0
+    private var u = 0.0
     private var z = 0.0
 
     fun update(inp1: Double, inp2: Double): Double {
@@ -20,4 +20,4 @@ class KalmanFilter(var x: Double, val Q: Double, val R: Double, var P: Double, v
         pprev = P
         return x
     }
-}
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/test/PIDFController.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/control/test/PIDFController.kt
@@ -1,8 +1,6 @@
 package org.firstinspires.ftc.teamcode.control.test
 
 import kotlin.math.abs
-import kotlin.math.max
-import kotlin.math.min
 import kotlin.math.sign
 
 class PIDFController(val kp: Double, val ki: Double, val kd: Double, val kv: Double, val ka: Double, val ks: Double) {
@@ -36,7 +34,7 @@ class PIDFController(val kp: Double, val ki: Double, val kd: Double, val kv: Dou
         val error = target - position
         val timestamp = System.currentTimeMillis()
 
-        return if(lastupdate.toInt() == 0) {
+        return if (lastupdate.toInt() == 0) {
             lasterror = error
             lastupdate = timestamp
             0.0
@@ -51,7 +49,7 @@ class PIDFController(val kp: Double, val ki: Double, val kd: Double, val kv: Dou
             val baseoutput = kp * error + ki * errorsum + kd * deriv + (refvel ?: targetVel) * kv + targetAccel * ka
             val output = if (abs(baseoutput) < 0.0000001) 0.0 else baseoutput + baseoutput.sign * ks
 
-            if(bounded) {
+            if (bounded) {
                 when {
                     output < lowerbound -> lowerbound
                     output > upperbound -> upperbound
@@ -62,4 +60,4 @@ class PIDFController(val kp: Double, val ki: Double, val kd: Double, val kv: Dou
             }
         }
     }
-}
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Azusa.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Azusa.kt
@@ -2,8 +2,6 @@ package org.firstinspires.ftc.teamcode.hardware
 
 import com.acmerobotics.dashboard.FtcDashboard
 import com.acmerobotics.dashboard.config.Config
-import com.qualcomm.robotcore.hardware.Gamepad
-import com.qualcomm.robotcore.hardware.HardwareMap
 import com.qualcomm.robotcore.util.Range
 import org.firstinspires.ftc.teamcode.control.localization.Speedometer
 import org.firstinspires.ftc.teamcode.control.localization.ThreeWheelOdometry
@@ -18,7 +16,6 @@ import org.openftc.revextensions2.ExpansionHubEx
 import org.openftc.revextensions2.ExpansionHubMotor
 import org.openftc.revextensions2.RevBulkData
 import java.util.*
-import kotlin.collections.ArrayList
 import kotlin.math.*
 
 @Config
@@ -81,7 +78,6 @@ class Azusa(dataPacket: OpModePacket) : BaseRobot(dataPacket) {
     override fun update() {
         if (dataPacket.debugging) {
             debugPhysics()
-
         } else {
             updateOdo()
         }

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/BaseRobot.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/BaseRobot.kt
@@ -6,4 +6,4 @@ abstract class BaseRobot(val dataPacket: OpModePacket) {
     abstract fun init()
     abstract fun update()
     val allHardware: ArrayList<Hardware> = ArrayList()
-}
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Firefly.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Firefly.kt
@@ -45,5 +45,5 @@ class Firefly(dataPacket: OpModePacket) : BaseRobot(dataPacket) {
         intake.setPowers(dataPacket.gamepad.left_bumper.b2d, dataPacket.gamepad.right_bumper.b2d)
     }
 
-    private val Boolean.b2d get() = if(this) 1.0 else 0.0
+    private val Boolean.b2d get() = if (this) 1.0 else 0.0
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.kt
@@ -4,7 +4,7 @@ import com.qualcomm.robotcore.hardware.DcMotor
 import org.firstinspires.ftc.teamcode.util.opmode.AzusaTelemetry
 import org.openftc.revextensions2.ExpansionHubMotor
 
-class Intake(private val leftIntake: ExpansionHubMotor, private val rightIntake: ExpansionHubMotor): Hardware() {
+class Intake(private val leftIntake: ExpansionHubMotor, private val rightIntake: ExpansionHubMotor) : Hardware() {
     private var leftPower = 0.0
     private var rightPower = 0.0
 
@@ -22,4 +22,4 @@ class Intake(private val leftIntake: ExpansionHubMotor, private val rightIntake:
         leftIntake.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER
         rightIntake.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER
     }
-}
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AzusaAuto.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AzusaAuto.kt
@@ -3,8 +3,6 @@ package org.firstinspires.ftc.teamcode.opmodes
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous
 import org.firstinspires.ftc.teamcode.control.path.Path
 import org.firstinspires.ftc.teamcode.control.path.builders.PurePursuitBuilder
-import org.firstinspires.ftc.teamcode.control.path.funcs.LoopUntilFunction
-import org.firstinspires.ftc.teamcode.control.path.funcs.SimpleFunction
 import org.firstinspires.ftc.teamcode.control.path.waypoints.LockedWaypoint
 import org.firstinspires.ftc.teamcode.control.path.waypoints.StopWaypoint
 import org.firstinspires.ftc.teamcode.control.path.waypoints.Waypoint
@@ -24,18 +22,18 @@ class AzusaAuto : BaseAuto() {
 
     override fun initialPath(): Path {
         return PurePursuitBuilder().addPoint(Waypoint(38.0, 58.0, 0.0))
-                .addPoint(Waypoint(15.0, 54.0, 14.0))
-                .addPoint(Waypoint(0.0, 46.0, 14.0))
-                .addPoint(LockedWaypoint(-8.0, 32.0, 14.0, Angle(270.0.toRadians, AngleUnit.RAD)))
-                .addPoint(LockedWaypoint(-8.0, 10.0, 14.0, Angle(270.0.toRadians, AngleUnit.RAD)))
-                .addPoint(LockedWaypoint(-8.0, -8.0, 14.0, Angle(270.0.toRadians, AngleUnit.RAD)))
-                .addPoint(Waypoint(0.0, -10.0, 14.0))
-                .addPoint(Waypoint(12.0, -2.0, 12.0))
-                .addPoint(Waypoint(16.0, 6.0, 12.0))
-                .addPoint(Waypoint(12.0, 16.0, 12.0))
-                .addPoint(Waypoint(0.0, 28.0, 10.0))
-                .addPoint(Waypoint(6.0, 42.0, 10.0))
-                .addPoint(Waypoint(5.0, 5.0, 10.0))
-                .addPoint(StopWaypoint(28.0, 54.0, 10.0, Angle(PI, AngleUnit.RAD), Angle(3.0.toRadians, AngleUnit.RAD))).build()
+            .addPoint(Waypoint(15.0, 54.0, 14.0))
+            .addPoint(Waypoint(0.0, 46.0, 14.0))
+            .addPoint(LockedWaypoint(-8.0, 32.0, 14.0, Angle(270.0.toRadians, AngleUnit.RAD)))
+            .addPoint(LockedWaypoint(-8.0, 10.0, 14.0, Angle(270.0.toRadians, AngleUnit.RAD)))
+            .addPoint(LockedWaypoint(-8.0, -8.0, 14.0, Angle(270.0.toRadians, AngleUnit.RAD)))
+            .addPoint(Waypoint(0.0, -10.0, 14.0))
+            .addPoint(Waypoint(12.0, -2.0, 12.0))
+            .addPoint(Waypoint(16.0, 6.0, 12.0))
+            .addPoint(Waypoint(12.0, 16.0, 12.0))
+            .addPoint(Waypoint(0.0, 28.0, 10.0))
+            .addPoint(Waypoint(6.0, 42.0, 10.0))
+            .addPoint(Waypoint(5.0, 5.0, 10.0))
+            .addPoint(StopWaypoint(28.0, 54.0, 10.0, Angle(PI, AngleUnit.RAD), Angle(3.0.toRadians, AngleUnit.RAD))).build()
     }
 }