Tune RR until feedback tuner

PathPlanning/src/main/kotlin/com/example/pathplanning/legacy/blue/basket/BlueHighBasket1.kt

@@ -1,32 +1,35 @@
 package com.example.pathplanning.legacy.blue.basket
 
-//object BlueHighBasket1 {
-//    @JvmStatic
-//    fun main(args: Array<String>) {
-//        val meepMeep = MeepMeep(800)
-//
-//        val myBot = DefaultBotBuilder(meepMeep) // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width
-//            .setConstraints(30.0, 30.0, Math.toRadians(170.0), Math.toRadians(170.0), 13.94)
-//            .followTrajectorySequence { drive ->
-//                drive.trajectorySequenceBuilder(Pose2d(-12.0, 62.0, (270.0).toRadians()))
-//                    .turn((90.0).toRadians())
-//                    .forward(60.0)
-//                    .waitSeconds(2.0)
-//                    .build()
-//            }
-//        var img: Image? = null
-//        try {
-//            img = ImageIO.read(File("C://Users//arava//Downloads//field.png/"))
-//        } catch (_ : IOException)  {
-//
-//        }
-//
-//        if (img != null) {
-//            meepMeep.setBackground(img)
-//                .setDarkMode(true)
-//                .setBackgroundAlpha(0.95f)
-//                .addEntity(myBot)
-//                .start()
-//        }
-//    }
-//}
+import com.acmerobotics.roadrunner.Pose2d
+import com.noahbres.meepmeep.MeepMeep
+import com.noahbres.meepmeep.core.toRadians
+import com.noahbres.meepmeep.roadrunner.DefaultBotBuilder
+
+object BlueHighBasket1 {
+    @JvmStatic
+    fun main(args: Array<String>) {
+        val meepMeep = MeepMeep(800)
+
+        val myBot = DefaultBotBuilder(meepMeep) // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width
+            .setStartPose(Pose2d(12.0, 62.0, (270.0).toRadians()))
+            .setConstraints(30.0, 30.0, Math.toRadians(170.0), Math.toRadians(170.0), 13.94)
+            .build()
+
+        myBot.runAction(
+            myBot.drive.actionBuilder(
+                Pose2d(12.0, 62.0, (270.0).toRadians())
+            )
+                .turn((90.0).toRadians())
+                .lineToX(35.0)
+                .waitSeconds(2.0)
+                .build()
+        )
+
+
+            meepMeep.setBackground(MeepMeep.Background.FIELD_INTO_THE_DEEP_JUICE_DARK)
+                .setDarkMode(true)
+                .setBackgroundAlpha(0.95f)
+                .addEntity(myBot)
+                .start()
+    }
+}

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/ActionCommand.kt

@@ -0,0 +1,33 @@
+package org.firstinspires.ftc.teamcode.commands
+
+import com.acmerobotics.dashboard.FtcDashboard
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket
+import com.acmerobotics.roadrunner.Action
+import com.arcrobotics.ftclib.command.CommandBase
+import com.arcrobotics.ftclib.command.Subsystem
+
+/**
+ * Generic command that wraps Roadrunner 1.0 Actions
+ */
+class ActionCommand(
+    private val action: Action,
+    private vararg val subsystem: Subsystem,
+    private val endBehavior: () -> Unit = { },
+) : CommandBase() {
+    var finished = false
+
+    override fun initialize() = addRequirements(*subsystem)
+
+    override fun execute() {
+        val packet = TelemetryPacket()
+
+        action.preview(packet.fieldOverlay())
+        finished = !action.run(packet)
+
+        FtcDashboard.getInstance().sendTelemetryPacket(packet)
+    }
+
+    override fun isFinished() = finished
+
+    override fun end(interrupted: Boolean) = endBehavior.invoke()
+}

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/ArmCommand.kt

@@ -3,12 +3,17 @@ package org.firstinspires.ftc.teamcode.commands.arm
 import com.arcrobotics.ftclib.command.CommandBase
 import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
 
+/**
+ * Arm Command to set PID position for the arm
+ * @see ArmSubsystem
+ */
 class ArmCommand(
     private val setpoint: Double,
     private val subsystem: ArmSubsystem
 ) : CommandBase() {
     override fun initialize() {
         subsystem.setpoint = setpoint
+        addRequirements(subsystem)
     }
 
     override fun execute() = subsystem.operateArm()

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/OpenArmCommand.kt

@@ -7,6 +7,9 @@ class OpenArmCommand(
     private val subsystem: ArmSubsystem,
     private val turn: Boolean,
 ) : CommandBase() {
+    override fun initialize() {
+            addRequirements(subsystem)
+    }
 
     override fun execute() {
         if (turn) {

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt

@@ -17,7 +17,7 @@ class DriveCommand(
 
     override fun execute() {
         subsystem.drive(
-            leftY = zonedDrive(-leftY.invoke() * 0.9, zoneVal).pow(3),
+            leftY = zonedDrive(leftY.invoke() * 0.9, zoneVal).pow(3),
             leftX = zonedDrive(leftX.invoke() * 0.9, zoneVal).pow(3),
             rightX = zonedDrive(rightX.invoke() * 0.9, zoneVal).pow(3),
         )

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/ElevatorCommand.kt

@@ -4,11 +4,12 @@ import com.arcrobotics.ftclib.command.CommandBase
 import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
 
 class ElevatorCommand(
+    private val setpoint: Double,
     private val subsystem: ElevatorSubsystem,
-    private val setpoint: Double
 ) : CommandBase() {
     override fun initialize() {
         subsystem.setpoint = setpoint
+        addRequirements(ElevatorSubsystem)
     }
 
     override fun execute() = subsystem.operateElevator()

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/GenericAuto.kt

@@ -0,0 +1,71 @@
+package org.firstinspires.ftc.teamcode.opModes.auto
+
+import com.acmerobotics.roadrunner.Pose2d
+import com.arcrobotics.ftclib.command.CommandOpMode
+import com.arcrobotics.ftclib.command.SequentialCommandGroup
+import com.arcrobotics.ftclib.command.WaitCommand
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous
+import org.firstinspires.ftc.teamcode.commands.ActionCommand
+import org.firstinspires.ftc.teamcode.commands.arm.ArmCommand
+import org.firstinspires.ftc.teamcode.commands.elevator.ElevatorCommand
+import org.firstinspires.ftc.teamcode.commands.intake.IntakeCommand
+import org.firstinspires.ftc.teamcode.constants.AutoStartPose
+import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
+
+@Autonomous(name = "Generic Auto")
+class GenericAuto : CommandOpMode() {
+    override fun initialize() {
+        DriveSubsystem.initialize(hardwareMap)
+        ArmSubsystem.initialize(hardwareMap)
+        ElevatorSubsystem.initialize(hardwareMap)
+        IntakeSubsystem.initialize(hardwareMap)
+
+        val scorePreloaded =
+            SequentialCommandGroup(
+                ActionCommand(
+                    DriveSubsystem.actionBuilder(AutoStartPose.BLUE_LEFT::startPose)
+                        .splineToLinearHeading(
+                            Pose2d(54.0, 56.0, Math.toRadians(225.0)),
+                            Math.toRadians(0.0)
+                        )
+                        .build(),
+                    DriveSubsystem
+                ),
+                ArmCommand(90.0, ArmSubsystem),
+                ElevatorCommand(30.0, ElevatorSubsystem),
+                IntakeCommand(true, IntakeSubsystem),
+                WaitCommand(500),
+                IntakeCommand(false, IntakeSubsystem),
+                ElevatorCommand(0.0, ElevatorSubsystem),
+                ArmCommand(0.0, ArmSubsystem),
+            )
+
+        val scoreFirst =
+            SequentialCommandGroup(
+                ActionCommand(
+                    DriveSubsystem.actionBuilder(DriveSubsystem::pose)
+                        .turn(Math.toRadians(37.0))
+                        .build(),
+                    DriveSubsystem
+                ),
+                ElevatorCommand(20.0, ElevatorSubsystem),
+                IntakeCommand(true, IntakeSubsystem),
+                WaitCommand(500),
+                IntakeCommand(false, IntakeSubsystem),
+                ElevatorCommand(0.0, ElevatorSubsystem),
+                ArmCommand(90.0, ArmSubsystem),
+                ElevatorCommand(30.0, ElevatorSubsystem)
+            )
+
+        schedule(
+            scorePreloaded
+            .andThen(
+                scoreFirst
+            )
+        )
+    }
+
+}

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.kt

@@ -16,92 +16,92 @@ import org.firstinspires.ftc.teamcode.roadrunner.messages.ThreeDeadWheelInputsMe
 
 @Config
 class ThreeDeadWheelLocalizer(hardwareMap: HardwareMap, val inPerTick: Double) : Localizer {
-    class Params {
-        var par0YTicks: Double = 0.0 // y position of the first parallel encoder (in tick units)
-        var par1YTicks: Double = 1.0 // y position of the second parallel encoder (in tick units)
-        var perpXTicks: Double = 0.0 // x position of the perpendicular encoder (in tick units)
-    }
+    data class Params(
+        @JvmField var leftYTicks: Double = -2547.028032399116, // y position of the first parallel encoder (in tick units)
+        @JvmField var rightYTicks: Double = 2556.586233603863, // y position of the second parallel encoder (in tick units)
+        @JvmField var strafeXTicks: Double = -490.4056033307692 // x position of the perpendicular encoder (in tick units)
+    )
 
     // TODO: make sure your config has **motors** with these names (or change them)
     //   the encoders should be plugged into the slot matching the named motor
     //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-    val par0 =
+    val left =
         OverflowEncoder(RawEncoder(hardwareMap.get(DcMotorEx::class.java, ControlBoard.ODO_LEFT_ENCODER.deviceName)))
-    val par1 =
+    val right =
         OverflowEncoder(RawEncoder(hardwareMap.get(DcMotorEx::class.java, ControlBoard.ODO_RIGHT_ENCODER.deviceName)))
-    val perp =
+    val strafe =
         OverflowEncoder(RawEncoder(hardwareMap.get(DcMotorEx::class.java, ControlBoard.ODO_STRAFE_ENCODER.deviceName)))
 
     // TODO: reverse encoder directions if needed
     //   par0.setDirection(DcMotorSimple.Direction.REVERSE);
 
-    private var lastPar0Pos = 0
-    private var lastPar1Pos = 0
-    private var lastPerpPos = 0
+    private var lastLeftPos = 0
+    private var lastRightPos = 0
+    private var lastStrafePos = 0
     private var initialized = false
 
     init {
         write("THREE_DEAD_WHEEL_PARAMS", PARAMS)
     }
 
     override fun update(): Twist2dDual<Time> {
-        val par0PosVel = par0.getPositionAndVelocity()
-        val par1PosVel = par1.getPositionAndVelocity()
-        val perpPosVel = perp.getPositionAndVelocity()
+        val leftPosVel = left.getPositionAndVelocity()
+        val rightPosVel = right.getPositionAndVelocity()
+        val strafePosVel = strafe.getPositionAndVelocity()
 
         write(
             "THREE_DEAD_WHEEL_INPUTS",
-            ThreeDeadWheelInputsMessage(par0PosVel, par1PosVel, perpPosVel)
+            ThreeDeadWheelInputsMessage(leftPosVel, rightPosVel, strafePosVel)
         )
 
         if (!initialized) {
             initialized = true
 
-            lastPar0Pos = par0PosVel.position
-            lastPar1Pos = par1PosVel.position
-            lastPerpPos = perpPosVel.position
+            lastLeftPos = leftPosVel.position
+            lastRightPos = rightPosVel.position
+            lastStrafePos = strafePosVel.position
 
             return Twist2dDual(
                 Vector2dDual.constant(Vector2d(0.0, 0.0), 2),
                 DualNum.constant(0.0, 2)
             )
         }
 
-        val par0PosDelta = par0PosVel.position - lastPar0Pos
-        val par1PosDelta = par1PosVel.position - lastPar1Pos
-        val perpPosDelta = perpPosVel.position - lastPerpPos
+        val leftPosDelta = leftPosVel.position - lastLeftPos
+        val rightPosDelta = rightPosVel.position - lastRightPos
+        val strafePosDelta = strafePosVel.position - lastStrafePos
 
         val twist = Twist2dDual(
             Vector2dDual(
                 DualNum<Time>(
                     doubleArrayOf(
-                        (PARAMS.par0YTicks * par1PosDelta - PARAMS.par1YTicks * par0PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
-                        (PARAMS.par0YTicks * par1PosVel.velocity - PARAMS.par1YTicks * par0PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
+                        (PARAMS.leftYTicks * rightPosDelta - PARAMS.rightYTicks * leftPosDelta) / (PARAMS.leftYTicks - PARAMS.rightYTicks),
+                        (PARAMS.leftYTicks * rightPosVel.velocity - PARAMS.rightYTicks * leftPosVel.velocity) / (PARAMS.leftYTicks - PARAMS.rightYTicks),
                     )
                 ) * inPerTick,
                 DualNum<Time>(
                     doubleArrayOf(
-                        (PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosDelta - par0PosDelta) + perpPosDelta),
-                        (PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosVel.velocity - par0PosVel.velocity) + perpPosVel.velocity),
+                        (PARAMS.strafeXTicks / (PARAMS.leftYTicks - PARAMS.rightYTicks) * (rightPosDelta - leftPosDelta) + strafePosDelta),
+                        (PARAMS.strafeXTicks / (PARAMS.leftYTicks - PARAMS.rightYTicks) * (rightPosVel.velocity - leftPosVel.velocity) + strafePosVel.velocity),
                     )
                 ) * inPerTick
             ),
             DualNum(
                 doubleArrayOf(
-                    (par0PosDelta - par1PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
-                    (par0PosVel.velocity - par1PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
+                    (leftPosDelta - rightPosDelta) / (PARAMS.leftYTicks - PARAMS.rightYTicks),
+                    (leftPosVel.velocity - rightPosVel.velocity) / (PARAMS.leftYTicks - PARAMS.rightYTicks),
                 )
             )
         )
 
-        lastPar0Pos = par0PosVel.position
-        lastPar1Pos = par1PosVel.position
-        lastPerpPos = perpPosVel.position
+        lastLeftPos = leftPosVel.position
+        lastRightPos = rightPosVel.position
+        lastStrafePos = strafePosVel.position
 
         return twist
     }
 
     companion object {
-        var PARAMS: Params = Params()
+        @JvmField var PARAMS = Params()
     }
 }

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.kt

@@ -4,11 +4,11 @@ import com.acmerobotics.roadrunner.PoseVelocity2dDual
 import com.acmerobotics.roadrunner.Time
 
 class DriveCommandMessage(poseVelocity: PoseVelocity2dDual<Time>) {
-    var timestamp: Long = System.nanoTime()
-    var forwardVelocity: Double = poseVelocity.linearVel.x[0]
-    var forwardAcceleration: Double = poseVelocity.linearVel.x[1]
-    var lateralVelocity: Double = poseVelocity.linearVel.y[0]
-    var lateralAcceleration: Double = poseVelocity.linearVel.y[1]
-    var angularVelocity: Double = poseVelocity.angVel[0]
-    var angularAcceleration: Double = poseVelocity.angVel[1]
+    @JvmField var timestamp: Long = System.nanoTime()
+    @JvmField var forwardVelocity: Double = poseVelocity.linearVel.x[0]
+    @JvmField var forwardAcceleration: Double = poseVelocity.linearVel.x[1]
+    @JvmField var lateralVelocity: Double = poseVelocity.linearVel.y[0]
+    @JvmField var lateralAcceleration: Double = poseVelocity.linearVel.y[1]
+    @JvmField var angularVelocity: Double = poseVelocity.angVel[0]
+    @JvmField var angularAcceleration: Double = poseVelocity.angVel[1]
 }