Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .github/workflows/build.yml
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ jobs:

steps:
- name: Checkout code
uses: actions/checkout@v5
uses: actions/checkout@v6

- name: Set up JDK 17
uses: actions/setup-java@v5
Expand Down
2 changes: 1 addition & 1 deletion .github/workflows/ktlint.yml
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ jobs:
contents: write

steps:
- uses: actions/checkout@v5
- uses: actions/checkout@v6
with:
repository: ${{ github.event.pull_request.head.repo.full_name || github.repository }}
ref: ${{ github.event.pull_request.head.ref || github.ref }}
Expand Down
3 changes: 1 addition & 2 deletions TeamCode/src/main/kotlin/pioneer/Constants.kt
Original file line number Diff line number Diff line change
Expand Up @@ -132,7 +132,7 @@ object Camera {
private const val PITCH_DEG = 0.0
private const val ROLL_DEG = 0.0

//Lens Intrinsics
// Lens Intrinsics
const val fx = 955.23
const val fy = 962.92
const val cx = 330.05
Expand All @@ -146,4 +146,3 @@ object Camera {
val ORIENTATION_RAD: YawPitchRollAngles
get() = YawPitchRollAngles(AngleUnit.RADIANS, YAW_DEG * DEG_TO_RAD, PITCH_DEG * DEG_TO_RAD, ROLL_DEG * DEG_TO_RAD, 0)
}

25 changes: 12 additions & 13 deletions TeamCode/src/main/kotlin/pioneer/hardware/Camera.kt
Original file line number Diff line number Diff line change
Expand Up @@ -13,31 +13,30 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase
import org.firstinspires.ftc.vision.apriltag.AprilTagLibrary
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor
import pioneer.constants.HardwareNames
import pioneer.constants.Camera as CameraConstants
import kotlin.jvm.java
import pioneer.constants.Camera as CameraConstants

class Camera(
private val hardwareMap: HardwareMap,
private val cameraName: String = HardwareNames.WEBCAM,
val processors: Array<VisionProcessor> = emptyArray(),
) : HardwareComponent {

override val name = "Camera"

private lateinit var portal: VisionPortal

override fun init() {
portal =
VisionPortal
.Builder()
.setCamera(hardwareMap.get(WebcamName::class.java, cameraName))
.setCameraResolution(Size(640, 480))
.enableLiveView(true)
.apply {
if (processors.isNotEmpty()) {
addProcessors(*processors)
}
}.build()
portal =
VisionPortal
.Builder()
.setCamera(hardwareMap.get(WebcamName::class.java, cameraName))
.setCameraResolution(Size(640, 480))
.enableLiveView(true)
.apply {
if (processors.isNotEmpty()) {
addProcessors(*processors)
}
}.build()
}

// Helper function to get a specific processor by type
Expand Down
81 changes: 41 additions & 40 deletions TeamCode/src/main/kotlin/pioneer/helpers/Indexer.kt
Original file line number Diff line number Diff line change
Expand Up @@ -5,43 +5,54 @@ interface Indexer {
var index: Int

fun next()

fun previous()

fun peek(): Any?

fun isEmpty(): Boolean

fun size(): Int

fun pop(): Any?

fun push(item: Any): Boolean

fun toList(): List<Any>
}

/**
* SpinDexer - A circular indexer implementation that wraps around when advancing or retreating
* @param maxSize Optional maximum size limit for the bin. If null, no limit is enforced.
*/
class SpinDexer(private val maxSize: Int? = null, private val initialItems: List<Any> = emptyList()) : Indexer {
class SpinDexer(
private val maxSize: Int? = null,
private val initialItems: List<Any> = emptyList(),
) : Indexer {
override var bin: List<Any> = initialItems
override var index: Int = 0

/**
* Calculate the next index in a circular manner
*/
private fun nextIndex(): Int {
return if (bin.isEmpty()) 0 else (index + 1) % bin.size
}
private fun nextIndex(): Int = if (bin.isEmpty()) 0 else (index + 1) % bin.size

/**
* Calculate the previous index in a circular manner
*/
private fun previousIndex(): Int {
return if (bin.isEmpty()) 0 else if (index - 1 < 0) bin.size - 1 else index - 1
}
private fun previousIndex(): Int =
if (bin.isEmpty()) {
0
} else if (index - 1 < 0) {
bin.size - 1
} else {
index - 1
}

/**
* Calculate a circular index offset from the current position
*/
private fun circularIndex(offset: Int): Int {
return if (bin.isEmpty()) 0 else (index + offset) % bin.size
}
private fun circularIndex(offset: Int): Int = if (bin.isEmpty()) 0 else (index + offset) % bin.size

/**
* Normalize the current index after bin modification
Expand All @@ -57,9 +68,7 @@ class SpinDexer(private val maxSize: Int? = null, private val initialItems: List
/**
* Check if there's capacity to add more items
*/
private fun hasCapacity(): Boolean {
return maxSize == null || bin.size < maxSize
}
private fun hasCapacity(): Boolean = maxSize == null || bin.size < maxSize

/**
* Move to the next position in a circular manner
Expand All @@ -82,30 +91,22 @@ class SpinDexer(private val maxSize: Int? = null, private val initialItems: List
/**
* Peek at the current item without removing it
*/
override fun peek(): Any? {
return bin.getOrNull(index)
}
override fun peek(): Any? = bin.getOrNull(index)

/**
* Check if the bin is empty
*/
override fun isEmpty(): Boolean {
return bin.isEmpty()
}
override fun isEmpty(): Boolean = bin.isEmpty()

/**
* Get the number of items in the bin
*/
override fun size(): Int {
return bin.size
}
override fun size(): Int = bin.size

/**
* Check if the bin is full (only applicable if maxSize is set)
*/
fun isFull(): Boolean {
return !hasCapacity()
}
fun isFull(): Boolean = !hasCapacity()

/**
* Remove and return the current item
Expand All @@ -124,21 +125,20 @@ class SpinDexer(private val maxSize: Int? = null, private val initialItems: List
* @param item The item to add to the bin
* @return true if the item was added successfully, false if the bin is full
*/
override fun push(item: Any): Boolean {
return if (hasCapacity()) {
override fun push(item: Any): Boolean =
if (hasCapacity()) {
bin = bin + item
true
} else {
false
}
}

/**
* Get the items as a list in order starting from the current position
*/
override fun toList(): List<Any> {
if (bin.isEmpty()) return emptyList()

return List(bin.size) { i ->
bin[circularIndex(i)]
}
Expand All @@ -149,20 +149,21 @@ class SpinDexer(private val maxSize: Int? = null, private val initialItems: List
*/
override fun toString(): String {
if (bin.isEmpty()) return "SpinDexer[]"

val prevIdx = previousIndex()
val nextIdx = nextIndex()

val items = bin.mapIndexed { idx, item ->
val marker = when (idx) {
index -> "(curr)"
nextIdx -> "(next)"
prevIdx -> "(prev)"
else -> ""

val items =
bin.mapIndexed { idx, item ->
val marker =
when (idx) {
index -> "(curr)"
nextIdx -> "(next)"
prevIdx -> "(prev)"
else -> ""
}
"$item$marker"
}
"$item$marker"
}
return "SpinDexer[${items.joinToString(", ")}]"
}

}
1 change: 0 additions & 1 deletion TeamCode/src/main/kotlin/pioneer/opmodes/BaseOpMode.kt
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@ import pioneer.Bot
import pioneer.hardware.MecanumBase
import pioneer.helpers.Chrono
import pioneer.helpers.FileLogger
import pioneer.localization.Localizer
import pioneer.localization.localizers.Pinpoint

// Base OpMode class to be extended by all user-defined OpModes
Expand Down
38 changes: 27 additions & 11 deletions TeamCode/src/main/kotlin/pioneer/opmodes/other/AprilTagsTest.kt
Original file line number Diff line number Diff line change
Expand Up @@ -10,13 +10,15 @@ import kotlin.math.*

@TeleOp(name = "April Tags Test")
class AprilTagsTest : BaseOpMode() {
private val processor : AprilTagProcessor = Camera.createAprilTagProcessor()
private val processor: AprilTagProcessor = Camera.createAprilTagProcessor()

override fun onInit() {
bot = Bot.builder()
.add(Pinpoint(hardwareMap))
.add(Camera(hardwareMap, processors = arrayOf(processor)))
.build()
bot =
Bot
.builder()
.add(Pinpoint(hardwareMap))
.add(Camera(hardwareMap, processors = arrayOf(processor)))
.build()
}

override fun onLoop() {
Expand All @@ -27,16 +29,31 @@ class AprilTagsTest : BaseOpMode() {

private fun fieldPosition() {
val detections = processor.detections
//TODO: Avg position if given multiple tags?
// TODO: Avg position if given multiple tags?
for (detection in detections) {
val tagPosition = listOf(detection.metadata.fieldPosition[0], detection.metadata.fieldPosition[1], detection.metadata.fieldPosition[2])
val fieldPositionWithTag = listOf((tagPosition[0]+detection.ftcPose.x).toFloat(), (tagPosition[1]+detection.ftcPose.y).toFloat(), (tagPosition[1]+detection.ftcPose.z).toFloat())
val tagPosition =
listOf(detection.metadata.fieldPosition[0], detection.metadata.fieldPosition[1], detection.metadata.fieldPosition[2])
val fieldPositionWithTag =
listOf(
(tagPosition[0] + detection.ftcPose.x).toFloat(),
(tagPosition[1] + detection.ftcPose.y).toFloat(),
(
tagPosition[1] +
detection.ftcPose.z
).toFloat(),
)

telemetry.addLine("--Field Position From Tag (x, y, z): (%.2f, %.2f, %.2f)".format(fieldPositionWithTag[0], fieldPositionWithTag[1], fieldPositionWithTag[2]))
telemetry.addLine(
"--Field Position From Tag (x, y, z): (%.2f, %.2f, %.2f)".format(
fieldPositionWithTag[0],
fieldPositionWithTag[1],
fieldPositionWithTag[2],
),
)
telemetry.addLine("--Bot Position (x, y): (%.2f, %.2f)".format(bot.pinpoint?.pose?.x, bot.pinpoint?.pose?.y))

}
}

@Deprecated("ts sucks just use the library")
private fun calculateAprilTag() {
val detections = processor.detections
Expand Down Expand Up @@ -88,5 +105,4 @@ class AprilTagsTest : BaseOpMode() {
}
}
}

}
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,9 @@ package pioneer.opmodes.other

import com.qualcomm.robotcore.eventloop.opmode.TeleOp
import pioneer.Bot
import pioneer.hardware.Camera
import pioneer.decode.Obelisk
import pioneer.general.AllianceColor
import pioneer.hardware.Camera
import pioneer.opmodes.BaseOpMode

@TeleOp(name = "Obelisk Test")
Expand Down
14 changes: 6 additions & 8 deletions TeamCode/src/main/kotlin/pioneer/vision/ColorBlob.kt
Original file line number Diff line number Diff line change
Expand Up @@ -11,23 +11,21 @@ import org.firstinspires.ftc.vision.opencv.ColorRange
import org.firstinspires.ftc.vision.opencv.ImageRegion

class ColorBlob(
targetColor: ColorRange? = null, // null = detects ALL blobs
targetColor: ColorRange? = null, // null = detects ALL blobs
draw: Boolean = false,
) : Processor {
override val processor: ColorBlobLocatorProcessor =
ColorBlobLocatorProcessor
.Builder()
.apply { targetColor?.let { setTargetColorRange(it) } } // Only set if provided
.apply { targetColor?.let { setTargetColorRange(it) } } // Only set if provided
.setContourMode(ContourMode.EXTERNAL_ONLY)

.setBlurSize(10) // Smooth the transitions between different colors in image
.setDilateSize(15) // Expand blobs to fill any divots on the edges
.setErodeSize(15) // Shrink blobs back to original size
.setBlurSize(10) // Smooth the transitions between different colors in image
.setDilateSize(15) // Expand blobs to fill any divots on the edges
.setErodeSize(15) // Shrink blobs back to original size
.setMorphOperationType(MorphOperationType.CLOSING)

.setRoi(ImageRegion.asUnityCenterCoordinates(-0.9, 0.9, 0.9, -0.9)) // Eliminate detection near edges
.setDrawContours(draw)
.setBoxFitColor(0) // Disable the drawing of rectangles
.setBoxFitColor(0) // Disable the drawing of rectangles
.setCircleFitColor(Color.rgb(255, 255, 0)) // Draw a circle
.build()

Expand Down
Loading