Velocity estimator fixes. Goal offset changes.

Author: nwazus

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

@@ -190,10 +190,11 @@ object Constants {
 
     object Turret {
         const val TICKS_PER_REV = 384.5 * 3
-        const val HEIGHT = 30.0
+        const val HEIGHT = 30.48
         const val THETA = 0.93
         const val ANGLE_TOLERANCE_RADIANS = 0.075
         const val LAUNCH_TIME = 0.125
+        const val OFFSET = -10.0
     }
 
     object ServoPositions {

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

@@ -45,14 +45,14 @@ enum class GoalTag(
     val height: Double
         get() = position[2].toDouble()
 
-    val shootingHeight = height + 20.0
+    val shootingHeight = height + 50.0
 
     // Tag pose plus half the goal depth (46.45 cm)
     val shootingPose: Pose
         get() =
             when (this) {
-                BLUE -> this.pose + Pose(x = -(46.45 / 2) / sqrt(2.0), y = (46.45 / 2) / sqrt(2.0)) // -X +Y
-                RED -> this.pose + Pose(x = (46.45 / 2) / sqrt(2.0), y = (46.45 / 2) / sqrt(2.0)) // +X +Y
+                BLUE -> this.pose + Pose(x = -32.0, y = 30.0) // -X +Y
+                RED -> this.pose + Pose(x = (46.45 / 2) / sqrt(2.0), y = (46.45 / 2) / sqrt(2.0)) // +X +Y //TODO: Update when back at HS
             }
 }
 

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

@@ -55,25 +55,40 @@ class Flywheel(
     }
 
     // https://www.desmos.com/calculator/uofqeqqyn1
+    //12/22: https://www.desmos.com/calculator/1kj8xzklqp
     fun estimateVelocity(
         target: Pose,
         pose: Pose,
+        targetHeight: Double
     ): Double {
-        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 shootPose = pose +
+                Pose(x = Constants.Turret.OFFSET * sin(-pose.theta), y = Constants.Turret.OFFSET * cos(-pose.theta)) +
+                Pose(pose.vx * Constants.Turret.LAUNCH_TIME, pose.vy * Constants.Turret.LAUNCH_TIME)
+
+//        val heightDiff = GoalTag.BLUE.shootingHeight - Constants.Turret.HEIGHT
+        val heightDiff = targetHeight - Constants.Turret.HEIGHT
+        //TODO Double check AprilTag height
         val groundDistance = shootPose distanceTo target
+        //Real world v0 of the ball
         val v0 =
             (groundDistance) / (
                 cos(
                     Constants.Turret.THETA,
                 ) * sqrt((2.0 * (heightDiff - tan(Constants.Turret.THETA) * (groundDistance))) / (-980))
             )
+        //Regression to convert real world velocity to flywheel speed
+//        val flywheelVelocity = 1.58901 * v0 + 17.2812 //From fall break testing
+        val flywheelVelocity = 1.583 * v0 - 9.86811 //From 12/22 testing
+//        val flywheelVelocity = 1.583 * v0 //Adjusting based on 12/22 testing
+
+        //Adjust for velocity of the bot when moving
+//        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))
 
-        val flywheelVelocity = 1.58901 * v0 + 17.2812
-        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))
+        val newTargetVelocity = flywheelVelocity
         return newTargetVelocity
     }
 }

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

@@ -10,6 +10,8 @@ import pioneer.helpers.MathUtils
 import pioneer.helpers.Pose
 import kotlin.math.PI
 import kotlin.math.abs
+import kotlin.math.cos
+import kotlin.math.sin
 
 class Turret(
     private val hardwareMap: HardwareMap,
@@ -93,7 +95,8 @@ class Turret(
         pose: Pose,
         target: Pose,
     ) {
-        val shootPose = pose + Pose(pose.vx * Constants.Turret.LAUNCH_TIME, pose.vy * Constants.Turret.LAUNCH_TIME)
+        val shootPose = pose + Pose(x = Constants.Turret.OFFSET * sin(-pose.theta), y = Constants.Turret.OFFSET * cos(-pose.theta)) +
+                Pose(pose.vx * Constants.Turret.LAUNCH_TIME, pose.vy * Constants.Turret.LAUNCH_TIME)
         // General Angle(From robot 0 to target):
         val targetTheta = (shootPose angleTo target)
         val turretTheta = (PI / 2 + targetTheta) - shootPose.theta

TeamCode/src/main/kotlin/pioneer/opmodes/other/FlywheelTesting.kt

@@ -0,0 +1,59 @@
+package pioneer.opmodes.auto
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp
+import pioneer.Bot
+import pioneer.hardware.Flywheel
+import pioneer.hardware.Launcher
+import pioneer.hardware.MecanumBase
+import pioneer.helpers.Pose
+import pioneer.helpers.Toggle
+import pioneer.localization.localizers.Pinpoint
+import pioneer.opmodes.BaseOpMode
+import pioneer.pathing.paths.LinearPath
+
+@TeleOp(name = "Flywheel Testing", group = "Testing")
+class FlywheelTesting : BaseOpMode() {
+
+    var incFlywheelSpeed = Toggle(false)
+    var decFlywheelSpeed = Toggle(false)
+    var flywheelSpeed = 0.0
+
+    var flywheelToggle = Toggle(false)
+
+    override fun onInit() {
+        bot =
+            Bot
+                .Builder()
+                .add(Flywheel(hardwareMap))
+                .add(Launcher(hardwareMap))
+                .build()
+    }
+
+    override fun onLoop() {
+
+        incFlywheelSpeed.toggle(gamepad1.right_bumper)
+        decFlywheelSpeed.toggle(gamepad1.left_bumper)
+        flywheelToggle.toggle(gamepad1.dpad_left)
+
+        if (incFlywheelSpeed.justChanged){
+            flywheelSpeed += 50.0
+        }
+        if (decFlywheelSpeed.justChanged){
+            flywheelSpeed -= 50.0
+        }
+
+        if (flywheelToggle.state){
+            bot.flywheel?.velocity = flywheelSpeed
+        } else {
+            bot.flywheel?.velocity = 0.0
+        }
+
+        if (gamepad1.square){
+            bot.launcher?.triggerLaunch()
+        }
+
+        telemetry.addData("Actual Flywheel Velocity", bot.flywheel?.velocity)
+        telemetry.addData("Target Velocity", flywheelSpeed)
+    }
+}

TeamCode/src/main/kotlin/pioneer/opmodes/other/ShootingTesting.kt

@@ -0,0 +1,77 @@
+package pioneer.opmodes.other
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit
+import pioneer.Bot
+import pioneer.BotType
+import pioneer.Constants
+import pioneer.hardware.Flywheel
+import pioneer.hardware.Launcher
+import pioneer.helpers.Toggle
+import pioneer.helpers.next
+import pioneer.opmodes.BaseOpMode
+import pioneer.opmodes.teleop.drivers.TeleopDriver1
+import pioneer.opmodes.teleop.drivers.TeleopDriver2
+import pioneer.opmodes.teleop.drivers.TeleopDriver2Testing
+
+@TeleOp(name = "Shooter Testing", group = "Testing")
+class ShootingTesting : BaseOpMode() {
+    private lateinit var driver1: TeleopDriver1
+    private lateinit var driver2: TeleopDriver2Testing
+    private val allianceToggle = Toggle(false)
+    private var changedAllianceColor = false
+
+    override fun onInit() {
+        bot = Bot.fromType(BotType.COMP_BOT, hardwareMap)
+
+        driver1 = TeleopDriver1(gamepad1, bot)
+        driver2 = TeleopDriver2Testing(gamepad2, bot)
+    }
+
+    override fun init_loop() {
+        allianceToggle.toggle(gamepad1.touchpad)
+        if (allianceToggle.justChanged) {
+            changedAllianceColor = true
+            bot.allianceColor = bot.allianceColor.next()
+        }
+        telemetry.addData("Alliance Color", bot.allianceColor)
+        telemetry.update()
+    }
+
+    override fun start() {
+        if (!changedAllianceColor) bot.allianceColor = Constants.TransferData.allianceColor
+//        bot.spindexer?.resetMotorPosition(Constants.TransferData.spindexerPositionTicks)
+//        bot.turret?.resetMotorPosition(Constants.TransferData.turretPositionTicks)
+    }
+
+    override fun onLoop() {
+        // Update gamepad inputs
+        driver1.update()
+        driver2.update()
+
+        // Add telemetry data
+        addTelemetryData()
+    }
+
+    private fun addTelemetryData() {
+//        telemetry.addData("Transfer Data", Constants.TransferData.turretPositionTicks)
+//        telemetry.addData("Turret Offset Ticks", bot.turret?.offsetTicks)
+//        telemetry.addData("Turret Angle", bot.turret?.currentAngle)
+        telemetry.addData("Artifacts", bot.spindexer?.artifacts.contentDeepToString())
+        telemetry.addData("Pose", bot.pinpoint!!.pose)
+        telemetry.addData("Turret Mode", bot.turret?.mode)
+        telemetry.addData("Shoot State", driver2.shootState)
+        telemetry.addData("Estimating Flywheel Speed", driver2.isEstimateSpeed.state)
+        telemetry.addData("Flywheel Target Speed", driver2.flywheelSpeed)
+        telemetry.addData("Flywheel Speed", driver2.flywheelVelocityEnum)
+        telemetry.addData("Flywheel TPS", bot.flywheel?.velocity)
+        telemetry.addData("Turret Angle", driver2.turretAngle)
+        telemetry.addData("Drive Power", driver1.drivePower)
+        telemetry.addData("Field Centric", driver1.fieldCentric)
+        telemetry.addData("Velocity", "vx: %.2f, vy: %.2f".format(bot.pinpoint?.pose?.vx, bot.pinpoint?.pose?.vy))
+        telemetry.addData("Voltage", bot.batteryMonitor?.voltage)
+        telemetry.addData("Flywheel Motor Current", bot.flywheel?.motor?.getCurrent(CurrentUnit.MILLIAMPS))
+        telemetryPacket.addLine("Flywheel TPS" + (bot.flywheel?.velocity ?: 0.0))
+    }
+
+}

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

@@ -59,7 +59,8 @@ 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("Estimating Flywheel Speed", driver2.isEstimateSpeed.state)
+        telemetry.addData("Flywheel Target Speed", driver2.flywheelSpeed)
         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/TeleopDriver2.kt

@@ -1,7 +1,9 @@
 package pioneer.opmodes.teleop.drivers
 
 import com.qualcomm.robotcore.hardware.Gamepad
+import org.firstinspires.ftc.robotcore.external.Const
 import pioneer.Bot
+import pioneer.Constants
 import pioneer.decode.Artifact
 import pioneer.decode.GoalTag
 import pioneer.decode.Points
@@ -13,7 +15,9 @@ import pioneer.helpers.Toggle
 import pioneer.helpers.next
 import kotlin.math.PI
 import kotlin.math.abs
+import kotlin.math.cos
 import kotlin.math.pow
+import kotlin.math.sin
 
 class TeleopDriver2(
     private val gamepad: Gamepad,
@@ -72,7 +76,7 @@ class TeleopDriver2(
 //        flywheelSpeed = flywheelSpeed.coerceIn(0.0, 1.0)
 
         if (isEstimateSpeed.state) {
-            flywheelSpeed = bot.flywheel!!.estimateVelocity(targetGoal.shootingPose, bot.pinpoint?.pose ?: Pose())
+            flywheelSpeed = bot.flywheel!!.estimateVelocity(targetGoal.shootingPose, bot.pinpoint?.pose ?: Pose(), targetGoal.shootingHeight)
         } else {
             flywheelSpeed = flywheelVelocityEnum.velocity
         }
@@ -201,9 +205,12 @@ class TeleopDriver2(
 
     private fun updateIndicatorLED() {
         bot.flywheel?.velocity?.let {
-            if (it > flywheelVelocityEnum.velocity) {
+            if (it >= flywheelSpeed-10 && it <=flywheelSpeed+20) {
                 gamepad.setLedColor(0.0, 1.0, 0.0, -1)
-            } else {
+            } else if (it <flywheelSpeed-10){
+                gamepad.setLedColor(255.0,165.0,0.0, -1)
+            }
+            else {
                 gamepad.setLedColor(1.0, 0.0, 0.0, -1)
             }
         }