From ade4fe1045f17b1a29196e35c896789e30750f22 Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Thu, 12 Dec 2024 14:57:50 -0500 Subject: [PATCH] Object structure --- .../blue/basket/left/BlueHighBasketLeft.kt | 76 +++++ .../blue/basket/right/BlueHighBasketRight.kt | 76 +++++ .../blue/chamber/left/BlueHighChamberLeft.kt | 76 +++++ .../chamber/right/BlueHighChamberRight.kt | 75 +++++ .../blue/park/left/BlueParkObservationLeft.kt | 26 ++ .../park/left/BlueParkSubmmersibleLeft.kt | 26 ++ .../park/right/BlueParkObservationRight.kt | 21 ++ .../park/right/BlueParkSubmmersibleRight.kt | 27 ++ .../auto/red/basket/left/RedHighBasketLeft.kt | 64 ++++ .../red/basket/right/RedHighBasketRight.kt | 63 ++++ .../red/chamber/left/RedHighChamberLeft.kt | 64 ++++ .../red/chamber/right/RedHighChamberRight.kt | 64 ++++ .../red/park/left/RedParkObservationLeft.kt | 23 ++ .../red/park/left/RedParkSubmmersibleLeft.kt | 25 ++ .../red/park/right/RedParkObservationRight.kt | 22 ++ .../park/right/RedParkSubmmersibleRight.kt | 26 ++ .../teamcode/legacy/drive/DriveSubsystem.kt | 306 ++++++++++++++++++ .../roadrunner/AutomaticFeedforwardTuner.kt | 210 ++++++++++++ .../legacy/roadrunner/BackAndForth.kt | 48 +++ .../roadrunner/DriveVelocityPIDTuner.kt | 153 +++++++++ .../legacy/roadrunner/FollowerPIDTuner.kt | 51 +++ .../legacy/roadrunner/LocalizationTest.kt | 41 +++ .../roadrunner/ManualFeedforwardTuner.kt | 130 ++++++++ .../legacy/roadrunner/MaxAngularVeloTuner.kt | 69 ++++ .../legacy/roadrunner/MaxVelocityTuner.kt | 86 +++++ .../roadrunner/MotorDirectionDebugger.kt | 78 +++++ .../teamcode/legacy/roadrunner/SplineTest.kt | 38 +++ .../teamcode/legacy/roadrunner/StrafeTest.kt | 38 +++ .../legacy/roadrunner/StraightTest.kt | 32 ++ .../legacy/roadrunner/TrackWidthTuner.kt | 68 ++++ .../TrackingWheelForwardOffsetTuner.kt | 85 +++++ .../TrackingWheelLateralDistanceTuner.kt | 124 +++++++ .../teamcode/legacy/roadrunner/TurnTest.kt | 22 ++ .../blue/basket/left/BlueHighBasketLeft.kt | 76 ----- .../blue/basket/right/BlueHighBasketRight.kt | 76 ----- .../blue/chamber/left/BlueHighChamberLeft.kt | 76 ----- .../chamber/right/BlueHighChamberRight.kt | 75 ----- .../blue/park/left/BlueParkObservationLeft.kt | 26 -- .../park/left/BlueParkSubmmersibleLeft.kt | 31 -- .../park/right/BlueParkObservationRight.kt | 26 -- .../park/right/BlueParkSubmmersibleRight.kt | 32 -- .../auto/red/basket/left/RedHighBasketLeft.kt | 75 ----- .../red/basket/right/RedHighBasketRight.kt | 74 ----- .../red/chamber/left/RedHighChamberLeft.kt | 75 ----- .../red/chamber/right/RedHighChamberRight.kt | 75 ----- .../red/park/left/RedParkObservationLeft.kt | 28 -- .../red/park/left/RedParkSubmmersibleLeft.kt | 30 -- .../red/park/right/RedParkObservationRight.kt | 27 -- .../park/right/RedParkSubmmersibleRight.kt | 31 -- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 7 +- .../roadrunner/AutomaticFeedforwardTuner.kt | 211 ------------ .../opModes/tuning/roadrunner/BackAndForth.kt | 49 --- .../roadrunner/DriveVelocityPIDTuner.kt | 154 --------- .../tuning/roadrunner/FollowerPIDTuner.kt | 52 --- .../tuning/roadrunner/LocalizationTest.kt | 41 --- .../roadrunner/ManualFeedforwardTuner.kt | 131 -------- .../tuning/roadrunner/MaxAngularVeloTuner.kt | 70 ---- .../tuning/roadrunner/MaxVelocityTuner.kt | 87 ----- .../roadrunner/MotorDirectionDebugger.kt | 79 ----- .../opModes/tuning/roadrunner/SplineTest.kt | 38 --- .../opModes/tuning/roadrunner/StrafeTest.kt | 47 --- .../opModes/tuning/roadrunner/StraightTest.kt | 41 --- .../tuning/roadrunner/TrackWidthTuner.kt | 81 ----- .../TrackingWheelForwardOffsetTuner.kt | 99 ------ .../TrackingWheelLateralDistanceTuner.kt | 134 -------- .../opModes/tuning/roadrunner/TurnTest.kt | 28 -- .../subsystems/drive/DriveSubsystem.kt | 304 +---------------- 67 files changed, 2342 insertions(+), 2477 deletions(-) create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/BlueHighBasketLeft.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/right/BlueHighBasketRight.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/chamber/left/BlueHighChamberLeft.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/chamber/right/BlueHighChamberRight.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/left/BlueParkObservationLeft.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/left/BlueParkSubmmersibleLeft.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/right/BlueParkObservationRight.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/right/BlueParkSubmmersibleRight.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/basket/left/RedHighBasketLeft.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/basket/right/RedHighBasketRight.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/chamber/left/RedHighChamberLeft.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/chamber/right/RedHighChamberRight.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/left/RedParkObservationLeft.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/left/RedParkSubmmersibleLeft.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/right/RedParkObservationRight.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/right/RedParkSubmmersibleRight.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/drive/DriveSubsystem.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/AutomaticFeedforwardTuner.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/BackAndForth.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/DriveVelocityPIDTuner.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/FollowerPIDTuner.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/LocalizationTest.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/ManualFeedforwardTuner.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MaxAngularVeloTuner.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MaxVelocityTuner.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MotorDirectionDebugger.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/SplineTest.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/StrafeTest.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/StraightTest.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackWidthTuner.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackingWheelForwardOffsetTuner.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackingWheelLateralDistanceTuner.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TurnTest.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/right/BlueHighBasketRight.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/right/BlueHighChamberRight.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/left/BlueParkObservationLeft.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/left/BlueParkSubmmersibleLeft.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/right/BlueParkObservationRight.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/right/BlueParkSubmmersibleRight.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/left/RedHighBasketLeft.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/right/RedHighBasketRight.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/left/RedHighChamberLeft.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/right/RedHighChamberRight.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/left/RedParkObservationLeft.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/left/RedParkSubmmersibleLeft.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/right/RedParkObservationRight.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/right/RedParkSubmmersibleRight.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/AutomaticFeedforwardTuner.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/BackAndForth.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/DriveVelocityPIDTuner.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/FollowerPIDTuner.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/LocalizationTest.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/ManualFeedforwardTuner.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxAngularVeloTuner.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MotorDirectionDebugger.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/SplineTest.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StrafeTest.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StraightTest.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackWidthTuner.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelForwardOffsetTuner.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelLateralDistanceTuner.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TurnTest.kt diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/BlueHighBasketLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/BlueHighBasketLeft.kt new file mode 100644 index 0000000..36b9c68 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/left/BlueHighBasketLeft.kt @@ -0,0 +1,76 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.blue.basket.left +// +//import com.arcrobotics.ftclib.hardware.motors.CRServo +//import com.arcrobotics.ftclib.hardware.motors.Motor +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous +//import com.qualcomm.robotcore.eventloop.opmode.OpMode +//import org.firstinspires.ftc.teamcode.constants.AutoStartPose +//import org.firstinspires.ftc.teamcode.constants.ControlBoard +//import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem +//import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem +//import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem +//import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem +// +//@Autonomous(name = "Blue High Basket Left", group = "Basket", preselectTeleOp = "MainTeleOp") +//class BlueHighBasketLeft: OpMode() { +// private lateinit var armLeft: Motor +// private lateinit var armRight: Motor +// private lateinit var elevatorLeft: Motor +// private lateinit var elevatorRight: Motor +// +// private lateinit var intake: CRServo +// +// private lateinit var driveSubsystem: DriveSubsystem +// private lateinit var intakeSubsystem: IntakeSubsystem +// private lateinit var armSubsystem: ArmSubsystem +// private lateinit var elevatorSubsystem: SlidesSubsytem +// +// override fun init() { +//// intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) +// +// armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) +// armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) +// +// elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) +// elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) +// +// driveSubsystem = DriveSubsystem(hardwareMap) +//// intakeSubsystem = IntakeSubsystem(intake) +// armSubsystem = ArmSubsystem(armRight, armLeft) +// elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) +// .turn(Math.toRadians(90.0)) +// .forward(60.0) +// .addTemporalMarker(3.0) { +// armSubsystem.setpoint = Math.toRadians(95.0) +// } +// .waitSeconds(1.0) +// .addTemporalMarker(6.0) { +// elevatorSubsystem.setpoint = 250.0 +// } +// .waitSeconds(2.0) +//// .addTemporalMarker(5.0){ +//// intakeSubsystem.outtake() +//// } +// .waitSeconds(1.0) +// .addTemporalMarker(10.0) { +// elevatorSubsystem.setpoint = 0.0 +// } +// .waitSeconds(2.0) +// .addTemporalMarker(13.0) { +// armSubsystem.setpoint = Math.toRadians(0.0) +// } +// .build() +// +// driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// } +// +// override fun loop() { +// driveSubsystem.update() +// +// elevatorSubsystem.periodic() +// armSubsystem.periodic() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/right/BlueHighBasketRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/right/BlueHighBasketRight.kt new file mode 100644 index 0000000..e9a86fe --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/basket/right/BlueHighBasketRight.kt @@ -0,0 +1,76 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.blue.basket.right +// +//import com.arcrobotics.ftclib.hardware.motors.CRServo +//import com.arcrobotics.ftclib.hardware.motors.Motor +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous +//import com.qualcomm.robotcore.eventloop.opmode.OpMode +//import org.firstinspires.ftc.teamcode.constants.AutoStartPose +//import org.firstinspires.ftc.teamcode.constants.ControlBoard +//import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem +//import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem +//import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem +//import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem +// +// +//@Autonomous(name = "Blue High Basket Right", group = "Basket", preselectTeleOp = "MainTeleOp") +//class BlueHighBasketRight() : OpMode() { +// +// private lateinit var armLeft: Motor +// private lateinit var armRight: Motor +// private lateinit var elevatorLeft: Motor +// private lateinit var elevatorRight: Motor +// +// private lateinit var intake: CRServo +// +// private lateinit var driveSubsystem: DriveSubsystem +// private lateinit var intakeSubsystem: IntakeSubsystem +// private lateinit var armSubsystem: ArmSubsystem +// private lateinit var elevatorSubsystem: SlidesSubsytem +// +// override fun init() { +// intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) +// +// armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) +// armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) +// +// elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) +// elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) +// +// driveSubsystem = DriveSubsystem(hardwareMap) +// intakeSubsystem = IntakeSubsystem(intake) +// armSubsystem = ArmSubsystem(armRight, armLeft) +// elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose) +// .turn(Math.toRadians(90.0)) +// .forward(35.0) +// .addTemporalMarker(2.0) { +// armSubsystem.setpoint = Math.toRadians(150.0) +// } +// .waitSeconds(1.0) +// .addTemporalMarker(3.0) { +// elevatorSubsystem.setpoint = 2.0 +// } +// .waitSeconds(2.0) +// .addTemporalMarker(5.0){ +// intakeSubsystem.outtake() +// } +// .waitSeconds(1.0) +// .addTemporalMarker(6.0) { +// elevatorSubsystem.setpoint = 0.0 +// } +// .waitSeconds(2.0) +// .addTemporalMarker(8.0) { +// armSubsystem.setpoint = Math.toRadians(0.0) +// } +// .build() +// driveSubsystem.poseEstimate = AutoStartPose.BLUE_RIGHT.startPose +// +// +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// } +// +// override fun loop() { +// driveSubsystem.update() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/chamber/left/BlueHighChamberLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/chamber/left/BlueHighChamberLeft.kt new file mode 100644 index 0000000..491ebbe --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/chamber/left/BlueHighChamberLeft.kt @@ -0,0 +1,76 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.blue.chamber.left +// +//import com.arcrobotics.ftclib.hardware.motors.CRServo +//import com.arcrobotics.ftclib.hardware.motors.Motor +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous +//import com.qualcomm.robotcore.eventloop.opmode.OpMode +//import org.firstinspires.ftc.teamcode.constants.AutoStartPose +//import org.firstinspires.ftc.teamcode.constants.ControlBoard +//import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem +//import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem +//import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem +//import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem +// +//@Autonomous(name = "Blue High Chamber Left", group = "Chamber", preselectTeleOp = "MainTeleOp") +//class BlueHighChamberLeft() : OpMode() { +// private lateinit var armLeft: Motor +// private lateinit var armRight: Motor +// private lateinit var elevatorLeft: Motor +// private lateinit var elevatorRight: Motor +// +// private lateinit var intake: CRServo +// +// private lateinit var driveSubsystem: DriveSubsystem +// private lateinit var intakeSubsystem: IntakeSubsystem +// private lateinit var armSubsystem: ArmSubsystem +// private lateinit var elevatorSubsystem: SlidesSubsytem +// +// override fun init() { +// intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) +// +// armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) +// armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) +// +// elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) +// elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) +// +// driveSubsystem = DriveSubsystem(hardwareMap) +// intakeSubsystem = IntakeSubsystem(intake) +// armSubsystem = ArmSubsystem(armRight, armLeft) +// elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) +// +// .forward(28.0) +// .strafeLeft(11.0) +// .addTemporalMarker(2.0) { +// armSubsystem.setpoint = Math.toRadians(150.0) +// } +// .waitSeconds(1.0) +// .addTemporalMarker(3.0) { +// elevatorSubsystem.setpoint = 2.0 +// } +// .waitSeconds(2.0) +// .addTemporalMarker(5.0) { +// intakeSubsystem.outtake() +// } +// .waitSeconds(1.0) +// .addTemporalMarker(6.0) { +// elevatorSubsystem.setpoint = 0.0 +// } +// .waitSeconds(2.0) +// .addTemporalMarker(8.0) { +// armSubsystem.setpoint = Math.toRadians(0.0) +// } +// .build() +// +// driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose +// +// +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// } +// +// override fun loop() { +// driveSubsystem.update() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/chamber/right/BlueHighChamberRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/chamber/right/BlueHighChamberRight.kt new file mode 100644 index 0000000..e828d12 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/chamber/right/BlueHighChamberRight.kt @@ -0,0 +1,75 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.blue.chamber.right + +//import com.arcrobotics.ftclib.hardware.motors.CRServo +//import com.arcrobotics.ftclib.hardware.motors.Motor +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous +//import com.qualcomm.robotcore.eventloop.opmode.OpMode +//import org.firstinspires.ftc.teamcode.constants.AutoStartPose +//import org.firstinspires.ftc.teamcode.constants.ControlBoard +//import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem +//import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem +//import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem +//import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem +// +//@Autonomous(name = "Blue High Chamber Right", group = "Chamber", preselectTeleOp = "MainTeleOp") +//class BlueHighChamberRight() : OpMode() { +// private lateinit var armLeft: Motor +// private lateinit var armRight: Motor +// private lateinit var elevatorLeft: Motor +// private lateinit var elevatorRight: Motor +// +// private lateinit var intake: CRServo +// +// private lateinit var driveSubsystem: DriveSubsystem +// private lateinit var intakeSubsystem: IntakeSubsystem +// private lateinit var armSubsystem: ArmSubsystem +// private lateinit var elevatorSubsystem: SlidesSubsytem +// +// override fun init() { +// intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) +// +// armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) +// armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) +// +// elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) +// elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) +// +// driveSubsystem = DriveSubsystem(hardwareMap) +// intakeSubsystem = IntakeSubsystem(intake) +// armSubsystem = ArmSubsystem(armRight, armLeft) +// elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose) +// +// .forward(28.0) +// .strafeRight(11.0) +// .addTemporalMarker(2.0) { +// armSubsystem.setpoint = Math.toRadians(150.0) +// } +// .waitSeconds(1.0) +// .addTemporalMarker(3.0) { +// elevatorSubsystem.setpoint = 2.0 +// } +// .waitSeconds(2.0) +// .addTemporalMarker(5.0) { +// intakeSubsystem.outtake() +// } +// .waitSeconds(1.0) +// .addTemporalMarker(6.0) { +// elevatorSubsystem.setpoint = 0.0 +// } +// .waitSeconds(2.0) +// .addTemporalMarker(8.0) { +// armSubsystem.setpoint = Math.toRadians(0.0) +// } +// .build() +// driveSubsystem.poseEstimate = AutoStartPose.BLUE_RIGHT.startPose +// +// +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// } +// +// override fun loop() { +// driveSubsystem.update() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/left/BlueParkObservationLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/left/BlueParkObservationLeft.kt new file mode 100644 index 0000000..5cecee8 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/left/BlueParkObservationLeft.kt @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.blue.park.left +// +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous +//import com.qualcomm.robotcore.eventloop.opmode.OpMode +//import org.firstinspires.ftc.teamcode.constants.AutoStartPose +//import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem +// +//@Autonomous(name = "Blue Park (Observation) Left", group = "Observation", preselectTeleOp = "MainTeleOp") +//class BlueParkObservationLeft : OpMode() { +// private lateinit var driveSubsystem: DriveSubsystem +// +// override fun init() { +// driveSubsystem = DriveSubsystem(hardwareMap) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) +// .strafeRight(48.0) +// .build() +// driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose +// +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// } +// +// override fun loop() { +// driveSubsystem.update() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/left/BlueParkSubmmersibleLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/left/BlueParkSubmmersibleLeft.kt new file mode 100644 index 0000000..050bd25 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/left/BlueParkSubmmersibleLeft.kt @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.blue.park.left + +//@Autonomous(name = "Blue Park (Submmersible) Left", group = "Submmersible", preselectTeleOp = "MainTeleOp") +//class BlueParkSubmmersibleLeft: OpMode() { +// +// private lateinit var driveSubsystem: DriveSubsystem +// +// override fun init() { +// driveSubsystem = DriveSubsystem(hardwareMap) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) +// .forward(25.0) +// .strafeLeft(48.0) +// .forward(25.0) +// .strafeRight(12.0) +// .build() +// driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose +// +// +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// } +// +// override fun loop() { +// driveSubsystem.update() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/right/BlueParkObservationRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/right/BlueParkObservationRight.kt new file mode 100644 index 0000000..65b9a87 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/right/BlueParkObservationRight.kt @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.blue.park.right + +//@Autonomous(name = "Blue Park (Observation) Right", group = "Observation", preselectTeleOp = "MainTeleOp") +//class BlueParkObservationRight: OpMode() { +// private lateinit var driveSubsystem: DriveSubsystem +// +// override fun init() { +// driveSubsystem = DriveSubsystem(hardwareMap) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose) +// .strafeRight(72.0) +// .build() +// driveSubsystem.poseEstimate = AutoStartPose.BLUE_RIGHT.startPose +// +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// } +// +// override fun loop() { +// driveSubsystem.update() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/right/BlueParkSubmmersibleRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/right/BlueParkSubmmersibleRight.kt new file mode 100644 index 0000000..c61c7dd --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/blue/park/right/BlueParkSubmmersibleRight.kt @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.blue.park.right + +//@Autonomous(name = "Blue Park (Submmersible) Right", group = "Submmersible", preselectTeleOp = "MainTeleOp") +//class BlueParkSubmmersibleRight: OpMode() { +// +// private lateinit var driveSubsystem: DriveSubsystem +// +// override fun init() { +// driveSubsystem = DriveSubsystem(hardwareMap) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose) +// .forward(27.0) +// .strafeLeft(24.0) +// .forward(24.0) +// .strafeRight(12.0) +// .build() +// driveSubsystem.poseEstimate = AutoStartPose.BLUE_RIGHT.startPose +// +// +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// +// } +// +// override fun loop() { +// driveSubsystem.update() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/basket/left/RedHighBasketLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/basket/left/RedHighBasketLeft.kt new file mode 100644 index 0000000..16aa4b7 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/basket/left/RedHighBasketLeft.kt @@ -0,0 +1,64 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.red.basket.left + +//@Autonomous(name = "Red High Basket Left", group = "Basket", preselectTeleOp = "MainTeleOp") +//class RedHighBasketLeft() : OpMode() { +// +// private lateinit var armLeft: Motor +// private lateinit var armRight: Motor +// private lateinit var elevatorLeft: Motor +// private lateinit var elevatorRight: Motor +// +// private lateinit var intake: CRServo +// +// private lateinit var driveSubsystem: DriveSubsystem +// private lateinit var intakeSubsystem: IntakeSubsystem +// private lateinit var armSubsystem: ArmSubsystem +// private lateinit var elevatorSubsystem: SlidesSubsytem +// +// override fun init() { +// intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) +// +// armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) +// armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) +// +// elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) +// elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) +// +// driveSubsystem = DriveSubsystem(hardwareMap) +// intakeSubsystem = IntakeSubsystem(intake) +// armSubsystem = ArmSubsystem(armRight, armLeft) +// elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose) +// .turn(Math.toRadians(90.0)) +// .forward(35.0) +// .addTemporalMarker(2.0) { +// armSubsystem.setpoint = Math.toRadians(150.0) +// } +// .waitSeconds(1.0) +// .addTemporalMarker(3.0) { +// elevatorSubsystem.setpoint = 2.0 +// } +// .waitSeconds(2.0) +// .addTemporalMarker(5.0){ +// intakeSubsystem.outtake() +// } +// .waitSeconds(1.0) +// .addTemporalMarker(6.0) { +// elevatorSubsystem.setpoint = 0.0 +// } +// .waitSeconds(2.0) +// .addTemporalMarker(8.0) { +// armSubsystem.setpoint = Math.toRadians(0.0) +// } +// .build() +// driveSubsystem.poseEstimate = AutoStartPose.RED_LEFT.startPose +// +// +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// } +// +// override fun loop() { +// driveSubsystem.update() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/basket/right/RedHighBasketRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/basket/right/RedHighBasketRight.kt new file mode 100644 index 0000000..591026f --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/basket/right/RedHighBasketRight.kt @@ -0,0 +1,63 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.red.basket.right + +//@Autonomous(name = "Red High Basket Right", group = "Basket", preselectTeleOp = "MainTeleOp") +//class RedHighBasketRight : OpMode() { +// private lateinit var armLeft: Motor +// private lateinit var armRight: Motor +// private lateinit var elevatorLeft: Motor +// private lateinit var elevatorRight: Motor +// +// private lateinit var intake: CRServo +// +// private lateinit var driveSubsystem: DriveSubsystem +// private lateinit var intakeSubsystem: IntakeSubsystem +// private lateinit var armSubsystem: ArmSubsystem +// private lateinit var elevatorSubsystem: SlidesSubsytem +// +// override fun init() { +// intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) +// +// armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) +// armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) +// +// elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) +// elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) +// +// driveSubsystem = DriveSubsystem(hardwareMap) +// intakeSubsystem = IntakeSubsystem(intake) +// armSubsystem = ArmSubsystem(armRight, armLeft) +// elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose) +// .turn(Math.toRadians(90.0)) +// .forward(60.0) +// .addTemporalMarker(2.0) { +// armSubsystem.setpoint = Math.toRadians(150.0) +// } +// .waitSeconds(1.0) +// .addTemporalMarker(3.0) { +// elevatorSubsystem.setpoint = 2.0 +// } +// .waitSeconds(2.0) +// .addTemporalMarker(5.0){ +// intakeSubsystem.outtake() +// } +// .waitSeconds(1.0) +// .addTemporalMarker(6.0) { +// elevatorSubsystem.setpoint = 0.0 +// } +// .waitSeconds(2.0) +// .addTemporalMarker(8.0) { +// armSubsystem.setpoint = Math.toRadians(0.0) +// } +// .build() +// driveSubsystem.poseEstimate = AutoStartPose.RED_RIGHT.startPose +// +// +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// } +// +// override fun loop() { +// driveSubsystem.update() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/chamber/left/RedHighChamberLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/chamber/left/RedHighChamberLeft.kt new file mode 100644 index 0000000..6a13429 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/chamber/left/RedHighChamberLeft.kt @@ -0,0 +1,64 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.red.chamber.left + +//@Autonomous(name = "Red High Chamber Left", group = "Chamber", preselectTeleOp = "MainTeleOp") +//class RedHighChamberLeft() : OpMode() { +// private lateinit var armLeft: Motor +// private lateinit var armRight: Motor +// private lateinit var elevatorLeft: Motor +// private lateinit var elevatorRight: Motor +// +// private lateinit var intake: CRServo +// +// private lateinit var driveSubsystem: DriveSubsystem +// private lateinit var intakeSubsystem: IntakeSubsystem +// private lateinit var armSubsystem: ArmSubsystem +// private lateinit var elevatorSubsystem: SlidesSubsytem +// +// override fun init() { +// intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) +// +// armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) +// armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) +// +// elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) +// elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) +// +// driveSubsystem = DriveSubsystem(hardwareMap) +// intakeSubsystem = IntakeSubsystem(intake) +// armSubsystem = ArmSubsystem(armRight, armLeft) +// elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose) +// +// .forward(28.0) +// .strafeRight(11.0) +// .addTemporalMarker(2.0) { +// armSubsystem.setpoint = Math.toRadians(150.0) +// } +// .waitSeconds(1.0) +// .addTemporalMarker(3.0) { +// elevatorSubsystem.setpoint = 2.0 +// } +// .waitSeconds(2.0) +// .addTemporalMarker(5.0) { +// intakeSubsystem.outtake() +// } +// .waitSeconds(1.0) +// .addTemporalMarker(6.0) { +// elevatorSubsystem.setpoint = 0.0 +// } +// .waitSeconds(2.0) +// .addTemporalMarker(8.0) { +// armSubsystem.setpoint = Math.toRadians(0.0) +// } +// .build() +// driveSubsystem.poseEstimate = AutoStartPose.RED_LEFT.startPose +// +// +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// } +// +// override fun loop() { +// driveSubsystem.update() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/chamber/right/RedHighChamberRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/chamber/right/RedHighChamberRight.kt new file mode 100644 index 0000000..72f2e7b --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/chamber/right/RedHighChamberRight.kt @@ -0,0 +1,64 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.red.chamber.right + +//@Autonomous(name = "Red High Chamber Right", group = "Chamber", preselectTeleOp = "MainTeleOp") +//class RedHighChamberRight : OpMode() { +// private lateinit var armLeft: Motor +// private lateinit var armRight: Motor +// private lateinit var elevatorLeft: Motor +// private lateinit var elevatorRight: Motor +// +// private lateinit var intake: CRServo +// +// private lateinit var driveSubsystem: DriveSubsystem +// private lateinit var intakeSubsystem: IntakeSubsystem +// private lateinit var armSubsystem: ArmSubsystem +// private lateinit var elevatorSubsystem: SlidesSubsytem +// +// override fun init() { +// intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) +// +// armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) +// armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) +// +// elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) +// elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) +// +// driveSubsystem = DriveSubsystem(hardwareMap) +// intakeSubsystem = IntakeSubsystem(intake) +// armSubsystem = ArmSubsystem(armRight, armLeft) +// elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose) +// +// .forward(28.0) +// .strafeLeft(11.0) +// .addTemporalMarker(2.0) { +// armSubsystem.setpoint = Math.toRadians(150.0) +// } +// .waitSeconds(1.0) +// .addTemporalMarker(3.0) { +// elevatorSubsystem.setpoint = 2.0 +// } +// .waitSeconds(2.0) +// .addTemporalMarker(5.0) { +// intakeSubsystem.outtake() +// } +// .waitSeconds(1.0) +// .addTemporalMarker(6.0) { +// elevatorSubsystem.setpoint = 0.0 +// } +// .waitSeconds(2.0) +// .addTemporalMarker(8.0) { +// armSubsystem.setpoint = Math.toRadians(0.0) +// } +// .build() +// driveSubsystem.poseEstimate = AutoStartPose.RED_RIGHT.startPose +// +// +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// } +// +// override fun loop() { +// driveSubsystem.update() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/left/RedParkObservationLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/left/RedParkObservationLeft.kt new file mode 100644 index 0000000..9cfc484 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/left/RedParkObservationLeft.kt @@ -0,0 +1,23 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.red.park.left + +//@Autonomous(name = "Red Park (Observation) Left", group = "Observation", preselectTeleOp = "MainTeleOp") +//class RedParkObservationLeft: OpMode() { +// +// private lateinit var driveSubsystem: DriveSubsystem +// +// override fun init() { +// driveSubsystem = DriveSubsystem(hardwareMap) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose) +// .strafeRight(72.0) +// .build() +// driveSubsystem.poseEstimate = AutoStartPose.RED_LEFT.startPose +// +// +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// } +// +// override fun loop() { +// driveSubsystem.update() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/left/RedParkSubmmersibleLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/left/RedParkSubmmersibleLeft.kt new file mode 100644 index 0000000..65e3312 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/left/RedParkSubmmersibleLeft.kt @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.red.park.left + +//@Autonomous(name = "Red Park (Submmersible) Left", group = "Submmersible", preselectTeleOp = "MainTeleOp") +//class RedParkSubmmersibleLeft: OpMode() { +// +// private lateinit var driveSubsystem: DriveSubsystem +// +// override fun init() { +// driveSubsystem = DriveSubsystem(hardwareMap) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose) +// .forward(27.0) +// .strafeLeft(24.0) +// .forward(24.0) +// .strafeRight(12.0) +// .build() +// driveSubsystem.poseEstimate = AutoStartPose.RED_LEFT.startPose +// +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// } +// +// override fun loop() { +// driveSubsystem.update() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/right/RedParkObservationRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/right/RedParkObservationRight.kt new file mode 100644 index 0000000..cf88282 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/right/RedParkObservationRight.kt @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.red.park.right + +//@Autonomous(name = "Red Park (Observation) Right", group = "Observation", preselectTeleOp = "MainTeleOp") +//class RedParkObservationRight: OpMode() { +// +// private lateinit var driveSubsystem: DriveSubsystem +// +// override fun init() { +// driveSubsystem = DriveSubsystem(hardwareMap) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose) +// .strafeRight(48.0) +// .build() +// driveSubsystem.poseEstimate = AutoStartPose.RED_RIGHT.startPose +// +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// } +// +// override fun loop() { +// driveSubsystem.update() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/right/RedParkSubmmersibleRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/right/RedParkSubmmersibleRight.kt new file mode 100644 index 0000000..a08ad7e --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/auto/red/park/right/RedParkSubmmersibleRight.kt @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.teamcode.legacy.auto.red.park.right + +//@Autonomous(name = "Red Park (Submmersible) Right", group = "Submmersible", preselectTeleOp = "MainTeleOp") +//class RedParkSubmmersibleRight: OpMode() { +// +// private lateinit var driveSubsystem: DriveSubsystem +// +// override fun init() { +// driveSubsystem = DriveSubsystem(hardwareMap) +// +// val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose) +// .forward(27.0) +// .strafeLeft(48.0) +// .forward(24.0) +// .strafeRight(12.0) +// .build() +// driveSubsystem.poseEstimate = AutoStartPose.RED_RIGHT.startPose +// +// +// driveSubsystem.followTrajectorySequenceAsync(trajectory) +// } +// +// override fun loop() { +// driveSubsystem.update() +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/drive/DriveSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/drive/DriveSubsystem.kt new file mode 100644 index 0000000..eed777a --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/drive/DriveSubsystem.kt @@ -0,0 +1,306 @@ +package org.firstinspires.ftc.teamcode.legacy.drive +// +//import com.acmerobotics.dashboard.config.Config +//import com.acmerobotics.roadrunner.control.PIDCoefficients +//import com.acmerobotics.roadrunner.drive.MecanumDrive +//import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower +//import com.acmerobotics.roadrunner.followers.TrajectoryFollower +//import com.acmerobotics.roadrunner.geometry.Pose2d +//import com.acmerobotics.roadrunner.geometry.Vector2d +//import com.acmerobotics.roadrunner.trajectory.Trajectory +//import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder +//import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint +//import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint +//import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint +//import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint +//import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint +//import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint +//import com.acmerobotics.roadrunner.util.Angle +//import com.arcrobotics.ftclib.command.CommandScheduler +//import com.arcrobotics.ftclib.command.Subsystem +//import com.qualcomm.hardware.lynx.LynxModule +//import com.qualcomm.robotcore.hardware.DcMotor +//import com.qualcomm.robotcore.hardware.DcMotorEx +//import com.qualcomm.robotcore.hardware.DcMotorSimple +//import com.qualcomm.robotcore.hardware.HardwareMap +//import com.qualcomm.robotcore.hardware.PIDFCoefficients +//import com.qualcomm.robotcore.hardware.VoltageSensor +//import org.firstinspires.ftc.teamcode.constants.ControlBoard +//import org.firstinspires.ftc.teamcode.constants.PoseStorage +//import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants +//import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.StandardTrackingWheelLocalizer +//import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.TrajectorySequence +//import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.TrajectorySequenceBuilder +//import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.TrajectorySequenceRunner +//import org.firstinspires.ftc.teamcode.utils.roadrunner.util.LynxModuleUtil +//import kotlin.math.PI +//import kotlin.math.abs +// +///* +// * Simple mecanum drive hardware implementation for REV hardware. +// */ +//@Config +//class DriveSubsystem @JvmOverloads constructor( +// hardwareMap: HardwareMap, +// private val fieldCentric: Boolean = false, +//) : MecanumDrive( +// DriveConstants.kV, +// DriveConstants.kA, +// DriveConstants.kStatic, +// DriveConstants.TRACK_WIDTH, +// DriveConstants.TRACK_WIDTH, +// LATERAL_MULTIPLIER +//), Subsystem { +// private val trajectorySequenceRunner: TrajectorySequenceRunner +// private val leftFront: DcMotorEx +// private val leftRear: DcMotorEx +// private val rightRear: DcMotorEx +// private val rightFront: DcMotorEx +// private val motors: List +// private val batteryVoltageSensor: VoltageSensor +// +// var m_name = this.javaClass.simpleName +// +// init { +// val follower: TrajectoryFollower = HolonomicPIDVAFollower( +// TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID, +// Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5 +// ) +// +// LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap) +// +// batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next() +// +// for (module in hardwareMap.getAll(LynxModule::class.java)) { +// module.bulkCachingMode = LynxModule.BulkCachingMode.AUTO +// } +// +// leftFront = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_LEFT_FRONT.deviceName) +// leftRear = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_LEFT_REAR.deviceName) +// rightRear = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_RIGHT_REAR.deviceName) +// rightFront = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_RIGHT_FRONT.deviceName) +// motors = listOf(leftFront, leftRear, rightRear, rightFront) +// for (motor in motors) { +// val motorConfigurationType = motor.motorType.clone() +// motorConfigurationType.achieveableMaxRPMFraction = 1.0 +// motor.motorType = motorConfigurationType +// } +// +// if (DriveConstants.RUN_USING_ENCODER) { +// setMode(DcMotor.RunMode.RUN_USING_ENCODER) +// } +// +// setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE) +// +// if (DriveConstants.RUN_USING_ENCODER) { +// setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID) +// } +// +// // leftFront.direction = DcMotorSimple.Direction.REVERSE +// leftRear.direction = DcMotorSimple.Direction.REVERSE +// +// localizer = StandardTrackingWheelLocalizer(hardwareMap) +// +// trajectorySequenceRunner = TrajectorySequenceRunner(follower, HEADING_PID) +// +// CommandScheduler.getInstance().registerSubsystem(this) +// } +// +// @JvmOverloads +// fun trajectoryBuilder( +// startPose: Pose2d, +// reversed: Boolean = false, +// startHeading: Double = startPose.heading, +// ): TrajectoryBuilder { +// return TrajectoryBuilder( +// startPose, +// Angle.norm(startHeading + if (reversed) PI else 0.0), +// VEL_CONSTRAINT, +// ACCEL_CONSTRAINT +// ) +// } +// +// fun drive(leftY: Double, leftX: Double, rightX: Double) { +// val (_, _, heading) = poseEstimate +// +// val (x, y) = Vector2d( +// -leftY, +// -leftX +// ).rotated( +// if (fieldCentric) -heading else 0.0 +// ) +// +// setWeightedDrivePower(Pose2d(x, y, -rightX)) +// +// update() +// PoseStorage.poseEstimate = poseEstimate +// } +// +// fun trajectorySequenceBuilder(startPose: Pose2d): TrajectorySequenceBuilder { +// return TrajectorySequenceBuilder( +// startPose, +// VEL_CONSTRAINT, ACCEL_CONSTRAINT, +// DriveConstants.MAX_ANG_VEL, DriveConstants.MAX_ANG_ACCEL +// ) +// } +// +// fun turnAsync(angle: Double) { +// trajectorySequenceRunner.followTrajectorySequenceAsync( +// trajectorySequenceBuilder(poseEstimate) +// .turn(angle) +// .build() +// ) +// } +// +// fun turn(angle: Double) { +// turnAsync(angle) +// waitForIdle() +// } +// +// fun followTrajectoryAsync(trajectory: Trajectory) { +// trajectorySequenceRunner.followTrajectorySequenceAsync( +// trajectorySequenceBuilder(trajectory.start()) +// .addTrajectory(trajectory) +// .build() +// ) +// } +// +// fun followTrajectory(trajectory: Trajectory) { +// followTrajectoryAsync(trajectory) +// waitForIdle() +// } +// +// fun followTrajectorySequenceAsync(trajectorySequence: TrajectorySequence) { +// trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence) +// } +// +// fun followTrajectorySequence(trajectorySequence: TrajectorySequence) { +// followTrajectorySequenceAsync(trajectorySequence) +// waitForIdle() +// } +// +// @Suppress("unused", "MemberVisibilityCanBePrivate") +// val lastError: Pose2d +// get() = trajectorySequenceRunner.lastPoseError +// +// fun update() { +// updatePoseEstimate() +// val signal = trajectorySequenceRunner.update(poseEstimate, poseVelocity) +// signal?.let { setDriveSignal(it) } +// } +// +// fun waitForIdle() { +// while (!Thread.currentThread().isInterrupted && isBusy) update() +// } +// +// val isBusy: Boolean +// get() = trajectorySequenceRunner.isBusy +// +// fun setMode(runMode: DcMotor.RunMode?) { +// for (motor in motors) { +// motor.mode = runMode +// } +// } +// +// fun setZeroPowerBehavior(zeroPowerBehavior: DcMotor.ZeroPowerBehavior) { +// for (motor in motors) { +// motor.zeroPowerBehavior = zeroPowerBehavior +// } +// } +// +// fun setPIDFCoefficients(runMode: DcMotor.RunMode?, coefficients: PIDFCoefficients) { +// val compensatedCoefficients = PIDFCoefficients( +// coefficients.p, coefficients.i, coefficients.d, +// coefficients.f * 12 / batteryVoltageSensor.voltage +// ) +// for (motor in motors) { +// motor.setPIDFCoefficients(runMode, compensatedCoefficients) +// } +// } +// +// fun setWeightedDrivePower(drivePower: Pose2d) { +// var vel = drivePower +// if ((abs(drivePower.x) + abs(drivePower.y) +// + abs(drivePower.heading)) > 1 +// ) { +// // re-normalize the powers according to the weights +// val denom = +// VX_WEIGHT * abs(drivePower.x) + VY_WEIGHT * abs(drivePower.y) + OMEGA_WEIGHT * abs( +// drivePower.heading +// ) +// vel = Pose2d( +// VX_WEIGHT * drivePower.x, +// VY_WEIGHT * drivePower.y, +// OMEGA_WEIGHT * drivePower.heading +// ).div(denom) +// } +// setDrivePower(vel) +// } +// +// override fun getWheelPositions(): List { +// val wheelPositions: MutableList = ArrayList() +// for (motor in motors) { +// wheelPositions.add(DriveConstants.encoderTicksToInches(motor.currentPosition.toDouble())) +// } +// return wheelPositions +// } +// +// @Suppress("unused") +// override fun getWheelVelocities(): List { +// val wheelVelocities: MutableList = ArrayList() +// for (motor in motors) { +// wheelVelocities.add(DriveConstants.encoderTicksToInches(motor.velocity)) +// } +// return wheelVelocities +// } +// +// override fun setMotorPowers( +// frontLeft: Double, +// rearLeft: Double, +// rearRight: Double, +// frontRight: Double, +// ) { +// leftFront.power = frontLeft +// leftRear.power = rearLeft +// rightRear.power = rearRight +// rightFront.power = frontRight +// } +// +// override val rawExternalHeading: Double +// get() = 0.0 +// +// override fun getExternalHeadingVelocity() = 0.0 +// +// companion object { +// @JvmField +// var TRANSLATIONAL_PID = PIDCoefficients(10.0, 0.0, 0.01) +// @JvmField +// var HEADING_PID = PIDCoefficients(10.0, 0.0, 0.01) +// var LATERAL_MULTIPLIER = 1.0179744056670780318264660644901 +// var VX_WEIGHT = 1.0 +// var VY_WEIGHT = 1.0 +// var OMEGA_WEIGHT = 1.0 +// private val VEL_CONSTRAINT = getVelocityConstraint( +// DriveConstants.MAX_VEL, +// DriveConstants.MAX_ANG_VEL, +// DriveConstants.TRACK_WIDTH +// ) +// private val ACCEL_CONSTRAINT = getAccelerationConstraint(DriveConstants.MAX_ACCEL) +// fun getVelocityConstraint( +// maxVel: Double, +// maxAngularVel: Double, +// trackWidth: Double, +// ): TrajectoryVelocityConstraint { +// return MinVelocityConstraint( +// listOf( +// AngularVelocityConstraint(maxAngularVel), +// MecanumVelocityConstraint(maxVel, trackWidth) +// ) +// ) +// } +// +// fun getAccelerationConstraint(maxAccel: Double): TrajectoryAccelerationConstraint { +// return ProfileAccelerationConstraint(maxAccel) +// } +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/AutomaticFeedforwardTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/AutomaticFeedforwardTuner.kt new file mode 100644 index 0000000..acfbd7d --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/AutomaticFeedforwardTuner.kt @@ -0,0 +1,210 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner +// +//import com.acmerobotics.dashboard.FtcDashboard +//import com.acmerobotics.dashboard.config.Config +//import com.acmerobotics.dashboard.telemetry.MultipleTelemetry +//import com.acmerobotics.roadrunner.geometry.Pose2d +//import com.acmerobotics.roadrunner.util.NanoClock +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode +//import com.qualcomm.robotcore.util.RobotLog +//import org.firstinspires.ftc.robotcore.internal.system.Misc +//import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem +//import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants +//import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.rpmToVelocity +//import org.firstinspires.ftc.teamcode.utils.roadrunner.util.LoggingUtil +//import org.firstinspires.ftc.teamcode.utils.roadrunner.util.RegressionUtil +// +///* +// * Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an +// * outline of the procedure: +// * 1. Slowly ramp the motor power and record encoder values along the way. +// * 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV) +// * and an optional intercept (kStatic). +// * 3. Accelerate the robot (apply constant power) and record the encoder counts. +// * 4. Adjust the encoder data based on the velocity tuning data and find kA with another linear +// * regression. +// */ +////@Disabled +//@Config +//@Autonomous(group = "drive") +//class AutomaticFeedforwardTuner : LinearOpMode() { +// @Throws(InterruptedException::class) +// override fun runOpMode() { +// if (DriveConstants.RUN_USING_ENCODER) { +// RobotLog.setGlobalErrorMsg( +// "Feedforward constants usually don't need to be tuned " + +// "when using the built-in drive motor velocity PID." +// ) +// } +// telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) +// val drive = DriveSubsystem(hardwareMap) +// val clock = NanoClock.system() +// telemetry.addLine("Press play to begin the feedforward tuning routine") +// telemetry.update() +// waitForStart() +// if (isStopRequested) return +// telemetry.clearAll() +// telemetry.addLine("Would you like to fit kStatic?") +// telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no") +// telemetry.update() +// var fitIntercept = false +// while (!isStopRequested) { +// if (gamepad1.y) { +// fitIntercept = true +// while (!isStopRequested && gamepad1.y) { +// idle() +// } +// break +// } else if (gamepad1.b) { +// while (!isStopRequested && gamepad1.b) { +// idle() +// } +// break +// } +// idle() +// } +// telemetry.clearAll() +// telemetry.addLine( +// Misc.formatInvariant( +// "Place your robot on the field with at least %.2f in of room in front", DISTANCE +// ) +// ) +// telemetry.addLine("Press (Y/Δ) to begin") +// telemetry.update() +// while (!isStopRequested && !gamepad1.y) { +// idle() +// } +// while (!isStopRequested && gamepad1.y) { +// idle() +// } +// telemetry.clearAll() +// telemetry.addLine("Running...") +// telemetry.update() +// val maxVel = rpmToVelocity(DriveConstants.MAX_RPM) +// val finalVel = MAX_POWER * maxVel +// val accel = finalVel * finalVel / (2.0 * DISTANCE) +// val rampTime = Math.sqrt(2.0 * DISTANCE / accel) +// val timeSamples: MutableList = ArrayList() +// val positionSamples: MutableList = ArrayList() +// val powerSamples: MutableList = ArrayList() +// drive.poseEstimate = Pose2d() +// var startTime = clock.seconds() +// while (!isStopRequested) { +// val elapsedTime = clock.seconds() - startTime +// if (elapsedTime > rampTime) { +// break +// } +// val vel = accel * elapsedTime +// val power = vel / maxVel +// timeSamples.add(elapsedTime) +// positionSamples.add(drive.poseEstimate.x) +// powerSamples.add(power) +// drive.setDrivePower(Pose2d(power, 0.0, 0.0)) +// drive.updatePoseEstimate() +// } +// drive.setDrivePower(Pose2d(0.0, 0.0, 0.0)) +// val rampResult = RegressionUtil.fitRampData( +// timeSamples, positionSamples, powerSamples, fitIntercept, +// LoggingUtil.getLogFile( +// Misc.formatInvariant( +// "DriveRampRegression-%d.csv", System.currentTimeMillis() +// ) +// ) +// ) +// telemetry.clearAll() +// telemetry.addLine("Quasi-static ramp up test complete") +// if (fitIntercept) { +// telemetry.addLine( +// Misc.formatInvariant( +// "kV = %.5f, kStatic = %.5f (R^2 = %.2f)", +// rampResult.kV, rampResult.kStatic, rampResult.rSquare +// ) +// ) +// } else { +// telemetry.addLine( +// Misc.formatInvariant( +// "kV = %.5f (R^2 = %.2f)", +// rampResult.kStatic, rampResult.rSquare +// ) +// ) +// } +// telemetry.addLine("Would you like to fit kA?") +// telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no") +// telemetry.update() +// var fitAccelFF = false +// while (!isStopRequested) { +// if (gamepad1.y) { +// fitAccelFF = true +// while (!isStopRequested && gamepad1.y) { +// idle() +// } +// break +// } else if (gamepad1.b) { +// while (!isStopRequested && gamepad1.b) { +// idle() +// } +// break +// } +// idle() +// } +// if (fitAccelFF) { +// telemetry.clearAll() +// telemetry.addLine("Place the robot back in its starting position") +// telemetry.addLine("Press (Y/Δ) to continue") +// telemetry.update() +// while (!isStopRequested && !gamepad1.y) { +// idle() +// } +// while (!isStopRequested && gamepad1.y) { +// idle() +// } +// telemetry.clearAll() +// telemetry.addLine("Running...") +// telemetry.update() +// val maxPowerTime = DISTANCE / maxVel +// timeSamples.clear() +// positionSamples.clear() +// powerSamples.clear() +// drive.poseEstimate = Pose2d() +// drive.setDrivePower(Pose2d(MAX_POWER, 0.0, 0.0)) +// startTime = clock.seconds() +// while (!isStopRequested) { +// val elapsedTime = clock.seconds() - startTime +// if (elapsedTime > maxPowerTime) { +// break +// } +// timeSamples.add(elapsedTime) +// positionSamples.add(drive.poseEstimate.x) +// powerSamples.add(MAX_POWER) +// drive.updatePoseEstimate() +// } +// drive.setDrivePower(Pose2d(0.0, 0.0, 0.0)) +// val accelResult = RegressionUtil.fitAccelData( +// timeSamples, positionSamples, powerSamples, rampResult, +// LoggingUtil.getLogFile( +// Misc.formatInvariant( +// "DriveAccelRegression-%d.csv", System.currentTimeMillis() +// ) +// ) +// ) +// telemetry.clearAll() +// telemetry.addLine("Constant power test complete") +// telemetry.addLine( +// Misc.formatInvariant( +// "kA = %.5f (R^2 = %.2f)", +// accelResult.kA, accelResult.rSquare +// ) +// ) +// telemetry.update() +// } +// while (!isStopRequested) { +// idle() +// } +// } +// +// companion object { +// var MAX_POWER = 0.7 +// var DISTANCE = 100.0 // in +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/BackAndForth.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/BackAndForth.kt new file mode 100644 index 0000000..994a146 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/BackAndForth.kt @@ -0,0 +1,48 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner +// +//import com.acmerobotics.dashboard.config.Config +//import com.acmerobotics.roadrunner.geometry.Pose2d +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode +//import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem +// +///* +// * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base +// * classes). The robot drives back and forth in a straight line indefinitely. Utilization of the +// * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer +// * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're +// * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once +// * you've successfully connected, start the program, and your robot will begin moving forward and +// * backward. You should observe the target position (green) and your pose estimate (blue) and adjust +// * your follower PID coefficients such that you follow the target position as accurately as possible. +// * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. +// * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. +// * These coefficients can be tuned live in dashboard. +// * +// * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It +// * is recommended that you use the FollowerPIDTuner opmode for further fine tuning. +// */ +////@Disabled +//@Config +//@Autonomous(group = "drive") +//class BackAndForth : LinearOpMode() { +// @Throws(InterruptedException::class) +// override fun runOpMode() { +// val drive = DriveSubsystem(hardwareMap) +// val trajectoryForward = drive.trajectoryBuilder(Pose2d()) +// .forward(DISTANCE) +// .build() +// val trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end()) +// .back(DISTANCE) +// .build() +// waitForStart() +// while (opModeIsActive() && !isStopRequested) { +// drive.followTrajectory(trajectoryForward) +// drive.followTrajectory(trajectoryBackward) +// } +// } +// +// companion object { +// var DISTANCE = 50.0 +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/DriveVelocityPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/DriveVelocityPIDTuner.kt new file mode 100644 index 0000000..31bcaaa --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/DriveVelocityPIDTuner.kt @@ -0,0 +1,153 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner +// +//import com.acmerobotics.dashboard.FtcDashboard +//import com.acmerobotics.dashboard.config.Config +//import com.acmerobotics.dashboard.telemetry.MultipleTelemetry +//import com.acmerobotics.roadrunner.geometry.Pose2d +//import com.acmerobotics.roadrunner.profile.MotionProfile +//import com.acmerobotics.roadrunner.profile.MotionProfileGenerator.generateSimpleMotionProfile +//import com.acmerobotics.roadrunner.profile.MotionState +//import com.acmerobotics.roadrunner.util.NanoClock +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode +//import com.qualcomm.robotcore.hardware.DcMotor +//import com.qualcomm.robotcore.util.RobotLog +//import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem +//import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants +// +///* +// * This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed- +// * loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as +// * important as the positional parameters. Like the other manual tuning routines, this op mode +// * relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's +// * WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC +// * phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully +// * connected, start the program, and your robot will begin moving forward and backward according to +// * a motion profile. Your job is to graph the velocity errors over time and adjust the PID +// * coefficients (note: the tuning variable will not appear until the op mode finishes initializing). +// * Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the +// * MOTOR_VELO_PID field. +// * +// * Recommended tuning process: +// * +// * 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to +// * mitigate oscillations. +// * 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached. +// * 3. Back off kP and kD a little until the response is less oscillatory (but without lag). +// * +// * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the +// * user to reset the position of the bot in the event that it drifts off the path. +// * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. +// */ +////@Disabled +//@Config +//@Autonomous(group = "drive") +//class DriveVelocityPIDTuner : LinearOpMode() { +// override fun runOpMode() { +// if (!DriveConstants.RUN_USING_ENCODER) { +// RobotLog.setGlobalErrorMsg( +// "%s does not need to be run if the built-in motor velocity" + +// "PID is not in use", javaClass.simpleName +// ) +// } +// telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) +// val drive = DriveSubsystem(hardwareMap) +// var mode = Mode.TUNING_MODE +// var lastKp = DriveConstants.MOTOR_VELO_PID.p +// var lastKi = DriveConstants.MOTOR_VELO_PID.i +// var lastKd = DriveConstants.MOTOR_VELO_PID.d +// var lastKf = DriveConstants.MOTOR_VELO_PID.f +// drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID) +// val clock = NanoClock.system() +// telemetry.addLine("Ready!") +// telemetry.update() +// telemetry.clearAll() +// waitForStart() +// if (isStopRequested) return +// var movingForwards = true +// var activeProfile = generateProfile(true) +// var profileStart = clock.seconds() +// while (!isStopRequested) { +// telemetry.addData("mode", mode) +// when (mode) { +// Mode.TUNING_MODE -> { +// if (gamepad1.y) { +// mode = Mode.DRIVER_MODE +// drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER) +// } +// +// // calculate and set the motor power +// val profileTime = clock.seconds() - profileStart +// if (profileTime > activeProfile.duration()) { +// // generate a new profile +// movingForwards = !movingForwards +// activeProfile = generateProfile(movingForwards) +// profileStart = clock.seconds() +// } +// val motionState = activeProfile[profileTime] +// val targetPower = DriveConstants.kV * motionState.v +// drive.setDrivePower(Pose2d(targetPower, 0.0, 0.0)) +// val velocities = drive.getWheelVelocities() +// +// // update telemetry +// telemetry.addData("targetVelocity", motionState.v) +// var i = 0 +// while (i < velocities.size) { +// telemetry.addData("measuredVelocity$i", velocities[i]) +// telemetry.addData( +// "error$i", +// motionState.v - velocities[i] +// ) +// i++ +// } +// } +// +// Mode.DRIVER_MODE -> { +// if (gamepad1.b) { +// drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER) +// mode = Mode.TUNING_MODE +// movingForwards = true +// activeProfile = generateProfile(movingForwards) +// profileStart = clock.seconds() +// } +// drive.setWeightedDrivePower( +// Pose2d( +// -gamepad1.left_stick_y.toDouble(), +// -gamepad1.left_stick_x.toDouble(), +// -gamepad1.right_stick_x.toDouble() +// ) +// ) +// } +// } +// if (lastKp != DriveConstants.MOTOR_VELO_PID.p || lastKd != DriveConstants.MOTOR_VELO_PID.d || lastKi != DriveConstants.MOTOR_VELO_PID.i || lastKf != DriveConstants.MOTOR_VELO_PID.f) { +// drive.setPIDFCoefficients( +// DcMotor.RunMode.RUN_USING_ENCODER, +// DriveConstants.MOTOR_VELO_PID +// ) +// lastKp = DriveConstants.MOTOR_VELO_PID.p +// lastKi = DriveConstants.MOTOR_VELO_PID.i +// lastKd = DriveConstants.MOTOR_VELO_PID.d +// lastKf = DriveConstants.MOTOR_VELO_PID.f +// } +// telemetry.update() +// } +// } +// +// internal enum class Mode { +// DRIVER_MODE, TUNING_MODE +// } +// +// companion object { +// var DISTANCE = 72.0 // in +// private fun generateProfile(movingForward: Boolean): MotionProfile { +// val start = MotionState(if (movingForward) 0.0 else DISTANCE, 0.0, 0.0, 0.0) +// val goal = MotionState(if (movingForward) DISTANCE else 0.0, 0.0, 0.0, 0.0) +// return generateSimpleMotionProfile( +// start, +// goal, +// DriveConstants.MAX_VEL, +// DriveConstants.MAX_ACCEL +// ) +// } +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/FollowerPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/FollowerPIDTuner.kt new file mode 100644 index 0000000..aa5894e --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/FollowerPIDTuner.kt @@ -0,0 +1,51 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner +// +//import com.acmerobotics.dashboard.config.Config +//import com.acmerobotics.roadrunner.geometry.Pose2d +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode +//import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem +// +///* +// * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base +// * classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the +// * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer +// * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're +// * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once +// * you've successfully connected, start the program, and your robot will begin driving in a square. +// * You should observe the target position (green) and your pose estimate (blue) and adjust your +// * follower PID coefficients such that you follow the target position as accurately as possible. +// * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. +// * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. +// * These coefficients can be tuned live in dashboard. +// */ +////@Disabled +//@Config +//@Autonomous(group = "drive") +//class FollowerPIDTuner : LinearOpMode() { +// @Throws(InterruptedException::class) +// override fun runOpMode() { +// val drive = DriveSubsystem(hardwareMap) +// val startPose = Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0.0) +// drive.poseEstimate = startPose +// waitForStart() +// if (isStopRequested) return +// while (!isStopRequested) { +// val trajSeq = drive.trajectorySequenceBuilder(startPose) +// .forward(DISTANCE) +// .turn(Math.toRadians(90.0)) +// .forward(DISTANCE) +// .turn(Math.toRadians(90.0)) +// .forward(DISTANCE) +// .turn(Math.toRadians(90.0)) +// .forward(DISTANCE) +// .turn(Math.toRadians(90.0)) +// .build() +// drive.followTrajectorySequence(trajSeq) +// } +// } +// +// companion object { +// var DISTANCE = 48.0 // in +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/LocalizationTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/LocalizationTest.kt new file mode 100644 index 0000000..ea9af50 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/LocalizationTest.kt @@ -0,0 +1,41 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner +// +//import com.acmerobotics.roadrunner.geometry.Pose2d +//import com.qualcomm.robotcore.eventloop.opmode.Disabled +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode +//import com.qualcomm.robotcore.eventloop.opmode.TeleOp +//import com.qualcomm.robotcore.hardware.DcMotor +//import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem +// +///** +// * This is a simple teleop routine for testing localization. Drive the robot around like a normal +// * teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight +// * errors are not out of the ordinary, especially with sudden drive motions). The goal of this +// * exercise is to ascertain whether the localizer has been configured properly (note: the pure +// * encoder localizer heading may be significantly off if the track width has not been tuned). +// */ +////@Disabled +//@TeleOp(group = "drive") +//class LocalizationTest : LinearOpMode() { +// @Throws(InterruptedException::class) +// override fun runOpMode() { +// val drive = DriveSubsystem(hardwareMap) +// drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER) +// waitForStart() +// while (!isStopRequested) { +// drive.setWeightedDrivePower( +// Pose2d( +// -gamepad1.left_stick_y.toDouble(), +// -gamepad1.left_stick_x.toDouble(), +// -gamepad1.right_stick_x.toDouble() +// ) +// ) +// drive.update() +// val (x, y, heading) = drive.poseEstimate +// telemetry.addData("x", x) +// telemetry.addData("y", y) +// telemetry.addData("heading", Math.toDegrees(heading)) +// telemetry.update() +// } +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/ManualFeedforwardTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/ManualFeedforwardTuner.kt new file mode 100644 index 0000000..1b1fa6a --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/ManualFeedforwardTuner.kt @@ -0,0 +1,130 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner +// +//import com.acmerobotics.dashboard.FtcDashboard +//import com.acmerobotics.dashboard.config.Config +//import com.acmerobotics.dashboard.telemetry.MultipleTelemetry +//import com.acmerobotics.roadrunner.geometry.Pose2d +//import com.acmerobotics.roadrunner.kinematics.Kinematics.calculateMotorFeedforward +//import com.acmerobotics.roadrunner.profile.MotionProfile +//import com.acmerobotics.roadrunner.profile.MotionProfileGenerator.generateSimpleMotionProfile +//import com.acmerobotics.roadrunner.profile.MotionState +//import com.acmerobotics.roadrunner.util.NanoClock +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode +//import com.qualcomm.robotcore.util.RobotLog +//import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem +//import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.MAX_ACCEL +//import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.MAX_VEL +//import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.RUN_USING_ENCODER +//import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.kA +//import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.kStatic +//import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.kV +// +///* +// * This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary, +// * tuning these coefficients is just as important as the positional parameters. Like the other +// * manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard, +// * connect your computer to the RC's WiFi network. In your browser, navigate to +// * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if +// * you are using the Control Hub. Once you've successfully connected, start the program, and your +// * robot will begin moving forward and backward according to a motion profile. Your job is to graph +// * the velocity errors over time and adjust the feedforward coefficients. Once you've found a +// * satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file. +// * +// * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the +// * user to reset the position of the bot in the event that it drifts off the path. +// * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. +// */ +////@Disabled +//@Config +//@Autonomous(group = "drive") +//class ManualFeedforwardTuner : LinearOpMode() { +// private val dashboard = FtcDashboard.getInstance() +// private lateinit var drive: DriveSubsystem +// private lateinit var mode: Mode +// override fun runOpMode() { +// if (RUN_USING_ENCODER) { +// RobotLog.setGlobalErrorMsg( +// "Feedforward constants usually don't need to be tuned " + +// "when using the built-in drive motor velocity PID." +// ) +// } +// telemetry = MultipleTelemetry(telemetry, dashboard.telemetry) +// drive = DriveSubsystem(hardwareMap) +// mode = Mode.TUNING_MODE +// val clock = NanoClock.system() +// telemetry.addLine("Ready!") +// telemetry.update() +// telemetry.clearAll() +// waitForStart() +// if (isStopRequested) return +// var movingForwards = true +// var activeProfile = generateProfile(true) +// var profileStart = clock.seconds() +// while (!isStopRequested) { +// telemetry.addData("mode", mode) +// when (mode) { +// Mode.TUNING_MODE -> { +// if (gamepad1.y) { +// mode = Mode.DRIVER_MODE +// } +// +// // calculate and set the motor power +// val profileTime = clock.seconds() - profileStart +// if (profileTime > activeProfile.duration()) { +// // generate a new profile +// movingForwards = !movingForwards +// activeProfile = generateProfile(movingForwards) +// profileStart = clock.seconds() +// } +// +// val motionState = activeProfile[profileTime] +// val targetPower = +// calculateMotorFeedforward(motionState.v, motionState.a, kV, kA, kStatic) +// +// drive.setDrivePower(Pose2d(targetPower, 0.0, 0.0)) +// drive.updatePoseEstimate() +// +// val currentVelo = requireNotNull( +// drive.poseVelocity +// ) { "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer." }.x +// +// // update telemetry +// telemetry.addData("targetVelocity", motionState.v) +// telemetry.addData("measuredVelocity", currentVelo) +// telemetry.addData("error", motionState.v - currentVelo) +// } +// +// Mode.DRIVER_MODE -> { +// if (gamepad1.b) { +// mode = Mode.TUNING_MODE +// movingForwards = true +// activeProfile = generateProfile(movingForwards) +// profileStart = clock.seconds() +// } +// drive.setWeightedDrivePower( +// Pose2d( +// -gamepad1.left_stick_y.toDouble(), +// -gamepad1.left_stick_x.toDouble(), +// -gamepad1.right_stick_x.toDouble() +// ) +// ) +// } +// } +// telemetry.update() +// } +// } +// +// internal enum class Mode { +// DRIVER_MODE, TUNING_MODE +// } +// +// companion object { +// private var DISTANCE = 90.0 // in +// private fun generateProfile(movingForward: Boolean): MotionProfile { +// val start = MotionState(if (movingForward) 0.0 else DISTANCE, 0.0, 0.0, 0.0) +// val goal = MotionState(if (movingForward) DISTANCE else 0.0, 0.0, 0.0, 0.0) +// return generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL) +// } +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MaxAngularVeloTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MaxAngularVeloTuner.kt new file mode 100644 index 0000000..69f1d4b --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MaxAngularVeloTuner.kt @@ -0,0 +1,69 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner +// +//import com.acmerobotics.dashboard.FtcDashboard +//import com.acmerobotics.dashboard.config.Config +//import com.acmerobotics.dashboard.telemetry.MultipleTelemetry +//import com.acmerobotics.roadrunner.geometry.Pose2d +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode +//import com.qualcomm.robotcore.hardware.DcMotor +//import com.qualcomm.robotcore.util.ElapsedTime +//import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem +// +///** +// * This routine is designed to calculate the maximum angular velocity your bot can achieve under load. +// * +// * +// * Upon pressing start, your bot will turn at max power for RUNTIME seconds. +// * +// * +// * Further fine tuning of MAX_ANG_VEL may be desired. +// */ +////@Disabled +//@Config +//@Autonomous(group = "drive") +//class MaxAngularVeloTuner : LinearOpMode() { +// private var timer: ElapsedTime? = null +// private var maxAngVelocity = 0.0 +// +// @Throws(InterruptedException::class) +// override fun runOpMode() { +// val drive = DriveSubsystem(hardwareMap) +// drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER) +// +// telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) +// telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds.") +// telemetry.addLine("Please ensure you have enough space cleared.") +// telemetry.addLine("") +// telemetry.addLine("Press start when ready.") +// telemetry.update() +// +// waitForStart() +// +// telemetry.clearAll() +// telemetry.update() +// +// drive.setDrivePower(Pose2d(0.0, 0.0, 1.0)) +// +// timer = ElapsedTime() +// +// while (!isStopRequested && timer!!.seconds() < RUNTIME) { +// drive.updatePoseEstimate() +// +// val (_, _, heading) = requireNotNull( +// drive.poseVelocity +// ) { "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer." } +// +// maxAngVelocity = heading.coerceAtLeast(maxAngVelocity) +// } +// drive.setDrivePower(Pose2d()) +// telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity) +// telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity)) +// telemetry.update() +// while (!isStopRequested) idle() +// } +// +// companion object { +// var RUNTIME = 4.0 +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MaxVelocityTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MaxVelocityTuner.kt new file mode 100644 index 0000000..f7ede79 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MaxVelocityTuner.kt @@ -0,0 +1,86 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner +// +//import com.acmerobotics.dashboard.FtcDashboard +//import com.acmerobotics.dashboard.telemetry.MultipleTelemetry +//import com.acmerobotics.roadrunner.geometry.Pose2d +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode +//import com.qualcomm.robotcore.hardware.DcMotor +//import com.qualcomm.robotcore.hardware.VoltageSensor +//import com.qualcomm.robotcore.util.ElapsedTime +//import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem +//import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants +//import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.getMotorVelocityF +// +///** +// * This routine is designed to calculate the maximum velocity your bot can achieve under load. It +// * will also calculate the effective kF value for your velocity PID. +// * +// * +// * Upon pressing start, your bot will run at max power for RUNTIME seconds. +// * +// * +// * Further fine tuning of kF may be desired. +// */ +////@Disabled //@Config +//@Autonomous(group = "drive") +//class MaxVelocityTuner : LinearOpMode() { +// private lateinit var timer: ElapsedTime +// private var maxVelocity = 0.0 +// private lateinit var batteryVoltageSensor: VoltageSensor +// +// @Throws(InterruptedException::class) +// override fun runOpMode() { +// val drive = DriveSubsystem(hardwareMap) +// drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER) +// +// batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next() +// +// telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) +// telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds.") +// telemetry.addLine("Please ensure you have enough space cleared.") +// telemetry.addLine("") +// telemetry.addLine("Press start when ready.") +// telemetry.update() +// +// waitForStart() +// +// telemetry.clearAll() +// telemetry.update() +// +// drive.setDrivePower(Pose2d(1.0, 0.0, 0.0)) +// +// timer = ElapsedTime() +// +// while (!isStopRequested && timer.seconds() < RUNTIME) { +// drive.updatePoseEstimate() +// val poseVelo = requireNotNull( +// drive.poseVelocity +// ) { "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer." } +// +// maxVelocity = poseVelo.vec().norm().coerceAtLeast(maxVelocity) +// } +// +// drive.setDrivePower(Pose2d()) +// +// val effectiveKf = getMotorVelocityF(veloInchesToTicks(maxVelocity)) +// +// telemetry.addData("Max Velocity", maxVelocity) +// telemetry.addData( +// "Voltage Compensated kF", +// effectiveKf * batteryVoltageSensor.voltage / 12 +// ) +// +// telemetry.update() +// +// while (!isStopRequested && opModeIsActive()) idle() +// } +// +// private fun veloInchesToTicks(inchesPerSec: Double): Double { +// return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV +// } +// +// companion object { +// var RUNTIME = 2.0 +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MotorDirectionDebugger.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MotorDirectionDebugger.kt new file mode 100644 index 0000000..85510d7 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/MotorDirectionDebugger.kt @@ -0,0 +1,78 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner +// +//import com.acmerobotics.dashboard.FtcDashboard +//import com.acmerobotics.dashboard.config.Config +//import com.acmerobotics.dashboard.telemetry.MultipleTelemetry +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode +//import com.qualcomm.robotcore.eventloop.opmode.TeleOp +//import org.firstinspires.ftc.robotcore.external.Telemetry +//import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem +// +///** +// * X / ▢ - Front Left +// * Y / Δ - Front Right +// * B / O - Rear Right +// * A / X - Rear Left +// * The buttons are mapped to match the wheels spatially if you +// * were to rotate the gamepad 45deg°. x/square is the front left +// * ________ and each button corresponds to the wheel as you go clockwise +// * / ______ \ +// * ------------.-' _ '-..+ Front of Bot +// * / _ ( Y ) _ \ ^ +// * | ( X ) _ ( B ) | Front Left \ Front Right +// * ___ '. ( A ) /| Wheel \ Wheel +// * .' '. '-._____.-' .' (x/▢) \ (Y/Δ) +// * | | | \ +// * '.___.' '. | Rear Left \ Rear Right +// * '. / Wheel \ Wheel +// * \. .' (A/X) \ (B/O) +// * \________/ +// * +// */ +////@Disabled +//@Config +//@TeleOp(group = "drive") +//class MotorDirectionDebugger : LinearOpMode() { +// @Throws(InterruptedException::class) +// override fun runOpMode() { +// telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) +// val drive = DriveSubsystem(hardwareMap) +// telemetry.addLine("Press play to begin the debugging opmode") +// telemetry.update() +// waitForStart() +// if (isStopRequested) return +// telemetry.clearAll() +// telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML) +// while (!isStopRequested) { +// telemetry.addLine("Press each button to turn on its respective motor") +// telemetry.addLine() +// telemetry.addLine("Xbox/PS4 Button - MotorPDController") +// telemetry.addLine("  X / ▢         - Front Left") +// telemetry.addLine("  Y / Δ         - Front Right") +// telemetry.addLine("  B / O         - Rear  Right") +// telemetry.addLine("  A / X         - Rear  Left") +// telemetry.addLine() +// if (gamepad1.x) { +// drive.setMotorPowers(MOTOR_POWER, 0.0, 0.0, 0.0) +// telemetry.addLine("Running MotorPDController: Front Left") +// } else if (gamepad1.y) { +// drive.setMotorPowers(0.0, 0.0, 0.0, MOTOR_POWER) +// telemetry.addLine("Running MotorPDController: Front Right") +// } else if (gamepad1.b) { +// drive.setMotorPowers(0.0, 0.0, MOTOR_POWER, 0.0) +// telemetry.addLine("Running MotorPDController: Rear Right") +// } else if (gamepad1.a) { +// drive.setMotorPowers(0.0, MOTOR_POWER, 0.0, 0.0) +// telemetry.addLine("Running MotorPDController: Rear Left") +// } else { +// drive.setMotorPowers(0.0, 0.0, 0.0, 0.0) +// telemetry.addLine("Running MotorPDController: None") +// } +// telemetry.update() +// } +// } +// +// companion object { +// var MOTOR_POWER = 0.7 +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/SplineTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/SplineTest.kt new file mode 100644 index 0000000..eb3893c --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/SplineTest.kt @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner +// +//import com.acmerobotics.roadrunner.geometry.Pose2d +//import com.acmerobotics.roadrunner.geometry.Vector2d +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous +//import com.qualcomm.robotcore.eventloop.opmode.Disabled +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode +//import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem +// +///* +// * This is an example of a more complex path to really test the tuning. +// */ +////@Disabled +//@Autonomous(group = "drive") +//class SplineTest : LinearOpMode() { +// @Throws(InterruptedException::class) +// override fun runOpMode() { +// val drive = DriveSubsystem(hardwareMap) +// +// waitForStart() +// +// if (isStopRequested) return +// +// val traj = drive.trajectoryBuilder(Pose2d()) +// .splineTo(Vector2d(30.0, 30.0), 0.0) +// .build() +// +// drive.followTrajectory(traj) +// +// sleep(2000) +// +// drive.followTrajectory( +// drive.trajectoryBuilder(traj.end(), true) +// .splineTo(Vector2d(0.0, 0.0), Math.toRadians(180.0)) +// .build() +// ) +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/StrafeTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/StrafeTest.kt new file mode 100644 index 0000000..7d70891 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/StrafeTest.kt @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner + +/* + * This is a simple routine to test translational drive capabilities. + */ +//@Disabled +//@Config +//@Autonomous(group = "drive") +//class StrafeTest : LinearOpMode() { +// @Throws(InterruptedException::class) +// override fun runOpMode() { +// telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) +// +// val drive = DriveSubsystem(hardwareMap) +// val trajectory = drive.trajectoryBuilder(Pose2d()) +// .strafeRight(DISTANCE) +// .build() +// +// waitForStart() +// +// if (isStopRequested) return +// +// drive.followTrajectory(trajectory) +// +// val (x, y, heading) = drive.poseEstimate +// +// telemetry.addData("finalX", x) +// telemetry.addData("finalY", y) +// telemetry.addData("finalHeading", heading) +// telemetry.update() +// +// while (!isStopRequested && opModeIsActive()); +// } +// +// companion object { +// var DISTANCE = 60.0 // in +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/StraightTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/StraightTest.kt new file mode 100644 index 0000000..b08ee95 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/StraightTest.kt @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner + +/* + * This is a simple routine to test translational drive capabilities. + */ +//@Disabled +//@Config +//@Autonomous(group = "drive") +//class StraightTest : LinearOpMode() { +// @Throws(InterruptedException::class) +// override fun runOpMode() { +// telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) +// val drive = DriveSubsystem(hardwareMap) +// val trajectory = drive.trajectoryBuilder(Pose2d()) +// .forward(DISTANCE) +// .build() +// waitForStart() +// if (isStopRequested) return +// drive.followTrajectory(trajectory) +// val (x, y, heading) = drive.poseEstimate +// telemetry.addData("finalX", x) +// telemetry.addData("finalY", y) +// telemetry.addData("finalHeading", heading) +// telemetry.update() +// while (!isStopRequested && opModeIsActive()); +// } +// +// companion object { +// @JvmField +// var DISTANCE = 60.0 // in +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackWidthTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackWidthTuner.kt new file mode 100644 index 0000000..f7a9cd2 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackWidthTuner.kt @@ -0,0 +1,68 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner + +/* + * This routine determines the effective track width. The procedure works by executing a point turn + * with a given angle and measuring the difference between that angle and the actual angle (as + * indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient + * given angle / actual angle gives a multiplicative adjustment to the estimated track width + * (effective track width = estimated track width * given angle / actual angle). The routine repeats + * this procedure a few times and averages the values for additional accuracy. Note: a relatively + * accurate track width estimate is important or else the angular constraints will be thrown off. + */ +//@Disabled +//@Config +//@Autonomous(group = "drive") +//class TrackWidthTuner : LinearOpMode() { +// @Throws(InterruptedException::class) +// override fun runOpMode() { +// telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) +// val drive = DriveSubsystem(hardwareMap) +// // TODO: if you haven't already, set the localizer to something that doesn't depend on +// // drive encoders for computing the heading +// telemetry.addLine("Press play to begin the track width tuner routine") +// telemetry.addLine("Make sure your robot has enough clearance to turn smoothly") +// telemetry.update() +// waitForStart() +// if (isStopRequested) return +// telemetry.clearAll() +// telemetry.addLine("Running...") +// telemetry.update() +// val trackWidthStats = MovingStatistics(NUM_TRIALS) +// for (i in 0 until NUM_TRIALS) { +// drive.poseEstimate = Pose2d() +// +// // it is important to handle heading wraparounds +// var headingAccumulator = 0.0 +// var lastHeading = 0.0 +// drive.turnAsync(Math.toRadians(ANGLE)) +// while (!isStopRequested && drive.isBusy) { +// val heading = drive.poseEstimate.heading +// headingAccumulator += norm(heading - lastHeading) +// lastHeading = heading +// drive.update() +// } +// val trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator +// trackWidthStats.add(trackWidth) +// sleep(DELAY.toLong()) +// } +// telemetry.clearAll() +// telemetry.addLine("Tuning complete") +// telemetry.addLine( +// Misc.formatInvariant( +// "Effective track width = %.2f (SE = %.3f)", +// trackWidthStats.mean, +// trackWidthStats.standardDeviation / Math.sqrt(NUM_TRIALS.toDouble()) +// ) +// ) +// telemetry.update() +// while (!isStopRequested) { +// idle() +// } +// } +// +// companion object { +// var ANGLE = 180.0 // deg +// var NUM_TRIALS = 5 +// var DELAY = 1000 // ms +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackingWheelForwardOffsetTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackingWheelForwardOffsetTuner.kt new file mode 100644 index 0000000..510de28 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackingWheelForwardOffsetTuner.kt @@ -0,0 +1,85 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner + +/** + * This routine determines the effective forward offset for the lateral tracking wheel. + * The procedure executes a point turn at a given angle for a certain number of trials, + * along with a specified delay in milliseconds. The purpose of this is to track the + * change in the y position during the turn. The offset, or distance, of the lateral tracking + * wheel from the center or rotation allows the wheel to spin during a point turn, leading + * to an incorrect measurement for the y position. This creates an arc around around + * the center of rotation with an arc length of change in y and a radius equal to the forward + * offset. We can compute this offset by calculating (change in y position) / (change in heading) + * which returns the radius if the angle (change in heading) is in radians. This is based + * on the arc length formula of length = theta * radius. + * + * + * To run this routine, simply adjust the desired angle and specify the number of trials + * and the desired delay. Then, run the procedure. Once it finishes, it will print the + * average of all the calculated forward offsets derived from the calculation. This calculated + * forward offset is then added onto the current forward offset to produce an overall estimate + * for the forward offset. You can run this procedure as many times as necessary until a + * satisfactory result is produced. + */ +//@Disabled +//@Config +//@Autonomous(group = "drive") +//class TrackingWheelForwardOffsetTuner : LinearOpMode() { +// @Throws(InterruptedException::class) +// override fun runOpMode() { +// telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) +// val drive = DriveSubsystem(hardwareMap) +// if (drive.localizer !is StandardTrackingWheelLocalizer) { +// RobotLog.setGlobalErrorMsg( +// "StandardTrackingWheelLocalizer is not being set in the " +// + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer" +// + "(hardwareMap));\" is called in SampleMecanumDrive.java" +// ) +// } +// telemetry.addLine("Press play to begin the forward offset tuner") +// telemetry.addLine("Make sure your robot has enough clearance to turn smoothly") +// telemetry.update() +// waitForStart() +// if (isStopRequested) return +// telemetry.clearAll() +// telemetry.addLine("Running...") +// telemetry.update() +// val forwardOffsetStats = MovingStatistics(NUM_TRIALS) +// for (i in 0 until NUM_TRIALS) { +// drive.poseEstimate = Pose2d() +// +// // it is important to handle heading wraparounds +// var headingAccumulator = 0.0 +// var lastHeading = 0.0 +// drive.turnAsync(Math.toRadians(ANGLE)) +// while (!isStopRequested && drive.isBusy) { +// val heading = drive.poseEstimate.heading +// headingAccumulator += norm(heading - lastHeading) +// lastHeading = heading +// drive.update() +// } +// val forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET + +// drive.poseEstimate.y / headingAccumulator +// forwardOffsetStats.add(forwardOffset) +// sleep(DELAY.toLong()) +// } +// telemetry.clearAll() +// telemetry.addLine("Tuning complete") +// telemetry.addLine( +// Misc.formatInvariant( +// "Effective forward offset = %.2f (SE = %.3f)", +// forwardOffsetStats.mean, +// forwardOffsetStats.standardDeviation / Math.sqrt(NUM_TRIALS.toDouble()) +// ) +// ) +// telemetry.update() +// while (!isStopRequested) { +// idle() +// } +// } +// +// companion object { +// var ANGLE = 180.0 // deg +// var NUM_TRIALS = 5 +// var DELAY = 1000 // ms +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackingWheelLateralDistanceTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackingWheelLateralDistanceTuner.kt new file mode 100644 index 0000000..20c5b3a --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TrackingWheelLateralDistanceTuner.kt @@ -0,0 +1,124 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner + +/** + * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s + * LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel + * wheels. + * + * + * Tuning Routine: + * + * + * 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical + * measured value. This need only be an estimated value as you will be tuning it anyways. + * + * + * 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an + * similar mark right below the indicator on your bot. This will be your reference point to + * ensure you've turned exactly 360°. + * + * + * 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help + * identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard, + * connect your computer to the RC's WiFi network. In your browser, navigate to + * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash + * if you are using the Control Hub. + * Ensure the field is showing (select the field view in top right of the page). + * + * + * 4. Press play to begin the tuning routine. + * + * + * 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise. + * + * + * 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns. + * + * + * 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators + * on the bot and on the ground you created earlier should be lined up. + * + * + * 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your + * StandardTrackingWheelLocalizer.java class. + * + * + * 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value + * yourself. Read the heading output and follow the advice stated in the note below to manually + * nudge the values yourself. + * + * + * Note: + * It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with + * a line from the circumference to the center should be present, representing the bot. The line + * indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in + * dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than + * the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the + * actual bot, the LATERAL_DISTANCE should be increased. + * + * + * If your drawn bot oscillates around a point in dashboard, don't worry. This is because the + * position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the + * effective center of rotation. You can ignore this effect. The center of rotation will be offset + * slightly but your heading will still be fine. This does not affect your overall tracking + * precision. The heading should still line up. + */ +//@Disabled +//@Config +//@TeleOp(group = "drive") +//class TrackingWheelLateralDistanceTuner : LinearOpMode() { +// @Throws(InterruptedException::class) +// override fun runOpMode() { +// val drive = DriveSubsystem(hardwareMap) +// if (drive.localizer !is StandardTrackingWheelLocalizer) { +// RobotLog.setGlobalErrorMsg( +// "StandardTrackingWheelLocalizer is not being set in the " +// + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer" +// + "(hardwareMap));\" is called in SampleMecanumDrive.java" +// ) +// } +// telemetry.addLine( +// "Prior to beginning the routine, please read the directions " +// + "located in the comments of the opmode file." +// ) +// telemetry.addLine("Press play to begin the tuning routine.") +// telemetry.addLine("") +// telemetry.addLine("Press Y/△ to stop the routine.") +// telemetry.update() +// waitForStart() +// if (isStopRequested) return +// telemetry.clearAll() +// telemetry.update() +// var headingAccumulator = 0.0 +// var lastHeading = 0.0 +// var tuningFinished = false +// while (!isStopRequested && !tuningFinished) { +// val vel = Pose2d(0.0, 0.0, -gamepad1.right_stick_x.toDouble()) +// drive.setDrivePower(vel) +// drive.update() +// val heading = drive.poseEstimate.heading +// val deltaHeading = heading - lastHeading +// headingAccumulator += normDelta(deltaHeading) +// lastHeading = heading +// telemetry.clearAll() +// telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator)) +// telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading)) +// telemetry.addLine() +// telemetry.addLine("Press Y/△ to conclude routine") +// telemetry.update() +// if (gamepad1.y) tuningFinished = true +// } +// telemetry.clearAll() +// telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°") +// telemetry.addLine( +// "Effective LATERAL_DISTANCE: " + +// headingAccumulator / (NUM_TURNS * Math.PI * 2) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE +// ) +// telemetry.update() +// while (!isStopRequested) idle() +// } +// +// companion object { +// var NUM_TURNS = 10 +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TurnTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TurnTest.kt new file mode 100644 index 0000000..9e36017 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/legacy/roadrunner/TurnTest.kt @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.teamcode.legacy.roadrunner + +/* + * This is a simple routine to test turning capabilities. + */ +//@Disabled +//@Config +//@Autonomous(group = "drive") +//class TurnTest : LinearOpMode() { +// @Throws(InterruptedException::class) +// override fun runOpMode() { +// val drive = DriveSubsystem(hardwareMap) +// waitForStart() +// if (isStopRequested) return +// drive.turn(Math.toRadians(ANGLE)) +// } +// +// companion object { +// @JvmField +// var ANGLE = 180.0 // deg +// } +//} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt deleted file mode 100644 index e3bd71d..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt +++ /dev/null @@ -1,76 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left - -import com.arcrobotics.ftclib.hardware.motors.CRServo -import com.arcrobotics.ftclib.hardware.motors.Motor -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.constants.ControlBoard -import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem - -@Autonomous(name = "Blue High Basket Left", group = "Basket", preselectTeleOp = "MainTeleOp") -class BlueHighBasketLeft: OpMode() { - private lateinit var armLeft: Motor - private lateinit var armRight: Motor - private lateinit var elevatorLeft: Motor - private lateinit var elevatorRight: Motor - - private lateinit var intake: CRServo - - private lateinit var driveSubsystem: DriveSubsystem - private lateinit var intakeSubsystem: IntakeSubsystem - private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem - - override fun init() { -// intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) - - armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) - armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) - - elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) - elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) - - driveSubsystem = DriveSubsystem(hardwareMap) -// intakeSubsystem = IntakeSubsystem(intake) - armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) - .turn(Math.toRadians(90.0)) - .forward(60.0) - .addTemporalMarker(3.0) { - armSubsystem.setpoint = Math.toRadians(95.0) - } - .waitSeconds(1.0) - .addTemporalMarker(6.0) { - elevatorSubsystem.setpoint = 250.0 - } - .waitSeconds(2.0) -// .addTemporalMarker(5.0){ -// intakeSubsystem.outtake() -// } - .waitSeconds(1.0) - .addTemporalMarker(10.0) { - elevatorSubsystem.setpoint = 0.0 - } - .waitSeconds(2.0) - .addTemporalMarker(13.0) { - armSubsystem.setpoint = Math.toRadians(0.0) - } - .build() - - driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose - driveSubsystem.followTrajectorySequenceAsync(trajectory) - } - - override fun loop() { - driveSubsystem.update() - - elevatorSubsystem.periodic() - armSubsystem.periodic() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/right/BlueHighBasketRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/right/BlueHighBasketRight.kt deleted file mode 100644 index 5247a0f..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/right/BlueHighBasketRight.kt +++ /dev/null @@ -1,76 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.right - -import com.arcrobotics.ftclib.hardware.motors.CRServo -import com.arcrobotics.ftclib.hardware.motors.Motor -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.constants.ControlBoard -import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem - - -@Autonomous(name = "Blue High Basket Right", group = "Basket", preselectTeleOp = "MainTeleOp") -class BlueHighBasketRight() : OpMode() { - - private lateinit var armLeft: Motor - private lateinit var armRight: Motor - private lateinit var elevatorLeft: Motor - private lateinit var elevatorRight: Motor - - private lateinit var intake: CRServo - - private lateinit var driveSubsystem: DriveSubsystem - private lateinit var intakeSubsystem: IntakeSubsystem - private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem - - override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) - - armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) - armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) - - elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) - elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) - - driveSubsystem = DriveSubsystem(hardwareMap) - intakeSubsystem = IntakeSubsystem(intake) - armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose) - .turn(Math.toRadians(90.0)) - .forward(35.0) - .addTemporalMarker(2.0) { - armSubsystem.setpoint = Math.toRadians(150.0) - } - .waitSeconds(1.0) - .addTemporalMarker(3.0) { - elevatorSubsystem.setpoint = 2.0 - } - .waitSeconds(2.0) - .addTemporalMarker(5.0){ - intakeSubsystem.outtake() - } - .waitSeconds(1.0) - .addTemporalMarker(6.0) { - elevatorSubsystem.setpoint = 0.0 - } - .waitSeconds(2.0) - .addTemporalMarker(8.0) { - armSubsystem.setpoint = Math.toRadians(0.0) - } - .build() - driveSubsystem.poseEstimate = AutoStartPose.BLUE_RIGHT.startPose - - - driveSubsystem.followTrajectorySequenceAsync(trajectory) - } - - override fun loop() { - driveSubsystem.update() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt deleted file mode 100644 index 3f88cc4..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt +++ /dev/null @@ -1,76 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.blue.chamber.left - -import com.arcrobotics.ftclib.hardware.motors.CRServo -import com.arcrobotics.ftclib.hardware.motors.Motor -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.constants.ControlBoard -import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem - -@Autonomous(name = "Blue High Chamber Left", group = "Chamber", preselectTeleOp = "MainTeleOp") -class BlueHighChamberLeft() : OpMode() { - private lateinit var armLeft: Motor - private lateinit var armRight: Motor - private lateinit var elevatorLeft: Motor - private lateinit var elevatorRight: Motor - - private lateinit var intake: CRServo - - private lateinit var driveSubsystem: DriveSubsystem - private lateinit var intakeSubsystem: IntakeSubsystem - private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem - - override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) - - armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) - armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) - - elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) - elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) - - driveSubsystem = DriveSubsystem(hardwareMap) - intakeSubsystem = IntakeSubsystem(intake) - armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) - - .forward(28.0) - .strafeLeft(11.0) - .addTemporalMarker(2.0) { - armSubsystem.setpoint = Math.toRadians(150.0) - } - .waitSeconds(1.0) - .addTemporalMarker(3.0) { - elevatorSubsystem.setpoint = 2.0 - } - .waitSeconds(2.0) - .addTemporalMarker(5.0) { - intakeSubsystem.outtake() - } - .waitSeconds(1.0) - .addTemporalMarker(6.0) { - elevatorSubsystem.setpoint = 0.0 - } - .waitSeconds(2.0) - .addTemporalMarker(8.0) { - armSubsystem.setpoint = Math.toRadians(0.0) - } - .build() - - driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose - - - driveSubsystem.followTrajectorySequenceAsync(trajectory) - } - - override fun loop() { - driveSubsystem.update() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/right/BlueHighChamberRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/right/BlueHighChamberRight.kt deleted file mode 100644 index 8b43c28..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/right/BlueHighChamberRight.kt +++ /dev/null @@ -1,75 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.blue.chamber.right - -import com.arcrobotics.ftclib.hardware.motors.CRServo -import com.arcrobotics.ftclib.hardware.motors.Motor -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.constants.ControlBoard -import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem - -@Autonomous(name = "Blue High Chamber Right", group = "Chamber", preselectTeleOp = "MainTeleOp") -class BlueHighChamberRight() : OpMode() { - private lateinit var armLeft: Motor - private lateinit var armRight: Motor - private lateinit var elevatorLeft: Motor - private lateinit var elevatorRight: Motor - - private lateinit var intake: CRServo - - private lateinit var driveSubsystem: DriveSubsystem - private lateinit var intakeSubsystem: IntakeSubsystem - private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem - - override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) - - armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) - armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) - - elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) - elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) - - driveSubsystem = DriveSubsystem(hardwareMap) - intakeSubsystem = IntakeSubsystem(intake) - armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose) - - .forward(28.0) - .strafeRight(11.0) - .addTemporalMarker(2.0) { - armSubsystem.setpoint = Math.toRadians(150.0) - } - .waitSeconds(1.0) - .addTemporalMarker(3.0) { - elevatorSubsystem.setpoint = 2.0 - } - .waitSeconds(2.0) - .addTemporalMarker(5.0) { - intakeSubsystem.outtake() - } - .waitSeconds(1.0) - .addTemporalMarker(6.0) { - elevatorSubsystem.setpoint = 0.0 - } - .waitSeconds(2.0) - .addTemporalMarker(8.0) { - armSubsystem.setpoint = Math.toRadians(0.0) - } - .build() - driveSubsystem.poseEstimate = AutoStartPose.BLUE_RIGHT.startPose - - - driveSubsystem.followTrajectorySequenceAsync(trajectory) - } - - override fun loop() { - driveSubsystem.update() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/left/BlueParkObservationLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/left/BlueParkObservationLeft.kt deleted file mode 100644 index f9c6677..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/left/BlueParkObservationLeft.kt +++ /dev/null @@ -1,26 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.blue.park.left - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -@Autonomous(name = "Blue Park (Observation) Left", group = "Observation", preselectTeleOp = "MainTeleOp") -class BlueParkObservationLeft : OpMode() { - private lateinit var driveSubsystem: DriveSubsystem - - override fun init() { - driveSubsystem = DriveSubsystem(hardwareMap) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) - .strafeRight(48.0) - .build() - driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose - - driveSubsystem.followTrajectorySequenceAsync(trajectory) - } - - override fun loop() { - driveSubsystem.update() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/left/BlueParkSubmmersibleLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/left/BlueParkSubmmersibleLeft.kt deleted file mode 100644 index 622fe65..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/left/BlueParkSubmmersibleLeft.kt +++ /dev/null @@ -1,31 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.blue.park.left - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -@Autonomous(name = "Blue Park (Submmersible) Left", group = "Submmersible", preselectTeleOp = "MainTeleOp") -class BlueParkSubmmersibleLeft: OpMode() { - - private lateinit var driveSubsystem: DriveSubsystem - - override fun init() { - driveSubsystem = DriveSubsystem(hardwareMap) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) - .forward(25.0) - .strafeLeft(48.0) - .forward(25.0) - .strafeRight(12.0) - .build() - driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose - - - driveSubsystem.followTrajectorySequenceAsync(trajectory) - } - - override fun loop() { - driveSubsystem.update() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/right/BlueParkObservationRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/right/BlueParkObservationRight.kt deleted file mode 100644 index c0f85d9..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/right/BlueParkObservationRight.kt +++ /dev/null @@ -1,26 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.blue.park.right - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -@Autonomous(name = "Blue Park (Observation) Right", group = "Observation", preselectTeleOp = "MainTeleOp") -class BlueParkObservationRight: OpMode() { - private lateinit var driveSubsystem: DriveSubsystem - - override fun init() { - driveSubsystem = DriveSubsystem(hardwareMap) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose) - .strafeRight(72.0) - .build() - driveSubsystem.poseEstimate = AutoStartPose.BLUE_RIGHT.startPose - - driveSubsystem.followTrajectorySequenceAsync(trajectory) - } - - override fun loop() { - driveSubsystem.update() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/right/BlueParkSubmmersibleRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/right/BlueParkSubmmersibleRight.kt deleted file mode 100644 index e33f9a1..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/park/right/BlueParkSubmmersibleRight.kt +++ /dev/null @@ -1,32 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.blue.park.right - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -@Autonomous(name = "Blue Park (Submmersible) Right", group = "Submmersible", preselectTeleOp = "MainTeleOp") -class BlueParkSubmmersibleRight: OpMode() { - - private lateinit var driveSubsystem: DriveSubsystem - - override fun init() { - driveSubsystem = DriveSubsystem(hardwareMap) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose) - .forward(27.0) - .strafeLeft(24.0) - .forward(24.0) - .strafeRight(12.0) - .build() - driveSubsystem.poseEstimate = AutoStartPose.BLUE_RIGHT.startPose - - - driveSubsystem.followTrajectorySequenceAsync(trajectory) - - } - - override fun loop() { - driveSubsystem.update() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/left/RedHighBasketLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/left/RedHighBasketLeft.kt deleted file mode 100644 index 2b92748..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/left/RedHighBasketLeft.kt +++ /dev/null @@ -1,75 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.red.basket.left - -import com.arcrobotics.ftclib.hardware.motors.CRServo -import com.arcrobotics.ftclib.hardware.motors.Motor -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.constants.ControlBoard -import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem - -@Autonomous(name = "Red High Basket Left", group = "Basket", preselectTeleOp = "MainTeleOp") -class RedHighBasketLeft() : OpMode() { - - private lateinit var armLeft: Motor - private lateinit var armRight: Motor - private lateinit var elevatorLeft: Motor - private lateinit var elevatorRight: Motor - - private lateinit var intake: CRServo - - private lateinit var driveSubsystem: DriveSubsystem - private lateinit var intakeSubsystem: IntakeSubsystem - private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem - - override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) - - armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) - armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) - - elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) - elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) - - driveSubsystem = DriveSubsystem(hardwareMap) - intakeSubsystem = IntakeSubsystem(intake) - armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose) - .turn(Math.toRadians(90.0)) - .forward(35.0) - .addTemporalMarker(2.0) { - armSubsystem.setpoint = Math.toRadians(150.0) - } - .waitSeconds(1.0) - .addTemporalMarker(3.0) { - elevatorSubsystem.setpoint = 2.0 - } - .waitSeconds(2.0) - .addTemporalMarker(5.0){ - intakeSubsystem.outtake() - } - .waitSeconds(1.0) - .addTemporalMarker(6.0) { - elevatorSubsystem.setpoint = 0.0 - } - .waitSeconds(2.0) - .addTemporalMarker(8.0) { - armSubsystem.setpoint = Math.toRadians(0.0) - } - .build() - driveSubsystem.poseEstimate = AutoStartPose.RED_LEFT.startPose - - - driveSubsystem.followTrajectorySequenceAsync(trajectory) - } - - override fun loop() { - driveSubsystem.update() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/right/RedHighBasketRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/right/RedHighBasketRight.kt deleted file mode 100644 index 7bb7607..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/right/RedHighBasketRight.kt +++ /dev/null @@ -1,74 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.red.basket.right - -import com.arcrobotics.ftclib.hardware.motors.CRServo -import com.arcrobotics.ftclib.hardware.motors.Motor -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.constants.ControlBoard -import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem - -@Autonomous(name = "Red High Basket Right", group = "Basket", preselectTeleOp = "MainTeleOp") -class RedHighBasketRight() : OpMode() { - private lateinit var armLeft: Motor - private lateinit var armRight: Motor - private lateinit var elevatorLeft: Motor - private lateinit var elevatorRight: Motor - - private lateinit var intake: CRServo - - private lateinit var driveSubsystem: DriveSubsystem - private lateinit var intakeSubsystem: IntakeSubsystem - private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem - - override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) - - armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) - armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) - - elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) - elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) - - driveSubsystem = DriveSubsystem(hardwareMap) - intakeSubsystem = IntakeSubsystem(intake) - armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose) - .turn(Math.toRadians(90.0)) - .forward(60.0) - .addTemporalMarker(2.0) { - armSubsystem.setpoint = Math.toRadians(150.0) - } - .waitSeconds(1.0) - .addTemporalMarker(3.0) { - elevatorSubsystem.setpoint = 2.0 - } - .waitSeconds(2.0) - .addTemporalMarker(5.0){ - intakeSubsystem.outtake() - } - .waitSeconds(1.0) - .addTemporalMarker(6.0) { - elevatorSubsystem.setpoint = 0.0 - } - .waitSeconds(2.0) - .addTemporalMarker(8.0) { - armSubsystem.setpoint = Math.toRadians(0.0) - } - .build() - driveSubsystem.poseEstimate = AutoStartPose.RED_RIGHT.startPose - - - driveSubsystem.followTrajectorySequenceAsync(trajectory) - } - - override fun loop() { - driveSubsystem.update() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/left/RedHighChamberLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/left/RedHighChamberLeft.kt deleted file mode 100644 index c624bea..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/left/RedHighChamberLeft.kt +++ /dev/null @@ -1,75 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.red.chamber.left - -import com.arcrobotics.ftclib.hardware.motors.CRServo -import com.arcrobotics.ftclib.hardware.motors.Motor -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.constants.ControlBoard -import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem - -@Autonomous(name = "Red High Chamber Left", group = "Chamber", preselectTeleOp = "MainTeleOp") -class RedHighChamberLeft() : OpMode() { - private lateinit var armLeft: Motor - private lateinit var armRight: Motor - private lateinit var elevatorLeft: Motor - private lateinit var elevatorRight: Motor - - private lateinit var intake: CRServo - - private lateinit var driveSubsystem: DriveSubsystem - private lateinit var intakeSubsystem: IntakeSubsystem - private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem - - override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) - - armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) - armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) - - elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) - elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) - - driveSubsystem = DriveSubsystem(hardwareMap) - intakeSubsystem = IntakeSubsystem(intake) - armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose) - - .forward(28.0) - .strafeRight(11.0) - .addTemporalMarker(2.0) { - armSubsystem.setpoint = Math.toRadians(150.0) - } - .waitSeconds(1.0) - .addTemporalMarker(3.0) { - elevatorSubsystem.setpoint = 2.0 - } - .waitSeconds(2.0) - .addTemporalMarker(5.0) { - intakeSubsystem.outtake() - } - .waitSeconds(1.0) - .addTemporalMarker(6.0) { - elevatorSubsystem.setpoint = 0.0 - } - .waitSeconds(2.0) - .addTemporalMarker(8.0) { - armSubsystem.setpoint = Math.toRadians(0.0) - } - .build() - driveSubsystem.poseEstimate = AutoStartPose.RED_LEFT.startPose - - - driveSubsystem.followTrajectorySequenceAsync(trajectory) - } - - override fun loop() { - driveSubsystem.update() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/right/RedHighChamberRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/right/RedHighChamberRight.kt deleted file mode 100644 index e92c9cc..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/right/RedHighChamberRight.kt +++ /dev/null @@ -1,75 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.red.chamber.right - -import com.arcrobotics.ftclib.hardware.motors.CRServo -import com.arcrobotics.ftclib.hardware.motors.Motor -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.constants.ControlBoard -import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem - -@Autonomous(name = "Red High Chamber Right", group = "Chamber", preselectTeleOp = "MainTeleOp") -class RedHighChamberRight() : OpMode() { - private lateinit var armLeft: Motor - private lateinit var armRight: Motor - private lateinit var elevatorLeft: Motor - private lateinit var elevatorRight: Motor - - private lateinit var intake: CRServo - - private lateinit var driveSubsystem: DriveSubsystem - private lateinit var intakeSubsystem: IntakeSubsystem - private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem - - override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) - - armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) - armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) - - elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) - elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) - - driveSubsystem = DriveSubsystem(hardwareMap) - intakeSubsystem = IntakeSubsystem(intake) - armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose) - - .forward(28.0) - .strafeLeft(11.0) - .addTemporalMarker(2.0) { - armSubsystem.setpoint = Math.toRadians(150.0) - } - .waitSeconds(1.0) - .addTemporalMarker(3.0) { - elevatorSubsystem.setpoint = 2.0 - } - .waitSeconds(2.0) - .addTemporalMarker(5.0) { - intakeSubsystem.outtake() - } - .waitSeconds(1.0) - .addTemporalMarker(6.0) { - elevatorSubsystem.setpoint = 0.0 - } - .waitSeconds(2.0) - .addTemporalMarker(8.0) { - armSubsystem.setpoint = Math.toRadians(0.0) - } - .build() - driveSubsystem.poseEstimate = AutoStartPose.RED_RIGHT.startPose - - - driveSubsystem.followTrajectorySequenceAsync(trajectory) - } - - override fun loop() { - driveSubsystem.update() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/left/RedParkObservationLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/left/RedParkObservationLeft.kt deleted file mode 100644 index 07ee117..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/left/RedParkObservationLeft.kt +++ /dev/null @@ -1,28 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.red.park.left - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -@Autonomous(name = "Red Park (Observation) Left", group = "Observation", preselectTeleOp = "MainTeleOp") -class RedParkObservationLeft: OpMode() { - - private lateinit var driveSubsystem: DriveSubsystem - - override fun init() { - driveSubsystem = DriveSubsystem(hardwareMap) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose) - .strafeRight(72.0) - .build() - driveSubsystem.poseEstimate = AutoStartPose.RED_LEFT.startPose - - - driveSubsystem.followTrajectorySequenceAsync(trajectory) - } - - override fun loop() { - driveSubsystem.update() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/left/RedParkSubmmersibleLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/left/RedParkSubmmersibleLeft.kt deleted file mode 100644 index 1079657..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/left/RedParkSubmmersibleLeft.kt +++ /dev/null @@ -1,30 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.red.park.left - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -@Autonomous(name = "Red Park (Submmersible) Left", group = "Submmersible", preselectTeleOp = "MainTeleOp") -class RedParkSubmmersibleLeft: OpMode() { - - private lateinit var driveSubsystem: DriveSubsystem - - override fun init() { - driveSubsystem = DriveSubsystem(hardwareMap) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose) - .forward(27.0) - .strafeLeft(24.0) - .forward(24.0) - .strafeRight(12.0) - .build() - driveSubsystem.poseEstimate = AutoStartPose.RED_LEFT.startPose - - driveSubsystem.followTrajectorySequenceAsync(trajectory) - } - - override fun loop() { - driveSubsystem.update() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/right/RedParkObservationRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/right/RedParkObservationRight.kt deleted file mode 100644 index 440d557..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/right/RedParkObservationRight.kt +++ /dev/null @@ -1,27 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.red.park.right - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -@Autonomous(name = "Red Park (Observation) Right", group = "Observation", preselectTeleOp = "MainTeleOp") -class RedParkObservationRight: OpMode() { - - private lateinit var driveSubsystem: DriveSubsystem - - override fun init() { - driveSubsystem = DriveSubsystem(hardwareMap) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose) - .strafeRight(48.0) - .build() - driveSubsystem.poseEstimate = AutoStartPose.RED_RIGHT.startPose - - driveSubsystem.followTrajectorySequenceAsync(trajectory) - } - - override fun loop() { - driveSubsystem.update() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/right/RedParkSubmmersibleRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/right/RedParkSubmmersibleRight.kt deleted file mode 100644 index c87a5ef..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/park/right/RedParkSubmmersibleRight.kt +++ /dev/null @@ -1,31 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.auto.red.park.right - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.OpMode -import org.firstinspires.ftc.teamcode.constants.AutoStartPose -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -@Autonomous(name = "Red Park (Submmersible) Right", group = "Submmersible", preselectTeleOp = "MainTeleOp") -class RedParkSubmmersibleRight: OpMode() { - - private lateinit var driveSubsystem: DriveSubsystem - - override fun init() { - driveSubsystem = DriveSubsystem(hardwareMap) - - val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose) - .forward(27.0) - .strafeLeft(48.0) - .forward(24.0) - .strafeRight(12.0) - .build() - driveSubsystem.poseEstimate = AutoStartPose.RED_RIGHT.startPose - - - driveSubsystem.followTrajectorySequenceAsync(trajectory) - } - - override fun loop() { - driveSubsystem.update() - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt index c7b4e29..22701ee 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt @@ -27,7 +27,6 @@ class MainTeleOp : CommandOpMode() { private lateinit var slidesSubsystem: OpenSlidesSubsystem private lateinit var armSubsystem: OpenArmSubsystem - private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem private lateinit var spinUpCommand: SpinUpCommand @@ -56,7 +55,7 @@ class MainTeleOp : CommandOpMode() { slidesSubsystem = OpenSlidesSubsystem(elevatorRight, elevatorLeft) armSubsystem = OpenArmSubsystem(armRight, armLeft) - driveSubsystem = DriveSubsystem(hardwareMap) + DriveSubsystem.initialize(hardwareMap) //intakeSubsystem = IntakeSubsystem(intake) spinUpCommand = SpinUpCommand(slidesSubsystem) @@ -65,7 +64,7 @@ class MainTeleOp : CommandOpMode() { armDownCommand = OpenArmCommand(armSubsystem, false) //intakeCommand = IntakeCommand(true, intakeSubsystem) //outtakeCommand = IntakeCommand(false, intakeSubsystem) - driveCommand = DriveCommand(driveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0) + driveCommand = DriveCommand(DriveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0) operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whileHeld(spinUpCommand) operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whileHeld(spinDownCommand) @@ -77,7 +76,7 @@ class MainTeleOp : CommandOpMode() { // operator.getGamepadButton(GamepadKeys.Button.A).whileHeld(outtakeCommand) - driveSubsystem.defaultCommand = driveCommand + DriveSubsystem.defaultCommand = driveCommand RunCommand({ telemetry.addData("Arm Position: ", armSubsystem.armAngle) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/AutomaticFeedforwardTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/AutomaticFeedforwardTuner.kt deleted file mode 100644 index ca3f37a..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/AutomaticFeedforwardTuner.kt +++ /dev/null @@ -1,211 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.dashboard.FtcDashboard -import com.acmerobotics.dashboard.config.Config -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.util.NanoClock -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.util.RobotLog -import org.firstinspires.ftc.robotcore.internal.system.Misc -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.rpmToVelocity -import org.firstinspires.ftc.teamcode.utils.roadrunner.util.LoggingUtil -import org.firstinspires.ftc.teamcode.utils.roadrunner.util.RegressionUtil - -/* - * Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an - * outline of the procedure: - * 1. Slowly ramp the motor power and record encoder values along the way. - * 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV) - * and an optional intercept (kStatic). - * 3. Accelerate the robot (apply constant power) and record the encoder counts. - * 4. Adjust the encoder data based on the velocity tuning data and find kA with another linear - * regression. - */ -//@Disabled -@Config -@Autonomous(group = "drive") -class AutomaticFeedforwardTuner : LinearOpMode() { - @Throws(InterruptedException::class) - override fun runOpMode() { - if (DriveConstants.RUN_USING_ENCODER) { - RobotLog.setGlobalErrorMsg( - "Feedforward constants usually don't need to be tuned " + - "when using the built-in drive motor velocity PID." - ) - } - telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) - val drive = DriveSubsystem(hardwareMap) - val clock = NanoClock.system() - telemetry.addLine("Press play to begin the feedforward tuning routine") - telemetry.update() - waitForStart() - if (isStopRequested) return - telemetry.clearAll() - telemetry.addLine("Would you like to fit kStatic?") - telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no") - telemetry.update() - var fitIntercept = false - while (!isStopRequested) { - if (gamepad1.y) { - fitIntercept = true - while (!isStopRequested && gamepad1.y) { - idle() - } - break - } else if (gamepad1.b) { - while (!isStopRequested && gamepad1.b) { - idle() - } - break - } - idle() - } - telemetry.clearAll() - telemetry.addLine( - Misc.formatInvariant( - "Place your robot on the field with at least %.2f in of room in front", DISTANCE - ) - ) - telemetry.addLine("Press (Y/Δ) to begin") - telemetry.update() - while (!isStopRequested && !gamepad1.y) { - idle() - } - while (!isStopRequested && gamepad1.y) { - idle() - } - telemetry.clearAll() - telemetry.addLine("Running...") - telemetry.update() - val maxVel = rpmToVelocity(DriveConstants.MAX_RPM) - val finalVel = MAX_POWER * maxVel - val accel = finalVel * finalVel / (2.0 * DISTANCE) - val rampTime = Math.sqrt(2.0 * DISTANCE / accel) - val timeSamples: MutableList = ArrayList() - val positionSamples: MutableList = ArrayList() - val powerSamples: MutableList = ArrayList() - drive.poseEstimate = Pose2d() - var startTime = clock.seconds() - while (!isStopRequested) { - val elapsedTime = clock.seconds() - startTime - if (elapsedTime > rampTime) { - break - } - val vel = accel * elapsedTime - val power = vel / maxVel - timeSamples.add(elapsedTime) - positionSamples.add(drive.poseEstimate.x) - powerSamples.add(power) - drive.setDrivePower(Pose2d(power, 0.0, 0.0)) - drive.updatePoseEstimate() - } - drive.setDrivePower(Pose2d(0.0, 0.0, 0.0)) - val rampResult = RegressionUtil.fitRampData( - timeSamples, positionSamples, powerSamples, fitIntercept, - LoggingUtil.getLogFile( - Misc.formatInvariant( - "DriveRampRegression-%d.csv", System.currentTimeMillis() - ) - ) - ) - telemetry.clearAll() - telemetry.addLine("Quasi-static ramp up test complete") - if (fitIntercept) { - telemetry.addLine( - Misc.formatInvariant( - "kV = %.5f, kStatic = %.5f (R^2 = %.2f)", - rampResult.kV, rampResult.kStatic, rampResult.rSquare - ) - ) - } else { - telemetry.addLine( - Misc.formatInvariant( - "kV = %.5f (R^2 = %.2f)", - rampResult.kStatic, rampResult.rSquare - ) - ) - } - telemetry.addLine("Would you like to fit kA?") - telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no") - telemetry.update() - var fitAccelFF = false - while (!isStopRequested) { - if (gamepad1.y) { - fitAccelFF = true - while (!isStopRequested && gamepad1.y) { - idle() - } - break - } else if (gamepad1.b) { - while (!isStopRequested && gamepad1.b) { - idle() - } - break - } - idle() - } - if (fitAccelFF) { - telemetry.clearAll() - telemetry.addLine("Place the robot back in its starting position") - telemetry.addLine("Press (Y/Δ) to continue") - telemetry.update() - while (!isStopRequested && !gamepad1.y) { - idle() - } - while (!isStopRequested && gamepad1.y) { - idle() - } - telemetry.clearAll() - telemetry.addLine("Running...") - telemetry.update() - val maxPowerTime = DISTANCE / maxVel - timeSamples.clear() - positionSamples.clear() - powerSamples.clear() - drive.poseEstimate = Pose2d() - drive.setDrivePower(Pose2d(MAX_POWER, 0.0, 0.0)) - startTime = clock.seconds() - while (!isStopRequested) { - val elapsedTime = clock.seconds() - startTime - if (elapsedTime > maxPowerTime) { - break - } - timeSamples.add(elapsedTime) - positionSamples.add(drive.poseEstimate.x) - powerSamples.add(MAX_POWER) - drive.updatePoseEstimate() - } - drive.setDrivePower(Pose2d(0.0, 0.0, 0.0)) - val accelResult = RegressionUtil.fitAccelData( - timeSamples, positionSamples, powerSamples, rampResult, - LoggingUtil.getLogFile( - Misc.formatInvariant( - "DriveAccelRegression-%d.csv", System.currentTimeMillis() - ) - ) - ) - telemetry.clearAll() - telemetry.addLine("Constant power test complete") - telemetry.addLine( - Misc.formatInvariant( - "kA = %.5f (R^2 = %.2f)", - accelResult.kA, accelResult.rSquare - ) - ) - telemetry.update() - } - while (!isStopRequested) { - idle() - } - } - - companion object { - var MAX_POWER = 0.7 - var DISTANCE = 100.0 // in - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/BackAndForth.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/BackAndForth.kt deleted file mode 100644 index 7441dfb..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/BackAndForth.kt +++ /dev/null @@ -1,49 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.dashboard.config.Config -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -/* - * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base - * classes). The robot drives back and forth in a straight line indefinitely. Utilization of the - * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer - * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're - * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once - * you've successfully connected, start the program, and your robot will begin moving forward and - * backward. You should observe the target position (green) and your pose estimate (blue) and adjust - * your follower PID coefficients such that you follow the target position as accurately as possible. - * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. - * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. - * These coefficients can be tuned live in dashboard. - * - * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It - * is recommended that you use the FollowerPIDTuner opmode for further fine tuning. - */ -//@Disabled -@Config -@Autonomous(group = "drive") -class BackAndForth : LinearOpMode() { - @Throws(InterruptedException::class) - override fun runOpMode() { - val drive = DriveSubsystem(hardwareMap) - val trajectoryForward = drive.trajectoryBuilder(Pose2d()) - .forward(DISTANCE) - .build() - val trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end()) - .back(DISTANCE) - .build() - waitForStart() - while (opModeIsActive() && !isStopRequested) { - drive.followTrajectory(trajectoryForward) - drive.followTrajectory(trajectoryBackward) - } - } - - companion object { - var DISTANCE = 50.0 - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/DriveVelocityPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/DriveVelocityPIDTuner.kt deleted file mode 100644 index 1448fbe..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/DriveVelocityPIDTuner.kt +++ /dev/null @@ -1,154 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.dashboard.FtcDashboard -import com.acmerobotics.dashboard.config.Config -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.profile.MotionProfile -import com.acmerobotics.roadrunner.profile.MotionProfileGenerator.generateSimpleMotionProfile -import com.acmerobotics.roadrunner.profile.MotionState -import com.acmerobotics.roadrunner.util.NanoClock -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.hardware.DcMotor -import com.qualcomm.robotcore.util.RobotLog -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants - -/* - * This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed- - * loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as - * important as the positional parameters. Like the other manual tuning routines, this op mode - * relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's - * WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC - * phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully - * connected, start the program, and your robot will begin moving forward and backward according to - * a motion profile. Your job is to graph the velocity errors over time and adjust the PID - * coefficients (note: the tuning variable will not appear until the op mode finishes initializing). - * Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the - * MOTOR_VELO_PID field. - * - * Recommended tuning process: - * - * 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to - * mitigate oscillations. - * 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached. - * 3. Back off kP and kD a little until the response is less oscillatory (but without lag). - * - * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the - * user to reset the position of the bot in the event that it drifts off the path. - * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. - */ -//@Disabled -@Config -@Autonomous(group = "drive") -class DriveVelocityPIDTuner : LinearOpMode() { - override fun runOpMode() { - if (!DriveConstants.RUN_USING_ENCODER) { - RobotLog.setGlobalErrorMsg( - "%s does not need to be run if the built-in motor velocity" + - "PID is not in use", javaClass.simpleName - ) - } - telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) - val drive = DriveSubsystem(hardwareMap) - var mode = Mode.TUNING_MODE - var lastKp = DriveConstants.MOTOR_VELO_PID.p - var lastKi = DriveConstants.MOTOR_VELO_PID.i - var lastKd = DriveConstants.MOTOR_VELO_PID.d - var lastKf = DriveConstants.MOTOR_VELO_PID.f - drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID) - val clock = NanoClock.system() - telemetry.addLine("Ready!") - telemetry.update() - telemetry.clearAll() - waitForStart() - if (isStopRequested) return - var movingForwards = true - var activeProfile = generateProfile(true) - var profileStart = clock.seconds() - while (!isStopRequested) { - telemetry.addData("mode", mode) - when (mode) { - Mode.TUNING_MODE -> { - if (gamepad1.y) { - mode = Mode.DRIVER_MODE - drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER) - } - - // calculate and set the motor power - val profileTime = clock.seconds() - profileStart - if (profileTime > activeProfile.duration()) { - // generate a new profile - movingForwards = !movingForwards - activeProfile = generateProfile(movingForwards) - profileStart = clock.seconds() - } - val motionState = activeProfile[profileTime] - val targetPower = DriveConstants.kV * motionState.v - drive.setDrivePower(Pose2d(targetPower, 0.0, 0.0)) - val velocities = drive.getWheelVelocities() - - // update telemetry - telemetry.addData("targetVelocity", motionState.v) - var i = 0 - while (i < velocities.size) { - telemetry.addData("measuredVelocity$i", velocities[i]) - telemetry.addData( - "error$i", - motionState.v - velocities[i] - ) - i++ - } - } - - Mode.DRIVER_MODE -> { - if (gamepad1.b) { - drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER) - mode = Mode.TUNING_MODE - movingForwards = true - activeProfile = generateProfile(movingForwards) - profileStart = clock.seconds() - } - drive.setWeightedDrivePower( - Pose2d( - -gamepad1.left_stick_y.toDouble(), - -gamepad1.left_stick_x.toDouble(), - -gamepad1.right_stick_x.toDouble() - ) - ) - } - } - if (lastKp != DriveConstants.MOTOR_VELO_PID.p || lastKd != DriveConstants.MOTOR_VELO_PID.d || lastKi != DriveConstants.MOTOR_VELO_PID.i || lastKf != DriveConstants.MOTOR_VELO_PID.f) { - drive.setPIDFCoefficients( - DcMotor.RunMode.RUN_USING_ENCODER, - DriveConstants.MOTOR_VELO_PID - ) - lastKp = DriveConstants.MOTOR_VELO_PID.p - lastKi = DriveConstants.MOTOR_VELO_PID.i - lastKd = DriveConstants.MOTOR_VELO_PID.d - lastKf = DriveConstants.MOTOR_VELO_PID.f - } - telemetry.update() - } - } - - internal enum class Mode { - DRIVER_MODE, TUNING_MODE - } - - companion object { - var DISTANCE = 72.0 // in - private fun generateProfile(movingForward: Boolean): MotionProfile { - val start = MotionState(if (movingForward) 0.0 else DISTANCE, 0.0, 0.0, 0.0) - val goal = MotionState(if (movingForward) DISTANCE else 0.0, 0.0, 0.0, 0.0) - return generateSimpleMotionProfile( - start, - goal, - DriveConstants.MAX_VEL, - DriveConstants.MAX_ACCEL - ) - } - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/FollowerPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/FollowerPIDTuner.kt deleted file mode 100644 index cd31e54..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/FollowerPIDTuner.kt +++ /dev/null @@ -1,52 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.dashboard.config.Config -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -/* - * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base - * classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the - * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer - * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're - * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once - * you've successfully connected, start the program, and your robot will begin driving in a square. - * You should observe the target position (green) and your pose estimate (blue) and adjust your - * follower PID coefficients such that you follow the target position as accurately as possible. - * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. - * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. - * These coefficients can be tuned live in dashboard. - */ -//@Disabled -@Config -@Autonomous(group = "drive") -class FollowerPIDTuner : LinearOpMode() { - @Throws(InterruptedException::class) - override fun runOpMode() { - val drive = DriveSubsystem(hardwareMap) - val startPose = Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0.0) - drive.poseEstimate = startPose - waitForStart() - if (isStopRequested) return - while (!isStopRequested) { - val trajSeq = drive.trajectorySequenceBuilder(startPose) - .forward(DISTANCE) - .turn(Math.toRadians(90.0)) - .forward(DISTANCE) - .turn(Math.toRadians(90.0)) - .forward(DISTANCE) - .turn(Math.toRadians(90.0)) - .forward(DISTANCE) - .turn(Math.toRadians(90.0)) - .build() - drive.followTrajectorySequence(trajSeq) - } - } - - companion object { - var DISTANCE = 48.0 // in - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/LocalizationTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/LocalizationTest.kt deleted file mode 100644 index c94f85d..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/LocalizationTest.kt +++ /dev/null @@ -1,41 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.eventloop.opmode.TeleOp -import com.qualcomm.robotcore.hardware.DcMotor -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -/** - * This is a simple teleop routine for testing localization. Drive the robot around like a normal - * teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight - * errors are not out of the ordinary, especially with sudden drive motions). The goal of this - * exercise is to ascertain whether the localizer has been configured properly (note: the pure - * encoder localizer heading may be significantly off if the track width has not been tuned). - */ -//@Disabled -@TeleOp(group = "drive") -class LocalizationTest : LinearOpMode() { - @Throws(InterruptedException::class) - override fun runOpMode() { - val drive = DriveSubsystem(hardwareMap) - drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER) - waitForStart() - while (!isStopRequested) { - drive.setWeightedDrivePower( - Pose2d( - -gamepad1.left_stick_y.toDouble(), - -gamepad1.left_stick_x.toDouble(), - -gamepad1.right_stick_x.toDouble() - ) - ) - drive.update() - val (x, y, heading) = drive.poseEstimate - telemetry.addData("x", x) - telemetry.addData("y", y) - telemetry.addData("heading", Math.toDegrees(heading)) - telemetry.update() - } - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/ManualFeedforwardTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/ManualFeedforwardTuner.kt deleted file mode 100644 index 2a731d9..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/ManualFeedforwardTuner.kt +++ /dev/null @@ -1,131 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.dashboard.FtcDashboard -import com.acmerobotics.dashboard.config.Config -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.kinematics.Kinematics.calculateMotorFeedforward -import com.acmerobotics.roadrunner.profile.MotionProfile -import com.acmerobotics.roadrunner.profile.MotionProfileGenerator.generateSimpleMotionProfile -import com.acmerobotics.roadrunner.profile.MotionState -import com.acmerobotics.roadrunner.util.NanoClock -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.util.RobotLog -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.MAX_ACCEL -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.MAX_VEL -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.RUN_USING_ENCODER -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.kA -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.kStatic -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.kV - -/* - * This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary, - * tuning these coefficients is just as important as the positional parameters. Like the other - * manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard, - * connect your computer to the RC's WiFi network. In your browser, navigate to - * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if - * you are using the Control Hub. Once you've successfully connected, start the program, and your - * robot will begin moving forward and backward according to a motion profile. Your job is to graph - * the velocity errors over time and adjust the feedforward coefficients. Once you've found a - * satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file. - * - * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the - * user to reset the position of the bot in the event that it drifts off the path. - * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. - */ -//@Disabled -@Config -@Autonomous(group = "drive") -class ManualFeedforwardTuner : LinearOpMode() { - private val dashboard = FtcDashboard.getInstance() - private lateinit var drive: DriveSubsystem - private lateinit var mode: Mode - override fun runOpMode() { - if (RUN_USING_ENCODER) { - RobotLog.setGlobalErrorMsg( - "Feedforward constants usually don't need to be tuned " + - "when using the built-in drive motor velocity PID." - ) - } - telemetry = MultipleTelemetry(telemetry, dashboard.telemetry) - drive = DriveSubsystem(hardwareMap) - mode = Mode.TUNING_MODE - val clock = NanoClock.system() - telemetry.addLine("Ready!") - telemetry.update() - telemetry.clearAll() - waitForStart() - if (isStopRequested) return - var movingForwards = true - var activeProfile = generateProfile(true) - var profileStart = clock.seconds() - while (!isStopRequested) { - telemetry.addData("mode", mode) - when (mode) { - Mode.TUNING_MODE -> { - if (gamepad1.y) { - mode = Mode.DRIVER_MODE - } - - // calculate and set the motor power - val profileTime = clock.seconds() - profileStart - if (profileTime > activeProfile.duration()) { - // generate a new profile - movingForwards = !movingForwards - activeProfile = generateProfile(movingForwards) - profileStart = clock.seconds() - } - - val motionState = activeProfile[profileTime] - val targetPower = - calculateMotorFeedforward(motionState.v, motionState.a, kV, kA, kStatic) - - drive.setDrivePower(Pose2d(targetPower, 0.0, 0.0)) - drive.updatePoseEstimate() - - val currentVelo = requireNotNull( - drive.poseVelocity - ) { "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer." }.x - - // update telemetry - telemetry.addData("targetVelocity", motionState.v) - telemetry.addData("measuredVelocity", currentVelo) - telemetry.addData("error", motionState.v - currentVelo) - } - - Mode.DRIVER_MODE -> { - if (gamepad1.b) { - mode = Mode.TUNING_MODE - movingForwards = true - activeProfile = generateProfile(movingForwards) - profileStart = clock.seconds() - } - drive.setWeightedDrivePower( - Pose2d( - -gamepad1.left_stick_y.toDouble(), - -gamepad1.left_stick_x.toDouble(), - -gamepad1.right_stick_x.toDouble() - ) - ) - } - } - telemetry.update() - } - } - - internal enum class Mode { - DRIVER_MODE, TUNING_MODE - } - - companion object { - private var DISTANCE = 90.0 // in - private fun generateProfile(movingForward: Boolean): MotionProfile { - val start = MotionState(if (movingForward) 0.0 else DISTANCE, 0.0, 0.0, 0.0) - val goal = MotionState(if (movingForward) DISTANCE else 0.0, 0.0, 0.0, 0.0) - return generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL) - } - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxAngularVeloTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxAngularVeloTuner.kt deleted file mode 100644 index f61aaac..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxAngularVeloTuner.kt +++ /dev/null @@ -1,70 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.dashboard.FtcDashboard -import com.acmerobotics.dashboard.config.Config -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.hardware.DcMotor -import com.qualcomm.robotcore.util.ElapsedTime -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -/** - * This routine is designed to calculate the maximum angular velocity your bot can achieve under load. - * - * - * Upon pressing start, your bot will turn at max power for RUNTIME seconds. - * - * - * Further fine tuning of MAX_ANG_VEL may be desired. - */ -//@Disabled -@Config -@Autonomous(group = "drive") -class MaxAngularVeloTuner : LinearOpMode() { - private var timer: ElapsedTime? = null - private var maxAngVelocity = 0.0 - - @Throws(InterruptedException::class) - override fun runOpMode() { - val drive = DriveSubsystem(hardwareMap) - drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER) - - telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) - telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds.") - telemetry.addLine("Please ensure you have enough space cleared.") - telemetry.addLine("") - telemetry.addLine("Press start when ready.") - telemetry.update() - - waitForStart() - - telemetry.clearAll() - telemetry.update() - - drive.setDrivePower(Pose2d(0.0, 0.0, 1.0)) - - timer = ElapsedTime() - - while (!isStopRequested && timer!!.seconds() < RUNTIME) { - drive.updatePoseEstimate() - - val (_, _, heading) = requireNotNull( - drive.poseVelocity - ) { "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer." } - - maxAngVelocity = heading.coerceAtLeast(maxAngVelocity) - } - drive.setDrivePower(Pose2d()) - telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity) - telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity)) - telemetry.update() - while (!isStopRequested) idle() - } - - companion object { - var RUNTIME = 4.0 - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt deleted file mode 100644 index 93dd051..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt +++ /dev/null @@ -1,87 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.dashboard.FtcDashboard -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.hardware.DcMotor -import com.qualcomm.robotcore.hardware.VoltageSensor -import com.qualcomm.robotcore.util.ElapsedTime -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.getMotorVelocityF - -/** - * This routine is designed to calculate the maximum velocity your bot can achieve under load. It - * will also calculate the effective kF value for your velocity PID. - * - * - * Upon pressing start, your bot will run at max power for RUNTIME seconds. - * - * - * Further fine tuning of kF may be desired. - */ -//@Disabled //@Config -@Autonomous(group = "drive") -class MaxVelocityTuner : LinearOpMode() { - private lateinit var timer: ElapsedTime - private var maxVelocity = 0.0 - private lateinit var batteryVoltageSensor: VoltageSensor - - @Throws(InterruptedException::class) - override fun runOpMode() { - val drive = DriveSubsystem(hardwareMap) - drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER) - - batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next() - - telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) - telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds.") - telemetry.addLine("Please ensure you have enough space cleared.") - telemetry.addLine("") - telemetry.addLine("Press start when ready.") - telemetry.update() - - waitForStart() - - telemetry.clearAll() - telemetry.update() - - drive.setDrivePower(Pose2d(1.0, 0.0, 0.0)) - - timer = ElapsedTime() - - while (!isStopRequested && timer.seconds() < RUNTIME) { - drive.updatePoseEstimate() - val poseVelo = requireNotNull( - drive.poseVelocity - ) { "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer." } - - maxVelocity = poseVelo.vec().norm().coerceAtLeast(maxVelocity) - } - - drive.setDrivePower(Pose2d()) - - val effectiveKf = getMotorVelocityF(veloInchesToTicks(maxVelocity)) - - telemetry.addData("Max Velocity", maxVelocity) - telemetry.addData( - "Voltage Compensated kF", - effectiveKf * batteryVoltageSensor.voltage / 12 - ) - - telemetry.update() - - while (!isStopRequested && opModeIsActive()) idle() - } - - private fun veloInchesToTicks(inchesPerSec: Double): Double { - return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV - } - - companion object { - var RUNTIME = 2.0 - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MotorDirectionDebugger.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MotorDirectionDebugger.kt deleted file mode 100644 index e035dcc..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MotorDirectionDebugger.kt +++ /dev/null @@ -1,79 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.dashboard.FtcDashboard -import com.acmerobotics.dashboard.config.Config -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.eventloop.opmode.TeleOp -import org.firstinspires.ftc.robotcore.external.Telemetry -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -/** - * X / ▢ - Front Left - * Y / Δ - Front Right - * B / O - Rear Right - * A / X - Rear Left - * The buttons are mapped to match the wheels spatially if you - * were to rotate the gamepad 45deg°. x/square is the front left - * ________ and each button corresponds to the wheel as you go clockwise - * / ______ \ - * ------------.-' _ '-..+ Front of Bot - * / _ ( Y ) _ \ ^ - * | ( X ) _ ( B ) | Front Left \ Front Right - * ___ '. ( A ) /| Wheel \ Wheel - * .' '. '-._____.-' .' (x/▢) \ (Y/Δ) - * | | | \ - * '.___.' '. | Rear Left \ Rear Right - * '. / Wheel \ Wheel - * \. .' (A/X) \ (B/O) - * \________/ - * - */ -//@Disabled -@Config -@TeleOp(group = "drive") -class MotorDirectionDebugger : LinearOpMode() { - @Throws(InterruptedException::class) - override fun runOpMode() { - telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) - val drive = DriveSubsystem(hardwareMap) - telemetry.addLine("Press play to begin the debugging opmode") - telemetry.update() - waitForStart() - if (isStopRequested) return - telemetry.clearAll() - telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML) - while (!isStopRequested) { - telemetry.addLine("Press each button to turn on its respective motor") - telemetry.addLine() - telemetry.addLine("Xbox/PS4 Button - MotorPDController") - telemetry.addLine("  X / ▢         - Front Left") - telemetry.addLine("  Y / Δ         - Front Right") - telemetry.addLine("  B / O         - Rear  Right") - telemetry.addLine("  A / X         - Rear  Left") - telemetry.addLine() - if (gamepad1.x) { - drive.setMotorPowers(MOTOR_POWER, 0.0, 0.0, 0.0) - telemetry.addLine("Running MotorPDController: Front Left") - } else if (gamepad1.y) { - drive.setMotorPowers(0.0, 0.0, 0.0, MOTOR_POWER) - telemetry.addLine("Running MotorPDController: Front Right") - } else if (gamepad1.b) { - drive.setMotorPowers(0.0, 0.0, MOTOR_POWER, 0.0) - telemetry.addLine("Running MotorPDController: Rear Right") - } else if (gamepad1.a) { - drive.setMotorPowers(0.0, MOTOR_POWER, 0.0, 0.0) - telemetry.addLine("Running MotorPDController: Rear Left") - } else { - drive.setMotorPowers(0.0, 0.0, 0.0, 0.0) - telemetry.addLine("Running MotorPDController: None") - } - telemetry.update() - } - } - - companion object { - var MOTOR_POWER = 0.7 - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/SplineTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/SplineTest.kt deleted file mode 100644 index afff7a9..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/SplineTest.kt +++ /dev/null @@ -1,38 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.geometry.Vector2d -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -/* - * This is an example of a more complex path to really test the tuning. - */ -//@Disabled -@Autonomous(group = "drive") -class SplineTest : LinearOpMode() { - @Throws(InterruptedException::class) - override fun runOpMode() { - val drive = DriveSubsystem(hardwareMap) - - waitForStart() - - if (isStopRequested) return - - val traj = drive.trajectoryBuilder(Pose2d()) - .splineTo(Vector2d(30.0, 30.0), 0.0) - .build() - - drive.followTrajectory(traj) - - sleep(2000) - - drive.followTrajectory( - drive.trajectoryBuilder(traj.end(), true) - .splineTo(Vector2d(0.0, 0.0), Math.toRadians(180.0)) - .build() - ) - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StrafeTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StrafeTest.kt deleted file mode 100644 index cf637cd..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StrafeTest.kt +++ /dev/null @@ -1,47 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.dashboard.FtcDashboard -import com.acmerobotics.dashboard.config.Config -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -/* - * This is a simple routine to test translational drive capabilities. - */ -//@Disabled -@Config -@Autonomous(group = "drive") -class StrafeTest : LinearOpMode() { - @Throws(InterruptedException::class) - override fun runOpMode() { - telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) - - val drive = DriveSubsystem(hardwareMap) - val trajectory = drive.trajectoryBuilder(Pose2d()) - .strafeRight(DISTANCE) - .build() - - waitForStart() - - if (isStopRequested) return - - drive.followTrajectory(trajectory) - - val (x, y, heading) = drive.poseEstimate - - telemetry.addData("finalX", x) - telemetry.addData("finalY", y) - telemetry.addData("finalHeading", heading) - telemetry.update() - - while (!isStopRequested && opModeIsActive()); - } - - companion object { - var DISTANCE = 60.0 // in - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StraightTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StraightTest.kt deleted file mode 100644 index cd63379..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StraightTest.kt +++ /dev/null @@ -1,41 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.dashboard.FtcDashboard -import com.acmerobotics.dashboard.config.Config -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -/* - * This is a simple routine to test translational drive capabilities. - */ -//@Disabled -@Config -@Autonomous(group = "drive") -class StraightTest : LinearOpMode() { - @Throws(InterruptedException::class) - override fun runOpMode() { - telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) - val drive = DriveSubsystem(hardwareMap) - val trajectory = drive.trajectoryBuilder(Pose2d()) - .forward(DISTANCE) - .build() - waitForStart() - if (isStopRequested) return - drive.followTrajectory(trajectory) - val (x, y, heading) = drive.poseEstimate - telemetry.addData("finalX", x) - telemetry.addData("finalY", y) - telemetry.addData("finalHeading", heading) - telemetry.update() - while (!isStopRequested && opModeIsActive()); - } - - companion object { - @JvmField - var DISTANCE = 60.0 // in - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackWidthTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackWidthTuner.kt deleted file mode 100644 index 740b73b..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackWidthTuner.kt +++ /dev/null @@ -1,81 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.dashboard.FtcDashboard -import com.acmerobotics.dashboard.config.Config -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.util.Angle.norm -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.util.MovingStatistics -import org.firstinspires.ftc.robotcore.internal.system.Misc -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants - -/* - * This routine determines the effective track width. The procedure works by executing a point turn - * with a given angle and measuring the difference between that angle and the actual angle (as - * indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient - * given angle / actual angle gives a multiplicative adjustment to the estimated track width - * (effective track width = estimated track width * given angle / actual angle). The routine repeats - * this procedure a few times and averages the values for additional accuracy. Note: a relatively - * accurate track width estimate is important or else the angular constraints will be thrown off. - */ -//@Disabled -@Config -@Autonomous(group = "drive") -class TrackWidthTuner : LinearOpMode() { - @Throws(InterruptedException::class) - override fun runOpMode() { - telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) - val drive = DriveSubsystem(hardwareMap) - // TODO: if you haven't already, set the localizer to something that doesn't depend on - // drive encoders for computing the heading - telemetry.addLine("Press play to begin the track width tuner routine") - telemetry.addLine("Make sure your robot has enough clearance to turn smoothly") - telemetry.update() - waitForStart() - if (isStopRequested) return - telemetry.clearAll() - telemetry.addLine("Running...") - telemetry.update() - val trackWidthStats = MovingStatistics(NUM_TRIALS) - for (i in 0 until NUM_TRIALS) { - drive.poseEstimate = Pose2d() - - // it is important to handle heading wraparounds - var headingAccumulator = 0.0 - var lastHeading = 0.0 - drive.turnAsync(Math.toRadians(ANGLE)) - while (!isStopRequested && drive.isBusy) { - val heading = drive.poseEstimate.heading - headingAccumulator += norm(heading - lastHeading) - lastHeading = heading - drive.update() - } - val trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator - trackWidthStats.add(trackWidth) - sleep(DELAY.toLong()) - } - telemetry.clearAll() - telemetry.addLine("Tuning complete") - telemetry.addLine( - Misc.formatInvariant( - "Effective track width = %.2f (SE = %.3f)", - trackWidthStats.mean, - trackWidthStats.standardDeviation / Math.sqrt(NUM_TRIALS.toDouble()) - ) - ) - telemetry.update() - while (!isStopRequested) { - idle() - } - } - - companion object { - var ANGLE = 180.0 // deg - var NUM_TRIALS = 5 - var DELAY = 1000 // ms - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelForwardOffsetTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelForwardOffsetTuner.kt deleted file mode 100644 index d21ba97..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelForwardOffsetTuner.kt +++ /dev/null @@ -1,99 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.dashboard.FtcDashboard -import com.acmerobotics.dashboard.config.Config -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.util.Angle.norm -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.util.MovingStatistics -import com.qualcomm.robotcore.util.RobotLog -import org.firstinspires.ftc.robotcore.internal.system.Misc -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.StandardTrackingWheelLocalizer - -/** - * This routine determines the effective forward offset for the lateral tracking wheel. - * The procedure executes a point turn at a given angle for a certain number of trials, - * along with a specified delay in milliseconds. The purpose of this is to track the - * change in the y position during the turn. The offset, or distance, of the lateral tracking - * wheel from the center or rotation allows the wheel to spin during a point turn, leading - * to an incorrect measurement for the y position. This creates an arc around around - * the center of rotation with an arc length of change in y and a radius equal to the forward - * offset. We can compute this offset by calculating (change in y position) / (change in heading) - * which returns the radius if the angle (change in heading) is in radians. This is based - * on the arc length formula of length = theta * radius. - * - * - * To run this routine, simply adjust the desired angle and specify the number of trials - * and the desired delay. Then, run the procedure. Once it finishes, it will print the - * average of all the calculated forward offsets derived from the calculation. This calculated - * forward offset is then added onto the current forward offset to produce an overall estimate - * for the forward offset. You can run this procedure as many times as necessary until a - * satisfactory result is produced. - */ -//@Disabled -@Config -@Autonomous(group = "drive") -class TrackingWheelForwardOffsetTuner : LinearOpMode() { - @Throws(InterruptedException::class) - override fun runOpMode() { - telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) - val drive = DriveSubsystem(hardwareMap) - if (drive.localizer !is StandardTrackingWheelLocalizer) { - RobotLog.setGlobalErrorMsg( - "StandardTrackingWheelLocalizer is not being set in the " - + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer" - + "(hardwareMap));\" is called in SampleMecanumDrive.java" - ) - } - telemetry.addLine("Press play to begin the forward offset tuner") - telemetry.addLine("Make sure your robot has enough clearance to turn smoothly") - telemetry.update() - waitForStart() - if (isStopRequested) return - telemetry.clearAll() - telemetry.addLine("Running...") - telemetry.update() - val forwardOffsetStats = MovingStatistics(NUM_TRIALS) - for (i in 0 until NUM_TRIALS) { - drive.poseEstimate = Pose2d() - - // it is important to handle heading wraparounds - var headingAccumulator = 0.0 - var lastHeading = 0.0 - drive.turnAsync(Math.toRadians(ANGLE)) - while (!isStopRequested && drive.isBusy) { - val heading = drive.poseEstimate.heading - headingAccumulator += norm(heading - lastHeading) - lastHeading = heading - drive.update() - } - val forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET + - drive.poseEstimate.y / headingAccumulator - forwardOffsetStats.add(forwardOffset) - sleep(DELAY.toLong()) - } - telemetry.clearAll() - telemetry.addLine("Tuning complete") - telemetry.addLine( - Misc.formatInvariant( - "Effective forward offset = %.2f (SE = %.3f)", - forwardOffsetStats.mean, - forwardOffsetStats.standardDeviation / Math.sqrt(NUM_TRIALS.toDouble()) - ) - ) - telemetry.update() - while (!isStopRequested) { - idle() - } - } - - companion object { - var ANGLE = 180.0 // deg - var NUM_TRIALS = 5 - var DELAY = 1000 // ms - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelLateralDistanceTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelLateralDistanceTuner.kt deleted file mode 100644 index 32e4c47..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelLateralDistanceTuner.kt +++ /dev/null @@ -1,134 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.dashboard.config.Config -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.util.Angle.normDelta -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import com.qualcomm.robotcore.eventloop.opmode.TeleOp -import com.qualcomm.robotcore.util.RobotLog -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.StandardTrackingWheelLocalizer - -/** - * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s - * LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel - * wheels. - * - * - * Tuning Routine: - * - * - * 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical - * measured value. This need only be an estimated value as you will be tuning it anyways. - * - * - * 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an - * similar mark right below the indicator on your bot. This will be your reference point to - * ensure you've turned exactly 360°. - * - * - * 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help - * identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard, - * connect your computer to the RC's WiFi network. In your browser, navigate to - * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash - * if you are using the Control Hub. - * Ensure the field is showing (select the field view in top right of the page). - * - * - * 4. Press play to begin the tuning routine. - * - * - * 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise. - * - * - * 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns. - * - * - * 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators - * on the bot and on the ground you created earlier should be lined up. - * - * - * 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your - * StandardTrackingWheelLocalizer.java class. - * - * - * 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value - * yourself. Read the heading output and follow the advice stated in the note below to manually - * nudge the values yourself. - * - * - * Note: - * It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with - * a line from the circumference to the center should be present, representing the bot. The line - * indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in - * dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than - * the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the - * actual bot, the LATERAL_DISTANCE should be increased. - * - * - * If your drawn bot oscillates around a point in dashboard, don't worry. This is because the - * position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the - * effective center of rotation. You can ignore this effect. The center of rotation will be offset - * slightly but your heading will still be fine. This does not affect your overall tracking - * precision. The heading should still line up. - */ -//@Disabled -@Config -@TeleOp(group = "drive") -class TrackingWheelLateralDistanceTuner : LinearOpMode() { - @Throws(InterruptedException::class) - override fun runOpMode() { - val drive = DriveSubsystem(hardwareMap) - if (drive.localizer !is StandardTrackingWheelLocalizer) { - RobotLog.setGlobalErrorMsg( - "StandardTrackingWheelLocalizer is not being set in the " - + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer" - + "(hardwareMap));\" is called in SampleMecanumDrive.java" - ) - } - telemetry.addLine( - "Prior to beginning the routine, please read the directions " - + "located in the comments of the opmode file." - ) - telemetry.addLine("Press play to begin the tuning routine.") - telemetry.addLine("") - telemetry.addLine("Press Y/△ to stop the routine.") - telemetry.update() - waitForStart() - if (isStopRequested) return - telemetry.clearAll() - telemetry.update() - var headingAccumulator = 0.0 - var lastHeading = 0.0 - var tuningFinished = false - while (!isStopRequested && !tuningFinished) { - val vel = Pose2d(0.0, 0.0, -gamepad1.right_stick_x.toDouble()) - drive.setDrivePower(vel) - drive.update() - val heading = drive.poseEstimate.heading - val deltaHeading = heading - lastHeading - headingAccumulator += normDelta(deltaHeading) - lastHeading = heading - telemetry.clearAll() - telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator)) - telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading)) - telemetry.addLine() - telemetry.addLine("Press Y/△ to conclude routine") - telemetry.update() - if (gamepad1.y) tuningFinished = true - } - telemetry.clearAll() - telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°") - telemetry.addLine( - "Effective LATERAL_DISTANCE: " + - headingAccumulator / (NUM_TURNS * Math.PI * 2) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE - ) - telemetry.update() - while (!isStopRequested) idle() - } - - companion object { - var NUM_TURNS = 10 - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TurnTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TurnTest.kt deleted file mode 100644 index 5345621..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TurnTest.kt +++ /dev/null @@ -1,28 +0,0 @@ -package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner - -import com.acmerobotics.dashboard.config.Config -import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode -import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem - -/* - * This is a simple routine to test turning capabilities. - */ -//@Disabled -@Config -@Autonomous(group = "drive") -class TurnTest : LinearOpMode() { - @Throws(InterruptedException::class) - override fun runOpMode() { - val drive = DriveSubsystem(hardwareMap) - waitForStart() - if (isStopRequested) return - drive.turn(Math.toRadians(ANGLE)) - } - - companion object { - @JvmField - var ANGLE = 180.0 // deg - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt index 2d22513..c3deca6 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt @@ -1,306 +1,14 @@ package org.firstinspires.ftc.teamcode.subsystems.drive -import com.acmerobotics.dashboard.config.Config -import com.acmerobotics.roadrunner.control.PIDCoefficients -import com.acmerobotics.roadrunner.drive.MecanumDrive -import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower -import com.acmerobotics.roadrunner.followers.TrajectoryFollower -import com.acmerobotics.roadrunner.geometry.Pose2d -import com.acmerobotics.roadrunner.geometry.Vector2d -import com.acmerobotics.roadrunner.trajectory.Trajectory -import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder -import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint -import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint -import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint -import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint -import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint -import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint -import com.acmerobotics.roadrunner.util.Angle -import com.arcrobotics.ftclib.command.CommandScheduler -import com.arcrobotics.ftclib.command.Subsystem -import com.qualcomm.hardware.lynx.LynxModule -import com.qualcomm.robotcore.hardware.DcMotor -import com.qualcomm.robotcore.hardware.DcMotorEx -import com.qualcomm.robotcore.hardware.DcMotorSimple +import com.arcrobotics.ftclib.command.SubsystemBase import com.qualcomm.robotcore.hardware.HardwareMap -import com.qualcomm.robotcore.hardware.PIDFCoefficients -import com.qualcomm.robotcore.hardware.VoltageSensor -import org.firstinspires.ftc.teamcode.constants.ControlBoard -import org.firstinspires.ftc.teamcode.constants.PoseStorage -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants -import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.StandardTrackingWheelLocalizer -import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.TrajectorySequence -import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.TrajectorySequenceBuilder -import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.TrajectorySequenceRunner -import org.firstinspires.ftc.teamcode.utils.roadrunner.util.LynxModuleUtil -import kotlin.math.PI -import kotlin.math.abs -/* - * Simple mecanum drive hardware implementation for REV hardware. - */ -@Config -class DriveSubsystem @JvmOverloads constructor( - hardwareMap: HardwareMap, - private val fieldCentric: Boolean = false, -) : MecanumDrive( - DriveConstants.kV, - DriveConstants.kA, - DriveConstants.kStatic, - DriveConstants.TRACK_WIDTH, - DriveConstants.TRACK_WIDTH, - LATERAL_MULTIPLIER -), Subsystem { - private val trajectorySequenceRunner: TrajectorySequenceRunner - private val leftFront: DcMotorEx - private val leftRear: DcMotorEx - private val rightRear: DcMotorEx - private val rightFront: DcMotorEx - private val motors: List - private val batteryVoltageSensor: VoltageSensor +object DriveSubsystem : SubsystemBase() { + private lateinit var hardwareMap: HardwareMap - var m_name = this.javaClass.simpleName - - init { - val follower: TrajectoryFollower = HolonomicPIDVAFollower( - TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID, - Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5 - ) - - LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap) - - batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next() - - for (module in hardwareMap.getAll(LynxModule::class.java)) { - module.bulkCachingMode = LynxModule.BulkCachingMode.AUTO - } - - leftFront = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_LEFT_FRONT.deviceName) - leftRear = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_LEFT_REAR.deviceName) - rightRear = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_RIGHT_REAR.deviceName) - rightFront = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_RIGHT_FRONT.deviceName) - motors = listOf(leftFront, leftRear, rightRear, rightFront) - for (motor in motors) { - val motorConfigurationType = motor.motorType.clone() - motorConfigurationType.achieveableMaxRPMFraction = 1.0 - motor.motorType = motorConfigurationType - } - - if (DriveConstants.RUN_USING_ENCODER) { - setMode(DcMotor.RunMode.RUN_USING_ENCODER) - } - - setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE) - - if (DriveConstants.RUN_USING_ENCODER) { - setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID) - } - - // leftFront.direction = DcMotorSimple.Direction.REVERSE - leftRear.direction = DcMotorSimple.Direction.REVERSE - - localizer = StandardTrackingWheelLocalizer(hardwareMap) - - trajectorySequenceRunner = TrajectorySequenceRunner(follower, HEADING_PID) - - CommandScheduler.getInstance().registerSubsystem(this) - } - - @JvmOverloads - fun trajectoryBuilder( - startPose: Pose2d, - reversed: Boolean = false, - startHeading: Double = startPose.heading, - ): TrajectoryBuilder { - return TrajectoryBuilder( - startPose, - Angle.norm(startHeading + if (reversed) PI else 0.0), - VEL_CONSTRAINT, - ACCEL_CONSTRAINT - ) - } - - fun drive(leftY: Double, leftX: Double, rightX: Double) { - val (_, _, heading) = poseEstimate - - val (x, y) = Vector2d( - -leftY, - -leftX - ).rotated( - if (fieldCentric) -heading else 0.0 - ) - - setWeightedDrivePower(Pose2d(x, y, -rightX)) - - update() - PoseStorage.poseEstimate = poseEstimate - } - - fun trajectorySequenceBuilder(startPose: Pose2d): TrajectorySequenceBuilder { - return TrajectorySequenceBuilder( - startPose, - VEL_CONSTRAINT, ACCEL_CONSTRAINT, - DriveConstants.MAX_ANG_VEL, DriveConstants.MAX_ANG_ACCEL - ) - } - - fun turnAsync(angle: Double) { - trajectorySequenceRunner.followTrajectorySequenceAsync( - trajectorySequenceBuilder(poseEstimate) - .turn(angle) - .build() - ) - } - - fun turn(angle: Double) { - turnAsync(angle) - waitForIdle() - } - - fun followTrajectoryAsync(trajectory: Trajectory) { - trajectorySequenceRunner.followTrajectorySequenceAsync( - trajectorySequenceBuilder(trajectory.start()) - .addTrajectory(trajectory) - .build() - ) - } - - fun followTrajectory(trajectory: Trajectory) { - followTrajectoryAsync(trajectory) - waitForIdle() + fun initialize(hardwareMap: HardwareMap) { + this.hardwareMap = hardwareMap } - fun followTrajectorySequenceAsync(trajectorySequence: TrajectorySequence) { - trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence) - } - - fun followTrajectorySequence(trajectorySequence: TrajectorySequence) { - followTrajectorySequenceAsync(trajectorySequence) - waitForIdle() - } - - @Suppress("unused", "MemberVisibilityCanBePrivate") - val lastError: Pose2d - get() = trajectorySequenceRunner.lastPoseError - - fun update() { - updatePoseEstimate() - val signal = trajectorySequenceRunner.update(poseEstimate, poseVelocity) - signal?.let { setDriveSignal(it) } - } - - fun waitForIdle() { - while (!Thread.currentThread().isInterrupted && isBusy) update() - } - - val isBusy: Boolean - get() = trajectorySequenceRunner.isBusy - - fun setMode(runMode: DcMotor.RunMode?) { - for (motor in motors) { - motor.mode = runMode - } - } - - fun setZeroPowerBehavior(zeroPowerBehavior: DcMotor.ZeroPowerBehavior) { - for (motor in motors) { - motor.zeroPowerBehavior = zeroPowerBehavior - } - } - - fun setPIDFCoefficients(runMode: DcMotor.RunMode?, coefficients: PIDFCoefficients) { - val compensatedCoefficients = PIDFCoefficients( - coefficients.p, coefficients.i, coefficients.d, - coefficients.f * 12 / batteryVoltageSensor.voltage - ) - for (motor in motors) { - motor.setPIDFCoefficients(runMode, compensatedCoefficients) - } - } - - fun setWeightedDrivePower(drivePower: Pose2d) { - var vel = drivePower - if ((abs(drivePower.x) + abs(drivePower.y) - + abs(drivePower.heading)) > 1 - ) { - // re-normalize the powers according to the weights - val denom = - VX_WEIGHT * abs(drivePower.x) + VY_WEIGHT * abs(drivePower.y) + OMEGA_WEIGHT * abs( - drivePower.heading - ) - vel = Pose2d( - VX_WEIGHT * drivePower.x, - VY_WEIGHT * drivePower.y, - OMEGA_WEIGHT * drivePower.heading - ).div(denom) - } - setDrivePower(vel) - } - - override fun getWheelPositions(): List { - val wheelPositions: MutableList = ArrayList() - for (motor in motors) { - wheelPositions.add(DriveConstants.encoderTicksToInches(motor.currentPosition.toDouble())) - } - return wheelPositions - } - - @Suppress("unused") - override fun getWheelVelocities(): List { - val wheelVelocities: MutableList = ArrayList() - for (motor in motors) { - wheelVelocities.add(DriveConstants.encoderTicksToInches(motor.velocity)) - } - return wheelVelocities - } - - override fun setMotorPowers( - frontLeft: Double, - rearLeft: Double, - rearRight: Double, - frontRight: Double, - ) { - leftFront.power = frontLeft - leftRear.power = rearLeft - rightRear.power = rearRight - rightFront.power = frontRight - } - - override val rawExternalHeading: Double - get() = 0.0 - - override fun getExternalHeadingVelocity() = 0.0 - - companion object { - @JvmField - var TRANSLATIONAL_PID = PIDCoefficients(10.0, 0.0, 0.01) - @JvmField - var HEADING_PID = PIDCoefficients(10.0, 0.0, 0.01) - var LATERAL_MULTIPLIER = 1.0179744056670780318264660644901 - var VX_WEIGHT = 1.0 - var VY_WEIGHT = 1.0 - var OMEGA_WEIGHT = 1.0 - private val VEL_CONSTRAINT = getVelocityConstraint( - DriveConstants.MAX_VEL, - DriveConstants.MAX_ANG_VEL, - DriveConstants.TRACK_WIDTH - ) - private val ACCEL_CONSTRAINT = getAccelerationConstraint(DriveConstants.MAX_ACCEL) - fun getVelocityConstraint( - maxVel: Double, - maxAngularVel: Double, - trackWidth: Double, - ): TrajectoryVelocityConstraint { - return MinVelocityConstraint( - listOf( - AngularVelocityConstraint(maxAngularVel), - MecanumVelocityConstraint(maxVel, trackWidth) - ) - ) - } - - fun getAccelerationConstraint(maxAccel: Double): TrajectoryAccelerationConstraint { - return ProfileAccelerationConstraint(maxAccel) - } - } + fun drive(leftY: Double, leftX: Double, rightX: Double) = 0.0 } \ No newline at end of file