State Machine Diffy

Cleaner + allows live updating finally

Author: AshOnDiscord

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystems/drive/AutoPickup.kt

@@ -60,14 +60,6 @@ class AutoPickup(
 
                 lastTarget = targetPose to (actualAngle / 360)
 
-//                robot.intake.diffy.roll = Diffy.hoverRoll + actualAngle / 360
-//                println("${Diffy.hoverRoll + actualAngle / 360} $actualAngle ${target.angle}")
-//                if (target.angle > 1) {
-//                    robot.intake.diffy.roll += rollSpeed * robot.deltaTime.deltaTime * target.angle
-//                } else if (target.angle < -1) {
-//                    robot.intake.diffy.roll += rollSpeed * robot.deltaTime.deltaTime * target.angle
-//                }
-
                 robot.telemetry.addData("target roll", Diffy.hoverRoll + actualAngle / 360)
                 robot.telemetry.addData("actual angle", actualAngle)
                 robot.telemetry.addData("target angle", target.angle)
@@ -81,17 +73,9 @@ class AutoPickup(
         }
     }
 
-    fun startScanning() =
-        InstantCommand({
-            scanning = true
-//            robot.intake.diffy.isManual = true
-        })
+    fun startScanning() = InstantCommand({ scanning = true })
 
-    fun stopScanning() =
-        InstantCommand({
-            scanning = false
-//            robot.intake.diffy.isManual = false
-        })
+    fun stopScanning() = InstantCommand({ scanning = false })
 
     fun rumble() {
         if (lastTarget == null) return
@@ -114,7 +98,8 @@ class AutoPickup(
                     robot.drive.pidManager.isOn = true
                     robot.drive.pidManager.isSamplePickup = true
                     robot.drive.pidManager.target = target.first
-                    robot.intake.diffy.pitch = Diffy.pickupPitch
+
+                    robot.intake.diffy.state = Diffy.State.PICKUP
                     robot.intake.diffy.roll = Diffy.hoverRoll + target.second
                 }),
                 WaitUntilCommand(robot.drive.pidManager::atTarget),

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystems/intake/Diffy.kt

@@ -11,61 +11,67 @@ import org.firstinspires.ftc.teamcode.common.utils.hardware.AxonCR
 class Diffy(
     val robot: Robot,
 ) : Subsystem() {
-    val leftServo = AxonCR(robot.hardware, "diffyLeft", "analog2", false)
-    val rightServo = AxonCR(robot.hardware, "diffyRight", "analog1")
+    enum class State {
+        TRANSFER,
+        HOVER,
+        PICKUP,
+        SWEEP,
+        MANUAL,
+        DIRECT,
+    }
+
+    val left = AxonCR(robot.hardware, "diffyLeft", "analog2", false)
+    val right = AxonCR(robot.hardware, "diffyRight", "analog1")
 
-    var isManual: Boolean = false
+    var state: State = State.TRANSFER
 
-    // each only has an effective range of +/- 0.25 aka 0-0.5 or half
-    // as you need to ensure that pitch + roll never exceeds the bounds of 0 to 1
-    var pitch: Double = transferPitch // up and down, -1 to 1
-    var roll: Double = transferRoll // rotate, -1 to 1
+    var pitch: Double = transferPitch // up and down, ideally -1 to 1
+    var roll: Double = transferRoll // rotate, ideally -1 to 1
 
-    override fun init() {
-//        periodic()
+    override fun init() {}
+
+    // run in a loo during init
+    fun initLoopable() {
+        periodic()
     }
 
     val pidLeft = PIDController(kP, kI, kD)
     val pidRight = PIDController(kP, kI, kD)
 
     override fun periodic() {
-        if (isManual) {
-            return
-        }
+        if (state == State.DIRECT) return // direct control
 
         pidLeft.setPID(kP, kI, kD)
         pidRight.setPID(kP, kI, kD)
 
-        val pitchPosition = pitch
-        val leftPosition = pitchPosition - roll
-        val rightPosition = pitchPosition + roll
-
-        leftServo.power = pidLeft.calculate(leftServo.position, leftPosition)
-        rightServo.power = pidLeft.calculate(rightServo.position, rightPosition)
+        val (pitch, roll) =
+            when (state) {
+                State.TRANSFER -> transferPitch to transferRoll
+                State.HOVER -> hoverPitch to hoverPitch // use pickup for when you're autorotating
+                State.PICKUP -> pickupPitch to this.roll
+                State.SWEEP -> sweepPitch to sweepRoll
+                else -> return
+            }
+
+        // make sure relative offsets work
+        this.pitch = pitch
+        this.roll = roll
+
+        val leftTarget = pitch - roll
+        val rightTarget = pitch + roll
+        left.power = pidLeft.calculate(left.position, leftTarget)
+        right.power = pidLeft.calculate(right.position, rightTarget)
     }
 
-    fun transfer() =
-        InstantCommand({
-            pitch = transferPitch
-            roll = transferRoll
-        })
-
-    fun hover() =
-        InstantCommand({
-            pitch = hoverPitch
-            roll = hoverRoll
-        })
-
-    fun pickup() =
-        InstantCommand({
-            pitch = pickupPitch
-        })
-
-    fun sweep() =
-        InstantCommand({
-            pitch = sweepPitch
-            roll = sweepRoll
-        })
+    fun transfer() = InstantCommand({ state = State.TRANSFER })
+
+    fun hover() = InstantCommand({ state = State.HOVER })
+
+    fun pickup() = InstantCommand({ state = State.PICKUP })
+
+    fun sweep() = InstantCommand({ state = State.SWEEP })
+
+    fun manual() = InstantCommand({ state = State.MANUAL })
 
     companion object {
         @JvmField

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/FasterSampleAuton.kt

@@ -34,7 +34,6 @@ class FasterSampleAuton : OpMode() {
         robot.outtake.claw.close()
         robot.outtake.claw.periodic()
         robot.intake.arm.periodic()
-        robot.intake.diffy.periodic()
         robot.intake.claw.periodic()
 
         val grab = {
@@ -148,6 +147,10 @@ class FasterSampleAuton : OpMode() {
         )
 
         schedule(SequentialCommandGroup(*commands.toTypedArray()))
+
+        while (!isStarted()) {
+            robot.intake.diffy.initLoopable()
+        }
     }
 
     companion object {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/SampleAuton.kt

@@ -34,7 +34,6 @@ class SampleAuton : OpMode() {
         robot.outtake.claw.close()
         robot.outtake.claw.periodic()
         robot.intake.arm.periodic()
-        robot.intake.diffy.periodic()
         robot.intake.claw.periodic()
 
         val grab = {
@@ -144,6 +143,10 @@ class SampleAuton : OpMode() {
         )
 
         schedule(SequentialCommandGroup(*commands.toTypedArray()))
+
+        while (!isStarted()) {
+            robot.intake.diffy.initLoopable()
+        }
     }
 
     companion object {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/auton/SpecimenAuton.kt

@@ -56,7 +56,6 @@ class SpecimenAuton : OpMode() {
             outtake.claw.close()
             outtake.claw.periodic()
             intake.arm.periodic()
-//            intake.diffy.periodic()
             intake.claw.periodic()
 
             fun readySpecimen() =
@@ -155,6 +154,10 @@ class SpecimenAuton : OpMode() {
                     park(),
                 ),
             )
+
+            while (!isStarted()) {
+                intake.diffy.initLoopable()
+            }
         }
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/tuning/CameraTuner.kt

@@ -212,9 +212,6 @@ class SampleDectectionTuner : OpMode() {
         }
         if (targetPose != null) {
             if (rotationEnabled) {
-//                if (targetAngle <= angleThres) {
-//                    robot.intake.diffy.roll = targetAngle
-//                }
                 val newAngle = -Math.toRadians(robot.intake.diffy.roll * 90)
                 rotationalOffset = Vec2d(cos(newAngle), sin(newAngle) - 1.0).rotate(-90.0) * clawRadius
                 robot.drive.pidManager.target = targetPose!! - rotationalOffset
@@ -227,9 +224,6 @@ class SampleDectectionTuner : OpMode() {
             roll = -centered.angle / 90.0
             robot.telemetry.addData("roll", roll)
             targetAngle = roll
-//            if (rotationEnabled) {
-//                robot.intake.diffy.roll = (robot.intake.diffy.roll + roll).clamp(-1.0, 1.0)
-//            }
         }
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/tuning/DiffyTuner.kt

@@ -20,11 +20,12 @@ class DiffyTuner : OpMode() {
     override val robot by lazy { DiffyOnly(this) }
 
     override fun exec() {
+        robot.diffy.state = Diffy.State.MANUAL
         robot.diffy.pitch = pitch
         robot.diffy.roll = roll
 
-        robot.telemetry.addData("left", robot.diffy.leftServo.position)
-        robot.telemetry.addData("right", robot.diffy.rightServo.position)
+        robot.telemetry.addData("left", robot.diffy.left.position)
+        robot.telemetry.addData("right", robot.diffy.right.position)
     }
 
     companion object {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/tuning/IntakeArmTuner.kt

@@ -41,6 +41,8 @@ class IntakeArmTuner : OpMode() {
                         IntakeArm.extendedPosition = (IntakeArm.extendedPosition + step).coerceIn(0.0, 1.0)
                     } else if (state == 2) {
                         IntakeArm.floorPosition = (IntakeArm.floorPosition + step).coerceIn(0.0, 1.0)
+                    } else if (state == 3) {
+                        IntakeArm.sweepPosition = (IntakeArm.sweepPosition + step).coerceIn(0.0, 1.0)
                     }
                 }
 
@@ -52,6 +54,8 @@ class IntakeArmTuner : OpMode() {
                         IntakeArm.extendedPosition = (IntakeArm.extendedPosition - step).coerceIn(0.0, 1.0)
                     } else if (state == 2) {
                         IntakeArm.floorPosition = (IntakeArm.floorPosition - step).coerceIn(0.0, 1.0)
+                    } else if (state == 3) {
+                        IntakeArm.sweepPosition = (IntakeArm.sweepPosition - step).coerceIn(0.0, 1.0)
                     }
                 }
         }
@@ -69,6 +73,9 @@ class IntakeArmTuner : OpMode() {
         } else if (state == 2) {
             robot.intakeArm.state = IntakeArmPosition.FLOOR
             robot.telemetry.addData("state", "floor")
+        } else if (state == 3) {
+            robot.intakeArm.state = IntakeArmPosition.SWEEP
+            robot.telemetry.addData("state", "sweep")
         }
 
         robot.telemetry.addData("position", robot.intakeArm.leftServo.position)