diff --git a/TeamCode/src/main/kotlin/pioneer/Constants.kt b/TeamCode/src/main/kotlin/pioneer/Constants.kt index 2004ca4..63a8318 100644 --- a/TeamCode/src/main/kotlin/pioneer/Constants.kt +++ b/TeamCode/src/main/kotlin/pioneer/Constants.kt @@ -104,7 +104,7 @@ object Constants { // -------- Follower (path following) -------- object Follower { /** The threshold in cm to consider the target reached. */ - const val POSITION_THRESHOLD = 1.25 + const val POSITION_THRESHOLD = 1.75 /** The threshold in radians to consider the target heading reached. */ const val ROTATION_THRESHOLD = 0.06 @@ -136,7 +136,7 @@ object Constants { @JvmField var Y_KD = 0.0 // Theta PID coefficients for heading interpolation - @JvmField var THETA_KP = 3.0 // was 5.0 + @JvmField var THETA_KP = 5.0 // was 5.0 @JvmField var THETA_KI = 0.0 @JvmField var THETA_KD = 0.0 @@ -173,14 +173,16 @@ object Constants { // val distortionCoefficients = floatArrayOf(0.0573F, 2.0205F, -0.0331F, 0.0021F, -14.6155F, 0F, 0F, 0F) } + @Config object Spindexer { - // External Encoder - @JvmField var KP = 0.00475 - @JvmField var KI = 0.00005 - @JvmField var KD = 0.0125 + @JvmField var KP = 0.004 + @JvmField var KI = 0.0 + @JvmField var KD = 0.00125 @JvmField var KS_START = 0.04 + @JvmField var SHOOT_POWER = 0.2 + @JvmField var MAX_POWER_RATE = 100.0 @JvmField var MOTOR_TOLERANCE_TICKS = 0 // was 75 // stops moving within tolerance (in outtake for magnets) @@ -188,13 +190,15 @@ object Constants { const val SHOOTING_TOLERANCE_TICKS = 4 const val DETECTION_TOLERANCE_TICKS = 18 const val VELOCITY_TOLERANCE_TPS = 50 + const val ALLOWED_REVERSE_TICKS = 50 // How far spindexer can reverse without doing a 360 const val TICKS_PER_REV = 537.7 // Time required to confirm an artifact has been intaken (ms) - const val CONFIRM_INTAKE_MS = 67.0 + const val CONFIRM_INTAKE_MS = 25.0 // Max time the artifact can disappear without resetting confirmation (ms) - const val CONFIRM_LOSS_MS = 10 + const val CONFIRM_LOSS_MS = 0.0 + const val MAX_VELOCITY = 2400 // tps } object Turret { @@ -239,14 +243,17 @@ object Constants { var spindexerMotorTicks = 0 } + @Config object Flywheel { - @JvmField var KP = 0.0075 + @JvmField var KP = 0.0001 @JvmField var KI = 0.0 - @JvmField var KD = 0.0 + @JvmField var KD = 0.001 @JvmField var KF = 0.000415 val idleVelocity = 300.0 + @JvmField var VELOCITY_COMPENSATION_FACTOR = 1.0 + enum class FlywheelOperatingMode{ ALWAYS_IDLE, FULL_OFF, diff --git a/TeamCode/src/main/kotlin/pioneer/decode/Points.kt b/TeamCode/src/main/kotlin/pioneer/decode/Points.kt index d1658c1..e0408a8 100644 --- a/TeamCode/src/main/kotlin/pioneer/decode/Points.kt +++ b/TeamCode/src/main/kotlin/pioneer/decode/Points.kt @@ -40,7 +40,7 @@ class Points( val START_GOAL = Pose(133.0, 134.0, theta = 0.67).T(color) val START_FAR = Pose(43.0, -157.0, theta = 0.0).T(color) - val SHOOT_GOAL_CLOSE = Pose(55.0, 30.0, theta = -PI/2).T(color) + val SHOOT_GOAL_CLOSE = Pose(55.0, 25.0, theta = -PI/2).T(color) val SHOOT_GOAL_FAR = Pose(43.0, -140.0, theta = -PI/2).T(color) val LEAVE_POSITION = Pose(60.0, -60.0).T(color) @@ -53,8 +53,8 @@ class Points( val PREP_COLLECT_MID = Pose(prepCollectX, collectY(1), theta=collectTheta).T(color) val PREP_COLLECT_AUDIENCE = Pose(prepCollectX, collectY(2), theta=collectTheta).T(color) - val PREP_COLLECT_START_VELOCITY = Pose(0.0, -75.0).T(color) - val PREP_COLLECT_END_VELOCITY = Pose(75.0, 0.0).T(color) + val PREP_COLLECT_START_VELOCITY = Pose(0.0, 0.0).T(color) + val PREP_COLLECT_END_VELOCITY = Pose(80.0, 0.0).T(color) val GOTO_SHOOT_VELOCITY = Pose(-175.0, 0.0).T(color) private val collectX = 125.0 diff --git a/TeamCode/src/main/kotlin/pioneer/hardware/Flywheel.kt b/TeamCode/src/main/kotlin/pioneer/hardware/Flywheel.kt index 95deaf6..47caefa 100644 --- a/TeamCode/src/main/kotlin/pioneer/hardware/Flywheel.kt +++ b/TeamCode/src/main/kotlin/pioneer/hardware/Flywheel.kt @@ -11,6 +11,7 @@ import pioneer.helpers.FileLogger import pioneer.helpers.PIDController import pioneer.helpers.Pose import kotlin.math.cos +import kotlin.math.pow import kotlin.math.sin import kotlin.math.sqrt import kotlin.math.tan @@ -88,10 +89,10 @@ class Flywheel( //TODO Double check AprilTag height val groundDistance = shootPose distanceTo target - return estimateVelocity(groundDistance, targetHeight) + return estimateVelocity(groundDistance, targetHeight, pose) } - fun estimateVelocity(targetDistance: Double, targetHeight: Double) : Double { + fun estimateVelocity(targetDistance: Double, targetHeight: Double, pose: Pose = Pose()) : Double { val heightDiff = targetHeight - Constants.Turret.HEIGHT //Real world v0 of the ball val v0 = @@ -99,18 +100,20 @@ class Flywheel( cos( Constants.Turret.THETA, ) * sqrt((2.0 * (heightDiff - tan(Constants.Turret.THETA) * (targetDistance))) / (-980)) - ) + ) //Regression to convert real world velocity to flywheel speed // val flywheelVelocity = 1.583 * v0 - 9.86811 // From 12/22 testing // val flywheelVelocity = 1.64545 * v0 - 51.56276 // From 2/4 testing - val flywheelVelocity = 2.05204 * v0 - 290.74829 // From 2/9 testing +// val flywheelVelocity = 2.05204 * v0 - 290.74829 // From 2/9 testing (Old flywheel) + val flywheelVelocity = 4.43524 * v0 - 1109.14177 // From 2/15 Testing (Lighter flywheel) + //https://www.desmos.com/calculator/6jwkhfmibd //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 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 thetaToTarget = -(shootPose angleTo Pose(0.0, 0.0)) + val newTargetVelocityX = sin(thetaToTarget) * flywheelVelocity - pose.vx * Constants.Flywheel.VELOCITY_COMPENSATION_FACTOR + val newTargetVelocityY = cos(thetaToTarget) * flywheelVelocity - pose.vy * Constants.Flywheel.VELOCITY_COMPENSATION_FACTOR + val newTargetVelocity = sqrt(newTargetVelocityX.pow(2) + newTargetVelocityY.pow(2)) - val newTargetVelocity = flywheelVelocity return newTargetVelocity } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/pioneer/hardware/Intake.kt b/TeamCode/src/main/kotlin/pioneer/hardware/Intake.kt index 3af641f..a75ee19 100644 --- a/TeamCode/src/main/kotlin/pioneer/hardware/Intake.kt +++ b/TeamCode/src/main/kotlin/pioneer/hardware/Intake.kt @@ -19,7 +19,7 @@ class Intake( intake.power = value } - var defaultPower: Double = 1.0 + var defaultPower: Double = 0.9 override fun init() { intake = diff --git a/TeamCode/src/main/kotlin/pioneer/hardware/spindexer/ArtifactIndexer.kt b/TeamCode/src/main/kotlin/pioneer/hardware/spindexer/ArtifactIndexer.kt index ca85b8a..07390df 100644 --- a/TeamCode/src/main/kotlin/pioneer/hardware/spindexer/ArtifactIndexer.kt +++ b/TeamCode/src/main/kotlin/pioneer/hardware/spindexer/ArtifactIndexer.kt @@ -3,6 +3,7 @@ package pioneer.hardware.spindexer import com.qualcomm.robotcore.util.ElapsedTime import pioneer.Constants import pioneer.decode.Artifact +import pioneer.decode.Motif import pioneer.helpers.FileLogger class ArtifactIndexer { @@ -29,14 +30,52 @@ class ArtifactIndexer { get() = artifacts.count { it != null } // --- Index Helpers --- // - fun nextOpenIntakeIndex(): Int? = - artifacts.indexOfFirst { it == null }.takeIf { it != -1 } + fun nextOpenIntakeIndex(currentIndex: Int): Int? = run { + for (i in 0..2) { + // -i for reversed direction + val index = Math.floorMod(currentIndex - i, 3) + if (artifacts[index] == null) return index + } + null + } fun findOuttakeIndex(target: Artifact? = null): Int? = target?.let { artifacts.indexOfFirst { a -> a == it }.takeIf { i -> i != -1 } } ?: artifacts.indexOfFirst { it != null }.takeIf { it != -1 } + fun motifStartIndex(motif: Motif?, currentIndex: Int): Int? { + // Find the best shift to match motif + if (motif == null || !motif.isValid()) return null + + val pattern = motif.getPattern() + + var bestShift: Int? = null + var bestScore = -1 + + // Try all 3 rotations + for (offset in 0 until 3) { + // -offset for reversed direction, -2 to account for shooting vs intake difference + val shift = Math.floorMod((currentIndex - offset - 2), 3) + var score = 0 + + // Score this rotation based on how many positions match the motif pattern + for (i in 0 until 3) { + val rotated = artifacts[(i + shift) % 3] + if (rotated == pattern[i]) { + score++ + } + } + + if (score > bestScore) { + bestScore = score + bestShift = shift + } + } + + return bestShift?.minus(2) + } + // --- Intake Logic --- // /** @@ -71,6 +110,7 @@ class ArtifactIndexer { private fun handleLoss() { if (!artifactWasSeenRecently) return + // FIXME: whatttt if (artifactLostTimer.milliseconds() == 0.0) { artifactLostTimer.reset() } diff --git a/TeamCode/src/main/kotlin/pioneer/hardware/spindexer/Spindexer.kt b/TeamCode/src/main/kotlin/pioneer/hardware/spindexer/Spindexer.kt index b7e45ca..d82cd15 100644 --- a/TeamCode/src/main/kotlin/pioneer/hardware/spindexer/Spindexer.kt +++ b/TeamCode/src/main/kotlin/pioneer/hardware/spindexer/Spindexer.kt @@ -4,35 +4,36 @@ import com.qualcomm.robotcore.hardware.DcMotorEx import com.qualcomm.robotcore.hardware.HardwareMap import pioneer.Constants import pioneer.decode.Artifact +import pioneer.decode.Motif import pioneer.hardware.HardwareComponent import pioneer.hardware.RevColorSensor +import kotlin.math.roundToInt class Spindexer( private val hardwareMap: HardwareMap, private val motorName: String = Constants.HardwareNames.SPINDEXER_MOTOR, private val intakeSensorName: String = Constants.HardwareNames.INTAKE_SENSOR, ) : HardwareComponent { + // --- Spindexer State --- // + private enum class State { + READY, + INTAKE, + } + + private var state = State.INTAKE + // --- Subsystems --- // - private lateinit var motion: SpindexerMotionController private lateinit var detector: ArtifactDetector + private lateinit var motion: SpindexerMotionController private val indexer = ArtifactIndexer() // --- Positions --- // - private val intakePositions = - listOf( - SpindexerMotionController.MotorPosition.INTAKE_1, - SpindexerMotionController.MotorPosition.INTAKE_2, - SpindexerMotionController.MotorPosition.INTAKE_3 - ) - - private val outtakePositions = - listOf( - SpindexerMotionController.MotorPosition.OUTTAKE_1, - SpindexerMotionController.MotorPosition.OUTTAKE_2, - SpindexerMotionController.MotorPosition.OUTTAKE_3 - ) - var manualOverride = false + var isSorting = false + val isShooting: Boolean get() = motion.isShooting + + private val ticksPerArtifact: Int + get() = (Constants.Spindexer.TICKS_PER_REV / 3.0).roundToInt() // --- Artifact Data --- // val artifacts: Array get() = indexer.snapshot() @@ -42,15 +43,9 @@ class Spindexer( val numStoredArtifacts: Int get() = indexer.count // --- Motor Getters --- // - val motorState: SpindexerMotionController.MotorPosition get() = motion.target - val reachedTarget: Boolean get() = motion.reachedTarget - val withinDetectionTolerance: Boolean get() = motion.withinDetectionTolerance val currentMotorTicks: Int get() = motion.currentTicks val targetMotorTicks: Int get() = motion.targetTicks - val currentMotorVelocity: Double get() = motion.velocity - val closestMotorPosition get() = motion.closestPosition - val isOuttakePosition get() = motion.target in outtakePositions - val isIntakePosition get() = motion.target in intakePositions + val reachedTarget: Boolean get() = motion.reachedTarget // --- Initialization --- // override fun init() { @@ -68,87 +63,93 @@ class Spindexer( // --- Update Loop --- // override fun update() { motion.update() - motion.manualOverride = this.manualOverride - checkForArtifact() + when (state) { + State.INTAKE -> intakeState() + State.READY -> readyState() + } } // --- Public Commands --- // - fun moveToNextOpenIntake(): Boolean { - manualOverride = false - val index = indexer.nextOpenIntakeIndex() ?: return false - motion.target = intakePositions[index] - return true + fun moveManual(power: Double) { + motion.moveManual(power) } - fun moveToNextOuttake(artifact: Artifact? = null): Boolean { - manualOverride = false - val index = indexer.findOuttakeIndex(artifact) ?: return false - motion.target = outtakePositions[index] - return true + fun moveToPosition(positionIndex: Int) { + motion.positionIndex = positionIndex } - fun moveToPosition(position: SpindexerMotionController.MotorPosition) { - manualOverride = false - motion.target = position + fun setArtifacts(vararg values: Artifact?) { + indexer.setAll(*values) } - fun popCurrentArtifact(autoSwitchToIntake: Boolean = true): Artifact? { - if (motion.target !in outtakePositions) return null - val index = outtakePositions.indexOf(motion.target) - val artifact = indexer.pop(index) - if (autoSwitchToIntake){ - if (indexer.isEmpty) moveToNextOpenIntake() - } - return artifact + fun moveToNextOpenIntake(): Boolean { + state = State.INTAKE + manualOverride = false + motion.stopShooting() + val index = indexer.nextOpenIntakeIndex(motion.positionIndex) ?: return false + motion.positionIndex = index + return true } - fun popArtifact(index: Int): Artifact? { - val artifact = indexer.pop(index) - if (indexer.isEmpty) moveToNextOpenIntake() - return artifact + fun readyOuttake(targetMotif: Motif? = null) { + state = State.READY + motion.stopShooting() + // Return if there aren't any artifacts + val startIndex = indexer.motifStartIndex(targetMotif, motion.positionIndex) ?: return + motion.positionIndex = startIndex } - fun cancelLastIntake() { - indexer.popLastIntake() + fun shootNext() { + if (state == State.INTAKE) return + if (motion.isShooting) return + indexer.pop(motion.positionIndex) + // Rotate 120 degrees at constant speed (no PID) then hold next outtake. + motion.positionIndex += 1 + motion.startShooting(ticksPerArtifact, Constants.Spindexer.SHOOT_POWER) } - fun reset() { + fun shootAll() { + if (state == State.INTAKE) return + if (motion.isShooting) return + // Rotate a full revolution plus one at constant speed (no PID) to shoot all 3. + motion.startShooting(Constants.Spindexer.TICKS_PER_REV.roundToInt() + ticksPerArtifact, Constants.Spindexer.SHOOT_POWER) indexer.resetAll() } - fun moveManual(power: Double) { - manualOverride = true - motion.moveManual(power) - } - - fun resetMotorPosition(resetTicks: Int = 0) { +// fun popCurrentArtifact(autoSwitchToIntake: Boolean = true): Artifact? { +// if (motion.target !in outtakePositions) return null +// val index = outtakePositions.indexOf(motion.target) //Needs to be current ticks, +// val artifact = indexer.pop(index) +// if (autoSwitchToIntake){ +// if (indexer.isEmpty) moveToNextOpenIntake() +// } +// return artifact +// } + + fun resetMotorPosition(resetTicks: Int) { motion.calibrateEncoder(resetTicks) } - fun setArtifacts(vararg artifacts: Artifact?) { - indexer.setAll(*artifacts) + fun reset() { + indexer.resetAll() } // --- Private Helpers --- // - private fun checkForArtifact() { - if (motion.target !in intakePositions) return + private fun intakeState() { + // Make sure the spindexer is at it's target position if (!motion.withinDetectionTolerance) return if (!motion.withinVelocityTolerance) return - val index = intakePositions.indexOf(motion.target) + // Detect artifact val detected = detector.detect() - + val index = motion.positionIndex if (indexer.processIntake(index, detected)) { - if (!moveToNextOpenIntake()) switchMode() + // If there aren't any more open intake positions + if (!moveToNextOpenIntake()) readyOuttake() } } - private fun switchMode() { - val index = intakePositions.indexOf(motion.target) - if (motion.target in intakePositions) { - motion.target = outtakePositions[(index - 1).mod(outtakePositions.size)] - } else { - motion.target = intakePositions[(index + 1).mod(intakePositions.size)] - } + private fun readyState() { + // TODO: check if an artifact has been shot (then pop it) } } diff --git a/TeamCode/src/main/kotlin/pioneer/hardware/spindexer/SpindexerMotionController.kt b/TeamCode/src/main/kotlin/pioneer/hardware/spindexer/SpindexerMotionController.kt index e3f126a..a849229 100644 --- a/TeamCode/src/main/kotlin/pioneer/hardware/spindexer/SpindexerMotionController.kt +++ b/TeamCode/src/main/kotlin/pioneer/hardware/spindexer/SpindexerMotionController.kt @@ -2,9 +2,12 @@ package pioneer.hardware.spindexer import com.qualcomm.robotcore.hardware.DcMotor import com.qualcomm.robotcore.hardware.DcMotorEx +import com.qualcomm.robotcore.hardware.DcMotorSimple import com.qualcomm.robotcore.util.ElapsedTime import pioneer.Constants import pioneer.helpers.Chrono +import pioneer.helpers.FileLogger +import pioneer.helpers.MathUtils import pioneer.helpers.PIDController import kotlin.math.PI import kotlin.math.abs @@ -13,40 +16,8 @@ import kotlin.math.sign class SpindexerMotionController( private val motor: DcMotorEx, ) { - - // --- Motor Positions --- // - enum class MotorPosition(val radians: Double) { - OUTTAKE_1(0 * PI / 3), - INTAKE_1(3 * PI / 3), - - OUTTAKE_2(2 * PI / 3), - INTAKE_2(5 * PI / 3), - - OUTTAKE_3(4 * PI / 3), - INTAKE_3(1 * PI / 3), - } - - // --- Positions --- // - private val intakePositions = - listOf( - MotorPosition.INTAKE_1, - MotorPosition.INTAKE_2, - MotorPosition.INTAKE_3 - ) - - private val outtakePositions = - listOf( - MotorPosition.OUTTAKE_1, - MotorPosition.OUTTAKE_2, - MotorPosition.OUTTAKE_3 - ) - // --- Configuration --- // - - private val ticksPerRadian = - (Constants.Spindexer.TICKS_PER_REV / (2 * PI)).toInt() private var calibrationTicks = 0 - private var lastPower = 0.0 private val pid = PIDController( Constants.Spindexer.KP, @@ -57,8 +28,7 @@ class SpindexerMotionController( private val chrono = Chrono(false) // --- Public State --- // - - var target: MotorPosition = MotorPosition.INTAKE_1 + var positionIndex = 0 set(value) { if (field != value) { pid.reset() @@ -68,6 +38,11 @@ class SpindexerMotionController( var manualOverride = false + private var shooting = false + private var shootStartTicks = 0 + private var shootDeltaTicks = 0 + private var shootPower = 0.0 + val currentTicks: Int get() = (-motor.currentPosition + calibrationTicks) @@ -75,13 +50,16 @@ class SpindexerMotionController( get() = motor.velocity val targetTicks: Int - get() = (target.radians * ticksPerRadian).toInt() + get() = (positionIndex * Constants.Spindexer.TICKS_PER_REV / 3).toInt() val errorTicks: Int get() = wrapTicks(targetTicks - currentTicks) val velocityTimer = ElapsedTime() + val isShooting: Boolean + get() = shooting + val reachedTarget: Boolean get() = abs(errorTicks) < Constants.Spindexer.SHOOTING_TOLERANCE_TICKS && @@ -95,33 +73,17 @@ class SpindexerMotionController( get() = abs(motor.velocity) < Constants.Spindexer.VELOCITY_TOLERANCE_TPS - val closestPosition: MotorPosition - get() { - var closest = MotorPosition.INTAKE_1 - var smallest = Double.MAX_VALUE - - for (pos in MotorPosition.entries) { - val ticks = (pos.radians * ticksPerRadian).toInt() - val error = wrapTicks(ticks - currentTicks).toDouble() - if (abs(error) < abs(smallest)) { - smallest = error - closest = pos - } - } - return closest - } - // --- Initialization --- // fun init() { motor.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER - motor.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER + motor.mode = DcMotor.RunMode.RUN_USING_ENCODER pid.integralClamp = 1_000.0 } fun calibrateEncoder(calibrationTicks: Int = 0) { motor.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER - motor.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER + motor.mode = DcMotor.RunMode.RUN_USING_ENCODER // Logical offset from encoder zero to real zero this.calibrationTicks = calibrationTicks @@ -132,38 +94,51 @@ class SpindexerMotionController( // --- Update Loop --- // fun update() { + if (shooting) { + val traveledTicks = abs(currentTicks - shootStartTicks) + if (traveledTicks >= shootDeltaTicks) { + stopShooting() + } else { + motor.velocity = -shootPower * Constants.Spindexer.MAX_VELOCITY + } + return + } if (manualOverride) return if (!withinVelocityTolerance) velocityTimer.reset() chrono.update() - var power = pid.update(errorTicks.toDouble(), chrono.dt) + // Correct error to only move in one direction (unless really small) + val correctedError: Int = run { + val full = Constants.Spindexer.TICKS_PER_REV.toInt() + val allowedReverse = Constants.Spindexer.ALLOWED_REVERSE_TICKS + // Wrap to enforce movement direction +// val range = Pair(-allowedReverse, full - allowedReverse) + // For reversed spindexer + val range = Pair(-(full + allowedReverse), allowedReverse) + MathUtils.wrap(errorTicks, range) + } - // inconsistent based on loop time - // power = rampPower(power, chrono.dt) +// FileLogger.debug("Spindexer Motor Control", "Corrected Error: $correctedError") + var power = -pid.update(correctedError.toDouble(), chrono.dt) val ks = Constants.Spindexer.KS_START if (abs(power) > 0.01) { power += ks * sign(power) } -// if (abs(errorTicks) < Constants.Spindexer.PID_TOLERANCE_TICKS && target in outtakePositions) { -// power = sign(errorTicks.toDouble()) * Constants.Spindexer.FINAL_ADJUSTMENT_POWER -// } - if (abs(errorTicks) < Constants.Spindexer.MOTOR_TOLERANCE_TICKS) { power = 0.0 -// pid.reset() } - motor.power = power.coerceIn(-0.6, 0.6) + motor.power = power.coerceIn(-0.75, 0.75) } // --- Manual Control --- // - fun moveManual(power: Double) { + if (shooting) stopShooting() manualOverride = true motor.power = power.coerceIn(-1.0, 1.0) } @@ -173,21 +148,25 @@ class SpindexerMotionController( pid.reset() } - // --- Helpers --- // - private fun rampPower( - desired: Double, - dt: Double, - ): Double { - val maxDelta = - Constants.Spindexer.MAX_POWER_RATE * dt / 1000.0 - - val delta = desired - lastPower - val clipped = delta.coerceIn(-maxDelta, maxDelta) - - lastPower += clipped - return lastPower + fun startShooting(deltaTicks: Int, power: Double) { + val clampedTicks = abs(deltaTicks) + if (clampedTicks == 0) return + manualOverride = false + shooting = true + shootStartTicks = currentTicks + shootDeltaTicks = clampedTicks + shootPower = power.coerceIn(-1.0, 1.0) + pid.reset() + + } + + fun stopShooting() { + shooting = false + motor.power = 0.0 + pid.reset() } + // --- Helpers --- // private fun wrapTicks( error: Int, ticksPerRev: Double = Constants.Spindexer.TICKS_PER_REV, diff --git a/TeamCode/src/main/kotlin/pioneer/helpers/MathUtils.kt b/TeamCode/src/main/kotlin/pioneer/helpers/MathUtils.kt index dfc33e3..625b39a 100644 --- a/TeamCode/src/main/kotlin/pioneer/helpers/MathUtils.kt +++ b/TeamCode/src/main/kotlin/pioneer/helpers/MathUtils.kt @@ -32,6 +32,17 @@ object MathUtils { return normalized } + fun wrap( + value: Int, + range: Pair, + ): Int { + val spread = range.second - range.first + var normalized = value + while (normalized > range.second) normalized -= spread + while (normalized <= range.first) normalized += spread + return normalized + } + /** * Creates a linearly spaced array of values. * @param start Starting value diff --git a/TeamCode/src/main/kotlin/pioneer/helpers/Misc.kt b/TeamCode/src/main/kotlin/pioneer/helpers/Misc.kt index 9676b4a..469735f 100644 --- a/TeamCode/src/main/kotlin/pioneer/helpers/Misc.kt +++ b/TeamCode/src/main/kotlin/pioneer/helpers/Misc.kt @@ -5,8 +5,8 @@ import kotlin.enums.enumEntries /** * Returns the next enum entry in a cyclic manner. */ -inline fun > T.next(): T { +inline fun > T.next(step: Int = 1): T { val entries = enumEntries() - val nextOrdinal = (this.ordinal + 1) % entries.size + val nextOrdinal = (this.ordinal + step) % entries.size return entries[nextOrdinal] } diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/auto/AudienceSideAuto.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/auto/AudienceSideAuto.kt index 025d3ab..030706e 100644 --- a/TeamCode/src/main/kotlin/pioneer/opmodes/auto/AudienceSideAuto.kt +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/auto/AudienceSideAuto.kt @@ -104,7 +104,7 @@ class AudienceSideAuto : BaseOpMode() { bot.apply{ pinpoint?.reset(P.START_FAR) spindexer?.setArtifacts(Artifact.GREEN, Artifact.PURPLE, Artifact.PURPLE) - spindexer?.moveToNextOuttake(motifOrder.currentArtifact) + spindexer?.readyOuttake(motifOrder) follower.reset() } targetGoal = if (bot.allianceColor == AllianceColor.RED) GoalTag.RED else GoalTag.BLUE @@ -160,7 +160,7 @@ class AudienceSideAuto : BaseOpMode() { private fun state_goto_shoot() { bot.flywheel?.velocity = targetVelocity if (!bot.follower.isFollowing) { // Starting path - bot.spindexer?.moveToNextOuttake(motifOrder.currentArtifact) + bot.spindexer?.readyOuttake(motifOrder) bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.SHOOT_GOAL_FAR), 150.0) } if (bot.follower.done) { // Ending path @@ -203,27 +203,30 @@ class AudienceSideAuto : BaseOpMode() { } private fun handle_shoot_all() { - when (launchState) { - LaunchState.READY -> { - bot.spindexer?.moveToNextOuttake(motifOrder.currentArtifact) - launchState = LaunchState.MOVING_TO_POSITION - } - LaunchState.MOVING_TO_POSITION -> { - if (bot.spindexer?.reachedTarget == true && flywheelAtSpeed()) { - bot.launcher?.triggerLaunch() - launchState = LaunchState.LAUNCHING - } - } + bot.spindexer?.shootAll() - LaunchState.LAUNCHING -> { - if (bot.launcher?.isReset == true) { - bot.spindexer?.popCurrentArtifact() - motifOrder.getNextArtifact() // Cycle to next artifact - launchState = LaunchState.READY - } - } - } +// when (launchState) { +// LaunchState.READY -> { +// bot.spindexer?.moveToNextOuttake(motifOrder.currentArtifact) +// launchState = LaunchState.MOVING_TO_POSITION +// } +// +// LaunchState.MOVING_TO_POSITION -> { +// if (bot.spindexer?.reachedTarget == true && flywheelAtSpeed()) { +// bot.launcher?.triggerLaunch() +// launchState = LaunchState.LAUNCHING +// } +// } +// +// LaunchState.LAUNCHING -> { +// if (bot.launcher?.isReset == true) { +// bot.spindexer?.popCurrentArtifact() +// motifOrder.getNextArtifact() // Cycle to next artifact +// launchState = LaunchState.READY +// } +// } +// } } private fun flywheelAtSpeed(): Boolean { diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/auto/AudienceSideAutoMiddle.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/auto/AudienceSideAutoMiddle.kt index f1e40bb..c452417 100644 --- a/TeamCode/src/main/kotlin/pioneer/opmodes/auto/AudienceSideAutoMiddle.kt +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/auto/AudienceSideAutoMiddle.kt @@ -1,310 +1,312 @@ -package pioneer.opmodes.auto +// TODO: Redo logic for passive spindexer -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.util.ElapsedTime -import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor -import pioneer.Bot -import pioneer.BotType -import pioneer.Constants -import pioneer.decode.Artifact -import pioneer.decode.GoalTag -import pioneer.decode.Motif -import pioneer.decode.Obelisk -import pioneer.decode.Points -import pioneer.general.AllianceColor -import org.firstinspires.ftc.teamcode.prism.Color -import pioneer.helpers.Pose -import pioneer.helpers.Toggle -import pioneer.helpers.next -import pioneer.opmodes.BaseOpMode -import pioneer.pathing.paths.HermitePath -import pioneer.pathing.paths.LinearPath -import java.util.Timer -import java.util.TimerTask - -@Autonomous(name = "Audience Side Auto Middle", group = "Autonomous") -class AudienceSideAutoMiddle : BaseOpMode() { - enum class AutoOptions { - PRELOAD_ONLY, - FIRST_ROW, - SECOND_ROW, - ALL, - } - - // Main state for auto - enum class State { - GOTO_SHOOT, - SHOOT, - GOTO_COLLECT, - COLLECT, - LEAVE, - STOP, - } - - enum class CollectState { - GOAL, - MID, - AUDIENCE, - DONE, - } - - enum class LaunchState { - READY, - MOVING_TO_POSITION, - LAUNCHING, - } - - private val allianceToggle = Toggle(false) - private lateinit var P: Points - private lateinit var targetGoal: GoalTag - private var startLeave = true - private var autoType = AutoOptions.ALL - private var state = State.GOTO_SHOOT - private var collectState = CollectState.MID - private var launchState = LaunchState.READY - private var targetVelocity = 980.0 - // Motif logic variables - private var motifOrder: Motif = Motif(21) - private var lookForTag = true - private val tagTimer = ElapsedTime() - private val tagTimeout = 3.0 - private var firstShot = true - - override fun onInit() { - Constants.TransferData.reset() - bot = Bot.fromType(BotType.COMP_BOT, hardwareMap) - } - - override fun init_loop() { - allianceToggle.toggle(gamepad1.touchpad) - if (allianceToggle.justChanged) { - bot.allianceColor = bot.allianceColor.next() - bot.led?.setColor( - when(bot.allianceColor) { - AllianceColor.RED -> Color.RED - AllianceColor.BLUE -> Color.BLUE - AllianceColor.NEUTRAL -> Color.PURPLE - }, - 0, - 23 - ) - } - - bot.camera?.getProcessor()?.detections?.let { detections -> - Obelisk.detectMotif(detections, bot.allianceColor)?.let { detectedMotif -> - motifOrder = detectedMotif - } - } - - telemetry.apply { - addData("Alliance Color", bot.allianceColor) - addData("Detected Motif", motifOrder.toString()) - update() - } - } - - override fun onStart() { - P = Points(bot.allianceColor) - bot.apply{ - pinpoint?.reset(P.START_FAR) - spindexer?.setArtifacts(Artifact.GREEN, Artifact.PURPLE, Artifact.PURPLE) - spindexer?.moveToNextOuttake(motifOrder.currentArtifact) - follower.reset() - } - targetGoal = if (bot.allianceColor == AllianceColor.RED) GoalTag.RED else GoalTag.BLUE - tagTimer.reset() - } - - override fun onLoop() { - when (state) { - State.GOTO_SHOOT -> state_goto_shoot() - State.SHOOT -> state_shoot() - State.GOTO_COLLECT -> state_goto_collect() - State.COLLECT -> state_collect() - State.LEAVE -> state_leave() - State.STOP -> state_stop() - } - - checkForTimeUp() - -// targetVelocity = bot.flywheel!!.estimateVelocity(bot.pinpoint!!.pose, targetGoal.shootingPose, targetGoal.shootingHeight) - handleTurret() - - telemetry.addData("Pose", bot.pinpoint!!.pose.toString()) - telemetry.addData("Follower Done", bot.follower.done) - telemetry.addData("Flywheel Speed", bot.flywheel?.velocity) - telemetry.addData("Next Artifact", motifOrder.currentArtifact) - telemetry.addData("Detected Motif", motifOrder.toString()) - telemetry.addData("Artifacts", bot.spindexer?.artifacts.contentDeepToString()) - telemetry.addData("State", state) - telemetry.addData("Collect State", collectState) - telemetry.addData("Launch State", launchState) - } - - private fun handleTurret() { - if (lookForTag && tagTimer.seconds() < tagTimeout) { - bot.turret?.autoTrack(bot.pinpoint!!.pose, Pose(0.0, 200.0)) - bot.camera?.getProcessor()?.detections?.let { detections -> - Obelisk.detectMotif(detections, bot.allianceColor)?.let { detectedMotif -> - motifOrder = detectedMotif - lookForTag = false // Detected the motif - } - } - } else { - bot.turret?.autoTrack(bot.pinpoint!!.pose, targetGoal.shootingPose) - } - } - - private fun checkForTimeUp() { - if ((30.0 - elapsedTime) < 1.5) { - state = State.LEAVE - } - } - - private fun state_goto_shoot() { - bot.flywheel?.velocity = targetVelocity - if (!bot.follower.isFollowing) { // Starting path - bot.spindexer?.moveToNextOuttake(motifOrder.currentArtifact) - if (firstShot) { - bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.SHOOT_GOAL_FAR), 150.0) - firstShot = false - } else { - bot.follower.followPath( - HermitePath( - bot.pinpoint!!.pose, - P.SHOOT_GOAL_FAR, - P.GOTO_SHOOT_VELOCITY, - Pose(0.0,0.0) - ), - 150.0 - ) - } - } - if (bot.follower.done) { // Ending path - bot.follower.reset() - state = State.SHOOT - } - } - - private fun state_shoot() { - handle_shoot_all() - if (bot.spindexer?.isEmpty == true) { - state = State.GOTO_COLLECT - - // Breakpoint for the different auto options - when (autoType) { - AutoOptions.PRELOAD_ONLY -> { - if (collectState == CollectState.GOAL) { - bot.follower.reset() - bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.LEAVE_POSITION)) - state = State.STOP - } - } - AutoOptions.FIRST_ROW -> { - if (collectState == CollectState.MID) { - bot.follower.reset() - bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.LEAVE_POSITION)) - state = State.STOP - } - } - AutoOptions.SECOND_ROW -> { - if (collectState == CollectState.AUDIENCE) { - bot.follower.reset() - bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.LEAVE_POSITION)) - state = State.STOP - } - } - AutoOptions.ALL -> {} - } - } - } - - private fun handle_shoot_all() { - when (launchState) { - LaunchState.READY -> { - bot.spindexer?.moveToNextOuttake(motifOrder.currentArtifact) - launchState = LaunchState.MOVING_TO_POSITION - } - - LaunchState.MOVING_TO_POSITION -> { - if (bot.spindexer?.reachedTarget == true && flywheelAtSpeed()) { - bot.launcher?.triggerLaunch() - launchState = LaunchState.LAUNCHING - } - } - - LaunchState.LAUNCHING -> { - if (bot.launcher?.isReset == true) { - bot.spindexer?.popCurrentArtifact() - motifOrder.getNextArtifact() // Cycle to next artifact - launchState = LaunchState.READY - } - } - } - } - - private fun flywheelAtSpeed(): Boolean { - return (bot.flywheel?.velocity ?: 0.0) > (targetVelocity - 10) && - (bot.flywheel?.velocity ?: 0.0) < (targetVelocity + 10) - } - - private fun state_goto_collect() { - if (!bot.follower.isFollowing) { // Starting path - bot.spindexer!!.moveToNextOpenIntake() - when (collectState) { - CollectState.GOAL -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.PREP_COLLECT_GOAL), 175.0) - CollectState.MID -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.PREP_COLLECT_MID), 175.0) - CollectState.AUDIENCE -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.PREP_COLLECT_AUDIENCE), 175.0) - CollectState.DONE -> state = State.STOP - } - } - - if (bot.follower.done) { // Ending path - bot.follower.reset() - state = State.COLLECT - } - } - - private fun state_collect() { - bot.intake?.forward() - if (!bot.follower.isFollowing) { // Starting path - when (collectState) { - CollectState.GOAL -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.COLLECT_GOAL), 10.0) - CollectState.MID -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.COLLECT_MID), 10.0) - CollectState.AUDIENCE -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.COLLECT_AUDIENCE), 10.0) - CollectState.DONE -> {} - } - } - - if (bot.follower.done || bot.spindexer?.isFull == true) { // Ending path - bot.follower.reset() - state = State.GOTO_SHOOT - Timer().schedule(object : TimerTask() { - override fun run() { - bot.intake?.stop() - } - }, 1000) - bot.flywheel?.velocity = targetVelocity - collectState = CollectState.DONE - } - } - - private fun state_leave() { - if (startLeave) { - bot.flywheel?.velocity = 0.0 - bot.intake?.stop() - bot.follower.followPath(LinearPath(bot.pinpoint?.pose ?: Pose(), P.LEAVE_POSITION)) - startLeave = false - } - if (bot.follower.done) { - bot.follower.reset() - } - } - - private fun state_stop() { - if (bot.follower.done) { - bot.follower.reset() - requestOpModeStop() - } - } -} +//package pioneer.opmodes.auto +// +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous +//import com.qualcomm.robotcore.util.ElapsedTime +//import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor +//import pioneer.Bot +//import pioneer.BotType +//import pioneer.Constants +//import pioneer.decode.Artifact +//import pioneer.decode.GoalTag +//import pioneer.decode.Motif +//import pioneer.decode.Obelisk +//import pioneer.decode.Points +//import pioneer.general.AllianceColor +//import org.firstinspires.ftc.teamcode.prism.Color +//import pioneer.helpers.Pose +//import pioneer.helpers.Toggle +//import pioneer.helpers.next +//import pioneer.opmodes.BaseOpMode +//import pioneer.pathing.paths.HermitePath +//import pioneer.pathing.paths.LinearPath +//import java.util.Timer +//import java.util.TimerTask +// +//@Autonomous(name = "Audience Side Auto Middle", group = "Autonomous") +//class AudienceSideAutoMiddle : BaseOpMode() { +// enum class AutoOptions { +// PRELOAD_ONLY, +// FIRST_ROW, +// SECOND_ROW, +// ALL, +// } +// +// // Main state for auto +// enum class State { +// GOTO_SHOOT, +// SHOOT, +// GOTO_COLLECT, +// COLLECT, +// LEAVE, +// STOP, +// } +// +// enum class CollectState { +// GOAL, +// MID, +// AUDIENCE, +// DONE, +// } +// +// enum class LaunchState { +// READY, +// MOVING_TO_POSITION, +// LAUNCHING, +// } +// +// private val allianceToggle = Toggle(false) +// private lateinit var P: Points +// private lateinit var targetGoal: GoalTag +// private var startLeave = true +// private var autoType = AutoOptions.ALL +// private var state = State.GOTO_SHOOT +// private var collectState = CollectState.MID +// private var launchState = LaunchState.READY +// private var targetVelocity = 980.0 +// // Motif logic variables +// private var motifOrder: Motif = Motif(21) +// private var lookForTag = true +// private val tagTimer = ElapsedTime() +// private val tagTimeout = 3.0 +// private var firstShot = true +// +// override fun onInit() { +// Constants.TransferData.reset() +// bot = Bot.fromType(BotType.COMP_BOT, hardwareMap) +// } +// +// override fun init_loop() { +// allianceToggle.toggle(gamepad1.touchpad) +// if (allianceToggle.justChanged) { +// bot.allianceColor = bot.allianceColor.next() +// bot.led?.setColor( +// when(bot.allianceColor) { +// AllianceColor.RED -> Color.RED +// AllianceColor.BLUE -> Color.BLUE +// AllianceColor.NEUTRAL -> Color.PURPLE +// }, +// 0, +// 23 +// ) +// } +// +// bot.camera?.getProcessor()?.detections?.let { detections -> +// Obelisk.detectMotif(detections, bot.allianceColor)?.let { detectedMotif -> +// motifOrder = detectedMotif +// } +// } +// +// telemetry.apply { +// addData("Alliance Color", bot.allianceColor) +// addData("Detected Motif", motifOrder.toString()) +// update() +// } +// } +// +// override fun onStart() { +// P = Points(bot.allianceColor) +// bot.apply{ +// pinpoint?.reset(P.START_FAR) +// spindexer?.setArtifacts(Artifact.GREEN, Artifact.PURPLE, Artifact.PURPLE) +// spindexer?.moveToNextOuttake(motifOrder.currentArtifact) +// follower.reset() +// } +// targetGoal = if (bot.allianceColor == AllianceColor.RED) GoalTag.RED else GoalTag.BLUE +// tagTimer.reset() +// } +// +// override fun onLoop() { +// when (state) { +// State.GOTO_SHOOT -> state_goto_shoot() +// State.SHOOT -> state_shoot() +// State.GOTO_COLLECT -> state_goto_collect() +// State.COLLECT -> state_collect() +// State.LEAVE -> state_leave() +// State.STOP -> state_stop() +// } +// +// checkForTimeUp() +// +//// targetVelocity = bot.flywheel!!.estimateVelocity(bot.pinpoint!!.pose, targetGoal.shootingPose, targetGoal.shootingHeight) +// handleTurret() +// +// telemetry.addData("Pose", bot.pinpoint!!.pose.toString()) +// telemetry.addData("Follower Done", bot.follower.done) +// telemetry.addData("Flywheel Speed", bot.flywheel?.velocity) +// telemetry.addData("Next Artifact", motifOrder.currentArtifact) +// telemetry.addData("Detected Motif", motifOrder.toString()) +// telemetry.addData("Artifacts", bot.spindexer?.artifacts.contentDeepToString()) +// telemetry.addData("State", state) +// telemetry.addData("Collect State", collectState) +// telemetry.addData("Launch State", launchState) +// } +// +// private fun handleTurret() { +// if (lookForTag && tagTimer.seconds() < tagTimeout) { +// bot.turret?.autoTrack(bot.pinpoint!!.pose, Pose(0.0, 200.0)) +// bot.camera?.getProcessor()?.detections?.let { detections -> +// Obelisk.detectMotif(detections, bot.allianceColor)?.let { detectedMotif -> +// motifOrder = detectedMotif +// lookForTag = false // Detected the motif +// } +// } +// } else { +// bot.turret?.autoTrack(bot.pinpoint!!.pose, targetGoal.shootingPose) +// } +// } +// +// private fun checkForTimeUp() { +// if ((30.0 - elapsedTime) < 1.5) { +// state = State.LEAVE +// } +// } +// +// private fun state_goto_shoot() { +// bot.flywheel?.velocity = targetVelocity +// if (!bot.follower.isFollowing) { // Starting path +// bot.spindexer?.moveToNextOuttake(motifOrder.currentArtifact) +// if (firstShot) { +// bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.SHOOT_GOAL_FAR), 150.0) +// firstShot = false +// } else { +// bot.follower.followPath( +// HermitePath( +// bot.pinpoint!!.pose, +// P.SHOOT_GOAL_FAR, +// P.GOTO_SHOOT_VELOCITY, +// Pose(0.0,0.0) +// ), +// 150.0 +// ) +// } +// } +// if (bot.follower.done) { // Ending path +// bot.follower.reset() +// state = State.SHOOT +// } +// } +// +// private fun state_shoot() { +// handle_shoot_all() +// if (bot.spindexer?.isEmpty == true) { +// state = State.GOTO_COLLECT +// +// // Breakpoint for the different auto options +// when (autoType) { +// AutoOptions.PRELOAD_ONLY -> { +// if (collectState == CollectState.GOAL) { +// bot.follower.reset() +// bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.LEAVE_POSITION)) +// state = State.STOP +// } +// } +// AutoOptions.FIRST_ROW -> { +// if (collectState == CollectState.MID) { +// bot.follower.reset() +// bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.LEAVE_POSITION)) +// state = State.STOP +// } +// } +// AutoOptions.SECOND_ROW -> { +// if (collectState == CollectState.AUDIENCE) { +// bot.follower.reset() +// bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.LEAVE_POSITION)) +// state = State.STOP +// } +// } +// AutoOptions.ALL -> {} +// } +// } +// } +// +// private fun handle_shoot_all() { +// when (launchState) { +// LaunchState.READY -> { +// bot.spindexer?.moveToNextOuttake(motifOrder.currentArtifact) +// launchState = LaunchState.MOVING_TO_POSITION +// } +// +// LaunchState.MOVING_TO_POSITION -> { +// if (bot.spindexer?.reachedTarget == true && flywheelAtSpeed()) { +// bot.launcher?.triggerLaunch() +// launchState = LaunchState.LAUNCHING +// } +// } +// +// LaunchState.LAUNCHING -> { +// if (bot.launcher?.isReset == true) { +// bot.spindexer?.popCurrentArtifact() +// motifOrder.getNextArtifact() // Cycle to next artifact +// launchState = LaunchState.READY +// } +// } +// } +// } +// +// private fun flywheelAtSpeed(): Boolean { +// return (bot.flywheel?.velocity ?: 0.0) > (targetVelocity - 10) && +// (bot.flywheel?.velocity ?: 0.0) < (targetVelocity + 10) +// } +// +// private fun state_goto_collect() { +// if (!bot.follower.isFollowing) { // Starting path +// bot.spindexer!!.moveToNextOpenIntake() +// when (collectState) { +// CollectState.GOAL -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.PREP_COLLECT_GOAL), 175.0) +// CollectState.MID -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.PREP_COLLECT_MID), 175.0) +// CollectState.AUDIENCE -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.PREP_COLLECT_AUDIENCE), 175.0) +// CollectState.DONE -> state = State.STOP +// } +// } +// +// if (bot.follower.done) { // Ending path +// bot.follower.reset() +// state = State.COLLECT +// } +// } +// +// private fun state_collect() { +// bot.intake?.forward() +// if (!bot.follower.isFollowing) { // Starting path +// when (collectState) { +// CollectState.GOAL -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.COLLECT_GOAL), 10.0) +// CollectState.MID -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.COLLECT_MID), 10.0) +// CollectState.AUDIENCE -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.COLLECT_AUDIENCE), 10.0) +// CollectState.DONE -> {} +// } +// } +// +// if (bot.follower.done || bot.spindexer?.isFull == true) { // Ending path +// bot.follower.reset() +// state = State.GOTO_SHOOT +// Timer().schedule(object : TimerTask() { +// override fun run() { +// bot.intake?.stop() +// } +// }, 1000) +// bot.flywheel?.velocity = targetVelocity +// collectState = CollectState.DONE +// } +// } +// +// private fun state_leave() { +// if (startLeave) { +// bot.flywheel?.velocity = 0.0 +// bot.intake?.stop() +// bot.follower.followPath(LinearPath(bot.pinpoint?.pose ?: Pose(), P.LEAVE_POSITION)) +// startLeave = false +// } +// if (bot.follower.done) { +// bot.follower.reset() +// } +// } +// +// private fun state_stop() { +// if (bot.follower.done) { +// bot.follower.reset() +// requestOpModeStop() +// } +// } +//} diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/auto/CollectionTest.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/auto/CollectionTest.kt new file mode 100644 index 0000000..4cec70d --- /dev/null +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/auto/CollectionTest.kt @@ -0,0 +1,35 @@ +package pioneer.opmodes.other + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous +import pioneer.Bot +import pioneer.BotType +import pioneer.helpers.DashboardPlotter +import pioneer.helpers.Pose +import pioneer.opmodes.BaseOpMode +import pioneer.pathing.paths.HermitePath +import kotlin.math.hypot + +//@Disabled +@Autonomous(name = "Collection Test", group = "Testing") +class CollectionTest : BaseOpMode() { + override fun onInit() { + bot = Bot.fromType(BotType.COMP_BOT, hardwareMap) + } + + override fun onLoop() { + bot.intake?.forward() + if (!bot.follower.isFollowing) { + bot.follower.followPath( + HermitePath + .Builder() + .addPoint(Pose(0.0, 0.0, theta = 0.0), Pose(0.0, 0.0)) + .addPoint(Pose(0.0, 60.0, theta = 0.0), Pose(0.0, 0.0)) + .build(), + 10.0 + ) + } + if (bot.follower.done) { + requestOpModeStop() + } + } +} diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/auto/GoalSideAuto.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/auto/GoalSideAuto.kt index 4a63b61..2a1d2f3 100644 --- a/TeamCode/src/main/kotlin/pioneer/opmodes/auto/GoalSideAuto.kt +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/auto/GoalSideAuto.kt @@ -19,8 +19,7 @@ import pioneer.helpers.next import pioneer.opmodes.BaseOpMode import pioneer.pathing.paths.HermitePath import pioneer.pathing.paths.LinearPath -import java.util.Timer -import java.util.TimerTask +import kotlin.math.hypot @Autonomous(name = "Goal Side Auto", group = "Autonomous") class GoalSideAuto : BaseOpMode() { @@ -50,8 +49,10 @@ class GoalSideAuto : BaseOpMode() { enum class LaunchState { READY, - MOVING_TO_POSITION, - LAUNCHING, +// MOVING_TO_POSITION, +// LAUNCHING, + SORTING, + SHOOTING } private val allianceToggle = Toggle(false) @@ -61,7 +62,7 @@ class GoalSideAuto : BaseOpMode() { private var state = State.GOTO_SHOOT private var collectState = CollectState.GOAL private var launchState = LaunchState.READY - private var targetVelocity = 815.0 + private var targetVelocity = 800.0 // Motif logic variables private var motifOrder: Motif = Motif(21) private var lookForTag = true @@ -69,6 +70,8 @@ class GoalSideAuto : BaseOpMode() { private var firstShoot = true private val tagTimer = ElapsedTime() private val tagTimeout = 3.0 + private val shootTimer = ElapsedTime() + private val shootTime = 2.5 // TODO: Tune override fun onInit() { Constants.TransferData.reset() @@ -110,11 +113,13 @@ class GoalSideAuto : BaseOpMode() { bot.apply{ pinpoint?.reset(P.START_GOAL) spindexer?.setArtifacts(Artifact.GREEN, Artifact.PURPLE, Artifact.PURPLE) - spindexer?.moveToNextOuttake(motifOrder.currentArtifact) follower.reset() } targetGoal = if (bot.allianceColor == AllianceColor.RED) GoalTag.RED else GoalTag.BLUE tagTimer.reset() + + // Constantly run intake to keep balls in spindexer + bot.intake?.forward() } override fun onLoop() { @@ -129,9 +134,10 @@ class GoalSideAuto : BaseOpMode() { checkForTimeUp() -// targetVelocity = bot.flywheel!!.estimateVelocity(bot.pinpoint!!.pose, targetGoal.shootingPose, targetGoal.shootingHeight) + targetVelocity = bot.flywheel!!.estimateVelocity(bot.pinpoint!!.pose, targetGoal.shootingPose, targetGoal.shootingHeight) handleTurret() +// handleFlywheel() telemetry.addData("Pose", bot.pinpoint!!.pose.toString()) telemetry.addData("Follower Done", bot.follower.done) @@ -145,12 +151,13 @@ class GoalSideAuto : BaseOpMode() { } private fun handleTurret() { - if (lookForTag && tagTimer.seconds() < tagTimeout && tagTimer.seconds() > 1.5) { + if (lookForTag && tagTimer.seconds() < tagTimeout && tagTimer.seconds() > 1.0) { bot.turret?.autoTrack(bot.pinpoint!!.pose, Pose(0.0, 200.0)) bot.camera?.getProcessor()?.detections?.let { detections -> Obelisk.detectMotif(detections, bot.allianceColor)?.let { detectedMotif -> motifOrder = detectedMotif lookForTag = false // Detected the motif + bot.spindexer?.readyOuttake(motifOrder) } } } else { @@ -158,8 +165,17 @@ class GoalSideAuto : BaseOpMode() { } } + private fun handleFlywheel() { + val distance = bot.pinpoint?.pose?.distanceTo(targetGoal.pose) + targetVelocity = bot.flywheel!!.estimateVelocity( + distance!!, + targetGoal.shootingHeight + ) + bot.flywheel?.velocity = targetVelocity + } + private fun checkForTimeUp() { - if ((30.0 - elapsedTime) < 1.5) { + if ((30.0 - elapsedTime) < 0.75) { state = State.LEAVE } } @@ -167,19 +183,21 @@ class GoalSideAuto : BaseOpMode() { private fun state_goto_shoot() { bot.flywheel?.velocity = targetVelocity if (!bot.follower.isFollowing) { // Starting path - bot.spindexer?.moveToNextOuttake(motifOrder.currentArtifact) - val endPose = if (firstShoot) P.SHOOT_GOAL_CLOSE.copy(theta=0.0) else P.SHOOT_GOAL_CLOSE + bot.spindexer?.readyOuttake(motifOrder) + val endPose = if (firstShoot) P.SHOOT_GOAL_CLOSE.copy(theta = 0.0) else P.SHOOT_GOAL_CLOSE + firstShoot = false bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, endPose)) } if (bot.follower.done) { // Ending path bot.follower.reset() + shootTimer.reset() state = State.SHOOT } } private fun state_shoot() { handle_shoot_all() - if (bot.spindexer?.isEmpty == true) { + if (shootTimer.seconds() > shootTime) { state = State.GOTO_COLLECT // Breakpoint for the different auto options @@ -211,27 +229,28 @@ class GoalSideAuto : BaseOpMode() { } private fun handle_shoot_all() { - when (launchState) { - LaunchState.READY -> { - bot.spindexer?.moveToNextOuttake(motifOrder.currentArtifact) - launchState = LaunchState.MOVING_TO_POSITION - } - - LaunchState.MOVING_TO_POSITION -> { - if (bot.spindexer?.reachedTarget == true && flywheelAtSpeed()) { - bot.launcher?.triggerLaunch() - launchState = LaunchState.LAUNCHING - } - } - - LaunchState.LAUNCHING -> { - if (bot.launcher?.isReset == true) { - bot.spindexer?.popCurrentArtifact() - motifOrder.getNextArtifact() // Cycle to next artifact - launchState = LaunchState.READY - } - } - } + bot.spindexer?.shootAll() +// when (launchState) { +// LaunchState.READY -> { +// bot.spindexer?.moveToNextOuttake(motifOrder.currentArtifact) +// launchState = LaunchState.MOVING_TO_POSITION +// } +// +// LaunchState.MOVING_TO_POSITION -> { +// if (bot.spindexer?.reachedTarget == true && flywheelAtSpeed()) { +// bot.launcher?.triggerLaunch() +// launchState = LaunchState.LAUNCHING +// } +// } +// +// LaunchState.LAUNCHING -> { +// if (bot.launcher?.isReset == true) { +// bot.spindexer?.popCurrentArtifact() +// motifOrder.getNextArtifact() // Cycle to next artifact +// launchState = LaunchState.READY +// } +// } +// } } private fun state_goto_collect() { @@ -266,13 +285,12 @@ class GoalSideAuto : BaseOpMode() { } private fun state_collect() { - bot.intake?.forward() bot.flywheel?.velocity = 0.0 if (!bot.follower.isFollowing) { // Starting path when (collectState) { - CollectState.GOAL -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.COLLECT_GOAL), 8.0) - CollectState.MID -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.COLLECT_MID), 8.0) - CollectState.AUDIENCE -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.COLLECT_AUDIENCE), 8.0) + CollectState.GOAL -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.COLLECT_GOAL), 10.0) + CollectState.MID -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.COLLECT_MID), 10.0) + CollectState.AUDIENCE -> bot.follower.followPath(LinearPath(bot.pinpoint!!.pose, P.COLLECT_AUDIENCE), 10.0) CollectState.DONE -> {} } } @@ -280,11 +298,6 @@ class GoalSideAuto : BaseOpMode() { if (bot.follower.done || bot.spindexer?.isFull == true) { // Ending path bot.follower.reset() state = State.GOTO_SHOOT - Timer().schedule(object : TimerTask() { - override fun run() { - bot.intake?.stop() - } - }, 1000) bot.flywheel?.velocity = targetVelocity when (collectState) { CollectState.GOAL -> collectState = CollectState.MID @@ -298,7 +311,6 @@ class GoalSideAuto : BaseOpMode() { private fun state_leave() { if (startLeave) { bot.flywheel?.velocity = 0.0 - bot.intake?.stop() bot.follower.followPath(LinearPath(bot.pinpoint?.pose ?: Pose(), P.LEAVE_POSITION)) startLeave = false } diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/calibration/ManualSpindexerControl.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/calibration/ManualSpindexerControl.kt index 42ff999..94ef937 100644 --- a/TeamCode/src/main/kotlin/pioneer/opmodes/calibration/ManualSpindexerControl.kt +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/calibration/ManualSpindexerControl.kt @@ -4,6 +4,8 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode import com.qualcomm.robotcore.eventloop.opmode.TeleOp import com.qualcomm.robotcore.hardware.DcMotorEx import pioneer.Constants +import pioneer.helpers.Chrono +import pioneer.helpers.PIDController //@Disabled @TeleOp(name = "Manual Spindexer Control", group = "Calibration") @@ -12,8 +14,10 @@ class ManualSpindexerControl : OpMode() { var power = 0.0 + val chrono = Chrono() + override fun init() { - motor = hardwareMap.get(DcMotorEx::class.java, Constants.HardwareNames.TURRET_MOTOR) + motor = hardwareMap.get(DcMotorEx::class.java, Constants.HardwareNames.INTAKE_MOTOR) } override fun loop() { @@ -25,8 +29,11 @@ class ManualSpindexerControl : OpMode() { // motor.power = 0.0 // } - if (gamepad1.right_bumper) power += 0.004 - if (gamepad1.left_bumper) power -= 0.004 + if (gamepad1.rightBumperWasPressed()) power += 0.05 + if (gamepad1.leftBumperWasPressed()) power -= 0.05 + +// val targetVelocity = power * Constants.Spindexer.MAX_VELOCITY +// motor.velocity = targetVelocity motor.power = power diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/calibration/SpindexerMotorControl.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/calibration/SpindexerMotorControl.kt index fab6c5c..ae072c7 100644 --- a/TeamCode/src/main/kotlin/pioneer/opmodes/calibration/SpindexerMotorControl.kt +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/calibration/SpindexerMotorControl.kt @@ -1,46 +1,46 @@ -package pioneer.opmodes.calibration - -import com.acmerobotics.dashboard.FtcDashboard -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import com.qualcomm.robotcore.eventloop.opmode.TeleOp -import pioneer.hardware.spindexer.Spindexer -import pioneer.hardware.spindexer.SpindexerMotionController -import pioneer.helpers.Toggle -import pioneer.helpers.next - -//@Disabled -@TeleOp(name = "Spindexer Motor Control", group = "Calibration") -class SpindexerMotorControl : OpMode() { - lateinit var spindexer: Spindexer - - val changePositionToggle = Toggle(false) - val applyPositionToggle = Toggle(false) - - var targetPosition = SpindexerMotionController.MotorPosition.INTAKE_1 - - override fun init() { - spindexer = Spindexer(hardwareMap).apply { init() } - } - - override fun loop() { - changePositionToggle.toggle(gamepad1.circle) - applyPositionToggle.toggle(gamepad1.cross) - - if (changePositionToggle.justChanged) { - targetPosition = targetPosition.next() - } - - if (applyPositionToggle.justChanged) { - spindexer.moveToPosition(targetPosition) - } - - spindexer.update() - - telemetry.addData("Target Position", targetPosition) - telemetry.update() - - FtcDashboard.getInstance().telemetry.addData("Motor position", spindexer.currentMotorTicks) - FtcDashboard.getInstance().telemetry.addData("Target position", spindexer.targetMotorTicks) - FtcDashboard.getInstance().telemetry.update() - } -} +//package pioneer.opmodes.calibration +// +//import com.acmerobotics.dashboard.FtcDashboard +//import com.qualcomm.robotcore.eventloop.opmode.OpMode +//import com.qualcomm.robotcore.eventloop.opmode.TeleOp +//import pioneer.hardware.spindexer.Spindexer +//import pioneer.hardware.spindexer.SpindexerMotionController +//import pioneer.helpers.Toggle +//import pioneer.helpers.next +// +////@Disabled +//@TeleOp(name = "Spindexer Motor Control", group = "Calibration") +//class SpindexerMotorControl : OpMode() { +// lateinit var spindexer: Spindexer +// +// val changePositionToggle = Toggle(false) +// val applyPositionToggle = Toggle(false) +// +// var targetPosition = SpindexerMotionController.MotorPosition.INTAKE_1 +// +// override fun init() { +// spindexer = Spindexer(hardwareMap).apply { init() } +// } +// +// override fun loop() { +// changePositionToggle.toggle(gamepad1.circle) +// applyPositionToggle.toggle(gamepad1.cross) +// +// if (changePositionToggle.justChanged) { +// targetPosition = targetPosition.next() +// } +// +// if (applyPositionToggle.justChanged) { +// spindexer.moveToPosition(targetPosition) +// } +// +// spindexer.update() +// +// telemetry.addData("Target Position", targetPosition) +// telemetry.update() +// +// FtcDashboard.getInstance().telemetry.addData("Motor position", spindexer.currentMotorTicks) +// FtcDashboard.getInstance().telemetry.addData("Target position", spindexer.targetMotorTicks) +// FtcDashboard.getInstance().telemetry.update() +// } +//} diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/other/FlywheelTesting.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/other/FlywheelTesting.kt index da7920b..8d7e02c 100644 --- a/TeamCode/src/main/kotlin/pioneer/opmodes/other/FlywheelTesting.kt +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/other/FlywheelTesting.kt @@ -1,11 +1,13 @@ package pioneer.opmodes.auto +import com.acmerobotics.dashboard.FtcDashboard 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.FileLogger import pioneer.helpers.Pose import pioneer.helpers.Toggle import pioneer.localization.localizers.Pinpoint @@ -55,5 +57,10 @@ class FlywheelTesting : BaseOpMode() { telemetry.addData("Actual Flywheel Velocity", bot.flywheel?.velocity) telemetry.addData("Target Velocity", flywheelSpeed) + + telemetryPacket.put("Flywheel Velocity", bot.flywheel?.velocity) + telemetryPacket.put("Target Velocity", flywheelSpeed) + + FileLogger.debug("Flywheel Velocity", bot.flywheel?.velocity.toString()) } } diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/other/ShootingTesting.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/other/ShootingTesting.kt index 6707bdf..7407a7e 100644 --- a/TeamCode/src/main/kotlin/pioneer/opmodes/other/ShootingTesting.kt +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/other/ShootingTesting.kt @@ -1,76 +1,76 @@ -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 onStart() { - 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)) - } -} +//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 onStart() { +// 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)) +// } +//} diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/SpindexerTest.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/SpindexerTest.kt index 399b5e7..83eb79c 100644 --- a/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/SpindexerTest.kt +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/SpindexerTest.kt @@ -2,38 +2,46 @@ package pioneer.opmodes.teleop import com.qualcomm.robotcore.eventloop.opmode.TeleOp import pioneer.Bot -import pioneer.decode.Artifact +import pioneer.decode.Motif +import pioneer.hardware.Flywheel +import pioneer.hardware.Intake import pioneer.hardware.spindexer.Spindexer import pioneer.opmodes.BaseOpMode -// Controls: -// D-Pad Down: Move spindexer to next open intake position -// Cross: Move spindexer to outtake start position -// Circle: Move spindexer to outtake next position - @TeleOp(name = "Spindexer Test") class SpindexerTest : BaseOpMode() { + var motifOrder: Motif? = Motif(22) + override fun onInit() { - bot = - Bot + bot = Bot .Builder() .add(Spindexer(hardwareMap)) + .add(Flywheel(hardwareMap)) + .add(Intake(hardwareMap)) .build() } override fun onLoop() { - if (gamepad1.dpad_down) bot.spindexer!!.moveToNextOpenIntake() - if (gamepad1.left_bumper) bot.spindexer!!.moveToNextOuttake(Artifact.GREEN) - if (gamepad1.right_bumper) bot.spindexer!!.moveToNextOuttake(Artifact.PURPLE) - if (gamepad1.touchpad) bot.spindexer!!.moveToNextOuttake() - if (gamepad1.circle) bot.spindexer!!.popCurrentArtifact() + if (gamepad1.dpad_down) bot.spindexer?.moveToNextOpenIntake() + if (gamepad1.left_bumper) { + motifOrder = motifOrder?.prevMotif() + bot.spindexer?.readyOuttake(motifOrder) + } + if (gamepad1.left_bumper) { + motifOrder = motifOrder?.nextMotif() + bot.spindexer?.readyOuttake(motifOrder) + } + if (gamepad1.touchpad) bot.spindexer?.shootAll() + if (gamepad1.circle) bot.spindexer?.shootNext() - bot.spindexer!!.update() + if (gamepad1.dpad_up) bot.intake?.forward() else bot.intake?.stop() + if (gamepad1.dpad_right) bot.flywheel?.velocity = 600.0 + if (gamepad1.dpad_left) bot.flywheel?.velocity = 0.0 - telemetry.addData("Current Position", bot.spindexer!!.currentMotorTicks) - telemetry.addData("Target Position", bot.spindexer!!.targetMotorTicks) - telemetry.addData("Spindexer Position", bot.spindexer!!.motorState.toString()) - telemetry.addData("Artifacts", bot.spindexer!!.artifacts.contentDeepToString()) - telemetry.addData("Reached Target", bot.spindexer!!.reachedTarget) + telemetry.addData("Current Position", bot.spindexer?.currentMotorTicks) + telemetry.addData("Target Position", bot.spindexer?.targetMotorTicks) + telemetry.addData("Target Motif", motifOrder.toString()) + telemetry.addData("Artifacts", bot.spindexer?.artifacts.contentDeepToString()) + telemetry.addData("Reached Target", bot.spindexer?.reachedTarget) } } diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/Teleop.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/Teleop.kt index ae147cd..190c0f4 100644 --- a/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/Teleop.kt +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/Teleop.kt @@ -65,13 +65,13 @@ class Teleop : BaseOpMode() { } private fun addTelemetryData() { + addTelemetryData("Target Motif", driver2.motif, Verbose.INFO) addTelemetryData("Alliance Color", bot.allianceColor, Verbose.INFO) addTelemetryData("Drive Power", driver1.drivePower, Verbose.INFO) addTelemetryData("Pose", bot.pinpoint!!.pose, Verbose.DEBUG) addTelemetryData("Artifacts", bot.spindexer?.artifacts.contentDeepToString(), Verbose.INFO) addTelemetryData("Turret Mode", bot.turret?.mode, Verbose.INFO) - addTelemetryData("Multishot Mode", driver2.multishotState, Verbose.INFO) addTelemetryData("Flywheel Operating", bot.flywheel?.operatingMode, Verbose.INFO) addTelemetryData("Use Auto Track Offset", driver2.useAutoTrackOffset, Verbose.DEBUG) addTelemetryData("Flywheel Speed Offset", driver2.flywheelSpeedOffset, Verbose.DEBUG) @@ -82,7 +82,6 @@ class Teleop : BaseOpMode() { // addTelemetryData("Turret Real Ticks", bot.turret?.currentTicks, Verbose.DEBUG) addTelemetryData("Drive Power", driver1.drivePower, Verbose.DEBUG) - addTelemetryData("Spindexer State", bot.spindexer?.motorState, Verbose.INFO) addTelemetryData("April Tag Relative Error", driver2.errorDegrees, Verbose.INFO) @@ -91,7 +90,7 @@ class Teleop : BaseOpMode() { telemetryPacket.put("Spindexer Target Ticks", bot.spindexer?.targetMotorTicks) telemetryPacket.put("Spindexer Ticks", bot.spindexer?.currentMotorTicks) - telemetryPacket.put("Spindexer Velocity", bot.spindexer?.currentMotorVelocity) +// telemetryPacket.put("Spindexer Velocity", bot.spindexer?.currentMotorVelocity) addTelemetryData("Field Centric", driver1.fieldCentric, Verbose.INFO) addTelemetryData("Velocity", "vx: %.2f, vy: %.2f".format(bot.pinpoint?.pose?.vx, bot.pinpoint?.pose?.vy), Verbose.DEBUG) diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/TestingTeleop.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/TestingTeleop.kt index 722bae7..51c51ce 100644 --- a/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/TestingTeleop.kt +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/TestingTeleop.kt @@ -1,77 +1,77 @@ -package pioneer.opmodes.teleop - -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.helpers.Toggle -import pioneer.helpers.next -import pioneer.opmodes.BaseOpMode -import pioneer.opmodes.teleop.drivers.TeleopDriver1 -import pioneer.opmodes.teleop.drivers.TeleopDriver2Testing - -@TeleOp(name = "Testing Teleop") -class TestingTeleop : 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 onStart() { - 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("Target Goal", driver2.targetGoal) - telemetry.addData("Turret Mode", bot.turret?.mode) - telemetry.addData("Shoot State", driver2.shootState) - telemetry.addData("Is Using Custom Target", driver2.customTrackingTarget.state) - telemetry.addData("Custom Target Distance", driver2.customTargetDistance) - telemetry.addData("Custom Target Height", driver2.customTargetHeight) - 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)) - } -} +//package pioneer.opmodes.teleop +// +//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.helpers.Toggle +//import pioneer.helpers.next +//import pioneer.opmodes.BaseOpMode +//import pioneer.opmodes.teleop.drivers.TeleopDriver1 +//import pioneer.opmodes.teleop.drivers.TeleopDriver2Testing +// +//@TeleOp(name = "Testing Teleop") +//class TestingTeleop : 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 onStart() { +// 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("Target Goal", driver2.targetGoal) +// telemetry.addData("Turret Mode", bot.turret?.mode) +// telemetry.addData("Shoot State", driver2.shootState) +// telemetry.addData("Is Using Custom Target", driver2.customTrackingTarget.state) +// telemetry.addData("Custom Target Distance", driver2.customTargetDistance) +// telemetry.addData("Custom Target Height", driver2.customTargetHeight) +// 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)) +// } +//} diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/drivers/TeleopDriver2.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/drivers/TeleopDriver2.kt index 1ae6992..b7c9156 100644 --- a/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/drivers/TeleopDriver2.kt +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/drivers/TeleopDriver2.kt @@ -1,3 +1,4 @@ +// TODO: redo logic for passive spindexer package pioneer.opmodes.teleop.drivers import com.qualcomm.robotcore.hardware.Gamepad @@ -5,11 +6,11 @@ import com.qualcomm.robotcore.util.ElapsedTime import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor import pioneer.Bot import pioneer.Constants -import pioneer.decode.Artifact import pioneer.decode.GoalTag import pioneer.general.AllianceColor import pioneer.hardware.Turret import org.firstinspires.ftc.teamcode.prism.Color +import pioneer.decode.Motif import pioneer.helpers.FileLogger import pioneer.helpers.Pose import pioneer.helpers.Toggle @@ -40,16 +41,9 @@ class TeleopDriver2( var manualFlywheelSpeed = 0.0 var flywheelSpeedOffset = 0.0 var errorDegrees: Double? = 0.0 - var shootCommanded = false - var triggerMultishot = false - var multishotState = MultishotState.IDLE + var motif: Motif = Motif(21) - enum class MultishotState { - IDLE, - MOVING, - SHOOTING - } var flywheelShouldFloat = true fun update() { @@ -58,9 +52,10 @@ class TeleopDriver2( updateTurretPose() handleFlywheel() handleTurret() + updateMotif() + readyOuttake() handleShootInput() - handleMultiShot() - processShooting() +// processShooting() updateIndicatorLED() } @@ -170,92 +165,28 @@ class TeleopDriver2( if (bot.turret?.mode == Turret.Mode.MANUAL) handleManualTrack() else handleAutoTrack() } - private fun handleShootInput() { - if (shootingArtifact) return - when { - gamepad.right_bumper -> shootArtifact(Artifact.PURPLE) - gamepad.left_bumper -> shootArtifact(Artifact.GREEN) -// gamepad.triangle -> shootArtifact() + private fun updateMotif(){ + if (gamepad.rightBumperWasPressed()){ + motif = motif.nextMotif()!! } - } - - private fun handleMultiShot() { - multiShotToggle.toggle(gamepad.touchpad) - - when(multishotState) { - MultishotState.IDLE -> { - if (multiShotToggle.justChanged && gamepad.touchpad) { - multishotState = MultishotState.MOVING - FileLogger.debug("Teleop Driver 2", "Should have changed to MOVING") - } - } - MultishotState.MOVING -> { - shootArtifact() - if (multiShotToggle.justChanged && gamepad.touchpad) { - FileLogger.debug("Teleop Driver 2", "Changed back to IDLE") - multishotState = MultishotState.IDLE - } - val atSpeed = bot.flywheel?.velocity?.let { - if (abs(finalFlywheelSpeed - it) < 20.0) { true } - else { false } - } - - if (bot.spindexer?.reachedTarget == true && atSpeed == true) { - multishotState = MultishotState.SHOOTING - triggerMultishot = true - } - } - MultishotState.SHOOTING -> { - if (multiShotToggle.justChanged && gamepad.touchpad) { - multishotState = MultishotState.IDLE - } - if (shootingArtifact) { - triggerMultishot = false - } - if (!shootingArtifact && !triggerMultishot) { - multishotState = MultishotState.MOVING - } - } - } - - if (bot.spindexer?.isEmpty == true) { - multishotState = MultishotState.IDLE + if (gamepad.leftBumperWasPressed()){ + motif = motif.prevMotif()!! } } - private fun processShooting() { - if (shootingArtifact && bot.launcher?.isReset == true ) { - shootingArtifact = false - bot.spindexer?.popCurrentArtifact(false) - } - if (!flywheelToggle.state) return - - launchToggle.toggle(gamepad.square) - shootCommanded = launchToggle.justChanged || triggerMultishot - - if (shootCommanded && - bot.spindexer?.withinDetectionTolerance == true && - bot.spindexer?.isOuttakePosition == true - ) { - bot.launcher?.triggerLaunch() - shootingArtifact = true - } else if (shootCommanded && launchPressedTimer.seconds() < 0.5) { - bot.launcher?.triggerLaunch() - shootingArtifact = true + private fun readyOuttake(){ + if (gamepad.circleWasPressed()){ + bot.spindexer?.readyOuttake(motif) } - if (shootCommanded) launchPressedTimer.reset() } - private fun shootArtifact(artifact: Artifact? = null) { - // Can't shoot when flywheel isn't moving - // Start artifact launch sequence - val moved = - if (artifact != null) { - bot.spindexer?.moveToNextOuttake(artifact) - } else { - bot.spindexer?.moveToNextOuttake() - } + private fun handleShootInput() { + if (shootingArtifact) return + when { + gamepad.square -> bot.spindexer?.shootNext() + gamepad.touchpad -> bot.spindexer?.shootAll() + } } private fun handleManualTrack() { diff --git a/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/drivers/TeleopDriver2Testing.kt b/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/drivers/TeleopDriver2Testing.kt index 0e8e30a..677eef2 100644 --- a/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/drivers/TeleopDriver2Testing.kt +++ b/TeamCode/src/main/kotlin/pioneer/opmodes/teleop/drivers/TeleopDriver2Testing.kt @@ -1,225 +1,225 @@ -package pioneer.opmodes.teleop.drivers - -import com.qualcomm.robotcore.hardware.Gamepad -import pioneer.Bot -import pioneer.decode.Artifact -import pioneer.decode.GoalTag -import pioneer.decode.Points -import pioneer.general.AllianceColor -import pioneer.hardware.Turret -import pioneer.helpers.Chrono -import pioneer.helpers.Pose -import pioneer.helpers.Toggle -import pioneer.helpers.next -import kotlin.math.PI -import kotlin.math.abs -import kotlin.math.pow - -class TeleopDriver2Testing( - private val gamepad: Gamepad, - private val bot: Bot, -) { - 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 incCustomTargetDistance = Toggle(false) - private val decCustomTargetDistance = Toggle(false) - private val incCustomTargetHeight = Toggle(false) - private val decCustomTargetHeight = Toggle(false) - - private val flywheelToggle = Toggle(false) - private val changeFlywheelRangeToggle = Toggle(false) - - var P = Points(bot.allianceColor) - - enum class ShootState { READY, MOVING_TO_POSITION, LAUNCHING } - - var flywheelVelocityEnum = FlywheelSpeedRange.SHORT_RANGE - var shootState = ShootState.READY - var targetGoal = GoalTag.RED - var customTrackingTarget = Toggle(false) - lateinit var shootingTarget: Pose - var shootingHeight = 0.0 - var customTargetHeight = 0.0 - var customTargetDistance = 0.0 - private var shootingAll = false - private var remainingShots = 0 - var turretAngle = 0.0 - var flywheelSpeed = 0.0 - - - fun update() { - checkTargetGoal() - checkShootingTarget() - customTargetHandling() - updateFlywheelSpeed() - handleFlywheel() - handleTurret() - handleShootInput() - processShooting() - updateIndicatorLED() - 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 checkShootingTarget(){ - customTrackingTarget.toggle(gamepad.dpad_right) - - if (customTrackingTarget.state){ - - if (gamepad.left_stick_button){ - shootingTarget = bot.turret?.setCustomTarget(bot.pinpoint?.pose ?: Pose(), customTargetDistance)!! - shootingHeight = customTargetHeight - } - } else { - shootingTarget = targetGoal.shootingPose - shootingHeight = targetGoal.shootingHeight - } - } - - private fun customTargetHandling(){ - incCustomTargetDistance.toggle(gamepad.right_trigger.toDouble() != 0.0) - decCustomTargetDistance.toggle(gamepad.left_trigger.toDouble() != 0.0) - - incCustomTargetHeight.toggle(gamepad.right_trigger.toDouble() != 0.0 && gamepad.circle) - decCustomTargetHeight.toggle(gamepad.left_trigger.toDouble() != 0.0 && gamepad.circle) - - if (incCustomTargetDistance.justChanged){ - customTargetDistance += 5.0 - } - if (decCustomTargetDistance.justChanged){ - customTargetDistance -= 5.0 - } - - if (incCustomTargetHeight.justChanged){ - customTargetHeight += 5.0 - } - if (decCustomTargetHeight.justChanged){ - customTargetHeight -= 5.0 - } - - } - - private fun updateFlywheelSpeed() { - isEstimateSpeed.toggle(gamepad.dpad_right) -// if (flywheelSpeed < 1.0 && gamepad.dpad_up) { -// flywheelSpeed += chrono.dt * 0.5 +//package pioneer.opmodes.teleop.drivers +// +//import com.qualcomm.robotcore.hardware.Gamepad +//import pioneer.Bot +//import pioneer.decode.Artifact +//import pioneer.decode.GoalTag +//import pioneer.decode.Points +//import pioneer.general.AllianceColor +//import pioneer.hardware.Turret +//import pioneer.helpers.Chrono +//import pioneer.helpers.Pose +//import pioneer.helpers.Toggle +//import pioneer.helpers.next +//import kotlin.math.PI +//import kotlin.math.abs +//import kotlin.math.pow +// +//class TeleopDriver2Testing( +// private val gamepad: Gamepad, +// private val bot: Bot, +//) { +// 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 incCustomTargetDistance = Toggle(false) +// private val decCustomTargetDistance = Toggle(false) +// private val incCustomTargetHeight = Toggle(false) +// private val decCustomTargetHeight = Toggle(false) +// +// private val flywheelToggle = Toggle(false) +// private val changeFlywheelRangeToggle = Toggle(false) +// +// var P = Points(bot.allianceColor) +// +// enum class ShootState { READY, MOVING_TO_POSITION, LAUNCHING } +// +// var flywheelVelocityEnum = FlywheelSpeedRange.SHORT_RANGE +// var shootState = ShootState.READY +// var targetGoal = GoalTag.RED +// var customTrackingTarget = Toggle(false) +// lateinit var shootingTarget: Pose +// var shootingHeight = 0.0 +// var customTargetHeight = 0.0 +// var customTargetDistance = 0.0 +// private var shootingAll = false +// private var remainingShots = 0 +// var turretAngle = 0.0 +// var flywheelSpeed = 0.0 +// +// +// fun update() { +// checkTargetGoal() +// checkShootingTarget() +// customTargetHandling() +// updateFlywheelSpeed() +// handleFlywheel() +// handleTurret() +// handleShootInput() +// processShooting() +// updateIndicatorLED() +// 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 checkShootingTarget(){ +// customTrackingTarget.toggle(gamepad.dpad_right) +// +// if (customTrackingTarget.state){ +// +// if (gamepad.left_stick_button){ +// shootingTarget = bot.turret?.setCustomTarget(bot.pinpoint?.pose ?: Pose(), customTargetDistance)!! +// shootingHeight = customTargetHeight +// } +// } else { +// shootingTarget = targetGoal.shootingPose +// shootingHeight = targetGoal.shootingHeight // } -// if (flywheelSpeed > 0.0 && gamepad.dpad_down) { -// flywheelSpeed -= chrono.dt * 0.5 +// } +// +// private fun customTargetHandling(){ +// incCustomTargetDistance.toggle(gamepad.right_trigger.toDouble() != 0.0) +// decCustomTargetDistance.toggle(gamepad.left_trigger.toDouble() != 0.0) +// +// incCustomTargetHeight.toggle(gamepad.right_trigger.toDouble() != 0.0 && gamepad.circle) +// decCustomTargetHeight.toggle(gamepad.left_trigger.toDouble() != 0.0 && gamepad.circle) +// +// if (incCustomTargetDistance.justChanged){ +// customTargetDistance += 5.0 // } -// flywheelSpeed = flywheelSpeed.coerceIn(0.0, 1.0) - - if (isEstimateSpeed.state) { - flywheelSpeed = bot.flywheel!!.estimateVelocity(bot.pinpoint?.pose ?: Pose(), shootingTarget, shootingHeight) - } 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 = flywheelSpeed - } else { - bot.flywheel?.velocity = 0.0 - } - } - - private fun handleTurret() { - isAutoTracking.toggle(gamepad.cross) - bot.turret?.mode = if (isAutoTracking.state) Turret.Mode.AUTO_TRACK else Turret.Mode.MANUAL - if (bot.turret?.mode == Turret.Mode.MANUAL) handleManualTrack() else handleAutoTrack() - } - - private fun handleShootInput() { - if (shootState == ShootState.READY && !shootingAll) { - when { - gamepad.right_bumper -> shootArtifact(Artifact.PURPLE) - gamepad.left_bumper -> shootArtifact(Artifact.GREEN) - gamepad.triangle -> shootArtifact() -// gamepad.touchpad -> startShootingAll() -// gamepad.touchpad -> startShootingAll() - } - } - } - - private fun processShooting() { - - if (!flywheelToggle.state) return - if (gamepad.square && - bot.spindexer?.reachedTarget == true && - bot.spindexer?.isOuttakePosition == true - ) { - bot.launcher?.triggerLaunch() - bot.spindexer?.popCurrentArtifact() - } - } - - private fun shootArtifact(artifact: Artifact? = null) { - // Can't shoot when flywheel isn't moving - // Start artifact launch sequence - val moved = - if (artifact != null) { - bot.spindexer?.moveToNextOuttake(artifact) - } else { - bot.spindexer?.moveToNextOuttake() - } -// if (moved == true) shootState = ShootState.MOVING_TO_POSITION - } - - private fun handleManualTrack() { - if (abs(gamepad.right_stick_x) > 0.02) { - turretAngle -= gamepad.right_stick_x.toDouble().pow(3) * chrono.dt/1000.0 * 5.0 - turretAngle.coerceIn( - -PI, - PI, - ) // FIX: This will break if the turret has a different range. Try to hand off this to the Turret class - } - - if (gamepad.right_stick_button) { - turretAngle = 0.0 - } - bot.turret?.gotoAngle(turretAngle) - } - - private fun handleAutoTrack() { - if (bot.turret?.mode == Turret.Mode.AUTO_TRACK) { - bot.turret?.autoTrack( - bot.pinpoint?.pose ?: Pose(), - shootingTarget, - ) - } - } - - private fun updateIndicatorLED() { - bot.flywheel?.velocity?.let { - if (it >= flywheelSpeed-10 && it <=flywheelSpeed+20) { - gamepad.setLedColor(0.0, 1.0, 0.0, -1) - } else if (it 0.0 && gamepad.dpad_down) { +//// flywheelSpeed -= chrono.dt * 0.5 +//// } +//// flywheelSpeed = flywheelSpeed.coerceIn(0.0, 1.0) +// +// if (isEstimateSpeed.state) { +// flywheelSpeed = bot.flywheel!!.estimateVelocity(bot.pinpoint?.pose ?: Pose(), shootingTarget, shootingHeight) +// } 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 = flywheelSpeed +// } else { +// bot.flywheel?.velocity = 0.0 +// } +// } +// +// private fun handleTurret() { +// isAutoTracking.toggle(gamepad.cross) +// bot.turret?.mode = if (isAutoTracking.state) Turret.Mode.AUTO_TRACK else Turret.Mode.MANUAL +// if (bot.turret?.mode == Turret.Mode.MANUAL) handleManualTrack() else handleAutoTrack() +// } +// +// private fun handleShootInput() { +// if (shootState == ShootState.READY && !shootingAll) { +// when { +// gamepad.right_bumper -> shootArtifact(Artifact.PURPLE) +// gamepad.left_bumper -> shootArtifact(Artifact.GREEN) +// gamepad.triangle -> shootArtifact() +//// gamepad.touchpad -> startShootingAll() +//// gamepad.touchpad -> startShootingAll() +// } +// } +// } +// +// private fun processShooting() { +// +// if (!flywheelToggle.state) return +// if (gamepad.square && +// bot.spindexer?.reachedTarget == true && +// bot.spindexer?.isOuttakePosition == true +// ) { +// bot.launcher?.triggerLaunch() +// bot.spindexer?.popCurrentArtifact() +// } +// } +// +// private fun shootArtifact(artifact: Artifact? = null) { +// // Can't shoot when flywheel isn't moving +// // Start artifact launch sequence +// val moved = +// if (artifact != null) { +// bot.spindexer?.moveToNextOuttake(artifact) +// } else { +// bot.spindexer?.moveToNextOuttake() +// } +//// if (moved == true) shootState = ShootState.MOVING_TO_POSITION +// } +// +// private fun handleManualTrack() { +// if (abs(gamepad.right_stick_x) > 0.02) { +// turretAngle -= gamepad.right_stick_x.toDouble().pow(3) * chrono.dt/1000.0 * 5.0 +// turretAngle.coerceIn( +// -PI, +// PI, +// ) // FIX: This will break if the turret has a different range. Try to hand off this to the Turret class +// } +// +// if (gamepad.right_stick_button) { +// turretAngle = 0.0 +// } +// bot.turret?.gotoAngle(turretAngle) +// } +// +// private fun handleAutoTrack() { +// if (bot.turret?.mode == Turret.Mode.AUTO_TRACK) { +// bot.turret?.autoTrack( +// bot.pinpoint?.pose ?: Pose(), +// shootingTarget, +// ) +// } +// } +// +// private fun updateIndicatorLED() { +// bot.flywheel?.velocity?.let { +// if (it >= flywheelSpeed-10 && it <=flywheelSpeed+20) { +// gamepad.setLedColor(0.0, 1.0, 0.0, -1) +// } else if (it