Skip to content

Commit fd09934

Browse files
Braedeng0Braedeng0
committed
Merge camera to master
1 parent f13394d commit fd09934

File tree

5 files changed

+68
-50
lines changed

5 files changed

+68
-50
lines changed

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -137,6 +137,7 @@ object Camera {
137137
const val fy = 962.92
138138
const val cx = 330.05
139139
const val cy = 186.05
140+
140141
val distortionCoefficients = floatArrayOf(0.0573F, 2.0205F, -0.0331F, 0.0021F, -14.6155F, 0F, 0F, 0F)
141142

142143
val POSITION_CM: Position

‎TeamCode/src/main/kotlin/pioneer/hardware/Camera.kt‎

Lines changed: 24 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -13,12 +13,13 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase
1313
import org.firstinspires.ftc.vision.apriltag.AprilTagLibrary
1414
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor
1515
import pioneer.constants.HardwareNames
16+
import pioneer.constants.Camera as CameraConstants
1617
import kotlin.jvm.java
1718

1819
class Camera(
1920
private val hardwareMap: HardwareMap,
20-
private val name: String = HardwareNames.WEBCAM,
21-
private val processors: Array<VisionProcessor> = emptyArray(),
21+
private val cameraName: String = HardwareNames.WEBCAM,
22+
val processors: Array<VisionProcessor> = emptyArray(),
2223
) : HardwareComponent {
2324

2425
override val name = "Camera"
@@ -29,7 +30,7 @@ class Camera(
2930
portal =
3031
VisionPortal
3132
.Builder()
32-
.setCamera(hardwareMap.get(WebcamName::class.java, name))
33+
.setCamera(hardwareMap.get(WebcamName::class.java, cameraName))
3334
.setCameraResolution(Size(640, 480))
3435
.enableLiveView(true)
3536
.apply {
@@ -40,7 +41,7 @@ class Camera(
4041
}
4142

4243
// Helper function to get a specific processor by type
43-
inline fun <reified T : VisionProcessor> getProcessor(): T? = processors.filterIsInstance<T>().firstOrNull()
44+
private inline fun <reified T : VisionProcessor> getProcessor(): T? = processors.filterIsInstance<T>().firstOrNull()
4445

4546
fun close() {
4647
portal.close()
@@ -53,32 +54,27 @@ class Camera(
5354
distanceUnit: DistanceUnit = DistanceUnit.CM,
5455
angleUnit: AngleUnit = AngleUnit.RADIANS,
5556
draw: Boolean = false,
56-
): VisionProcessor {
57-
fun createAprilTagProcessor(
58-
position: Position = Position(DistanceUnit.CM, 0.0, 0.0, 0.0, 0),
59-
orientation: YawPitchRollAngles = YawPitchRollAngles(AngleUnit.RADIANS, 0.0, 0.0, 0.0, 0),
60-
): VisionProcessor {
61-
private val library: AprilTagLibrary =
62-
AprilTagLibrary
63-
.Builder()
64-
.addTags(AprilTagGameDatabase.getCurrentGameTagLibrary())
65-
.build()
57+
): AprilTagProcessor {
58+
val library: AprilTagLibrary =
59+
AprilTagLibrary
60+
.Builder()
61+
.addTags(AprilTagGameDatabase.getCurrentGameTagLibrary())
62+
.build()
6663

67-
override val processor: AprilTagProcessor =
68-
AprilTagProcessor
69-
.Builder()
70-
.setTagLibrary(library)
71-
.setCameraPose(position, orientation)
72-
.setLensIntrinsics(Camera.fx, Camera.fy, Camera.cx, Camera.cy)
73-
.setOutputUnits(distanceUnit, angleUnit)
74-
.setDrawTagID(draw)
75-
.setDrawTagOutline(draw)
76-
.setDrawAxes(draw)
77-
.setDrawCubeProjection(draw)
78-
.build()
64+
val processor: AprilTagProcessor =
65+
AprilTagProcessor
66+
.Builder()
67+
.setTagLibrary(library)
68+
.setCameraPose(position, orientation)
69+
.setLensIntrinsics(CameraConstants.fx, CameraConstants.fy, CameraConstants.cx, CameraConstants.cy)
70+
.setOutputUnits(distanceUnit, angleUnit)
71+
.setDrawTagID(draw)
72+
.setDrawTagOutline(draw)
73+
.setDrawAxes(draw)
74+
.setDrawCubeProjection(draw)
75+
.build()
7976

80-
return processor
81-
}
77+
return processor
8278
}
8379
}
8480
}

‎TeamCode/src/main/kotlin/pioneer/opmodes/BaseOpMode.kt‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,11 @@ package pioneer.opmodes
33
import com.acmerobotics.dashboard.telemetry.TelemetryPacket
44
import com.qualcomm.robotcore.eventloop.opmode.OpMode
55
import pioneer.Bot
6+
import pioneer.hardware.MecanumBase
67
import pioneer.helpers.Chrono
78
import pioneer.helpers.FileLogger
9+
import pioneer.localization.Localizer
10+
import pioneer.localization.localizers.Pinpoint
811

912
// Base OpMode class to be extended by all user-defined OpModes
1013
abstract class BaseOpMode : OpMode() {
@@ -41,7 +44,9 @@ abstract class BaseOpMode : OpMode() {
4144
onLoop()
4245

4346
// Update path follower
44-
bot.follower.update(dt)
47+
if (bot.has<Pinpoint>() && bot.has<MecanumBase>()) {
48+
bot.follower.update(dt)
49+
}
4550

4651
// Automatically handle telemetry updates
4752
updateTelemetry()

‎TeamCode/src/main/kotlin/pioneer/opmodes/other/AprilTagsTest.kt‎

Lines changed: 28 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -1,55 +1,66 @@
11
package pioneer.opmodes.other
22

33
import com.qualcomm.robotcore.eventloop.opmode.TeleOp
4-
import pioneer.BotType
4+
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor
5+
import pioneer.Bot
6+
import pioneer.hardware.Camera
7+
import pioneer.localization.localizers.Pinpoint
58
import pioneer.opmodes.BaseOpMode
9+
import kotlin.math.*
610

711
@TeleOp(name = "April Tags Test")
8-
class AprilTagsTest : BaseOpMode(BotType.GOBILDA_STARTER_BOT) {
12+
class AprilTagsTest : BaseOpMode() {
13+
private val processor : AprilTagProcessor = Camera.createAprilTagProcessor()
14+
915
override fun onInit() {
16+
bot = Bot.builder()
17+
.add(Pinpoint(hardwareMap))
18+
.add(Camera(hardwareMap, processors = arrayOf(processor)))
19+
.build()
1020
}
1121

1222
override fun onLoop() {
1323
calculateAprilTag()
1424
addAprilTagTelemetryData()
25+
fieldPosition()
1526
}
1627

1728
private fun fieldPosition() {
18-
val detections = bot.aprilTagProcessor.detections
29+
val detections = processor.detections
1930
//TODO: Avg position if given multiple tags?
2031
for (detection in detections) {
21-
var tagPosition = listOf(detection.metadata.fieldPosition[0], detection.metadata.fieldPosition[1], detection.metadata.fieldPosition[2])
22-
var fieldPositionWithTag = listOf((tagPosition[0]+detection.ftcPose.x).toFloat(), (tagPosition[1]+detection.ftcPose.y).toFloat(), (tagPosition[1]+detection.ftcPose.z).toFloat())
32+
val tagPosition = listOf(detection.metadata.fieldPosition[0], detection.metadata.fieldPosition[1], detection.metadata.fieldPosition[2])
33+
val fieldPositionWithTag = listOf((tagPosition[0]+detection.ftcPose.x).toFloat(), (tagPosition[1]+detection.ftcPose.y).toFloat(), (tagPosition[1]+detection.ftcPose.z).toFloat())
2334

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

2738
}
2839
}
2940
@Deprecated("ts sucks just use the library")
3041
private fun calculateAprilTag() {
31-
val detections = bot.aprilTagProcessor.detections
42+
val detections = processor.detections
3243
for (detection in detections) {
33-
if (detection != null && detection.ftcPose != null) {
34-
var rho = detection.ftcPose.range
35-
var phi = detection.ftcPose.elevation
36-
var theta = detection.ftcPose.bearing
44+
if (detection?.ftcPose != null) {
45+
val rho = detection.ftcPose.range
46+
val phi = detection.ftcPose.elevation
47+
val theta = detection.ftcPose.bearing
3748

38-
var height = Math.sin(phi) * rho
39-
var hypotenuseXY = height / Math.tan(phi)
40-
var dX = Math.sin(theta) * hypotenuseXY
41-
var dY = Math.cos(theta) * hypotenuseXY
49+
val height = sin(phi) * rho
50+
val hypotenuseXY = height / tan(phi)
51+
val dX = sin(theta) * hypotenuseXY
52+
val dY = cos(theta) * hypotenuseXY
4253

4354
telemetry.addLine("--Calculated Rel (x, y, z): (%.2f, %.2f,%.2f)".format(dX, dY, height))
4455
}
4556
}
4657
}
4758

4859
private fun addAprilTagTelemetryData() {
49-
val detections = bot.aprilTagProcessor.getDetections()
60+
val detections = processor.detections
5061
for (detection in detections) {
5162
// Check if tag or its properties are null to avoid null pointer exceptions
52-
if (detection != null && detection.ftcPose != null) {
63+
if (detection?.ftcPose != null) {
5364
telemetry.addData("Detection", detection.id)
5465
telemetry.addLine(
5566
"--Rel (x, y, z): (%.2f, %.2f, %.2f)".format(

‎TeamCode/src/main/kotlin/pioneer/opmodes/other/ObeliskTest.kt‎

Lines changed: 9 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,21 @@
11
package pioneer.opmodes.other
22

33
import com.qualcomm.robotcore.eventloop.opmode.TeleOp
4-
import pioneer.BotType
4+
import pioneer.Bot
5+
import pioneer.hardware.Camera
56
import pioneer.decode.Obelisk
67
import pioneer.general.AllianceColor
78
import pioneer.opmodes.BaseOpMode
89

910
@TeleOp(name = "Obelisk Test")
10-
class ObeliskTest : BaseOpMode(BotType.GOBILDA_STARTER_BOT) {
11+
class ObeliskTest : BaseOpMode() {
1112
private var alliance = AllianceColor.BLUE
1213

14+
private val processor = Camera.createAprilTagProcessor()
15+
1316
override fun onInit() {
17+
bot = Bot.builder().build()
18+
1419
telemetry.addData("Instructions", "D-pad Up=Blue, Down=Red")
1520
telemetry.addData("Alliance", alliance)
1621
telemetry.update()
@@ -30,7 +35,7 @@ class ObeliskTest : BaseOpMode(BotType.GOBILDA_STARTER_BOT) {
3035
}
3136

3237
private fun displayAllDetections() {
33-
val detections = bot.aprilTagProcessor.detections
38+
val detections = processor.detections
3439
telemetry.addData("Tags Detected", detections.size)
3540

3641
if (detections.isEmpty()) {
@@ -57,7 +62,7 @@ class ObeliskTest : BaseOpMode(BotType.GOBILDA_STARTER_BOT) {
5762
}
5863

5964
private fun findMotif() {
60-
val detections = bot.aprilTagProcessor.detections
65+
val detections = processor.detections
6166
val motif = Obelisk.detectMotif(detections, alliance)
6267

6368
if (motif != null && motif.isValid()) {

0 commit comments

Comments
 (0)