WoEN: AiR Into The Deep v3.0

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

@@ -1,26 +1,20 @@
 package org.firstinspires.ftc.teamcode.collectors
 
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
-import com.qualcomm.robotcore.util.ElapsedTime
 import org.firstinspires.ftc.teamcode.collectors.events.EventBus
-import org.firstinspires.ftc.teamcode.modules.camera.Camera
 import org.firstinspires.ftc.teamcode.modules.driveTrain.DriveTrain
-import org.firstinspires.ftc.teamcode.modules.intake.Intake
 import org.firstinspires.ftc.teamcode.modules.intake.IntakeManager
+import org.firstinspires.ftc.teamcode.modules.navigation.HardwareOdometers
 import org.firstinspires.ftc.teamcode.modules.navigation.gyro.IMUGyro
 import org.firstinspires.ftc.teamcode.modules.navigation.gyro.MergeGyro
 import org.firstinspires.ftc.teamcode.modules.navigation.gyro.OdometerGyro
-import org.firstinspires.ftc.teamcode.modules.navigation.HardwareOdometers
-import org.firstinspires.ftc.teamcode.modules.navigation.odometry.CVOdometry
 import org.firstinspires.ftc.teamcode.modules.navigation.odometry.MergeOdometry
 import org.firstinspires.ftc.teamcode.modules.navigation.odometry.OdometersOdometry
 import org.firstinspires.ftc.teamcode.utils.bulk.Bulk
 import org.firstinspires.ftc.teamcode.utils.devices.Battery
 import org.firstinspires.ftc.teamcode.utils.devices.Devices
-import org.firstinspires.ftc.teamcode.utils.telemetry.StaticTelemetry
 import org.firstinspires.ftc.teamcode.utils.timer.Timers
 import org.firstinspires.ftc.teamcode.utils.units.Angle
-import org.firstinspires.ftc.teamcode.utils.units.Orientation
 import org.firstinspires.ftc.teamcode.utils.units.Vec2
 import org.firstinspires.ftc.teamcode.utils.updateListener.UpdateHandler
 
@@ -29,8 +23,13 @@ import org.firstinspires.ftc.teamcode.utils.updateListener.UpdateHandler
  *
  * @author tikhonsmovzh
  */
-open class BaseCollector(val robot: LinearOpMode, private val gameSettings: GameSettings, val isAuto: Boolean, val _allModules: MutableList<IRobotModule>) {
-    companion object{
+open class BaseCollector(
+    val robot: LinearOpMode,
+    private val gameSettings: GameSettings,
+    val isAuto: Boolean,
+    val _allModules: MutableList<IRobotModule>
+) {
+    companion object {
         private val staticParameters = StaticParameters(GameStartPosition.NONE)
     }
 
@@ -41,17 +40,19 @@ open class BaseCollector(val robot: LinearOpMode, private val gameSettings: Game
     val devices = Devices(robot.hardwareMap)
 
     init {
-        _allModules.addAll(arrayOf(/*ся модули*/HardwareOdometers(),
-            IMUGyro(),
-            OdometerGyro(),
-            MergeGyro(),
-            OdometersOdometry(),
-            MergeOdometry(),
-            DriveTrain(),
-            IntakeManager(),
-            Camera(),
-            CVOdometry()
-        ))
+        _allModules.addAll(
+            arrayOf(
+                /*ся модули*/HardwareOdometers(),
+                IMUGyro(),
+                OdometerGyro(),
+                MergeGyro(),
+                OdometersOdometry(),
+                MergeOdometry(),
+                DriveTrain(),
+                IntakeManager(),
+                //CVOdometry()
+            )
+        )
     }
 
     private val _updateHandler = UpdateHandler()
@@ -62,25 +63,87 @@ open class BaseCollector(val robot: LinearOpMode, private val gameSettings: Game
 
     data class GameSettings(val startPosition: GameStartPosition = GameStartPosition.NONE)
 
-    enum class GameColor{ RED, BLUE }
+    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() {
-        if(gameSettings.startPosition != GameStartPosition.NONE)
+        if (gameSettings.startPosition != GameStartPosition.NONE)
             staticParameters.oldStartPosition = gameSettings.startPosition
 
         for (i in _allModules)
             i.init(this, _eventBus)
 
         _updateHandler.init(InitContext(devices.battery))
+
+        devices.teamLED.power = 0.9
     }
 
     fun start() {
@@ -110,12 +173,12 @@ open class BaseCollector(val robot: LinearOpMode, private val gameSettings: Game
         _updateHandler.stop()
     }
 
-    fun initUpdate(){
+    fun initUpdate() {
         _timers.update()
         _bulkAdapter.update()
         devices.battery.update()
 
-        for(i in _allModules)
+        for (i in _allModules)
             i.initUpdate()
 
         _updateHandler.update()

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

@@ -3,34 +3,32 @@ 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)
+    fun <T : IEvent> subscribe(event: KClass<T>, callback: (T) -> Unit) {
+        if (_events[event] == null)
             _events[event] = arrayListOf()
 
         _events[event]?.add(callback as (IEvent) -> Unit)
     }
 
-    fun <T: IEvent> invoke(event: T): T{
-        if(_events[Any::class] != null){
-            for(i in _events[Any::class]!!)
-                i.invoke(event)
-        }
+    fun <T : IEvent> invoke(event: T): T {
+        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)
+    fun anySubscribe(callback: (IEvent) -> Unit) {
+        _anyCallbacks.add(callback)
     }
 }

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

@@ -1,3 +1,3 @@
 package org.firstinspires.ftc.teamcode.collectors.events
 
-interface IEvent {}
+interface IEvent

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

@@ -3,33 +3,46 @@ 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")
+open class AutoOpMode(val startPos: GameStartPosition) : LinearOpModeBase() {
+    override fun getOpModeSettings() = OpModeSettings(
+        isAutoStart = false,
+        isPreInit = true,
+        preInitOpModeName = "TeleOpMode",
+        gamepadStart = false
+    )
 
     override fun getCollector(): BaseCollector {
-        val collector = BaseCollector(this,
+        val collector = BaseCollector(
+            this,
             BaseCollector.GameSettings(
                 startPosition = startPos,
             ),
             isAuto = true,
-            mutableListOf(/*ся модули для автонома*/ TrajectorySegmentRunner(), ActionsRunner())
+            mutableListOf(TrajectorySegmentRunner(), ActionsRunner(), Camera())
         )
 
         return collector
     }
 }
 
 @Autonomous
-class AutoOpModeRedBasket: AutoOpMode(GameStartPosition.RED_BASKET)
+class AutoOpModeRedBasket : AutoOpMode(GameStartPosition.RED_BASKET)
 
 @Autonomous
-class AutoOpModeRedHuman: AutoOpMode(GameStartPosition.RED_HUMAN)
+class AutoOpModeRedBasketBrick : AutoOpMode(GameStartPosition.RED_BASKET_BRICK)
 
 @Autonomous
-class AutoOpModeBlueHuman: AutoOpMode(GameStartPosition.BLUE_HUMAN)
+class AutoOpModeRedHuman : AutoOpMode(GameStartPosition.RED_HUMAN)
 
 @Autonomous
-class AutoOpModeBlueBasket: AutoOpMode(GameStartPosition.BLUE_BASKET)
+class AutoOpModeBlueHuman : AutoOpMode(GameStartPosition.BLUE_HUMAN)
+
+@Autonomous
+class AutoOpModeBlueBasket : AutoOpMode(GameStartPosition.BLUE_BASKET)
+
+@Autonomous
+class AutoOpModeBlueBasketBrick : AutoOpMode(GameStartPosition.BLUE_BASKET_BRICK)

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

@@ -7,11 +7,12 @@ import org.firstinspires.ftc.robotcore.internal.system.AppUtil
 import org.firstinspires.ftc.teamcode.utils.configs.Configs
 
 @TeleOp
-class DisableTelemetry: LinearOpMode() {
+class DisableTelemetry : LinearOpMode() {
     override fun runOpMode() {
         Configs.TelemetryConfig.ENABLE = false
 
-        OpModeManagerImpl.getOpModeManagerOfActivity(AppUtil.getInstance().getActivity()).startActiveOpMode()
+        OpModeManagerImpl.getOpModeManagerOfActivity(AppUtil.getInstance().activity)
+            .startActiveOpMode()
 
         waitForStart()
 

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

@@ -8,11 +8,12 @@ import com.qualcomm.robotcore.hardware.DcMotorEx
 import org.firstinspires.ftc.robotcore.internal.system.AppUtil
 
 @TeleOp
-class LiftDownOpMode: LinearOpMode() {
+class LiftDownOpMode : LinearOpMode() {
     override fun runOpMode() {
         val motor = hardwareMap.get("liftExtensionMotor") as DcMotorEx
 
-        OpModeManagerImpl.getOpModeManagerOfActivity(AppUtil.getInstance().getActivity()).startActiveOpMode()
+        OpModeManagerImpl.getOpModeManagerOfActivity(AppUtil.getInstance().activity)
+            .startActiveOpMode()
 
         waitForStart()
         resetRuntime()

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

@@ -2,12 +2,10 @@ package org.firstinspires.ftc.teamcode.linearOpModes
 
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
 import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl
-import com.qualcomm.robotcore.util.ElapsedTime
 import org.firstinspires.ftc.robotcore.internal.system.AppUtil
 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 +16,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,
@@ -41,11 +41,23 @@ open class LinearOpModeBase : LinearOpMode() {
         collector.init()
 
         if (settings.isAutoStart)
-            OpModeManagerImpl.getOpModeManagerOfActivity(AppUtil.getInstance().getActivity())
+            OpModeManagerImpl.getOpModeManagerOfActivity(AppUtil.getInstance().activity)
                 .startActiveOpMode()
 
         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().activity)
+                    .startActiveOpMode()
+            }
         }
 
         resetRuntime()
@@ -61,7 +73,7 @@ open class LinearOpModeBase : LinearOpMode() {
         collector.stop()
 
         if (settings.isPreInit)
-            OpModeManagerImpl.getOpModeManagerOfActivity(AppUtil.getInstance().getActivity())
+            OpModeManagerImpl.getOpModeManagerOfActivity(AppUtil.getInstance().activity)
                 .initOpMode(settings.preInitOpModeName)
     }
 }