Add camera aim adjustment (doesn't wrap)

Author: Braedeng0

TeamCode/src/main/kotlin/pioneer/hardware/Turret.kt

@@ -5,6 +5,7 @@ import com.qualcomm.robotcore.hardware.DcMotor
 import com.qualcomm.robotcore.hardware.DcMotorEx
 import com.qualcomm.robotcore.hardware.DcMotorSimple
 import com.qualcomm.robotcore.hardware.HardwareMap
+import com.qualcomm.robotcore.hardware.PIDFCoefficients
 import pioneer.Constants
 import pioneer.helpers.Chrono
 import pioneer.helpers.MathUtils
@@ -62,6 +63,7 @@ class Turret(
         require(motorRange.first < motorRange.second) {
             "Motor range must be valid: ${motorRange.first} to ${motorRange.second}"
         }
+        tagTrackPID.integralClamp = 1.0
     }
 
     override fun init() {
@@ -71,6 +73,10 @@ class Turret(
                 mode = DcMotor.RunMode.RUN_USING_ENCODER
                 zeroPowerBehavior = DcMotor.ZeroPowerBehavior.FLOAT
                 direction = DcMotorSimple.Direction.FORWARD
+//                setPIDFCoefficients(
+//                    DcMotor.RunMode.RUN_USING_ENCODER,
+//                    PIDFCoefficients(1.0, 0.0, 0.0, 0.0)
+//                )
             }
     }
 

TeamCode/src/main/kotlin/pioneer/opmodes/calibration/ManualSpindexerControl.kt

@@ -13,7 +13,7 @@ class ManualSpindexerControl : OpMode() {
     var power = 0.0
 
     override fun init() {
-        motor = hardwareMap.get(DcMotorEx::class.java, Constants.HardwareNames.SPINDEXER_MOTOR)
+        motor = hardwareMap.get(DcMotorEx::class.java, Constants.HardwareNames.TURRET_MOTOR)
     }
 
     override fun loop() {

TeamCode/src/main/kotlin/pioneer/opmodes/teleop/Teleop.kt

@@ -62,13 +62,6 @@ class Teleop : BaseOpMode() {
     }
 
     private fun addTelemetryData() {
-        val detection = bot.camera?.getProcessor<AprilTagProcessor>()?.detections?.firstOrNull()?.ftcPose
-        addTelemetryData("Multishot state", driver2.multishotState, Verbose.DEBUG)
-        addTelemetryData("April Tag Relative Position", "x: ${detection?.x} y: ${detection?.y} bearing: ${detection?.bearing}", Verbose.FATAL)
-        addTelemetryData("Shooting Distance", hypot(detection?.x ?: 0.0, detection?.y ?: 0.0), Verbose.FATAL)
-//        telemetry.addData("Transfer Data", Constants.TransferData.turretPositionTicks)
-//        telemetry.addData("Turret Offset Ticks", bot.turret?.offsetTicks)
-//        telemetry.addData("Turret Angle", bot.turret?.currentAngle)
         addTelemetryData("Alliance Color", bot.allianceColor, Verbose.INFO)
         addTelemetryData("Drive Power", driver1.drivePower, Verbose.INFO)
         addTelemetryData("Pose", bot.pinpoint!!.pose, Verbose.DEBUG)
@@ -86,8 +79,7 @@ class Teleop : BaseOpMode() {
         addTelemetryData("Drive Power", driver1.drivePower, Verbose.DEBUG)
         addTelemetryData("Spindexer State", bot.spindexer?.motorState, Verbose.INFO)
 
-        addTelemetryData("Relative Tag Pose", Pose(driver1.detection?.ftcPose?.x ?: 0.0, driver1.detection?.ftcPose?.y ?: 0.0), Verbose.FATAL)
-        addTelemetryData("Robot Pose Tag", driver1.robotPoseTag, Verbose.FATAL)
+        addTelemetryData("April Tag Relative Error", driver2.errorDegrees, Verbose.INFO)
 
         addTelemetryData("Spindexer Target Ticks", bot.spindexer?.targetMotorTicks, Verbose.DEBUG)
         addTelemetryData("Spindexer Ticks", bot.spindexer?.currentMotorTicks, Verbose.DEBUG)

TeamCode/src/main/kotlin/pioneer/opmodes/teleop/drivers/TeleopDriver2.kt

@@ -3,6 +3,7 @@ package pioneer.opmodes.teleop.drivers
 import com.qualcomm.robotcore.hardware.Gamepad
 import com.qualcomm.robotcore.util.ElapsedTime
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor
+import org.openftc.apriltag.AprilTagDetection
 import pioneer.Bot
 import pioneer.decode.Artifact
 import pioneer.decode.GoalTag
@@ -15,6 +16,7 @@ import pioneer.helpers.Pose
 import pioneer.helpers.Toggle
 import pioneer.vision.AprilTag
 import kotlin.math.*
+import kotlin.run
 
 class TeleopDriver2(
     private val gamepad: Gamepad,
@@ -36,8 +38,7 @@ class TeleopDriver2(
     var flywheelSpeed = 0.0
     var manualFlywheelSpeed = 0.0
     var flywheelSpeedOffset = 0.0
-    var turretTargetAngle = 0.0
-    var errorDegrees = 0.0
+    var errorDegrees: Double? = 0.0
     var shootCommanded = false
     var triggerMultishot = false
 
@@ -226,26 +227,31 @@ class TeleopDriver2(
         if (bot.turret?.mode == Turret.Mode.AUTO_TRACK) {
             val tagDetections = bot.camera?.getProcessor<AprilTagProcessor>()?.detections
 
+            FileLogger.debug("TeleopDriver2", "Tag Detections: ${tagDetections?.map { it.id }?.joinToString { ", " }}")
+
             // Only look at goal tag for the current alliance
-            tagDetections?.filter{
+            val filteredDetections = tagDetections?.filter{
                 it.id == when (bot.allianceColor) {
                     AllianceColor.RED -> GoalTag.RED.id
                     AllianceColor.BLUE -> GoalTag.BLUE.id
                     AllianceColor.NEUTRAL -> GoalTag.RED.id
                 }
             }
 
-            val errorDegrees = tagDetections?.firstOrNull()?.ftcPose?.bearing?.times(-1.0)
-            val currentRobotAngle = bot.pinpoint?.pose?.theta ?: 0.0
+            FileLogger.debug("TeleopDriver2", "Tag Detections: ${filteredDetections?.map { it.id }?.joinToString { ", " }}")
+
+            errorDegrees = filteredDetections?.firstOrNull()?.ftcPose?.bearing
             if (errorDegrees != null) {
-                turretTargetAngle = currentRobotAngle + errorDegrees
                 bot.turret?.tagTrack(
-                    errorDegrees,
+                    errorDegrees!!.times(-1.0),
                 )
             } else {
                 // No tag detected, use last known target
-                // TODO: Test tag loss logic
-                bot.turret?.gotoAngle(turretTargetAngle-currentRobotAngle)
+                // TODO: fix tag loss logic
+                bot.turret?.autoTrack(
+                    bot.pinpoint?.pose ?: Pose(),
+                    targetGoal.shootingPose
+                )
             }
         }
 //        if (abs(gamepad.right_stick_x) > 0.02) {