WoEN: AiR Into The Deep v3.0

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/collectors/BaseCollector.kt

@@ -49,8 +49,7 @@ open class BaseCollector(val robot: LinearOpMode, private val gameSettings: Game
             MergeOdometry(),
             DriveTrain(),
             IntakeManager(),
-            Camera(),
-            CVOdometry()
+            //CVOdometry()
         ))
     }
 
@@ -65,12 +64,18 @@ open class BaseCollector(val robot: LinearOpMode, private val gameSettings: Game
     enum class GameColor{ RED, BLUE }
     enum class GameOrientation { HUMAN, BASKET }
 
-    enum class GameStartPosition(val position: Vec2, val angle: Angle, val color: GameColor, val orientation: GameOrientation){
-        RED_HUMAN(Vec2(80.0 - 28.8, -156.5 + 1.5), Angle.ofDeg(90.0), GameColor.RED, GameOrientation.HUMAN),
-        RED_BASKET(Vec2(-80.0, -156.0 + 1.5), Angle.ofDeg(90.0), GameColor.RED, GameOrientation.BASKET),
-        BLUE_HUMAN(Vec2(-40.0, 156.5 - 1.5), Angle.ofDeg(-90.0), GameColor.BLUE, GameOrientation.HUMAN),
-        BLUE_BASKET(Vec2(80.0, 156.5 - 1.5), Angle.ofDeg(-90.0), GameColor.BLUE, GameOrientation.BASKET),
-        NONE(Vec2.ZERO, Angle(0.0), GameColor.BLUE, GameOrientation.BASKET)
+    class TeammateSate(val brick: Boolean)
+
+    enum class GameStartPosition(val position: Vec2, val angle: Angle, val color: GameColor, val orientation: GameOrientation, val teammate: TeammateSate){
+        RED_HUMAN(Vec2(-40.0, 156.5 - 1.5 + 0.2 - 1.2), Angle.ofDeg(180.0), GameColor.RED, GameOrientation.HUMAN, TeammateSate(false)),
+        RED_BASKET(Vec2(80.0, 156.5 - 1.5 + 0.2 - 1.2), Angle.ofDeg(-90.0), GameColor.RED, GameOrientation.BASKET, TeammateSate(false)),
+        BLUE_HUMAN(Vec2(-40.0, 156.5 - 1.5 + 0.2 - 1.2), Angle.ofDeg(180.0), GameColor.BLUE, GameOrientation.HUMAN, TeammateSate(false)),
+        BLUE_BASKET(Vec2(80.0, 156.5 - 1.5 + 0.2 - 1.2), Angle.ofDeg(-90.0), GameColor.BLUE, GameOrientation.BASKET, TeammateSate(false)),
+        NONE(Vec2.ZERO, Angle(0.0), GameColor.BLUE, GameOrientation.BASKET, TeammateSate(false)),
+        RED_HUMAN_BRICK(Vec2(-40.0, 156.5 - 1.5 + 0.2 - 1.2), Angle.ofDeg(180.0), GameColor.RED, GameOrientation.HUMAN, TeammateSate(true)),
+        RED_BASKET_BRICK(Vec2(80.0, 156.5 - 1.5 + 0.2 - 1.2), Angle.ofDeg(-90.0), GameColor.RED, GameOrientation.BASKET, TeammateSate(true)),
+    BLUE_HUMAN_BRICK(Vec2(-40.0, 156.5 - 1.5 + 0.2 - 1.2), Angle.ofDeg(180.0), GameColor.BLUE, GameOrientation.HUMAN, TeammateSate(true)),
+        BLUE_BASKET_BRICK(Vec2(80.0, 156.5 - 1.5 + 0.2 - 1.2), Angle.ofDeg(-90.0), GameColor.BLUE, GameOrientation.BASKET, TeammateSate(true)),
     }
 
     fun init() {
@@ -81,6 +86,8 @@ open class BaseCollector(val robot: LinearOpMode, private val gameSettings: Game
             i.init(this, _eventBus)
 
         _updateHandler.init(InitContext(devices.battery))
+
+        devices.teamLED.power = 0.9
     }
 
     fun start() {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/collectors/events/EventBus.kt

@@ -3,7 +3,8 @@ package org.firstinspires.ftc.teamcode.collectors.events
 import kotlin.reflect.KClass
 
 class EventBus {
-    private val _events = mutableMapOf<KClass<*>, ArrayList<(IEvent) -> Unit>>()
+    private val _events = hashMapOf<KClass<*>, ArrayList<(IEvent) -> Unit>>()
+    private val _anyCallbacks = mutableListOf<(IEvent) -> Unit>()
 
     fun <T: IEvent> subscribe(event: KClass<T>, callback: (T) -> Unit){
         if(_events[event] == null)
@@ -13,24 +14,21 @@ class EventBus {
     }
 
     fun <T: IEvent> invoke(event: T): T{
-        if(_events[Any::class] != null){
-            for(i in _events[Any::class]!!)
-                i.invoke(event)
-        }
+        for(i in _anyCallbacks)
+            i.invoke(event)
+
+        val callbacks = _events[event::class]
 
-        if(_events[event::class] == null)
+        if(callbacks == null)
             return event
 
-        for(i in _events[event::class]!!)
+        for(i in callbacks)
             i.invoke(event)
 
         return event
     }
 
     fun anySubscribe(callback: (IEvent) -> Unit){
-        if(_events[Any::class] == null)
-            _events[Any::class] = arrayListOf()
-
-        _events[Any::class]?.add(callback)
+        _anyCallbacks.add(callback)
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/AutoOpMode.kt

@@ -3,19 +3,20 @@ package org.firstinspires.ftc.teamcode.linearOpModes
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous
 import org.firstinspires.ftc.teamcode.collectors.BaseCollector
 import org.firstinspires.ftc.teamcode.collectors.BaseCollector.GameStartPosition
+import org.firstinspires.ftc.teamcode.modules.camera.Camera
 import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ActionsRunner
 import org.firstinspires.ftc.teamcode.modules.mainControl.runner.TrajectorySegmentRunner
 
 open class AutoOpMode(val startPos: GameStartPosition): LinearOpModeBase() {
-    override fun getOpModeSettings() = OpModeSettings(isAutoStart = false, isPreInit = false, preInitOpModeName = "TeleOpMode")
+    override fun getOpModeSettings() = OpModeSettings(isAutoStart = false, isPreInit = true, preInitOpModeName = "TeleOpMode", gamepadStart = false)
 
     override fun getCollector(): BaseCollector {
         val collector = BaseCollector(this,
             BaseCollector.GameSettings(
                 startPosition = startPos,
             ),
             isAuto = true,
-            mutableListOf(/*ся модули для автонома*/ TrajectorySegmentRunner(), ActionsRunner())
+            mutableListOf(/*ся модули для автонома*/ TrajectorySegmentRunner(), ActionsRunner(), Camera())
         )
 
         return collector
@@ -25,11 +26,17 @@ open class AutoOpMode(val startPos: GameStartPosition): LinearOpModeBase() {
 @Autonomous
 class AutoOpModeRedBasket: AutoOpMode(GameStartPosition.RED_BASKET)
 
+@Autonomous
+class AutoOpModeRedBasketBrick: AutoOpMode(GameStartPosition.RED_BASKET_BRICK)
+
 @Autonomous
 class AutoOpModeRedHuman: AutoOpMode(GameStartPosition.RED_HUMAN)
 
 @Autonomous
 class AutoOpModeBlueHuman: AutoOpMode(GameStartPosition.BLUE_HUMAN)
 
 @Autonomous
-class AutoOpModeBlueBasket: AutoOpMode(GameStartPosition.BLUE_BASKET)
+class AutoOpModeBlueBasket: AutoOpMode(GameStartPosition.BLUE_BASKET)
+
+@Autonomous
+class AutoOpModeBlueBasketBrick: AutoOpMode(GameStartPosition.BLUE_BASKET_BRICK)

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/LinearOpModeBase.kt

@@ -8,6 +8,7 @@ import org.firstinspires.ftc.teamcode.collectors.BaseCollector
 import org.firstinspires.ftc.teamcode.utils.telemetry.StaticTelemetry
 import org.firstinspires.ftc.teamcode.utils.units.Angle
 import org.firstinspires.ftc.teamcode.utils.units.Vec2
+import kotlin.math.abs
 
 /**
  * Класс для всех опмодов который запускает всю программу
@@ -18,11 +19,13 @@ open class LinearOpModeBase : LinearOpMode() {
     data class OpModeSettings(
         val isAutoStart: Boolean,
         val isPreInit: Boolean,
+        val gamepadStart: Boolean,
         val preInitOpModeName: String = "",
         val initTime: Double = 1.5
     )
 
-    protected open fun getOpModeSettings() = OpModeSettings(isAutoStart = false, isPreInit = false)
+    protected open fun getOpModeSettings() =
+        OpModeSettings(isAutoStart = false, isPreInit = false, gamepadStart = false)
 
     protected open fun getCollector() = BaseCollector(
         this,
@@ -46,6 +49,17 @@ open class LinearOpModeBase : LinearOpMode() {
 
         while (!isStarted()) {
             collector.initUpdate()
+            Thread.yield()
+
+            if ((gamepad1.options ||
+                abs(gamepad1.left_stick_x) > 0.01 ||
+                abs(gamepad1.left_stick_y) > 0.01 ||
+                abs(gamepad1.right_stick_x) > 0.01 ||
+                abs(gamepad1.right_stick_y) > 0.01 ||
+                gamepad1.touchpad) && settings.gamepadStart
+            )
+                OpModeManagerImpl.getOpModeManagerOfActivity(AppUtil.getInstance().getActivity())
+                    .startActiveOpMode()
         }
 
         resetRuntime()

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/TeleOpMode.kt

@@ -8,8 +8,9 @@ import org.firstinspires.ftc.teamcode.modules.mainControl.gamepad.Gamepad
 @TeleOp
 class TeleOpMode : LinearOpModeBase() {
     override fun getOpModeSettings() = OpModeSettings(
-        isAutoStart = true,
-        isPreInit = false
+        isAutoStart = false,
+        isPreInit = false,
+        gamepadStart = true
     )
 
     override fun getCollector(): BaseCollector {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/camera/Camera.kt

@@ -5,54 +5,63 @@ import org.firstinspires.ftc.teamcode.collectors.BaseCollector
 import org.firstinspires.ftc.teamcode.collectors.IRobotModule
 import org.firstinspires.ftc.teamcode.collectors.events.EventBus
 import org.firstinspires.ftc.teamcode.collectors.events.IEvent
+import org.firstinspires.ftc.teamcode.utils.configs.Configs
 import org.firstinspires.ftc.teamcode.utils.units.Orientation
 import org.firstinspires.ftc.vision.VisionPortal
 import org.firstinspires.ftc.vision.VisionProcessor
 
 
 class Camera : IRobotModule {
-    class RequestAllianceDetectedSticks(var sticks: Array<Orientation>? = null): IEvent
-    class RequestYellowDetectedSticks(var sticks: Array<Orientation>? = null): IEvent
-    class SetStickDetectEnable(val enable: Boolean): IEvent
-    class AddCameraProcessor(val processor: VisionProcessor): IEvent
+    class RequestAllianceDetectedSticks(var sticks: Array<Orientation>? = null) : IEvent
+    class RequestYellowDetectedSticks(var sticks: Array<Orientation>? = null) : IEvent
+    class AddCameraProcessor(val processor: VisionProcessor) : IEvent
+    class WaitFrameProcessed: IEvent
 
     private lateinit var _processor: StickProcessor
     private lateinit var _visionPortal: VisionPortal
 
     private var _visionPortalBuilder = VisionPortal.Builder()
 
     override fun init(collector: BaseCollector, bus: EventBus) {
-        bus.subscribe(RequestAllianceDetectedSticks::class){
+        bus.subscribe(WaitFrameProcessed::class){
+            _processor.waitFrame()
+        }
+
+        bus.subscribe(RequestAllianceDetectedSticks::class) {
             it.sticks = _processor.allianceSticks.get()
         }
 
-        bus.subscribe(RequestYellowDetectedSticks::class){
+        bus.subscribe(RequestYellowDetectedSticks::class) {
             it.sticks = _processor.yellowSticks.get()
         }
 
-        bus.subscribe(SetStickDetectEnable::class){
-            _processor.enableDetect.set(it.enable)
-        }
 
-        bus.subscribe(AddCameraProcessor::class){
+        bus.subscribe(AddCameraProcessor::class) {
             _visionPortalBuilder.addProcessor(it.processor)
         }
 
         _processor = StickProcessor()
 
         _processor.gameColor.set(collector.parameters.oldStartPosition.color)
 
-        _visionPortalBuilder = _visionPortalBuilder.addProcessor(_processor).setCamera(collector.devices.camera)
+        _visionPortalBuilder =
+            _visionPortalBuilder.addProcessor(_processor).setCamera(collector.devices.camera)
     }
 
     override fun start() {
         _visionPortal = _visionPortalBuilder.build()
-        FtcDashboard.getInstance().startCameraStream(_processor, 30.0)
-        _processor.enableDetect.set(true)
+
+        if (Configs.TelemetryConfig.ENABLE)
+            FtcDashboard.getInstance().startCameraStream(_processor, 15.0)
     }
 
     override fun stop() {
-        _visionPortal.stopStreaming()
-        FtcDashboard.getInstance().stopCameraStream()
+        if (Configs.TelemetryConfig.ENABLE)
+            try {
+                _visionPortal.stopStreaming()
+                FtcDashboard.getInstance().stopCameraStream()
+            } catch (_: Exception) {
+
+            }
     }
 }