Estimate flywheel velocity.

Added shooting while moving, untested.

Changed field reset position and some shooting constants.

Author: nwazus

TeamCode/src/main/kotlin/pioneer/Constants.kt

@@ -190,9 +190,10 @@ object Constants {
 
     object Turret {
         const val TICKS_PER_REV = 384.5 * 3
-        const val HEIGHT = 0.0 // TODO MEASURE
+        const val HEIGHT = 30.0
         const val THETA = 0.93
         const val ANGLE_TOLERANCE_RADIANS = 0.075
+        const val LAUNCH_TIME = 0.125
     }
 
     object ServoPositions {

TeamCode/src/main/kotlin/pioneer/decode/GoalTag.kt

@@ -45,6 +45,8 @@ enum class GoalTag(
     val height: Double
         get() = position[2].toDouble()
 
+    val shootingHeight = height + 20.0
+
     // Tag pose plus half the goal depth (46.45 cm)
     val shootingPose: Pose
         get() =

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

@@ -7,9 +7,12 @@ import com.qualcomm.robotcore.hardware.HardwareMap
 import com.qualcomm.robotcore.hardware.PIDFCoefficients
 import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit
 import pioneer.Constants
+import pioneer.decode.GoalTag
 import pioneer.helpers.FileLogger
 import pioneer.helpers.Pose
 import kotlin.math.cos
+import kotlin.math.pow
+import kotlin.math.sin
 import kotlin.math.sqrt
 import kotlin.math.tan
 
@@ -56,9 +59,9 @@ class Flywheel(
         target: Pose,
         pose: Pose,
     ): Double {
-        val tempTargetHeight = 0 // TODO Replace with GoalTag info when merged with CV branch
-        val heightDiff = tempTargetHeight - Constants.Turret.HEIGHT
-        val groundDistance = pose distanceTo pose
+        val shootPose = pose + Pose(pose.vx * Constants.Turret.LAUNCH_TIME, pose.vy * Constants.Turret.LAUNCH_TIME)
+        val heightDiff = GoalTag.BLUE.shootingHeight - Constants.Turret.HEIGHT
+        val groundDistance = shootPose distanceTo target
         val v0 =
             (groundDistance) / (
                 cos(
@@ -67,6 +70,10 @@ class Flywheel(
             )
 
         val flywheelVelocity = 1.58901 * v0 + 17.2812
-        return flywheelVelocity
+        val thetaToTarget = -(shootPose angleTo target)
+        val newTargetVelocityX = sin(thetaToTarget) * flywheelVelocity - pose.vx
+        val newTargetVelocityY = cos(thetaToTarget) * flywheelVelocity - pose.vy
+        val newTargetVelocity = sqrt(newTargetVelocityX.pow(2) + newTargetVelocityY.pow(2))
+        return newTargetVelocity
     }
 }

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

@@ -4,6 +4,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 org.firstinspires.ftc.robotcore.external.Const
 import pioneer.Constants
 import pioneer.helpers.MathUtils
 import pioneer.helpers.Pose
@@ -92,9 +93,10 @@ class Turret(
         pose: Pose,
         target: Pose,
     ) {
+        val shootPose = pose + Pose(pose.vx * Constants.Turret.LAUNCH_TIME, pose.vy * Constants.Turret.LAUNCH_TIME)
         // General Angle(From robot 0 to target):
-        val targetTheta = (pose angleTo target)
-        val turretTheta = (PI / 2 + targetTheta) - pose.theta
+        val targetTheta = (shootPose angleTo target)
+        val turretTheta = (PI / 2 + targetTheta) - shootPose.theta
         gotoAngle(MathUtils.normalizeRadians(turretTheta), 0.85)
     }
 }

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

@@ -59,6 +59,7 @@ class Teleop : BaseOpMode() {
         telemetry.addData("Target Goal", driver2.targetGoal)
         telemetry.addData("Turret Mode", bot.turret?.mode)
         telemetry.addData("Shoot State", driver2.shootState)
+        telemetry.addData("Estimating Flywheel Speed", driver2.isEstimateSpeed)
         telemetry.addData("Flywheel Speed", driver2.flywheelVelocityEnum)
         telemetry.addData("Flywheel TPS", bot.flywheel?.velocity)
         telemetry.addData("Turret Angle", driver2.turretAngle)

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

@@ -117,9 +117,9 @@ class TeleopDriver1(
     private fun handleResetPose() {
         if (gamepad.options) {
             if (bot.allianceColor == AllianceColor.RED) {
-                bot.pinpoint?.reset(Pose(-80.0, -95.0, theta = 0.1))
+                bot.pinpoint?.reset(Pose(-86.7, -99.0, theta = 0.1))
             } else {
-                bot.pinpoint?.reset(Pose(80.0, -95.0, theta = 0.1))
+                bot.pinpoint?.reset(Pose(86.7, -99.0, theta = 0.1))
             }
         }
     }

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

@@ -19,30 +19,33 @@ class TeleopDriver2(
     private val gamepad: Gamepad,
     private val bot: Bot,
 ) {
-    enum class FlywheelSpeed(
+    enum class FlywheelSpeedRange(
         val velocity: Double,
     ) {
         SHORT_RANGE(800.0),
         LONG_RANGE(1000.0),
     }
 
+    val isEstimateSpeed = Toggle(true)
     private val chrono = Chrono(autoUpdate = false)
     private val isAutoTracking = Toggle(false)
     private val flywheelToggle = Toggle(false)
-    private val changeFlywheelSpeedToggle = Toggle(false)
+    private val changeFlywheelRangeToggle = Toggle(false)
 
     var P = Points(bot.allianceColor)
 
     enum class ShootState { READY, MOVING_TO_POSITION, LAUNCHING }
 
-    var flywheelVelocityEnum = FlywheelSpeed.SHORT_RANGE
+    var flywheelVelocityEnum = FlywheelSpeedRange.SHORT_RANGE
     var shootState = ShootState.READY
-    var targetGoal = Pose()
+    var targetGoal = GoalTag.RED
     private var shootingAll = false
     private var remainingShots = 0
     var turretAngle = 0.0
+    var flywheelSpeed = 0.0
 
     fun update() {
+        checkTargetGoal()
         updateFlywheelSpeed()
         handleFlywheel()
         handleTurret()
@@ -52,24 +55,39 @@ class TeleopDriver2(
         chrono.update() // Manual update to allow dt to match across the loop.
     }
 
+    private fun checkTargetGoal() {
+        if (bot.allianceColor == AllianceColor.BLUE) {
+            targetGoal = GoalTag.BLUE
+        } else { return }
+    }
+
     private fun updateFlywheelSpeed() {
+        isEstimateSpeed.toggle(gamepad.dpad_right)
 //        if (flywheelSpeed < 1.0 && gamepad.dpad_up) {
 //            flywheelSpeed += chrono.dt * 0.5
 //        }
 //        if (flywheelSpeed > 0.0 && gamepad.dpad_down) {
 //            flywheelSpeed -= chrono.dt * 0.5
 //        }
 //        flywheelSpeed = flywheelSpeed.coerceIn(0.0, 1.0)
-        changeFlywheelSpeedToggle.toggle(gamepad.dpad_up)
-        if (changeFlywheelSpeedToggle.justChanged) {
+
+        if (isEstimateSpeed.state) {
+            flywheelSpeed = bot.flywheel!!.estimateVelocity(targetGoal.shootingPose, bot.pinpoint?.pose ?: Pose())
+        } else {
+            flywheelSpeed = flywheelVelocityEnum.velocity
+        }
+
+        changeFlywheelRangeToggle.toggle(gamepad.dpad_up)
+
+        if (changeFlywheelRangeToggle.justChanged) {
             flywheelVelocityEnum = flywheelVelocityEnum.next()
         }
     }
 
     private fun handleFlywheel() {
         flywheelToggle.toggle(gamepad.dpad_left)
         if (flywheelToggle.state) {
-            bot.flywheel?.velocity = flywheelVelocityEnum.velocity
+            bot.flywheel?.velocity = flywheelSpeed
         } else {
             bot.flywheel?.velocity = 0.0
         }
@@ -174,20 +192,9 @@ class TeleopDriver2(
 
     private fun handleAutoTrack() {
         if (bot.turret?.mode == Turret.Mode.AUTO_TRACK) {
-            if (bot.allianceColor == AllianceColor.RED) {
-                targetGoal = GoalTag.RED.pose
-            } else {
-                targetGoal = GoalTag.BLUE.pose
-            }
             bot.turret?.autoTrack(
                 bot.pinpoint?.pose ?: Pose(),
-                if (bot.allianceColor ==
-                    AllianceColor.BLUE
-                ) {
-                    GoalTag.BLUE.shootingPose
-                } else {
-                    GoalTag.RED.shootingPose
-                },
+                targetGoal.shootingPose,
             )
         }
     }