diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/collectors/BaseCollector.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/collectors/BaseCollector.kt index ba82c8df..129fa71b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/collectors/BaseCollector.kt +++ b/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) { - companion object{ +open class BaseCollector( + val robot: LinearOpMode, + private val gameSettings: GameSettings, + val isAuto: Boolean, + val _allModules: MutableList +) { + 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() diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/collectors/events/EventBus.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/collectors/events/EventBus.kt index 348a5733..f1315b35 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/collectors/events/EventBus.kt +++ b/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, ArrayList<(IEvent) -> Unit>>() + private val _events = hashMapOf, ArrayList<(IEvent) -> Unit>>() + private val _anyCallbacks = mutableListOf<(IEvent) -> Unit>() - fun subscribe(event: KClass, callback: (T) -> Unit){ - if(_events[event] == null) + fun subscribe(event: KClass, callback: (T) -> Unit) { + if (_events[event] == null) _events[event] = arrayListOf() _events[event]?.add(callback as (IEvent) -> Unit) } - fun invoke(event: T): T{ - if(_events[Any::class] != null){ - for(i in _events[Any::class]!!) - i.invoke(event) - } + fun 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) } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/collectors/events/IEvent.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/collectors/events/IEvent.kt index b87c5352..d4a18b05 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/collectors/events/IEvent.kt +++ b/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 {} \ No newline at end of file +interface IEvent \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/AutoOpMode.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/AutoOpMode.kt index 7c9743a3..ac9a3ef3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/AutoOpMode.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/AutoOpMode.kt @@ -3,19 +3,26 @@ 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 @@ -23,13 +30,19 @@ open class AutoOpMode(val startPos: GameStartPosition): LinearOpModeBase() { } @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) \ No newline at end of file +class AutoOpModeBlueHuman : AutoOpMode(GameStartPosition.BLUE_HUMAN) + +@Autonomous +class AutoOpModeBlueBasket : AutoOpMode(GameStartPosition.BLUE_BASKET) + +@Autonomous +class AutoOpModeBlueBasketBrick : AutoOpMode(GameStartPosition.BLUE_BASKET_BRICK) \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/DisableTelemetry.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/DisableTelemetry.kt index e1bdec14..0eecf0d9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/DisableTelemetry.kt +++ b/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() diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/LiftDownOpMode.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/LiftDownOpMode.kt index 89089554..c352e123 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/LiftDownOpMode.kt +++ b/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() diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/LinearOpModeBase.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/LinearOpModeBase.kt index 6e412eef..951e6111 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/LinearOpModeBase.kt +++ b/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) } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/TeleOpMode.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/TeleOpMode.kt index c877a868..5d9d5c74 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/linearOpModes/TeleOpMode.kt +++ b/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 { @@ -19,7 +20,7 @@ class TeleOpMode : LinearOpModeBase() { startPosition = BaseCollector.GameStartPosition.NONE ), isAuto = false, - mutableListOf(/*ся модули для телеопа*/Gamepad(), Hook()) + mutableListOf(Gamepad(), Hook()) ) return collector diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/camera/Camera.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/camera/Camera.kt index e448c669..01893fd1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/camera/Camera.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/camera/Camera.kt @@ -5,16 +5,17 @@ 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? = null): IEvent - class RequestYellowDetectedSticks(var sticks: Array? = null): IEvent - class SetStickDetectEnable(val enable: Boolean): IEvent - class AddCameraProcessor(val processor: VisionProcessor): IEvent + class RequestAllianceDetectedSticks(var sticks: Array? = null) : IEvent + class RequestYellowDetectedSticks(var sticks: Array? = null) : IEvent + class AddCameraProcessor(val processor: VisionProcessor) : IEvent + class WaitFrameProcessed : IEvent private lateinit var _processor: StickProcessor private lateinit var _visionPortal: VisionPortal @@ -22,19 +23,20 @@ class Camera : IRobotModule { 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) } @@ -42,17 +44,24 @@ class Camera : IRobotModule { _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) { + + } } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/camera/StickProcessor.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/camera/StickProcessor.kt index fec87483..9e20812c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/camera/StickProcessor.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/camera/StickProcessor.kt @@ -1,20 +1,27 @@ package org.firstinspires.ftc.teamcode.modules.camera import android.graphics.Bitmap -import android.graphics.Canvas -import org.firstinspires.ftc.robotcore.external.function.Consumer +import androidx.core.graphics.createBitmap import org.firstinspires.ftc.robotcore.external.function.Continuation import org.firstinspires.ftc.robotcore.external.stream.CameraStreamSource import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration import org.firstinspires.ftc.teamcode.collectors.BaseCollector import org.firstinspires.ftc.teamcode.utils.configs.Configs -import org.firstinspires.ftc.teamcode.utils.configs.Configs.CameraConfig.StickDetectConfig import org.firstinspires.ftc.teamcode.utils.units.Angle +import org.firstinspires.ftc.teamcode.utils.units.Color import org.firstinspires.ftc.teamcode.utils.units.Orientation import org.firstinspires.ftc.teamcode.utils.units.Vec2 import org.firstinspires.ftc.vision.VisionProcessor import org.opencv.android.Utils +import org.opencv.core.Core.ROTATE_180 +import org.opencv.core.Core.add +import org.opencv.core.Core.bitwise_and import org.opencv.core.Core.inRange +import org.opencv.core.Core.multiply +import org.opencv.core.Core.rotate +import org.opencv.core.Core.split +import org.opencv.core.Core.subtract +import org.opencv.core.CvType.CV_8U import org.opencv.core.Mat import org.opencv.core.MatOfPoint import org.opencv.core.MatOfPoint2f @@ -23,11 +30,9 @@ import org.opencv.core.RotatedRect import org.opencv.core.Scalar import org.opencv.core.Size import org.opencv.imgproc.Imgproc.CHAIN_APPROX_SIMPLE -import org.opencv.imgproc.Imgproc.COLOR_RGB2HSV import org.opencv.imgproc.Imgproc.MORPH_ERODE import org.opencv.imgproc.Imgproc.RETR_TREE import org.opencv.imgproc.Imgproc.blur -import org.opencv.imgproc.Imgproc.cvtColor import org.opencv.imgproc.Imgproc.dilate import org.opencv.imgproc.Imgproc.erode import org.opencv.imgproc.Imgproc.findContours @@ -36,199 +41,269 @@ import org.opencv.imgproc.Imgproc.line import org.opencv.imgproc.Imgproc.minAreaRect import org.opencv.imgproc.Imgproc.putText import org.opencv.imgproc.Imgproc.resize -import java.util.concurrent.Callable -import java.util.concurrent.ExecutorService -import java.util.concurrent.Executors import java.util.concurrent.atomic.AtomicReference +import kotlin.math.abs +import kotlin.math.atan2 class StickProcessor : VisionProcessor, CameraStreamSource { var allianceSticks = AtomicReference>(arrayOf()) var yellowSticks = AtomicReference>(arrayOf()) - var enableDetect = AtomicReference(false) - var gameColor = AtomicReference(BaseCollector.GameColor.BLUE) + var gameColor = AtomicReference(BaseCollector.GameColor.BLUE) + + private var _isOneFrame = AtomicReference(false) private var lastFrame: AtomicReference = AtomicReference(Bitmap.createBitmap(1, 1, Bitmap.Config.RGB_565)) - private val _executorService: ExecutorService = - Executors.newWorkStealingPool(Configs.CameraConfig.DETECT_THREADS_COUNT) - override fun init(width: Int, height: Int, calibration: CameraCalibration?) { } private var _drawFrame = Mat() - private var _hsvFrame = Mat() - private var _resizedFrame = Mat() - override fun processFrame(frame: Mat, captureTimeNanos: Long): Any { - if(!enableDetect.get()) { - allianceSticks.set(arrayOf()) + override fun processFrame(frm: Mat?, captureTimeNanos: Long): Any { + val frame = frm!!.clone() - return frame - } + if (!_isOneFrame.get()) + return frm resize( frame, - _resizedFrame, + frame, Size( frame.width() * Configs.CameraConfig.COMPRESSION_COEF, frame.height() * Configs.CameraConfig.COMPRESSION_COEF ) ) - _resizedFrame.copyTo(_drawFrame) - _hsvFrame = _resizedFrame + rotate(frame, frame, ROTATE_180) + - blur(_hsvFrame, _hsvFrame, Size(5.0, 5.0)) - cvtColor(_hsvFrame, _hsvFrame, COLOR_RGB2HSV) + frame.copyTo(_drawFrame) - fun getSticks(config: StickDetectConfig): Array { - val rects = detect(config, _hsvFrame.clone()) + blur( + frame, + frame, + Size(Configs.CameraConfig.BLUR_SIZE, Configs.CameraConfig.BLUR_SIZE) + ) - val rectsList = rects.toList() + val colors = mutableListOf() - val res = Array(rectsList.size) { - val pos = rectsList[it].center + split(frame, colors) - Orientation(Vec2(pos.x, pos.y), Angle.ofDeg(rectsList[it].angle)) - } + val r = colors[0] + val g = colors[1] + val b = colors[2] + + val yellowRects = detectElements( + r, g, b, + Configs.CameraConfig.YELLOW_STICK_DETECT_CONFIG.KR, + Configs.CameraConfig.YELLOW_STICK_DETECT_CONFIG.KG, + Configs.CameraConfig.YELLOW_STICK_DETECT_CONFIG.KB, + Configs.CameraConfig.YELLOW_STICK_DETECT_CONFIG.THRESHOLD + ) + val gameCol = gameColor.get() + + val allianceRects = + if (gameCol == BaseCollector.GameColor.BLUE) + detectElements( + r, g, b, + Configs.CameraConfig.BLUE_STICK_DETECT_CONFIG.KR, + Configs.CameraConfig.BLUE_STICK_DETECT_CONFIG.KG, + Configs.CameraConfig.BLUE_STICK_DETECT_CONFIG.KB, + Configs.CameraConfig.BLUE_STICK_DETECT_CONFIG.THRESHOLD + ) + else + detectElements( + r, g, b, + Configs.CameraConfig.RED_STICK_DETECT_CONFIG.KR, + Configs.CameraConfig.RED_STICK_DETECT_CONFIG.KG, + Configs.CameraConfig.RED_STICK_DETECT_CONFIG.KB, + Configs.CameraConfig.RED_STICK_DETECT_CONFIG.THRESHOLD + ) + + drawRotatedRects( + _drawFrame, yellowRects, + Configs.CameraConfig.YELLOW_STICK_DETECT_CONFIG.CONTOUR_COLOR, + Configs.CameraConfig.YELLOW_STICK_DETECT_CONFIG.TEXT_COLOR + ) + + if (gameCol == BaseCollector.GameColor.BLUE) drawRotatedRects( - _drawFrame, - rects, - config.CONTOUR_COLOR.getScalarColor(), - config.TEXT_COLOR.getScalarColor() + _drawFrame, allianceRects, + Configs.CameraConfig.BLUE_STICK_DETECT_CONFIG.CONTOUR_COLOR, + Configs.CameraConfig.BLUE_STICK_DETECT_CONFIG.TEXT_COLOR + ) + else + drawRotatedRects( + _drawFrame, allianceRects, + Configs.CameraConfig.RED_STICK_DETECT_CONFIG.CONTOUR_COLOR, + Configs.CameraConfig.RED_STICK_DETECT_CONFIG.TEXT_COLOR ) - return res - } + yellowSticks.set(rotatedRectToOrientation(yellowRects)) + allianceSticks.set(rotatedRectToOrientation(allianceRects)) - allianceSticks.set(getSticks(if(gameColor.get() == BaseCollector.GameColor.RED) Configs.CameraConfig.RED_STICK_DETECT else Configs.CameraConfig.BLUE_STICK_DETECT)) - yellowSticks.set(getSticks(Configs.CameraConfig.YELLOW_STICK_DETECT)) + val bitmap = createBitmap(_drawFrame.width(), _drawFrame.height(), Bitmap.Config.RGB_565) - val b = Bitmap.createBitmap( - _drawFrame.width(), - _drawFrame.height(), - Bitmap.Config.RGB_565 - ) - Utils.matToBitmap(_drawFrame, b) - lastFrame.set(b) + Utils.matToBitmap(_drawFrame, bitmap) + lastFrame.set(bitmap) + + _isOneFrame.set(false) - return frame + return frm } - private fun detect( - parameters: StickDetectConfig, - hsvFrame: Mat - ): Collection { - inRange( - hsvFrame, - Scalar(parameters.H_MIN, parameters.S_MIN, parameters.V_MIN), - Scalar(parameters.H_MAX, parameters.S_MAX, parameters.V_MAX), - hsvFrame - ) + fun waitFrame() { + _isOneFrame.set(true) + + while (_isOneFrame.get()); + } - erodeDilate(hsvFrame, parameters.ERODE_DILATE) + override fun onDrawFrame( + canvas: android.graphics.Canvas?, + onscreenWidth: Int, + onscreenHeight: Int, + scaleBmpPxToCanvasPx: Float, + scaleCanvasDensity: Float, + userContext: Any? + ) { - erode( - hsvFrame, - hsvFrame, - getStructuringElement( - MORPH_ERODE, - Size(parameters.PRECOMPRESSION, parameters.PRECOMPRESSION) - ) - ) + } + + private fun rotatedRectToOrientation(rects: List): Array { + val rectsList = rects.toList() + + return Array(rectsList.size) { + val pos = Vec2( + rectsList[it].center.x, + rectsList[it].center.y + ) - Configs.AutoClamp.FRAME_SIZE / 2.0 + + var polL = pos.length() + val polA = atan2(pos.y, pos.x) + + polL += polL * polL * polL * Configs.CameraConfig.FIX_K - dilateErode(hsvFrame, parameters.DILATE_ERODE) + val fixedPos = Vec2(polL, 0.0).setRot(polA) + Configs.AutoClamp.FRAME_SIZE / 2.0 - dilate( - hsvFrame, - hsvFrame, - getStructuringElement( - MORPH_ERODE, - Size(parameters.PRECOMPRESSION, parameters.PRECOMPRESSION) + Orientation( + fixedPos, Angle.ofDeg( + (if (rectsList[it].size.width < rectsList[it].size.height) + rectsList[it].angle + else + rectsList[it].angle - 90.0) + ) ) - ) + } + } + + private fun detectElements( + r: Mat, + g: Mat, + b: Mat, + kr: Double, + kg: Double, + kb: Double, + threshold: Double + ): List { + val uR = Mat() + val uG = Mat() + val uB = Mat() - val contours = arrayListOf() + val matOfOnes = Mat.ones(r.size(), CV_8U) - findContours(hsvFrame, contours, Mat(), RETR_TREE, CHAIN_APPROX_SIMPLE) + multiply(r, matOfOnes, uR, kr) + multiply(g, matOfOnes, uG, kg) + multiply(b, matOfOnes, uB, kb) - val tasks = arrayListOf>() + val combined = Mat() - for (i in contours) - tasks.add { processContour(i) } + add(uR, uB, combined) + add(combined, uG, combined) - val result = _executorService.invokeAll(tasks) + multiply(r, matOfOnes, uR, 1.0 - kr) + multiply(g, matOfOnes, uG, 1.0 - kg) + multiply(b, matOfOnes, uB, 1.0 - kb) - return result.mapNotNull { it.get() } + subtract(combined, uR, uR) + subtract(combined, uG, uG) + subtract(combined, uB, uB) + + inRange(uR, Scalar(threshold), Scalar(255.0), uR) + inRange(uG, Scalar(threshold), Scalar(255.0), uG) + inRange(uB, Scalar(threshold), Scalar(255.0), uB) + + bitwise_and(uR, uG, combined) + bitwise_and(combined, uB, combined) + + erodeDilate(combined, Configs.CameraConfig.ERODE_DILATE_K) + + val contours = mutableListOf() + + findContours(combined, contours, Mat(), RETR_TREE, CHAIN_APPROX_SIMPLE) + + val points = MatOfPoint2f() + + return MutableList(contours.size) { + points.fromArray(*contours[it].toArray()) + val rect = minAreaRect(points) + + val attitude = + if (rect.size.width > rect.size.height) + rect.size.width / rect.size.height + else + rect.size.height / rect.size.width + + if (abs(attitude - Configs.CameraConfig.STICK_ATTITUDE) < Configs.CameraConfig.STICK_ATTITUDE_SENS && rect.size.area() < Configs.CameraConfig.MIN_STICK_AREA) + rect + else + null + }.filterNotNull() } - fun drawRotatedRects( + private fun erodeDilate(mat: Mat, kSize: Double) { + erode(mat, mat, getStructuringElement(MORPH_ERODE, Size(kSize, kSize))) + dilate(mat, mat, getStructuringElement(MORPH_ERODE, Size(kSize, kSize))) + } + + private fun drawRotatedRects( mat: Mat, rects: Collection, - rectColor: Scalar, - textColor: Scalar + rectColor: Color, + textColor: Color ) { + val scalarRectColor = + Scalar(rectColor.r.toDouble(), rectColor.g.toDouble(), rectColor.b.toDouble()) + for (i in rects) { val points = Array(4) { null } i.points(points) - line(mat, points[0], points[1], rectColor, 5) - line(mat, points[1], points[2], rectColor, 5) - line(mat, points[2], points[3], rectColor, 5) - line(mat, points[3], points[0], rectColor, 5) + line(mat, points[0], points[1], scalarRectColor, 5) + line(mat, points[1], points[2], scalarRectColor, 5) + line(mat, points[2], points[3], scalarRectColor, 5) + line(mat, points[3], points[0], scalarRectColor, 5) putText( mat, - i.angle.toInt().toString(), + ((if (i.size.width < i.size.height) + i.angle + else + i.angle - 90.0)).toInt().toString(), i.center, 5, 2.0, - textColor + Scalar(textColor.r.toDouble(), textColor.g.toDouble(), textColor.b.toDouble()) ) } } - private fun processContour(contour: MatOfPoint): RotatedRect? { - val points = MatOfPoint2f() - - points.fromArray(*contour.toArray()) - - val rect = minAreaRect(points) - - if (rect.size.height * rect.size.width > Configs.CameraConfig.MIN_STICK_AREA) - return rect - - return null - } - - override fun onDrawFrame( - canvas: Canvas?, - onscreenWidth: Int, - onscreenHeight: Int, - scaleBmpPxToCanvasPx: Float, - scaleCanvasDensity: Float, - userContext: Any? - ) { - - } - - fun erodeDilate(mat: Mat, kSize: Double) { - erode(mat, mat, getStructuringElement(MORPH_ERODE, Size(kSize, kSize))) - dilate(mat, mat, getStructuringElement(MORPH_ERODE, Size(kSize, kSize))) - } - - fun dilateErode(mat: Mat, kSize: Double) { - dilate(mat, mat, getStructuringElement(MORPH_ERODE, Size(kSize, kSize))) - erode(mat, mat, getStructuringElement(MORPH_ERODE, Size(kSize, kSize))) - } - - override fun getFrameBitmap(continuation: Continuation>?) { - continuation!!.dispatch { bitmapConsumer -> bitmapConsumer.accept(lastFrame.get()) } + override fun getFrameBitmap(continuation: Continuation?>?) { + continuation!!.dispatch { bitmapConsumer -> bitmapConsumer?.accept(lastFrame.get()) } } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/driveTrain/DriveTrain.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/driveTrain/DriveTrain.kt index 1be0cc4d..059e9961 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/driveTrain/DriveTrain.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/driveTrain/DriveTrain.kt @@ -1,6 +1,5 @@ package org.firstinspires.ftc.teamcode.modules.driveTrain -import com.acmerobotics.roadrunner.clamp import com.qualcomm.robotcore.hardware.DcMotor import com.qualcomm.robotcore.hardware.DcMotorEx import com.qualcomm.robotcore.hardware.DcMotorSimple @@ -17,9 +16,7 @@ import org.firstinspires.ftc.teamcode.utils.pidRegulator.PIDRegulator import org.firstinspires.ftc.teamcode.utils.telemetry.StaticTelemetry import org.firstinspires.ftc.teamcode.utils.units.Vec2 import kotlin.math.abs -import kotlin.math.cos import kotlin.math.max -import kotlin.math.sin class DriveTrain : IRobotModule { private lateinit var _leftForwardDrive: DcMotorEx @@ -56,40 +53,52 @@ class DriveTrain : IRobotModule { _rightBackDrive.direction = DcMotorSimple.Direction.REVERSE _rightForwardDrive.direction = DcMotorSimple.Direction.REVERSE - bus.subscribe(SetDrivePowerEvent::class){ + bus.subscribe(SetDrivePowerEvent::class) { var dir = it.direction var rot = it.rotate - if(_eventBus.invoke(IntakeManager.RequestLiftPosEvent()).pos != IntakeManager.LiftPosition.TRANSPORT) { + if (_eventBus.invoke(IntakeManager.RequestLiftPosEvent()).pos != IntakeManager.LiftPosition.TRANSPORT) { dir *= Configs.DriveTrainConfig.LIFT_MAX_SPEED_K rot *= Configs.DriveTrainConfig.LIFT_MAX_ROTATE_SPEED_K } - bus.invoke(SetDriveCmEvent( - dir * Vec2(Configs.DriveTrainConfig.MAX_TRANSLATION_VELOCITY, Configs.DriveTrainConfig.MAX_TRANSLATION_VELOCITY), - rot * Configs.DriveTrainConfig.MAX_ROTATE_VELOCITY) + bus.invoke( + SetDriveCmEvent( + dir * Vec2( + Configs.DriveTrainConfig.MAX_TRANSLATION_VELOCITY, + Configs.DriveTrainConfig.MAX_TRANSLATION_VELOCITY + ), + rot * Configs.DriveTrainConfig.MAX_ROTATE_VELOCITY + ) ) } - bus.subscribe(SetDriveCmEvent::class){ - var clampedDirLength = clamp(it.direction.length(), 0.0, Configs.DriveTrainConfig.MAX_TRANSLATION_VELOCITY) - val dirRot = it.direction.rot() - - _targetDirectionVelocity = Vec2(clampedDirLength, 0.0).setRot(dirRot) - _targetRotateVelocity = clamp( - it.rotate, - -Configs.DriveTrainConfig.MAX_ROTATE_VELOCITY, - Configs.DriveTrainConfig.MAX_ROTATE_VELOCITY - ) + bus.subscribe(SetDriveCmEvent::class) { + _targetDirectionVelocity = it.direction + _targetRotateVelocity = it.rotate } - bus.subscribe(MergeOdometry.UpdateMergeOdometryEvent::class){ + bus.subscribe(MergeOdometry.UpdateMergeOdometryEvent::class) { + + val gyro = bus.invoke(MergeGyro.RequestMergeGyroEvent()) - driveSimpleDirection(Vec2( - _velocityPidfForward.update(_targetDirectionVelocity.x - it.velocity.x, _targetDirectionVelocity.x), - _velocityPidfSide.update(_targetDirectionVelocity.y - it.velocity.y, _targetDirectionVelocity.y)), - _velocityPidfRotate.update(_targetRotateVelocity - gyro.velocity!!, _targetRotateVelocity)) + driveSimpleDirection( + Vec2( + _velocityPidfForward.update( + _targetDirectionVelocity.x - it.velocity.x, + _targetDirectionVelocity.x + ), + _velocityPidfSide.update( + _targetDirectionVelocity.y - it.velocity.y, + _targetDirectionVelocity.y + ) + ), + _velocityPidfRotate.update( + _targetRotateVelocity - gyro.velocity!!, + _targetRotateVelocity + ) + ) StaticTelemetry.addData("targetXVel", _targetDirectionVelocity.x) StaticTelemetry.addData("currentXVel", it.velocity.x) @@ -103,19 +112,26 @@ class DriveTrain : IRobotModule { } private fun driveSimpleDirection(direction: Vec2, rotate: Double) { - val leftFrontVoltage = (direction.x - direction.y - rotate) * Configs.DriveTrainConfig.BELT_RATIO - val rightBackVoltage = (direction.x - direction.y + rotate) * Configs.DriveTrainConfig.BELT_RATIO - val leftBackVoltage = (direction.x + direction.y - rotate) * Configs.DriveTrainConfig.BELT_RATIO - val rightForwardVoltage = (direction.x + direction.y + rotate) * Configs.DriveTrainConfig.BELT_RATIO + val leftFrontVoltage = + (direction.x - direction.y - rotate) * Configs.DriveTrainConfig.BELT_RATIO + val rightBackVoltage = + (direction.x - direction.y + rotate) * Configs.DriveTrainConfig.BELT_RATIO + val leftBackVoltage = + (direction.x + direction.y - rotate) * Configs.DriveTrainConfig.BELT_RATIO + val rightForwardVoltage = + (direction.x + direction.y + rotate) * Configs.DriveTrainConfig.BELT_RATIO var leftFrontPower = _battery.voltageToPower(leftFrontVoltage) var rightBackPower = _battery.voltageToPower(rightBackVoltage) var leftBackPower = _battery.voltageToPower(leftBackVoltage) var rightForwardPower = _battery.voltageToPower(rightForwardVoltage) - val max = max(abs(leftFrontPower), max(abs(rightBackPower), max(abs(leftBackPower), abs(rightForwardPower)))) + val max = max( + abs(leftFrontPower), + max(abs(rightBackPower), max(abs(leftBackPower), abs(rightForwardPower))) + ) - if(max > 1.0){ + if (max > 1.0) { leftFrontPower /= max rightBackPower /= max leftBackPower /= max @@ -136,6 +152,6 @@ class DriveTrain : IRobotModule { _targetRotateVelocity = 0.0 } - class SetDrivePowerEvent(val direction: Vec2, val rotate: Double): IEvent - class SetDriveCmEvent(val direction: Vec2, val rotate: Double): IEvent + class SetDrivePowerEvent(val direction: Vec2, val rotate: Double) : IEvent + class SetDriveCmEvent(val direction: Vec2, val rotate: Double) : IEvent } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/hook/Hook.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/hook/Hook.kt index 52d8289e..35e5460f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/hook/Hook.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/hook/Hook.kt @@ -2,51 +2,69 @@ package org.firstinspires.ftc.teamcode.modules.hook import com.qualcomm.robotcore.hardware.CRServo import com.qualcomm.robotcore.hardware.DcMotorSimple -import com.qualcomm.robotcore.hardware.Servo import com.qualcomm.robotcore.util.ElapsedTime 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.modules.intake.IntakeManager +import org.firstinspires.ftc.teamcode.modules.navigation.gyro.MergeGyro import org.firstinspires.ftc.teamcode.utils.configs.Configs -class Hook: IRobotModule { - class HookRun: IEvent - class HookStop: IEvent - class HookRunRevers: IEvent +class Hook : IRobotModule { + class HookRun : IEvent + class HookStop : IEvent + class HookRunRevers : IEvent private lateinit var _leftHook: CRServo private lateinit var _rightHook: CRServo private val _gameTimer = ElapsedTime() + private var _useSync = false + + private lateinit var _eventBus: EventBus + override fun init(collector: BaseCollector, bus: EventBus) { + _eventBus = bus + _leftHook = collector.devices.servoHookLeft _rightHook = collector.devices.servoHookRight _rightHook.direction = DcMotorSimple.Direction.REVERSE - bus.subscribe(HookRun::class){ - if(_gameTimer.seconds() > Configs.HookConfig.ACTIVATION_TIME_SEC && bus.invoke(IntakeManager.RequestLiftPosEvent()).pos == IntakeManager.LiftPosition.UP_LAYER) { - _rightHook.power = Configs.HookConfig.HOOK_POWER - _leftHook.power = Configs.HookConfig.HOOK_POWER + bus.subscribe(HookRun::class) { + if (_gameTimer.seconds() > Configs.HookConfig.ACTIVATION_TIME_SEC) { + _useSync = true + + writeHookPower(Configs.HookConfig.HOOK_POWER) } } - bus.subscribe(HookStop::class){ - _rightHook.power = 0.0 - _leftHook.power = 0.0 + bus.subscribe(HookStop::class) { + writeHookPower(0.0) } - bus.subscribe(HookRunRevers::class){ - if(_gameTimer.seconds() > Configs.HookConfig.ACTIVATION_TIME_SEC && bus.invoke(IntakeManager.RequestLiftPosEvent()).pos == IntakeManager.LiftPosition.UP_LAYER) { - _rightHook.power = -Configs.HookConfig.HOOK_POWER - _leftHook.power = -Configs.HookConfig.HOOK_POWER + bus.subscribe(HookRunRevers::class) { + if (_gameTimer.seconds() > Configs.HookConfig.ACTIVATION_TIME_SEC) { + _useSync = true + + writeHookPower(-Configs.HookConfig.HOOK_POWER) } } } + private fun writeHookPower(power: Double) { + if (_useSync) { + val yAngle = _eventBus.invoke(MergeGyro.RequestMergeGyroEvent()).yRot!! + + _leftHook.power = -yAngle.angle * Configs.HookConfig.SYNC_K + power + _rightHook.power = yAngle.angle * Configs.HookConfig.SYNC_K + power + } else { + _leftHook.power = power + _rightHook.power = power + } + } + override fun start() { _gameTimer.reset() } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/intake/Intake.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/intake/Intake.kt index 476381cb..bb962d54 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/intake/Intake.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/intake/Intake.kt @@ -2,16 +2,12 @@ package org.firstinspires.ftc.teamcode.modules.intake import com.acmerobotics.roadrunner.clamp import com.qualcomm.robotcore.hardware.PwmControl -import com.qualcomm.robotcore.hardware.Servo -import com.qualcomm.robotcore.hardware.ServoImplEx -import com.qualcomm.robotcore.util.ElapsedTime import org.firstinspires.ftc.teamcode.collectors.BaseCollector -import org.firstinspires.ftc.teamcode.collectors.events.EventBus import org.firstinspires.ftc.teamcode.utils.LEDLine.LEDLine import org.firstinspires.ftc.teamcode.utils.configs.Configs import org.firstinspires.ftc.teamcode.utils.softServo.SoftServo -class Intake{ +class Intake { private lateinit var _servoClamp: SoftServo private lateinit var _servoDifLeft: SoftServo @@ -20,13 +16,15 @@ class Intake{ private lateinit var _leftLED: LEDLine private lateinit var _rightLED: LEDLine - fun atTarget() = _servoClamp.isEnd && _servoDifLeft.isEnd && _servoDifRight.isEnd + fun difAtTarget() = _servoDifLeft.isEnd && _servoDifRight.isEnd + fun clampAtTarget() = _servoClamp.isEnd var xPos = 0.0 var yPos = 0.0 fun init(collector: BaseCollector) { - collector.devices.servoClamp.position = Configs.IntakeConfig.SERVO_CLAMP + if (collector.isAuto) + collector.devices.servoClamp.position = Configs.IntakeConfig.SERVO_CLAMP _servoClamp = SoftServo(collector.devices.servoClamp, Configs.IntakeConfig.SERVO_CLAMP) @@ -47,8 +45,7 @@ class Intake{ _leftLED.power = Configs.Lighting.ON_POWER _rightLED.power = Configs.Lighting.ON_POWER - } - else { + } else { _servoClamp.targetPosition = Configs.IntakeConfig.SERVO_UNCLAMP _leftLED.power = Configs.Lighting.OFF_POWER @@ -58,20 +55,19 @@ class Intake{ field = value } - fun setDifPos(xRot: Double, yRot: Double) - { + fun setDifPos(xRot: Double, yRot: Double) { xPos = xRot yPos = yRot val x = xRot + Configs.IntakeConfig.DIF_DIFFERENCE_X val y = (yRot + Configs.IntakeConfig.DIF_DIFFERENCE_Y) * Configs.IntakeConfig.GEAR_RATIO - _servoDifRight.targetPosition = clamp((y + x) / Configs.IntakeConfig.MAX, 0.0, 1.0) - _servoDifLeft.targetPosition = clamp(1.0 - (x - y) / Configs.IntakeConfig.MAX, 0.0, 1.0) + _servoDifRight.targetPosition = clamp((y + x) / Configs.IntakeConfig.SERVO_MAX, 0.0, 1.0) + _servoDifLeft.targetPosition = + clamp(1.0 - (x - y) / Configs.IntakeConfig.SERVO_MAX, 0.0, 1.0) } - enum class ClampPosition - { + enum class ClampPosition { SERVO_CLAMP, SERVO_UNCLAMP } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/intake/IntakeManager.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/intake/IntakeManager.kt index 5dd2ce96..176edb36 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/intake/IntakeManager.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/intake/IntakeManager.kt @@ -1,16 +1,29 @@ package org.firstinspires.ftc.teamcode.modules.intake import androidx.core.math.MathUtils.clamp +import com.qualcomm.robotcore.util.ElapsedTime 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.modules.camera.Camera import org.firstinspires.ftc.teamcode.modules.camera.Camera.RequestAllianceDetectedSticks +import org.firstinspires.ftc.teamcode.modules.driveTrain.DriveTrain +import org.firstinspires.ftc.teamcode.modules.navigation.gyro.MergeGyro import org.firstinspires.ftc.teamcode.utils.configs.Configs +import org.firstinspires.ftc.teamcode.utils.currentSensor.CurrentSensor +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 java.lang.Math.toRadians import kotlin.math.PI +import kotlin.math.abs +import kotlin.math.cos +import kotlin.math.sin import kotlin.math.sqrt +import kotlin.math.tan class IntakeManager : IRobotModule { class EventSetClampPose(val pos: Intake.ClampPosition) : IEvent @@ -22,7 +35,10 @@ class IntakeManager : IRobotModule { class PreviousDifPos : IEvent class EventSetExtensionPosition(val pos: Double) : IEvent class RequestLiftAtTargetEvent(var target: Boolean? = null) : IEvent - class RequestIntakeAtTarget(var target: Boolean? = null): IEvent + class RequestIntakeAtTarget(var target: Boolean? = null) : IEvent + class ClampDefendedEvent : IEvent + class AutoClamp : IEvent + class SetDifPosEvent(val pos: Double) : IEvent enum class LiftPosition { CLAMP_CENTER, @@ -30,53 +46,56 @@ class IntakeManager : IRobotModule { UP_LAYER, TRANSPORT, HUMAN_ADD, - CLAMP_WALL + CLAMP_WALL, + LOW_BASKET, + AUTO_CLAMP_CENTER } private lateinit var _eventBus: EventBus + private lateinit var _clampCurrentSensor: CurrentSensor + private val _intake = Intake() private val _lift = Lift() + private var _isAuto = false + private var _liftPosition = LiftPosition.TRANSPORT + private var _defendedIntegrations = 0 + override fun initUpdate() { - _lift.update() + if (_isAuto) + _lift.update() } override fun init(collector: BaseCollector, bus: EventBus) { + _isAuto = collector.isAuto + _eventBus = bus _lift.init(collector) _intake.init(collector) + _clampCurrentSensor = collector.devices.clampCurrentSensor + var isClampBusy = false - if(collector.isAuto) + if (collector.isAuto) _lift.aimTargetPosition = Configs.LiftConfig.INIT_POS bus.subscribe(EventSetClampPose::class) { - fun setPos() { - _intake.clamp = it.pos - if (_liftPosition != LiftPosition.TRANSPORT) { - Timers.newTimer().start(Configs.IntakeConfig.CLAMP_TIME) { - setDownState() + if (!isClampBusy) { + isClampBusy = true + + when (_liftPosition) { + LiftPosition.HUMAN_ADD -> { + _intake.clamp = it.pos isClampBusy = false } - } else { - setDownState() - isClampBusy = false - } - } - - if(!isClampBusy) { - isClampBusy = true - if (_liftPosition != LiftPosition.HUMAN_ADD) { - if (_liftPosition != LiftPosition.UP_LAYER && _liftPosition != LiftPosition.CLAMP_WALL) { - setPos() - } else if (_liftPosition == LiftPosition.UP_LAYER) { + LiftPosition.UP_LAYER -> { _lift.aimTargetPosition = Configs.LiftConfig.UP_LAYER_UNCLAMP_AIM _lift.extensionTargetPosition = Configs.LiftConfig.UP_LAYER_UNCLAMP_EXTENSION @@ -86,39 +105,128 @@ class IntakeManager : IRobotModule { ) Timers.newTimer().start(Configs.IntakeConfig.UP_LAYER_DOWN_TIME) { - setPos() + _intake.clamp = Intake.ClampPosition.SERVO_UNCLAMP + setDownState() + + isClampBusy = false + } + } + + LiftPosition.CLAMP_WALL -> { + _intake.clamp = Intake.ClampPosition.SERVO_CLAMP + + Timers.newTimer().start({ !_intake.clampAtTarget() }) { + Timers.newTimer().start(Configs.IntakeConfig.CURRENT_SENSOR_DELAY) { + if (_clampCurrentSensor.current > Configs.IntakeConfig.CLAMP_CURRENT || + !Configs.IntakeConfig.USE_CURRENT_SENSOR || collector.isAuto || + _defendedIntegrations >= Configs.IntakeConfig.MAX_DEFENDED_INTEGRATIOS + ) { + + _lift.aimTargetPosition = + Configs.LiftConfig.CLAMP_WALL_CLAMPED_AIM_POS + _lift.extensionTargetPosition = + Configs.LiftConfig.CLAMP_WALL_CLAMPED_EXTENSION_POS + + _intake.setDifPos( + Configs.IntakeConfig.CLAMP_WALL_CLAMPED_DIF_POS_X, + Configs.IntakeConfig.CLAMP_WALL_CLAMPED_DIF_POS_Y + ) + + Timers.newTimer() + .start(Configs.IntakeConfig.CLAMP_WALL_UP_TIME) { + setDownState() + isClampBusy = false + } + } else { + bus.invoke(ClampDefendedEvent()) + + _intake.clamp = Intake.ClampPosition.SERVO_UNCLAMP + + isClampBusy = false + _defendedIntegrations++ + } + } } - } else { + } + + LiftPosition.TRANSPORT -> { _intake.clamp = it.pos - Timers.newTimer().start({ !_intake.atTarget() }) { - _lift.aimTargetPosition = Configs.LiftConfig.CLAMP_WALL_CLAMPED_AIM_POS - _lift.extensionTargetPosition = - Configs.LiftConfig.CLAMP_WALL_CLAMPED_EXTENSION_POS - _intake.setDifPos( - Configs.IntakeConfig.CLAMP_WALL_CLAMPED_DIF_POS_X, - Configs.IntakeConfig.CLAMP_WALL_CLAMPED_DIF_POS_Y - ) + setDownState() + isClampBusy = false + } - Timers.newTimer().start(Configs.IntakeConfig.CLAMP_WALL_UP_TIME) { - setPos() + LiftPosition.AUTO_CLAMP_CENTER -> { + _liftPosition = LiftPosition.TRANSPORT + _lift.deltaExtension = 0.0 + + Timers.newTimer().start({ !_lift.atTarget() }) { + Timers.newTimer() + .start(Configs.AutoClamp.AUTO_CLAMP_CENTER_CLAMP_TIMER) { + _intake.clamp = Intake.ClampPosition.SERVO_CLAMP + Timers.newTimer().start({ !_intake.clampAtTarget() }) { + isClampBusy = false + setDownState() + } + } + } + } + + LiftPosition.CLAMP_CENTER -> { + _intake.clamp = Intake.ClampPosition.SERVO_CLAMP + + Timers.newTimer().start({ !_intake.clampAtTarget() }) { + Timers.newTimer().start(Configs.IntakeConfig.CURRENT_SENSOR_DELAY) { + if ((_clampCurrentSensor.current > Configs.IntakeConfig.CLAMP_CURRENT + && _clampCurrentSensor.current < Configs.IntakeConfig.CLAMP_CURRENT_TWO) || + !Configs.IntakeConfig.USE_CURRENT_SENSOR || collector.isAuto || + _defendedIntegrations >= Configs.IntakeConfig.MAX_DEFENDED_INTEGRATIOS + ) + setDownState() + else { + _intake.clamp = Intake.ClampPosition.SERVO_UNCLAMP + + _defendedIntegrations++ + + bus.invoke(ClampDefendedEvent()) + } + + isClampBusy = false } } } - } else { - _intake.clamp = it.pos - isClampBusy = false + LiftPosition.UP_BASKED, LiftPosition.LOW_BASKET -> { + _intake.clamp = Intake.ClampPosition.SERVO_UNCLAMP + + Timers.newTimer().start({ !_intake.clampAtTarget() }) { + Timers.newTimer().start(Configs.LiftConfig.BASKET_DELAY) { + setDownState() + isClampBusy = false + } + } + } } } } + bus.subscribe(SetDifPosEvent::class) { + if (_liftPosition == LiftPosition.CLAMP_CENTER) + _intake.setDifPos( + _intake.xPos, + clamp( + it.pos, -Configs.IntakeConfig.MAX_DIF_POS_Y, + Configs.IntakeConfig.MAX_DIF_POS_Y + ) + ) + } + bus.subscribe(RequestClampPosEvent::class) { it.pos = _intake.clamp } bus.subscribe(EventSetExtensionVel::class) { - if (_liftPosition == LiftPosition.CLAMP_CENTER) { + if (_liftPosition == LiftPosition.CLAMP_CENTER || _liftPosition == LiftPosition.AUTO_CLAMP_CENTER) { _lift.extensionVelocity = it.vel } else { _lift.extensionVelocity = 0.0 @@ -136,61 +244,92 @@ class IntakeManager : IRobotModule { } bus.subscribe(NextDifPos::class) { - if (_liftPosition == LiftPosition.CLAMP_CENTER && !Configs.IntakeConfig.USE_CAMERA) + if (_liftPosition == LiftPosition.CLAMP_CENTER) _intake.setDifPos( _intake.xPos, - clamp(_intake.yPos + Configs.IntakeConfig.GAMEPADE_DIF_STEP, + clamp( + _intake.yPos + Configs.IntakeConfig.GAMEPADE_DIF_STEP, -Configs.IntakeConfig.MAX_DIF_POS_Y, Configs.IntakeConfig.MAX_DIF_POS_Y - )) + ) + ) } bus.subscribe(PreviousDifPos::class) { - if (_liftPosition == LiftPosition.CLAMP_CENTER && !Configs.IntakeConfig.USE_CAMERA) + if (_liftPosition == LiftPosition.CLAMP_CENTER) _intake.setDifPos( _intake.xPos, - clamp(_intake.yPos - Configs.IntakeConfig.GAMEPADE_DIF_STEP, + clamp( + _intake.yPos - Configs.IntakeConfig.GAMEPADE_DIF_STEP, -Configs.IntakeConfig.MAX_DIF_POS_Y, Configs.IntakeConfig.MAX_DIF_POS_Y - )) + ) + ) } bus.subscribe(EventSetLiftPose::class) { - if(_lift.atTarget() || collector.isAuto) { - if (it.pos == LiftPosition.UP_BASKED && _intake.clamp == Intake.ClampPosition.SERVO_CLAMP && _liftPosition == LiftPosition.TRANSPORT) { + if ((_lift.atTarget() || collector.isAuto) && !isClampBusy) { + if (it.pos == LiftPosition.UP_BASKED && _intake.clamp == Intake.ClampPosition.SERVO_CLAMP && (_liftPosition == LiftPosition.TRANSPORT || _liftPosition == LiftPosition.LOW_BASKET)) { _lift.aimTargetPosition = Configs.LiftConfig.UP_BASKED_AIM _lift.extensionTargetPosition = Configs.LiftConfig.UP_BASKED_EXTENSION - _intake.setDifPos(xRot = Configs.IntakeConfig.UP_BASKET_DIF_POS_X, yRot = Configs.IntakeConfig.UP_BASKET_DIF_POS_Y) + _intake.setDifPos( + xRot = Configs.IntakeConfig.UP_BASKET_DIF_POS_X, + yRot = Configs.IntakeConfig.UP_BASKET_DIF_POS_Y + ) + _liftPosition = it.pos + _lift.deltaExtension = 0.0 + _defendedIntegrations = 0 + } else if (it.pos == LiftPosition.LOW_BASKET && _intake.clamp == Intake.ClampPosition.SERVO_CLAMP && (_liftPosition == LiftPosition.TRANSPORT || _liftPosition == LiftPosition.UP_BASKED)) { + _lift.aimTargetPosition = Configs.LiftConfig.LOW_BASKED_AIM + _lift.extensionTargetPosition = Configs.LiftConfig.LOW_BASKED_EXTENSION + _intake.setDifPos( + xRot = Configs.IntakeConfig.UP_BASKET_DIF_POS_X, + yRot = Configs.IntakeConfig.UP_BASKET_DIF_POS_Y + ) _liftPosition = it.pos _lift.deltaExtension = 0.0 + _defendedIntegrations = 0 } else if (it.pos == LiftPosition.UP_LAYER && _intake.clamp == Intake.ClampPosition.SERVO_CLAMP && _liftPosition == LiftPosition.TRANSPORT) { _lift.aimTargetPosition = Configs.LiftConfig.UP_LAYER_AIM _lift.extensionTargetPosition = Configs.LiftConfig.UP_LAYER_EXTENSION - _intake.setDifPos(xRot = Configs.IntakeConfig.UP_LAYER_DIF_POS_X, yRot = Configs.IntakeConfig.UP_LAYER_DIF_POS_Y) + _intake.setDifPos( + xRot = Configs.IntakeConfig.UP_LAYER_DIF_POS_X, + yRot = Configs.IntakeConfig.UP_LAYER_DIF_POS_Y + ) _liftPosition = it.pos _lift.deltaExtension = 0.0 + _defendedIntegrations = 0 } else if (it.pos == LiftPosition.CLAMP_CENTER && _intake.clamp == Intake.ClampPosition.SERVO_UNCLAMP && _liftPosition == LiftPosition.TRANSPORT) { _lift.aimTargetPosition = Configs.LiftConfig.CLAMP_CENTER_AIM _lift.extensionTargetPosition = Configs.LiftConfig.CLAMP_CENTER_EXTENSION - _intake.setDifPos(xRot = Configs.IntakeConfig.CLAMP_CENTER_DIF_POS_X, yRot = Configs.IntakeConfig.CLAMP_CENTER_DIF_POS_Y) + _intake.setDifPos( + xRot = Configs.IntakeConfig.CLAMP_CENTER_DIF_POS_X, + yRot = Configs.IntakeConfig.CLAMP_CENTER_DIF_POS_Y + ) _liftPosition = it.pos _lift.deltaExtension = 0.0 - } - else if(it.pos == LiftPosition.CLAMP_WALL && _liftPosition == LiftPosition.TRANSPORT && _intake.clamp == Intake.ClampPosition.SERVO_UNCLAMP){ + _defendedIntegrations = 0 + } else if (it.pos == LiftPosition.CLAMP_WALL && _liftPosition == LiftPosition.TRANSPORT && _intake.clamp == Intake.ClampPosition.SERVO_UNCLAMP) { _lift.aimTargetPosition = Configs.LiftConfig.CLAMP_WALL_AIM_POS _lift.extensionTargetPosition = Configs.LiftConfig.CLAMP_WALL_EXTENSION_POS - _intake.setDifPos(xRot = Configs.IntakeConfig.CLAMP_WALL_DIF_POS_X, yRot = Configs.IntakeConfig.CLAMP_WALL_DIF_POS_Y) + _intake.setDifPos( + xRot = Configs.IntakeConfig.CLAMP_WALL_DIF_POS_X, + yRot = Configs.IntakeConfig.CLAMP_WALL_DIF_POS_Y + ) _liftPosition = it.pos _lift.deltaExtension = 0.0 - } - else if(it.pos == LiftPosition.HUMAN_ADD && _liftPosition == LiftPosition.TRANSPORT){ + _defendedIntegrations = 0 + } else if (it.pos == LiftPosition.HUMAN_ADD && _liftPosition == LiftPosition.TRANSPORT) { _lift.aimTargetPosition = Configs.LiftConfig.HUMAN_ADD_AIM_POS _lift.extensionTargetPosition = Configs.LiftConfig.HUMAN_ADD_EXTENSION_POS - _intake.setDifPos(xRot = Configs.IntakeConfig.HUMAN_ADD_DIF_POS_X, yRot = Configs.IntakeConfig.HUMAN_ADD_DIF_POS_Y) + _intake.setDifPos( + xRot = Configs.IntakeConfig.HUMAN_ADD_DIF_POS_X, + yRot = Configs.IntakeConfig.HUMAN_ADD_DIF_POS_Y + ) _liftPosition = it.pos _lift.deltaExtension = 0.0 - } - else if (it.pos == LiftPosition.TRANSPORT) + _defendedIntegrations = 0 + } else if (it.pos == LiftPosition.TRANSPORT) setDownState() } } @@ -199,60 +338,181 @@ class IntakeManager : IRobotModule { it.target = _lift.atTarget() && !isClampBusy } - bus.subscribe(RequestIntakeAtTarget::class){ - it.target = _intake.atTarget() && !isClampBusy + bus.subscribe(RequestIntakeAtTarget::class) { + it.target = _intake.clampAtTarget() && _intake.difAtTarget() && !isClampBusy } - } - override fun update() { - _lift.update() - - if (Configs.IntakeConfig.USE_CAMERA) { - if (_liftPosition == LiftPosition.CLAMP_CENTER) { - _eventBus.invoke(Camera.SetStickDetectEnable(true)) + bus.subscribe(AutoClamp::class) { + if (!isClampBusy) { + isClampBusy = true + _liftPosition = LiftPosition.AUTO_CLAMP_CENTER + _lift.aimTargetPosition = 45.0 + _lift.extensionTargetPosition = 0.0 + _lift.deltaExtension = 0.0 + _intake.setDifPos( + xRot = Configs.IntakeConfig.CLAMP_CENTER_DIF_POS_X, + yRot = Configs.IntakeConfig.CLAMP_CENTER_DIF_POS_Y + ) + + _liftPosition = LiftPosition.AUTO_CLAMP_CENTER + _intake.clamp = Intake.ClampPosition.SERVO_UNCLAMP + + _isCameraDetected = false + + Timers.newTimer().start({ !_lift.atTarget() }) { + Timers.newTimer().start(Configs.AutoClamp.CAMERA_ENABLE_TIMER) { + bus.invoke(Camera.WaitFrameProcessed()) + + val yellowSticks = bus.invoke(Camera.RequestYellowDetectedSticks()).sticks!! + val allianceSticks = bus.invoke(RequestAllianceDetectedSticks()).sticks!! + + val sticks = + (yellowSticks + allianceSticks).filter { Configs.AutoClamp.FRAME_SIZE.y - it.y > Configs.AutoClamp.MIN_ELEMENT_Y } + .toTypedArray() + + if (sticks.isNotEmpty()) { + for (i in _closesSticks.indices) + _closesSticks[i] = Pair(Orientation.ZERO, Double.MAX_VALUE) + + for (i in sticks) { + val yAngle = + 90.0 - (180.0 - 90.0 - _lift.currentAimPos) + (Configs.AutoClamp.FRAME_SIZE.y / 2.0 - i.y) * Configs.AutoClamp.PIXEL_TO_ANGLE + val xAngle = + (Configs.AutoClamp.FRAME_SIZE.x / 2.0 - i.x) * Configs.AutoClamp.PIXEL_TO_ANGLE + + val cameraH = + sin(toRadians(_lift.currentAimPos)) * Configs.AutoClamp.EXTENSION_LENGHT + Configs.AutoClamp.LIFT_H + val cameraX = + cos(toRadians(_lift.currentAimPos)) * Configs.AutoClamp.EXTENSION_LENGHT + + val xPos = + cameraH * tan(toRadians(yAngle)) + cameraX - Configs.AutoClamp.LIFT_CENTER_POS + val yPos = cameraH * tan(toRadians(xAngle)) + + val l = sqrt(xPos * xPos + yPos * yPos) + + for (j in _closesSticks.indices) { + if (_closesSticks[j].second > l) { + _closesSticks[j] = + Pair(Orientation(Vec2(xPos, yPos), i.angl), l) + break + } + } + } - val allianceSticks = _eventBus.invoke(RequestAllianceDetectedSticks()).sticks!! - val yellowSticks = _eventBus.invoke(Camera.RequestYellowDetectedSticks()).sticks!! + _clampStartRot = + _eventBus.invoke(MergeGyro.RequestMergeGyroEvent()).rotation!! - if (allianceSticks.isEmpty() && yellowSticks.isEmpty()) - return + _intake.setDifPos( + _intake.xPos, clamp( + _closesSticks[0].first.angl.toDegree(), + -Configs.IntakeConfig.MAX_DIF_POS_Y, + Configs.IntakeConfig.MAX_DIF_POS_Y + ) + ) - var closesdStick = allianceSticks[0] - var closesdStickL = Double.MAX_VALUE + _isCameraDetected = true + } else + setDownState() - for (i in allianceSticks) { - val catetX = i.x - Configs.IntakeConfig.CAMERA_CLAMP_POS_X - val catetY = i.y - Configs.IntakeConfig.CAMERA_CLAMP_POS_Y - val l = sqrt(catetX * catetX + catetY * catetY) - if (l < closesdStickL) { - closesdStick = i - closesdStickL = l + isClampBusy = false } } + } + } + } - _intake.setDifPos(90.0, closesdStick.angl.toDegree()) + val _closesSticks = Array(Configs.AutoClamp.NON_FOTO_INTEGRATIONS) { + Pair( + Orientation.ZERO, Double.MAX_VALUE + ) + } + private var _isCameraDetected = false + private var _clampStartRot = Angle.ZERO + private var _targetTime = ElapsedTime() + override fun update() { + _lift.update() + + StaticTelemetry.addData( + "liftTargetExtensionPos", + _lift.extensionTargetPosition + _lift.deltaExtension + ) + StaticTelemetry.addData("clamp current", _clampCurrentSensor.current) + + if (Configs.AutoClamp.ENABLE_AUTO_CLAMP && _liftPosition == LiftPosition.AUTO_CLAMP_CENTER && _isCameraDetected) { + val targetAngle = + Angle(kotlin.math.atan2(_closesSticks[0].first.y, _closesSticks[0].first.x)) + val err = + (targetAngle - (_eventBus.invoke(MergeGyro.RequestMergeGyroEvent()).rotation!! - _clampStartRot)).angle + + _eventBus.invoke( + DriveTrain.SetDriveCmEvent( + Vec2.ZERO, + err * Configs.AutoClamp.DRIVE_ROTATE_P + ) + ) + + _lift.aimTargetPosition = Configs.LiftConfig.CLAMP_CENTER_AIM + _lift.extensionTargetPosition = + clamp( + (_closesSticks[0].second + Configs.AutoClamp.LIFT_CENTER_POS - Configs.AutoClamp.EXTENSION_LENGHT) / (Configs.AutoClamp.LIFT_CIRCLE_R * PI * 2.0) * Configs.AutoClamp.MOTOR_TICKS, + 0.0, + Configs.LiftConfig.MAX_EXTENSION_POS + ) + + if (_lift.atTarget() && _intake.difAtTarget() && abs(err) < Configs.AutoClamp.ROTATE_SENS) { + if (_targetTime.seconds() > Configs.AutoClamp.TARGET_DELAY) { + _intake.clamp = Intake.ClampPosition.SERVO_CLAMP + + if (_intake.clampAtTarget()) { + Timers.newTimer().start(Configs.IntakeConfig.CURRENT_SENSOR_DELAY) { + if ((_clampCurrentSensor.current > Configs.IntakeConfig.CLAMP_CURRENT + && _clampCurrentSensor.current < Configs.IntakeConfig.CLAMP_CURRENT_TWO) || + !Configs.IntakeConfig.USE_CURRENT_SENSOR || _defendedIntegrations >= Configs.IntakeConfig.MAX_DEFENDED_INTEGRATIOS + ) + setDownState() + else { + _defendedIntegrations++ + _eventBus.invoke(AutoClamp()) + } + } + + _isCameraDetected = false + } + } } else - _eventBus.invoke(Camera.SetStickDetectEnable(false)) + _targetTime.reset() } } - fun setDownState(){ + fun setDownState() { _lift.aimTargetPosition = Configs.LiftConfig.TRANSPORT_AIM _lift.extensionTargetPosition = Configs.LiftConfig.TRANSPORT_EXTENSION if (_liftPosition == LiftPosition.UP_BASKED) { - _intake.setDifPos(xRot = Configs.IntakeConfig.UP_BASKET_DOWN_MOVE_DIF_POS_X, yRot = Configs.IntakeConfig.UP_BASKET_DOWN_MOVE_DIF_POS_Y) + _intake.setDifPos( + xRot = Configs.IntakeConfig.UP_BASKET_DOWN_MOVE_DIF_POS_X, + yRot = Configs.IntakeConfig.UP_BASKET_DOWN_MOVE_DIF_POS_Y + ) Timers.newTimer().start(Configs.IntakeConfig.UP_BASKET_DOWN_TIME) { - _intake.setDifPos(xRot = Configs.IntakeConfig.TRANSPORT_DIF_POS_X, yRot = Configs.IntakeConfig.TRANSPORT_DIF_POS_Y) + _intake.setDifPos( + xRot = Configs.IntakeConfig.TRANSPORT_DIF_POS_X, + yRot = Configs.IntakeConfig.TRANSPORT_DIF_POS_Y + ) } } else - _intake.setDifPos(xRot = Configs.IntakeConfig.TRANSPORT_DIF_POS_X, yRot = Configs.IntakeConfig.TRANSPORT_DIF_POS_Y) + _intake.setDifPos( + xRot = Configs.IntakeConfig.TRANSPORT_DIF_POS_X, + yRot = Configs.IntakeConfig.TRANSPORT_DIF_POS_Y + ) _liftPosition = LiftPosition.TRANSPORT _lift.deltaExtension = 0.0 + + _defendedIntegrations = 0 } override fun start() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/intake/Lift.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/intake/Lift.kt index d556edfe..57766846 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/intake/Lift.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/intake/Lift.kt @@ -6,7 +6,6 @@ import com.qualcomm.robotcore.hardware.DcMotor import com.qualcomm.robotcore.hardware.DcMotor.ZeroPowerBehavior.BRAKE import com.qualcomm.robotcore.hardware.DcMotorEx import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction.REVERSE -import com.qualcomm.robotcore.hardware.DigitalChannel import com.qualcomm.robotcore.util.ElapsedTime import org.firstinspires.ftc.teamcode.collectors.BaseCollector import org.firstinspires.ftc.teamcode.utils.configs.Configs @@ -34,6 +33,8 @@ class Lift { var aimTargetPosition = 0.0 var extensionTargetPosition = 0.0 + private var _isResetExtension = false + fun init(collector: BaseCollector) { _battery = collector.devices.battery @@ -45,10 +46,7 @@ class Lift { _aimMotor.zeroPowerBehavior = BRAKE _extensionMotor.zeroPowerBehavior = BRAKE - if (collector.isAuto) { - _extensionMotor.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER - _extensionMotor.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER - } + _isResetExtension = collector.isAuto } fun getCurrentExtensionPos() = _extensionMotor.currentPosition.toDouble() @@ -58,12 +56,17 @@ class Lift { var deltaExtension = 0.0 private var _oldTargetAimPos = 0.0 - fun getAimPos() = _aimPotentiometer.voltage / + var currentAimPos = 0.0 + private set + + fun readRawAimPos() = _aimPotentiometer.voltage / Configs.LiftConfig.MAX_POTENTIOMETER_VOLTAGE * Configs.LiftConfig.MAX_POTENTIOMETER_ANGLE + Configs.LiftConfig.AIM_POTENTIOMETER_DIFFERENCE fun update() { - StaticTelemetry.addData("aim pos", getAimPos()) + currentAimPos = readRawAimPos() + + StaticTelemetry.addData("aimPos", currentAimPos) deltaExtension += _deltaTime.seconds() * extensionVelocity @@ -80,7 +83,10 @@ class Lift { val targetDefencedAimPos: Double - if (abs(Configs.LiftConfig.MIN_EXTENSION_POS - getCurrentExtensionPos()) < Configs.LiftConfig.EXTENSION_SENS) { + if (abs(Configs.LiftConfig.MIN_EXTENSION_POS - getCurrentExtensionPos()) < Configs.LiftConfig.DEFENDED_EXTENSION_SENS || abs( + currentAimPos - targetAimPos + ) < Configs.LiftConfig.AIM_DEFEND_TRIGGER_POS + ) { targetDefencedAimPos = targetAimPos _oldTargetAimPos = targetAimPos } else @@ -88,22 +94,26 @@ class Lift { val targetDefencedExtensionPos: Double - if (abs(targetAimPos - getAimPos()) > Configs.LiftConfig.AIM_SENS) + if (abs(targetAimPos - currentAimPos) > Configs.LiftConfig.DEFENDED_AIM_SENS) targetDefencedExtensionPos = Configs.LiftConfig.MIN_EXTENSION_POS else targetDefencedExtensionPos = targetExtensionPos - _aimErr = targetDefencedAimPos - getAimPos() + StaticTelemetry.addData("targetAimLiftPos", targetDefencedAimPos) + + _aimErr = targetDefencedAimPos - currentAimPos _extensionErr = (targetDefencedExtensionPos + deltaExtension) - getCurrentExtensionPos() val triggerMinPower = - if (getAimPos() > Configs.LiftConfig.TRIGET_SLOW_POS) + if (currentAimPos > Configs.LiftConfig.TRIGET_SLOW_POS) Configs.LiftConfig.MAX_SPEED_DOWN else Configs.LiftConfig.MAX_TRIGGER_SPEED_DOWN val aimPower = - _battery.voltageToPower(_aimPID.update(_aimErr) - .coerceAtLeast(triggerMinPower).coerceAtMost(Configs.LiftConfig.MIN_SPEED_UP)) + _battery.voltageToPower( + _aimPID.update(_aimErr) + .coerceAtLeast(triggerMinPower).coerceAtMost(Configs.LiftConfig.MIN_SPEED_UP) + ) val extensionPower = _battery.voltageToPower(_extensionPID.update(_extensionErr)) @@ -112,9 +122,15 @@ class Lift { } fun atTarget() = - abs(_aimErr) < Configs.LiftConfig.AIM_SENS && abs(_extensionErr) < Configs.LiftConfig.EXTENSION_SENS + abs(readRawAimPos() - aimTargetPosition) < Configs.LiftConfig.AIM_SENS && + abs(getCurrentExtensionPos() - (extensionTargetPosition + deltaExtension)) < Configs.LiftConfig.EXTENSION_SENS fun start() { _deltaTime.reset() + + if (_isResetExtension) { + _extensionMotor.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER + _extensionMotor.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER + } } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/Actions.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/Actions.kt index b4cb1f34..a620cbb1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/Actions.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/Actions.kt @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.modules.mainControl.actions import com.acmerobotics.roadrunner.Trajectory import com.qualcomm.robotcore.util.ElapsedTime import org.firstinspires.ftc.teamcode.collectors.events.EventBus -import org.firstinspires.ftc.teamcode.collectors.events.IEvent import org.firstinspires.ftc.teamcode.modules.intake.Intake import org.firstinspires.ftc.teamcode.modules.intake.IntakeManager import org.firstinspires.ftc.teamcode.modules.mainControl.runner.RRTrajectorySegment @@ -13,13 +12,13 @@ import org.firstinspires.ftc.teamcode.utils.units.Angle import org.firstinspires.ftc.teamcode.utils.units.Orientation interface IAction { - fun update() + fun update() {} - fun end() + fun end() {} - fun isEnd(): Boolean + fun isEnd(): Boolean = true - fun start() + fun start() {} } interface ITransportAction : IAction { @@ -41,10 +40,6 @@ class FollowRRTrajectory(private val _eventBus: EventBus, trajectory: List _secTime override fun start() { @@ -95,17 +78,11 @@ class WaitAction(private val _secTime: Double) : IAction { class LiftAction( private val _eventBus: EventBus, val pos: IntakeManager.LiftPosition, - val extensionPos: Double = 0.0 + val extensionPos: Double = 0.0, + val waitEnd: Boolean = true ) : IAction { - override fun update() { - - } - - override fun end() { - - } - - override fun isEnd() = _eventBus.invoke(IntakeManager.RequestLiftAtTargetEvent()).target!! + override fun isEnd() = (_eventBus.invoke(IntakeManager.RequestLiftAtTargetEvent()).target!! && + _eventBus.invoke(IntakeManager.RequestIntakeAtTarget()).target!!) || !waitEnd override fun start() { _eventBus.invoke(IntakeManager.EventSetLiftPose(pos)) @@ -113,30 +90,21 @@ class LiftAction( } } -class ClampAction(private val _eventBus: EventBus, val pos: Intake.ClampPosition) : IAction { - override fun update() { - - } - - override fun end() { - - } - - override fun isEnd() = _eventBus.invoke(IntakeManager.RequestIntakeAtTarget()).target!! +class ClampAction( + private val _eventBus: EventBus, val pos: Intake.ClampPosition, + val waitEnd: Boolean = true +) : IAction { + override fun isEnd() = (_eventBus.invoke(IntakeManager.RequestIntakeAtTarget()).target!! && + _eventBus.invoke(IntakeManager.RequestLiftAtTargetEvent()).target!!) || !waitEnd override fun start() { _eventBus.invoke(IntakeManager.EventSetClampPose(pos)) } } -class WaitLiftAction(private val _eventBus: EventBus) : IAction { - override fun update() {} - - override fun end() {} - - override fun isEnd() = _eventBus.invoke(IntakeManager.RequestLiftAtTargetEvent()).target!! - - override fun start() {} +class WaitIntakeAction(private val _eventBus: EventBus) : IAction { + override fun isEnd() = _eventBus.invoke(IntakeManager.RequestLiftAtTargetEvent()).target!! && + _eventBus.invoke(IntakeManager.RequestIntakeAtTarget()).target!! } class ParallelActions( @@ -175,10 +143,6 @@ class ParallelActions( } } - override fun end() { - - } - override fun isEnd(): Boolean { for (i in _actions) if (_exitType == ExitType.OR && i.isEmpty()) @@ -196,26 +160,20 @@ class ParallelActions( } } -class DifAction(val eventBus: EventBus, val dir: DifDirection) : IAction { - enum class DifDirection { - NEXT, - PREVIOUS - } - - override fun update() { +class DifAction(val eventBus: EventBus, val pos: Double) : IAction { + override fun isEnd() = eventBus.invoke(IntakeManager.RequestIntakeAtTarget()).target!! + override fun start() { + eventBus.invoke(IntakeManager.SetDifPosEvent(pos)) } - override fun end() { - } +} - override fun isEnd() = eventBus.invoke(IntakeManager.RequestIntakeAtTarget()).target!! +class AutoClampAction(val eventBus: EventBus) : IAction { + override fun isEnd() = + eventBus.invoke(IntakeManager.RequestLiftPosEvent()).pos!! != IntakeManager.LiftPosition.AUTO_CLAMP_CENTER override fun start() { - if (dir == DifDirection.NEXT) - eventBus.invoke(IntakeManager.NextDifPos()) - else - eventBus.invoke(IntakeManager.PreviousDifPos()) + eventBus.invoke(IntakeManager.AutoClamp()) } - } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/ActionsRunner.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/ActionsRunner.kt index a8cf24aa..67d10790 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/ActionsRunner.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/ActionsRunner.kt @@ -4,20 +4,12 @@ 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.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.mainControl.actions.trajectoryes.BlueBaskedTrajectory -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.trajectoryes.BlueHumanTrajectory -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.trajectoryes.RedBaskedTrajectory -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.trajectoryes.RedHumanTrajectory -import org.firstinspires.ftc.teamcode.utils.timer.Timers +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.trajectories.NewBasketTrajectory import org.firstinspires.ftc.teamcode.utils.units.Orientation -import org.firstinspires.ftc.teamcode.utils.units.Vec2 -class ActionsRunner: IRobotModule { - class RunActionsEvent(val actions: List): IEvent - class RequestIsEndActionsRun(var isEnd: Boolean): IEvent +class ActionsRunner : IRobotModule { + class RunActionsEvent(val actions: List) : IEvent + class RequestIsEndActionsRun(var isEnd: Boolean) : IEvent private val _actions = ArrayList() @@ -26,40 +18,45 @@ class ActionsRunner: IRobotModule { override fun init(collector: BaseCollector, bus: EventBus) { _eventBus = bus - bus.subscribe(RunActionsEvent::class){ + bus.subscribe(RunActionsEvent::class) { _actions.addAll(it.actions) } - bus.subscribe(RequestIsEndActionsRun::class){ + bus.subscribe(RequestIsEndActionsRun::class) { it.isEnd = _actions.isEmpty() } - when(collector.parameters.oldStartPosition){ - BaseCollector.GameStartPosition.RED_HUMAN -> RedHumanTrajectory() - BaseCollector.GameStartPosition.RED_BASKET -> RedBaskedTrajectory() - BaseCollector.GameStartPosition.BLUE_HUMAN -> BlueHumanTrajectory() - BaseCollector.GameStartPosition.BLUE_BASKET -> BlueBaskedTrajectory() + when (collector.parameters.oldStartPosition) { + BaseCollector.GameStartPosition.RED_HUMAN, BaseCollector.GameStartPosition.RED_HUMAN_BRICK, BaseCollector.GameStartPosition.BLUE_HUMAN, BaseCollector.GameStartPosition.BLUE_HUMAN_BRICK -> NewBasketTrajectory() + BaseCollector.GameStartPosition.RED_BASKET, BaseCollector.GameStartPosition.RED_BASKET_BRICK, BaseCollector.GameStartPosition.BLUE_BASKET, BaseCollector.GameStartPosition.BLUE_BASKET_BRICK -> NewBasketTrajectory() BaseCollector.GameStartPosition.NONE -> throw Exception("none is not start auto pos") - }.runTrajectory(bus, Orientation(collector.parameters.oldStartPosition.position, collector.parameters.oldStartPosition.angle)) + }.runTrajectory( + bus, + Orientation( + collector.parameters.oldStartPosition.position, + collector.parameters.oldStartPosition.angle + ), + collector.parameters.oldStartPosition.teammate + ) } override fun update() { - if(_actions.isEmpty()) + if (_actions.isEmpty()) return _actions[0].update() - if(_actions[0].isEnd()) { + if (_actions[0].isEnd()) { _actions[0].end() _actions.removeAt(0) - if(!_actions.isEmpty()) + if (!_actions.isEmpty()) _actions[0].start() } } override fun start() { - if(!_actions.isEmpty()) + if (!_actions.isEmpty()) _actions[0].start() } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectories/BasketTrajectory.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectories/BasketTrajectory.kt new file mode 100644 index 00000000..ba455bd0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectories/BasketTrajectory.kt @@ -0,0 +1,269 @@ +package org.firstinspires.ftc.teamcode.modules.mainControl.actions.trajectories + +import com.acmerobotics.roadrunner.Pose2d +import com.acmerobotics.roadrunner.Vector2d +import org.firstinspires.ftc.teamcode.collectors.BaseCollector +import org.firstinspires.ftc.teamcode.collectors.events.EventBus +import org.firstinspires.ftc.teamcode.modules.intake.Intake +import org.firstinspires.ftc.teamcode.modules.intake.IntakeManager +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ActionsRunner +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.AutoClampAction +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ClampAction +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.DifAction +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.FollowRRTrajectory +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.IAction +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ITransportAction.Companion.getEndOrientation +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.LiftAction +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ParallelActions +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.WaitAction +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.WaitIntakeAction +import org.firstinspires.ftc.teamcode.modules.mainControl.runner.TrajectorySegmentRunner.Companion.newRRTrajectory +import org.firstinspires.ftc.teamcode.utils.units.Orientation +import java.lang.Math.toRadians + +class BasketTrajectory : ITrajectoryBuilder { + override fun runTrajectory( + eventBus: EventBus, + startOrientation: Orientation, + teammate: BaseCollector.TeammateSate + ) { + val actions = arrayListOf() + + fun runToBasket(startOrientation: Orientation): ArrayList { + val acts = arrayListOf() + + acts.add( + ParallelActions( + arrayOf( + arrayListOf( + FollowRRTrajectory( + eventBus, newRRTrajectory(startOrientation) + .strafeToLinearHeading( + Vector2d(131.1, 133.4), + toRadians(-90.0 - 45.0) + ) + .build() + ) + ), + arrayListOf( + WaitIntakeAction(eventBus), + WaitAction(0.2), + LiftAction(eventBus, IntakeManager.LiftPosition.UP_BASKED) + ) + ), + ParallelActions.ExitType.AND + ) + ) + + return acts + } + + fun basket(extension: Double = -1.0): ArrayList { + val acts = arrayListOf() + + acts.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_UNCLAMP)) + + acts.add(WaitIntakeAction(eventBus)) + + if (extension > 0.0) + acts.add( + LiftAction( + eventBus, + IntakeManager.LiftPosition.CLAMP_CENTER, + extension + ) + ) + + return acts + } + + + fun clampStick(isDif: Boolean = false): ArrayList { + val acts = arrayListOf() + + if (isDif) + acts.add(DifAction(eventBus, 40.0)) + + acts.add(WaitIntakeAction(eventBus)) + + acts.add(WaitAction(0.2)) + + acts.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_CLAMP)) + + acts.add(WaitIntakeAction(eventBus)) + + return acts + } + + fun paralelClamp(isDif: Boolean): ArrayList { + val clampActions = + arrayListOf(WaitIntakeAction(eventBus), WaitAction(if (isDif) 0.7 else 0.4)) + + clampActions.addAll(runToBasket(getEndOrientation(actions))) + + return arrayListOf( + ParallelActions( + arrayOf(clampStick(isDif), clampActions), + ParallelActions.ExitType.AND + ) + ) + } + + actions.addAll(runToBasket(startOrientation)) + + actions.add( + ParallelActions( + arrayOf( + basket(950.0), arrayListOf( + WaitAction(0.2), + FollowRRTrajectory( + eventBus, newRRTrajectory(getEndOrientation(actions)) + .strafeToLinearHeading(Vector2d(121.6, 126.9), toRadians(-90.0)) + .build() + ) + ) + ), ParallelActions.ExitType.AND + ) + ) + + actions.addAll(paralelClamp(false)) + + actions.add( + ParallelActions( + arrayOf( + arrayListOf( + WaitAction(0.2), + FollowRRTrajectory( + eventBus, newRRTrajectory(getEndOrientation(actions)) + .strafeToLinearHeading(Vector2d(144.2, 119.6), toRadians(-90.0)) + .build() + ) + ), basket(780.0) + ), ParallelActions.ExitType.AND + ) + ) + + actions.add(WaitAction(0.2)) + + actions.addAll(paralelClamp(false)) + + actions.add( + ParallelActions( + arrayOf( + arrayListOf( + WaitAction(0.2), + FollowRRTrajectory( + eventBus, newRRTrajectory(getEndOrientation(actions)) + .strafeToLinearHeading( + Vector2d(137.7, 105.7), + toRadians(-90.0 + 39.5) + ) + .build() + ) + ), basket(780.0) + ), ParallelActions.ExitType.AND + ) + ) + + actions.add(WaitAction(0.3)) + + actions.addAll(paralelClamp(true)) + + if (teammate.brick) { + actions.add( + ParallelActions( + arrayOf( + basket(1000.0), arrayListOf( + WaitAction(0.2), FollowRRTrajectory( + eventBus, + newRRTrajectory(getEndOrientation(actions)).strafeToLinearHeading( + Vector2d(117.0, 142.3), toRadians(180.0) + ).build() + ) + ) + ), ParallelActions.ExitType.AND + ) + ) + + actions.addAll(paralelClamp(false)) + } + + actions.add( + ParallelActions( + arrayOf( + basket(1.0), arrayListOf( + WaitAction(0.1), + FollowRRTrajectory( + eventBus, newRRTrajectory( + getEndOrientation(actions) + ) + .setTangent(toRadians(-90.0)) + .splineToLinearHeading( + Pose2d(60.0, 6.0, toRadians(180.0)), + toRadians(-90.0 - 45.0) + ).build() + ) + ) + ), ParallelActions.ExitType.AND + ) + ) + + actions.add(AutoClampAction(eventBus)) + actions.add(WaitAction(0.1)) + + actions.addAll(runToBasket(getEndOrientation(actions))) + + if (!teammate.brick) { + actions.add( + ParallelActions( + arrayOf( + basket(1.0), arrayListOf( + WaitAction(0.1), + FollowRRTrajectory( + eventBus, newRRTrajectory( + getEndOrientation(actions) + ) + .setTangent(toRadians(-90.0)) + .splineToLinearHeading( + Pose2d(50.0, -6.0, toRadians(180.0)), + toRadians(-90.0 - 45.0) + ).build() + ) + ) + ), ParallelActions.ExitType.AND + ) + ) + + actions.add(AutoClampAction(eventBus)) + actions.add(WaitAction(0.1)) + + actions.addAll(runToBasket(getEndOrientation(actions))) + actions.addAll(basket()) + } else + actions.addAll(basket()) + + actions.add( + ParallelActions( + arrayOf( + arrayListOf( + FollowRRTrajectory( + eventBus, newRRTrajectory(getEndOrientation(actions)) + .setTangent(toRadians(180.0)) + .splineToLinearHeading( + Pose2d(50.0, 0.0, toRadians(0.0)), + toRadians(180.0) + ) + .build() + ) + ), + arrayListOf( + ClampAction(eventBus, Intake.ClampPosition.SERVO_CLAMP), + LiftAction(eventBus, IntakeManager.LiftPosition.UP_LAYER) + ) + ), ParallelActions.ExitType.AND + ) + ) + + eventBus.invoke(ActionsRunner.RunActionsEvent(actions)) + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectoryes/BlueHumanTrajectory.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectories/HumanTrajectory.kt similarity index 74% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectoryes/BlueHumanTrajectory.kt rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectories/HumanTrajectory.kt index dc8ccd54..779f8eea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectoryes/BlueHumanTrajectory.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectories/HumanTrajectory.kt @@ -1,6 +1,9 @@ -package org.firstinspires.ftc.teamcode.modules.mainControl.actions.trajectoryes +package org.firstinspires.ftc.teamcode.modules.mainControl.actions.trajectories +import com.acmerobotics.roadrunner.Pose2d +import com.acmerobotics.roadrunner.ProfileAccelConstraint import com.acmerobotics.roadrunner.Vector2d +import org.firstinspires.ftc.teamcode.collectors.BaseCollector import org.firstinspires.ftc.teamcode.collectors.events.EventBus import org.firstinspires.ftc.teamcode.modules.intake.Intake import org.firstinspires.ftc.teamcode.modules.intake.IntakeManager @@ -13,15 +16,22 @@ import org.firstinspires.ftc.teamcode.modules.mainControl.actions.LiftAction import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ParallelActions import org.firstinspires.ftc.teamcode.modules.mainControl.actions.TurnAction import org.firstinspires.ftc.teamcode.modules.mainControl.actions.WaitAction -import org.firstinspires.ftc.teamcode.modules.mainControl.runner.TrajectorySegmentRunner import org.firstinspires.ftc.teamcode.modules.mainControl.runner.TrajectorySegmentRunner.Companion.newRRTrajectory import org.firstinspires.ftc.teamcode.utils.configs.Configs import org.firstinspires.ftc.teamcode.utils.units.Angle import org.firstinspires.ftc.teamcode.utils.units.Orientation import java.lang.Math.toRadians -class BlueHumanTrajectory : ITrajectoryBuilder { - override fun runTrajectory(eventBus: EventBus, startOrientation: Orientation) { +class HumanTrajectory : ITrajectoryBuilder { + override fun runTrajectory( + eventBus: EventBus, + startOrientation: Orientation, + teammate: BaseCollector.TeammateSate + ) { + val layerAccelConstrain = + ProfileAccelConstraint(-150.0, Configs.DriveTrainConfig.MAX_TRANSLATION_ACCEL) + val humanUnclampPos = ProfileAccelConstraint(-150.0, 150.0) + val actions = arrayListOf() actions.add( @@ -31,8 +41,9 @@ class BlueHumanTrajectory : ITrajectoryBuilder { FollowRRTrajectory( eventBus, newRRTrajectory(startOrientation) .strafeToLinearHeading( - Vector2d(19.2, 77.7), - toRadians(90.0) + Vector2d(14.2, 72.0), + toRadians(90.0), + accelConstraintOverride = layerAccelConstrain ) .build() ) @@ -56,8 +67,8 @@ class BlueHumanTrajectory : ITrajectoryBuilder { FollowRRTrajectory( eventBus, newRRTrajectory(getEndOrientation(actions)) .strafeToLinearHeading( - Vector2d(-74.7, 118.2), - toRadians(-88.0 - 45.0) + Vector2d(-79.2, 118.2), + toRadians(-90.0 - 45.0) ) .build() ) @@ -75,52 +86,34 @@ class BlueHumanTrajectory : ITrajectoryBuilder { actions.add(TurnAction(eventBus, getEndOrientation(actions), Angle.ofDeg(180.0 - 35.0))) - actions.add(ParallelActions(arrayOf( - arrayListOf( - ClampAction(eventBus, Intake.ClampPosition.SERVO_UNCLAMP) - ), - arrayListOf( - FollowRRTrajectory( - eventBus, newRRTrajectory(getEndOrientation(actions)) - .strafeToLinearHeading( - Vector2d(-98.2, 116.7), - toRadians(-90.0 - 45.0) + actions.add( + ParallelActions( + arrayOf( + arrayListOf( + ClampAction(eventBus, Intake.ClampPosition.SERVO_UNCLAMP) + ), + arrayListOf( + FollowRRTrajectory( + eventBus, newRRTrajectory(getEndOrientation(actions)) + .strafeToLinearHeading( + Vector2d(-105.2, 116.2), + toRadians(-90.0 - 45.0), + accelConstraintOverride = humanUnclampPos + ) + .build() ) - .build() - ) + ) + ), ParallelActions.ExitType.AND ) - ), ParallelActions.ExitType.AND)) + ) + + actions.add(WaitAction(0.1)) actions.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_CLAMP)) actions.add(TurnAction(eventBus, getEndOrientation(actions), Angle.ofDeg(180.0 - 35.0))) actions.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_UNCLAMP)) - /* - actions.add(TurnAction(eventBus, getEndOrientation(actions), Angle.ofDeg(-90.0 - 45.0))) - - actions.add( - FollowRRTrajectory( - eventBus, newRRTrajectory(getEndOrientation(actions)) - .strafeTo(Vector2d(-78.0 - 27.0 - 25.0, 122.0)) - .build() - ) - ) - - actions.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_CLAMP)) - - actions.add( - FollowRRTrajectory( - eventBus, newRRTrajectory(getEndOrientation(actions)) - .strafeToLinearHeading( - Vector2d(-73.0, 124.0), - toRadians(180.0 - 45.0) - ) - .build() - ) - ) - - actions.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_UNCLAMP))*/ actions.add( ParallelActions( @@ -128,9 +121,13 @@ class BlueHumanTrajectory : ITrajectoryBuilder { arrayListOf( FollowRRTrajectory( eventBus, newRRTrajectory(getEndOrientation(actions)) - .strafeToLinearHeading( - Vector2d(-83.5, 135.3), - toRadians(90.0) + .setTangent(toRadians(-90.0)) + .splineToLinearHeading( + Pose2d( + Vector2d(-95.5, 130.0), + toRadians(90.0) + ), + toRadians(180.0 - 35.0) ) .build() ) @@ -152,7 +149,11 @@ class BlueHumanTrajectory : ITrajectoryBuilder { FollowRRTrajectory( eventBus, newRRTrajectory(getEndOrientation(actions)) .setTangent(toRadians(0.0)) - .splineToConstantHeading(Vector2d(12.0, 81.8), toRadians(-90.0)) + .splineToConstantHeading( + Vector2d(12.0, 80.5), + toRadians(-90.0), + accelConstraintOverride = layerAccelConstrain + ) .build() ) ), @@ -172,7 +173,7 @@ class BlueHumanTrajectory : ITrajectoryBuilder { FollowRRTrajectory( eventBus, newRRTrajectory(getEndOrientation(actions)) .strafeToLinearHeading( - Vector2d(-81.1, 144.1), + Vector2d(-90.1, 139.6), toRadians(90.0) ) .build() @@ -195,7 +196,11 @@ class BlueHumanTrajectory : ITrajectoryBuilder { FollowRRTrajectory( eventBus, newRRTrajectory(getEndOrientation(actions)) .setTangent(toRadians(0.0)) - .splineToConstantHeading(Vector2d(9.2, 100.2), toRadians(-90.0)) + .splineToConstantHeading( + Vector2d(9.2, 90.7), + toRadians(-90.0), + accelConstraintOverride = layerAccelConstrain + ) .build() ) ), @@ -215,7 +220,7 @@ class BlueHumanTrajectory : ITrajectoryBuilder { FollowRRTrajectory( eventBus, newRRTrajectory(getEndOrientation(actions)) .strafeToLinearHeading( - Vector2d(-78.1, 151.0), + Vector2d(-87.1, 150.1), toRadians(90.0) ) .build() @@ -239,8 +244,8 @@ class BlueHumanTrajectory : ITrajectoryBuilder { eventBus, newRRTrajectory(getEndOrientation(actions)) .setTangent(toRadians(0.0)) .splineToConstantHeading( - Vector2d(10.1, 102.9), - toRadians(-90.0) + Vector2d(8.1, 97.3), + toRadians(-90.0), accelConstraintOverride = layerAccelConstrain ) .build() ) @@ -261,7 +266,7 @@ class BlueHumanTrajectory : ITrajectoryBuilder { FollowRRTrajectory( eventBus, newRRTrajectory(getEndOrientation(actions)) .strafeToLinearHeading( - Vector2d(-42.0, 147.4), + Vector2d(-42.0, 140.4), toRadians(90.0 + 45.0) ) .build() @@ -274,17 +279,6 @@ class BlueHumanTrajectory : ITrajectoryBuilder { ) ) - -// actions.add(FollowRRTrajectory( -// eventBus, newRRTrajectory(getEndOrientation(actions)) -// .strafeToLinearHeading( -// Vector2d(0.0, 140.0), -// toRadians(0.0) -// ) -// .strafeTo(Vector2d(-98.0, 140.0)) -// .build() -// )) - eventBus.invoke(RunActionsEvent(actions)) } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectoryes/ITrajectoryBuilder.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectories/ITrajectoryBuilder.kt similarity index 50% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectoryes/ITrajectoryBuilder.kt rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectories/ITrajectoryBuilder.kt index 232a1645..301a51d4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectoryes/ITrajectoryBuilder.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectories/ITrajectoryBuilder.kt @@ -1,8 +1,13 @@ -package org.firstinspires.ftc.teamcode.modules.mainControl.actions.trajectoryes +package org.firstinspires.ftc.teamcode.modules.mainControl.actions.trajectories +import org.firstinspires.ftc.teamcode.collectors.BaseCollector import org.firstinspires.ftc.teamcode.collectors.events.EventBus import org.firstinspires.ftc.teamcode.utils.units.Orientation interface ITrajectoryBuilder { - fun runTrajectory(eventBus: EventBus, startOrientation: Orientation) + fun runTrajectory( + eventBus: EventBus, + startOrientation: Orientation, + teammate: BaseCollector.TeammateSate + ) } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectories/NewBasketTrajectory.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectories/NewBasketTrajectory.kt new file mode 100644 index 00000000..dee5a583 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectories/NewBasketTrajectory.kt @@ -0,0 +1,271 @@ +package org.firstinspires.ftc.teamcode.modules.mainControl.actions.trajectories + +import com.acmerobotics.roadrunner.Pose2d +import com.acmerobotics.roadrunner.Vector2d +import org.firstinspires.ftc.teamcode.collectors.BaseCollector +import org.firstinspires.ftc.teamcode.collectors.events.EventBus +import org.firstinspires.ftc.teamcode.modules.intake.Intake +import org.firstinspires.ftc.teamcode.modules.intake.IntakeManager +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ActionsRunner +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.AutoClampAction +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ClampAction +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.DifAction +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.FollowRRTrajectory +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.IAction +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ITransportAction.Companion.getEndOrientation +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.LiftAction +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ParallelActions +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.WaitAction +import org.firstinspires.ftc.teamcode.modules.mainControl.actions.WaitIntakeAction +import org.firstinspires.ftc.teamcode.modules.mainControl.runner.TrajectorySegmentRunner.Companion.newRRTrajectory +import org.firstinspires.ftc.teamcode.utils.units.Orientation +import java.lang.Math.toRadians + +class NewBasketTrajectory : ITrajectoryBuilder { + override fun runTrajectory( + eventBus: EventBus, + startOrientation: Orientation, + teammate: BaseCollector.TeammateSate + ) { + val actions = arrayListOf() + + val basketDelay = 0.1 + + fun runToBasket(): ArrayList { + val acts = arrayListOf() + + acts.add( + ParallelActions( + arrayOf( + arrayListOf( + FollowRRTrajectory( + eventBus, newRRTrajectory(startOrientation) + .strafeToLinearHeading( + Vector2d(129.2, 133.6), + toRadians(-90.0 - 45.0) + ) + .build() + ) + ), + arrayListOf( + WaitIntakeAction(eventBus), + LiftAction(eventBus, IntakeManager.LiftPosition.UP_BASKED) + ) + ), + ParallelActions.ExitType.AND + ) + ) + + return acts + } + + fun basket(extensionPos: Double = -1.0, rotateDif: Boolean = false): ArrayList { + val acts = arrayListOf() + + acts.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_UNCLAMP)) + + if (extensionPos > 0.0) + acts.add( + LiftAction( + eventBus, + IntakeManager.LiftPosition.CLAMP_CENTER, + extensionPos + ) + ) + + if (rotateDif) + acts.add(DifAction(eventBus, 40.0)) + + return acts + } + + actions.addAll(runToBasket()) + + actions.add( + ParallelActions( + arrayOf( + basket(990.0), arrayListOf( + WaitAction(basketDelay), FollowRRTrajectory( + eventBus, newRRTrajectory(getEndOrientation(actions)) + .strafeToLinearHeading(Vector2d(117.6, 126.9), toRadians(-90.0)) + .build() + ) + ) + ), ParallelActions.ExitType.AND + ) + ) + + actions.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_CLAMP, false)) + actions.addAll(runToBasket()) + + actions.add( + ParallelActions( + arrayOf( + basket(770.0), arrayListOf( + WaitAction(basketDelay), + FollowRRTrajectory( + eventBus, newRRTrajectory(getEndOrientation(actions)) + .strafeToLinearHeading(Vector2d(141.2, 119.6), toRadians(-90.0)) + .build() + ) + ) + ), ParallelActions.ExitType.AND + ) + ) + + actions.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_CLAMP, false)) + actions.addAll(runToBasket()) + + actions.add( + ParallelActions( + arrayOf( + basket(780.0, true), arrayListOf( + WaitAction(basketDelay), + FollowRRTrajectory( + eventBus, newRRTrajectory(getEndOrientation(actions)) + .strafeToLinearHeading( + Vector2d(129.7, 106.7), + toRadians(-90.0 + 39.5) + ) + .build() + ) + ) + ), ParallelActions.ExitType.AND + ) + ) + + actions.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_CLAMP, false)) + actions.add(WaitAction(0.1)) + actions.addAll(runToBasket()) + + if (teammate.brick) { + actions.add( + ParallelActions( + arrayOf( + basket(1000.0), arrayListOf( + WaitAction(basketDelay), + FollowRRTrajectory( + eventBus, newRRTrajectory(getEndOrientation(actions)) + .strafeToLinearHeading( + Vector2d(112.0, 142.3), toRadians(180.0) + ).build() + ) + ) + ), ParallelActions.ExitType.AND + ) + ) + + actions.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_CLAMP, false)) + actions.add(WaitAction(0.1)) + actions.add( + ParallelActions( + arrayOf( + arrayListOf( + FollowRRTrajectory( + eventBus, newRRTrajectory(startOrientation) + .strafeToLinearHeading( + Vector2d(130.1, 122.5), + toRadians(-90.0 - 45.0) + ) + .build() + ) + ), + arrayListOf( + WaitIntakeAction(eventBus), + LiftAction(eventBus, IntakeManager.LiftPosition.UP_BASKED) + ) + ), + ParallelActions.ExitType.AND + ) + ) + } + + actions.add( + ParallelActions( + arrayOf( + basket(), arrayListOf( + FollowRRTrajectory( + eventBus, + newRRTrajectory(getEndOrientation(actions)) + .setTangent(toRadians(-90.0)) + .splineToLinearHeading( + Pose2d(47.0, 10.0, toRadians(180.0)), + toRadians(180.0) + ).build() + ) + ) + ), ParallelActions.ExitType.AND + ) + ) + + actions.add(AutoClampAction(eventBus)) + + actions.add( + ParallelActions( + arrayOf( + arrayListOf( + FollowRRTrajectory( + eventBus, newRRTrajectory(getEndOrientation(actions)) + .setReversed(true) + .splineToLinearHeading( + Pose2d(91.0, 149.0, toRadians(-90.0 - 45.0)), + toRadians(90.0) + ) + .build() + ) + ), + arrayListOf( + WaitIntakeAction(eventBus), + LiftAction(eventBus, IntakeManager.LiftPosition.UP_BASKED) + ) + ), ParallelActions.ExitType.AND + ) + ) + + actions.add( + ParallelActions( + arrayOf( + basket(), arrayListOf( + WaitAction(basketDelay), + FollowRRTrajectory( + eventBus, + newRRTrajectory(getEndOrientation(actions)) + .setTangent(toRadians(-90.0)) + .splineToLinearHeading( + Pose2d(20.0, 30.0, toRadians(180.0)), + toRadians(180.0) + ).build() + ) + ) + ), ParallelActions.ExitType.AND + ) + ) + + actions.add(AutoClampAction(eventBus)) + + actions.add( + ParallelActions( + arrayOf( + arrayListOf( + FollowRRTrajectory( + eventBus, newRRTrajectory(getEndOrientation(actions)) + .setReversed(true) + .splineToLinearHeading( + Pose2d(70.0, 153.0, toRadians(-90.0 - 45.0)), + toRadians(90.0) + ) + .build() + ) + ), + arrayListOf( + WaitIntakeAction(eventBus), + LiftAction(eventBus, IntakeManager.LiftPosition.UP_BASKED) + ) + ), ParallelActions.ExitType.AND + ) + ) + actions.addAll(basket()) + + eventBus.invoke(ActionsRunner.RunActionsEvent(actions)) + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectoryes/BlueBaskedTrajectory.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectoryes/BlueBaskedTrajectory.kt deleted file mode 100644 index 5f458d6b..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectoryes/BlueBaskedTrajectory.kt +++ /dev/null @@ -1,163 +0,0 @@ -package org.firstinspires.ftc.teamcode.modules.mainControl.actions.trajectoryes - -import com.acmerobotics.roadrunner.Pose2d -import com.acmerobotics.roadrunner.Vector2d -import org.firstinspires.ftc.teamcode.collectors.events.EventBus -import org.firstinspires.ftc.teamcode.modules.intake.Intake -import org.firstinspires.ftc.teamcode.modules.intake.IntakeManager -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ActionsRunner -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ClampAction -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.DifAction -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.FollowRRTrajectory -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.IAction -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ITransportAction.Companion.getEndOrientation -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.LiftAction -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ParallelActions -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.TurnAction -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.WaitAction -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.WaitLiftAction -import org.firstinspires.ftc.teamcode.modules.mainControl.runner.TrajectorySegmentRunner.Companion.newRRTrajectory -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 java.lang.Math.toRadians - -class BlueBaskedTrajectory : ITrajectoryBuilder { - override fun runTrajectory(eventBus: EventBus, startOrientation: Orientation) { - val actions = arrayListOf() - - fun runToBasket(startOrientation: Orientation) { - actions.add( - ParallelActions( - arrayOf( - arrayListOf( - FollowRRTrajectory( - eventBus, newRRTrajectory(startOrientation) - .strafeToLinearHeading(Vector2d(135.0, 132.3), toRadians(-90.0 - 45.0)) - .build() - ) - ) - , - arrayListOf( - WaitAction(0.1), - LiftAction(eventBus, IntakeManager.LiftPosition.UP_BASKED) - ) - ), - ParallelActions.ExitType.AND - ) - ) - } - - fun basket() { - actions.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_UNCLAMP)) - - actions.add(WaitLiftAction(eventBus)) - } - - fun clampStick(extension: Double, isDif: Boolean = false){ - actions.add(WaitLiftAction(eventBus)) - - actions.add(WaitAction(0.15)) - - actions.add(LiftAction(eventBus, IntakeManager.LiftPosition.CLAMP_CENTER, extension)) - - actions.add(WaitLiftAction(eventBus)) - - if(isDif) - for(i in 0..2) - actions.add(DifAction(eventBus, DifAction.DifDirection.NEXT)) - - actions.add(WaitAction(0.1)) - - actions.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_CLAMP)) - - actions.add(WaitLiftAction(eventBus)) - } - - runToBasket(startOrientation) - basket() - - actions.add( - FollowRRTrajectory( - eventBus, newRRTrajectory(getEndOrientation(actions)) - .strafeToLinearHeading(Vector2d(119.6, 126.9), toRadians(-90.0)) - .build() - ) - ) - - clampStick(950.0) - runToBasket(getEndOrientation(actions)) - - basket() - - actions.add( - FollowRRTrajectory( - eventBus, newRRTrajectory(getEndOrientation(actions)) - .strafeToLinearHeading(Vector2d(142.1, 119.6), toRadians(-90.0)) - .build() - ) - ) - - clampStick(800.0) - runToBasket(getEndOrientation(actions)) - basket() - - actions.add( - FollowRRTrajectory( - eventBus, newRRTrajectory(getEndOrientation(actions)) - .strafeToLinearHeading(Vector2d(137.7, 106.7), toRadians(-90.0 + 39.5)) - .build() - ) - ) - - clampStick(780.0, true) - runToBasket(getEndOrientation(actions)) - basket() -// -// actions.add( -// FollowRRTrajectory( -// eventBus, newRRTrajectory(getEndOrientation(actions)) -// .strafeToLinearHeading(Vector2d(105.8, 132.8), toRadians(180.0)) -// .build() -// ) -// ) -// -// clampStick(700.0) -// -// actions.add( -// ParallelActions( -// arrayOf( -// arrayListOf( -// FollowRRTrajectory( -// eventBus, newRRTrajectory(startOrientation) -// .strafeToLinearHeading(Vector2d(136.2, 128.2), toRadians(-90.0 - 45.0)) -// .build() -// ) -// ) -// , -// arrayListOf( -// WaitAction(0.1), -// LiftAction(eventBus, IntakeManager.LiftPosition.UP_BASKED) -// ) -// ), -// ParallelActions.ExitType.AND -// ) -// ) -// -// basket() - - actions.add( - FollowRRTrajectory( - eventBus, newRRTrajectory(getEndOrientation(actions)) - .setTangent(toRadians(180.0)) - .splineToLinearHeading(Pose2d(62.0, 0.0, toRadians(0.0)), toRadians(180.0)) - .build() - ) - ) - - actions.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_CLAMP)) - actions.add(LiftAction(eventBus, IntakeManager.LiftPosition.UP_LAYER)) - - eventBus.invoke(ActionsRunner.RunActionsEvent(actions)) - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectoryes/RedBaskedTrajectory.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectoryes/RedBaskedTrajectory.kt deleted file mode 100644 index 7058cdc1..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectoryes/RedBaskedTrajectory.kt +++ /dev/null @@ -1,113 +0,0 @@ -package org.firstinspires.ftc.teamcode.modules.mainControl.actions.trajectoryes - -import com.acmerobotics.roadrunner.Vector2d -import org.firstinspires.ftc.teamcode.collectors.events.EventBus -import org.firstinspires.ftc.teamcode.modules.intake.Intake -import org.firstinspires.ftc.teamcode.modules.intake.IntakeManager -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ActionsRunner -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ClampAction -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.DifAction -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.FollowRRTrajectory -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.IAction -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ITransportAction.Companion.getEndOrientation -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.LiftAction -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.WaitAction -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.WaitLiftAction -import org.firstinspires.ftc.teamcode.modules.mainControl.runner.TrajectorySegmentRunner.Companion.newRRTrajectory -import org.firstinspires.ftc.teamcode.utils.units.Orientation -import java.lang.Math.toRadians - -class RedBaskedTrajectory: ITrajectoryBuilder { - override fun runTrajectory(eventBus: EventBus, startOrientation: Orientation){ - val actions = arrayListOf() - - fun runToBasket(startOrientation: Orientation) { - actions.add( - FollowRRTrajectory( - eventBus, newRRTrajectory(startOrientation) - .strafeToLinearHeading(Vector2d(-129.5, -121.5), toRadians(90.0 - 45.0)) - .build() - ) - ) - } - - fun basket() { - actions.add(LiftAction(eventBus, IntakeManager.LiftPosition.UP_BASKED)) - - actions.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_UNCLAMP)) - - actions.add(WaitLiftAction(eventBus)) - } - - fun clampStick(extension: Double, isDif: Boolean = false){ - actions.add(WaitLiftAction(eventBus)) - - actions.add(WaitAction(0.1)) - - actions.add(LiftAction(eventBus, IntakeManager.LiftPosition.CLAMP_CENTER, extension)) - - actions.add(WaitLiftAction(eventBus)) - - if(isDif) - for(i in 0..2) - actions.add(DifAction(eventBus, DifAction.DifDirection.NEXT)) - - actions.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_CLAMP)) - - actions.add(WaitLiftAction(eventBus)) - } - - runToBasket(startOrientation) - basket() - - actions.add( - FollowRRTrajectory( - eventBus, newRRTrajectory(getEndOrientation(actions)) - .strafeToLinearHeading(Vector2d(-119.9, -126.7), toRadians(90.0)) - .build() - ) - ) - - clampStick(850.0) - runToBasket(getEndOrientation(actions)) - - basket() - - actions.add( - FollowRRTrajectory( - eventBus, newRRTrajectory(getEndOrientation(actions)) - .strafeToLinearHeading(Vector2d(-138.7, -118.6), toRadians(90.0)) - .build() - ) - ) - - clampStick(750.0) - runToBasket(getEndOrientation(actions)) - basket() - - actions.add( - FollowRRTrajectory( - eventBus, newRRTrajectory(getEndOrientation(actions)) - .strafeToLinearHeading(Vector2d(-139.0, -118.6), toRadians(90.0 - 40)) - .build() - ) - ) - - clampStick(750.0, true) - runToBasket(getEndOrientation(actions)) - basket() - - actions.add( - FollowRRTrajectory( - eventBus, newRRTrajectory(getEndOrientation(actions)) - .strafeToLinearHeading(Vector2d(-31.0, 0.0), toRadians(180.0)) - .build() - ) - ) - - actions.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_CLAMP)) - actions.add(LiftAction(eventBus, IntakeManager.LiftPosition.UP_LAYER)) - - eventBus.invoke(ActionsRunner.RunActionsEvent(actions)) - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectoryes/RedHumanTrajectory.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectoryes/RedHumanTrajectory.kt deleted file mode 100644 index a19a1683..00000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/actions/trajectoryes/RedHumanTrajectory.kt +++ /dev/null @@ -1,55 +0,0 @@ -package org.firstinspires.ftc.teamcode.modules.mainControl.actions.trajectoryes - -import com.acmerobotics.roadrunner.Vector2d -import com.acmerobotics.roadrunner.clamp -import org.firstinspires.ftc.teamcode.collectors.events.EventBus -import org.firstinspires.ftc.teamcode.modules.intake.Intake -import org.firstinspires.ftc.teamcode.modules.intake.IntakeManager -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ActionsRunner -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ClampAction -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.FollowRRTrajectory -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.IAction -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.ITransportAction.Companion.getEndOrientation -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.LiftAction -import org.firstinspires.ftc.teamcode.modules.mainControl.actions.WaitAction -import org.firstinspires.ftc.teamcode.modules.mainControl.runner.TrajectorySegmentRunner.Companion.newRRTrajectory -import org.firstinspires.ftc.teamcode.utils.units.Orientation - -class RedHumanTrajectory: ITrajectoryBuilder { - override fun runTrajectory(eventBus: EventBus, startOrientation: Orientation){ - val actions = arrayListOf() - - extracted(actions, eventBus, startOrientation) - - - extracted(actions, eventBus, startOrientation) - - actions.add(WaitAction(5.0)) - - actions.add(LiftAction(eventBus, IntakeManager.LiftPosition.UP_BASKED)) - - actions.add(FollowRRTrajectory(eventBus, newRRTrajectory(getEndOrientation(actions)) - .strafeTo(Vector2d(20.0, -85.0)) - .build()) - ) - actions.add(ClampAction(eventBus, Intake.ClampPosition.SERVO_UNCLAMP)) - actions.add(FollowRRTrajectory(eventBus, newRRTrajectory(getEndOrientation(actions)) - .strafeTo(Vector2d(150.0,-159.0)) - .build()) - ) - - eventBus.invoke(ActionsRunner.RunActionsEvent(actions)) - } - private fun extracted( - actions: ArrayList, - eventBus: EventBus, - startOrientation: Orientation - ) { - actions.add( - FollowRRTrajectory( - eventBus, newRRTrajectory(startOrientation) - .build() - ) - ) - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/gamepad/Gamepad.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/gamepad/Gamepad.kt index e610a7e7..6b647a11 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/gamepad/Gamepad.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/gamepad/Gamepad.kt @@ -18,27 +18,34 @@ class Gamepad : IRobotModule { override fun init(collector: BaseCollector, bus: EventBus) { _gamepad = collector.robot.gamepad1 _eventBus = bus + + bus.subscribe(IntakeManager.ClampDefendedEvent::class) { + _gamepad.rumble(Configs.IntakeConfig.GAMEPAD_DEFENDED_RUMPLE_MS) + } } private var _oldClamp = false - private var _basketOld = false + private var _upBasketOld = false + private var _lowBasketOld = false private var _centerOld = false private var _layerOld = false private var _clampWallOld = false + private var _autoClampOld = false private var _oldNextDifPos = false private var _oldPreviousDifPos = false override fun update() { - _eventBus.invoke( - SetDrivePowerEvent( - Vec2( - (-_gamepad.left_stick_y).toDouble(), - (-_gamepad.left_stick_x).toDouble() - ), (-_gamepad.right_stick_x).toDouble() + if (_eventBus.invoke(IntakeManager.RequestLiftPosEvent()).pos!! != IntakeManager.LiftPosition.AUTO_CLAMP_CENTER) + _eventBus.invoke( + SetDrivePowerEvent( + Vec2( + (-_gamepad.left_stick_y).toDouble(), + (-_gamepad.left_stick_x).toDouble() + ), (-_gamepad.right_stick_x).toDouble() + ) ) - ) if (_gamepad.circle && !_oldClamp) { if (_eventBus.invoke(IntakeManager.RequestClampPosEvent()).pos == ClampPosition.SERVO_UNCLAMP) @@ -56,7 +63,7 @@ class Gamepad : IRobotModule { else _eventBus.invoke(Hook.HookStop()) - if (!_basketOld && _gamepad.dpad_up) + if (!_upBasketOld && _gamepad.dpad_up) _eventBus.invoke(IntakeManager.EventSetLiftPose(IntakeManager.LiftPosition.UP_BASKED)) if (!_centerOld && _gamepad.dpad_down) @@ -68,17 +75,26 @@ class Gamepad : IRobotModule { if (!_clampWallOld && _gamepad.dpad_left) _eventBus.invoke(IntakeManager.EventSetLiftPose(IntakeManager.LiftPosition.CLAMP_WALL)) - _basketOld = _gamepad.dpad_up + if (!_lowBasketOld && _gamepad.cross) + _eventBus.invoke(IntakeManager.EventSetLiftPose(IntakeManager.LiftPosition.LOW_BASKET)) + + _upBasketOld = _gamepad.dpad_up + _lowBasketOld = _gamepad.cross _centerOld = _gamepad.dpad_down _layerOld = _gamepad.dpad_right _clampWallOld = _gamepad.dpad_left + _autoClampOld = _gamepad.ps - _eventBus.invoke(IntakeManager.EventSetExtensionVel((_gamepad.right_trigger - _gamepad.left_trigger).toDouble() * Configs.LiftConfig.GAMEPAD_EXTENSION_SENS)) + _eventBus.invoke( + IntakeManager.EventSetExtensionVel( + (_gamepad.right_trigger - _gamepad.left_trigger) * Configs.LiftConfig.GAMEPAD_EXTENSION_SENS + ) + ) - if(_gamepad.right_bumper && !_oldNextDifPos) + if (_gamepad.right_bumper && !_oldNextDifPos) _eventBus.invoke(IntakeManager.NextDifPos()) - if(_gamepad.left_bumper && !_oldPreviousDifPos) + if (_gamepad.left_bumper && !_oldPreviousDifPos) _eventBus.invoke(IntakeManager.PreviousDifPos()) _oldNextDifPos = _gamepad.right_bumper diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/runner/TrajectorySegmentRunner.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/runner/TrajectorySegmentRunner.kt index f651d0ff..b5baefd3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/runner/TrajectorySegmentRunner.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/runner/TrajectorySegmentRunner.kt @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.modules.mainControl.runner import com.acmerobotics.roadrunner.AngularVelConstraint -import com.acmerobotics.roadrunner.HolonomicController import com.acmerobotics.roadrunner.MinVelConstraint import com.acmerobotics.roadrunner.Pose2d import com.acmerobotics.roadrunner.ProfileAccelConstraint @@ -14,6 +13,7 @@ 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.modules.driveTrain.DriveTrain +import org.firstinspires.ftc.teamcode.modules.intake.IntakeManager import org.firstinspires.ftc.teamcode.modules.navigation.gyro.MergeGyro import org.firstinspires.ftc.teamcode.modules.navigation.odometry.MergeOdometry import org.firstinspires.ftc.teamcode.utils.configs.Configs @@ -133,12 +133,13 @@ class TrajectorySegmentRunner : IRobotModule { val headingVelU = if (abs(velHeadingErr) > Configs.RoadRunnerConfig.HEADING_VEL_SENS) velHeadingErr * Configs.RoadRunnerConfig.HEADING_VEL_P else 0.0 - _eventBus.invoke( - DriveTrain.SetDriveCmEvent( - localizedTransVelocity + uPos + uPosVel, - _targetHeadingVelocity + headingU + headingVelU + if (_eventBus.invoke(IntakeManager.RequestLiftPosEvent()).pos!! != IntakeManager.LiftPosition.AUTO_CLAMP_CENTER) + _eventBus.invoke( + DriveTrain.SetDriveCmEvent( + localizedTransVelocity + uPos + uPosVel, + _targetHeadingVelocity + headingU + headingVelU + ) ) - ) StaticTelemetry.drawRect( _targetOrientation.pos, diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/runner/TrajectorySegments.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/runner/TrajectorySegments.kt index 6169e36c..dc8ee8dd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/runner/TrajectorySegments.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/modules/mainControl/runner/TrajectorySegments.kt @@ -24,9 +24,15 @@ interface ITrajectorySegment { fun duration(): Double } -class TurnSegment(angle: Double, private val _startOrientation: Orientation): ITrajectorySegment{ - private val _turn = TimeTurn(Pose2d(_startOrientation.x, _startOrientation.y, _startOrientation.angl.angle), angle, - TurnConstraints(Configs.DriveTrainConfig.MAX_ROTATE_VELOCITY, -Configs.DriveTrainConfig.ROTATE_ACCEL, Configs.DriveTrainConfig.ROTATE_ACCEL)) +class TurnSegment(angle: Double, private val _startOrientation: Orientation) : ITrajectorySegment { + private val _turn = TimeTurn( + Pose2d(_startOrientation.x, _startOrientation.y, _startOrientation.angl.angle), angle, + TurnConstraints( + Configs.RoadRunnerConfig.ROAD_RUNNER_ROTATE_VELOCITY, + -Configs.RoadRunnerConfig.ROAD_RUNNER_ROTATE_VELOCITY, + Configs.DriveTrainConfig.ROTATE_ACCEL + ) + ) override fun isEnd(time: Double) = time > duration() @@ -34,19 +40,21 @@ class TurnSegment(angle: Double, private val _startOrientation: Orientation): IT override fun turnVelocity(time: Double) = _turn[time].velocity().angVel.value() - override fun targetOrientation(time: Double) = Orientation(_startOrientation.pos, Angle(_turn[time].value().heading.toDouble())) + override fun targetOrientation(time: Double) = + Orientation(_startOrientation.pos, Angle(_turn[time].value().heading.toDouble())) override fun duration() = _turn.duration } -class RRTrajectorySegment(rawBuildedTrajectory: List): ITrajectorySegment{ - private val _trajectory = Array(rawBuildedTrajectory.size){TimeTrajectory(rawBuildedTrajectory[it])} +class RRTrajectorySegment(rawBuildedTrajectory: List) : ITrajectorySegment { + private val _trajectory = + Array(rawBuildedTrajectory.size) { TimeTrajectory(rawBuildedTrajectory[it]) } private fun getPoseTime(time: Double): Pose2dDual