diff --git a/.idea/misc.xml b/.idea/misc.xml index 3b0be22..6c5519f 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,4 +1,3 @@ - diff --git a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/basket/BlueHighBasket1.kt b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/basket/BlueHighBasket1.kt index 8b8915a..ec37eec 100644 --- a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/basket/BlueHighBasket1.kt +++ b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/basket/BlueHighBasket1.kt @@ -25,7 +25,7 @@ object BlueHighBasket1 { } var img: Image? = null try { - img = ImageIO.read(File("/Users/ishaanghaskadbi/Downloads/field.png")) + img = ImageIO.read(File("C://Users//arava//Downloads//field.png/")) } catch (_ : IOException) { } diff --git a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt new file mode 100644 index 0000000..87f70a3 --- /dev/null +++ b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt @@ -0,0 +1,117 @@ +package com.example.pathplanning.blue.main + +import com.acmerobotics.roadrunner.geometry.Pose2d +import com.noahbres.meepmeep.MeepMeep +import com.noahbres.meepmeep.core.toRadians +import com.noahbres.meepmeep.roadrunner.DefaultBotBuilder +import java.awt.Image +import java.io.File +import java.io.IOException +import javax.imageio.ImageIO + +object BlueAuto { + @JvmStatic + + fun main(args: Array) { + val meepMeep = MeepMeep(800) + + val myBot = + DefaultBotBuilder(meepMeep) // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(30.0, 30.0, Math.toRadians(170.0), Math.toRadians(170.0), 13.94) + .followTrajectorySequence { drive -> + drive.trajectorySequenceBuilder(Pose2d(-12.0, 62.0, (270.0).toRadians())) + .addTemporalMarker(0.1,) { + //add the command for the arm to go up + //add command for belt servo to go backwards + //add the command to add the slides go up + } + .splineToLinearHeading(Pose2d(55.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) + .addDisplacementMarker { + //add command for rollers servo to outtake + } + .turn(Math.toRadians(30.0)) // + .addTemporalMarker(6.0) { + //add command for arm to go down + //add command for slides to go out + //add command to belt servo to go at [x] angle + } + .addDisplacementMarker { + //add command to intake + } + .turn(Math.toRadians(-30.0)) // + .addTemporalMarker() { + //add command to bring slides in + //add command to bring arm up + //add command to put slides out + //add command to turn belt servo + } + .addDisplacementMarker { + //add command to outtake + } + .turn(Math.toRadians(55.0)) // + .addTemporalMarker() { + //add command to bring slides in + //add command to bring arm down + //add command to put slides out + //add command to turn belt servo + } + .addDisplacementMarker { + //add command to intake + } + .turn(Math.toRadians(-55.0)) // + .addTemporalMarker() { + //add command to bring slides in + //add command to bring arm up + //add command to put slides out + //add command to turn belt servo + } + .addDisplacementMarker { + //add command to outtake + } + .turn(Math.toRadians(80.0)) // + .addTemporalMarker() { + //add command to bring slides in + //add command to bring arm down + //add command to put slides out + //add command to turn belt servo + } + .addDisplacementMarker { + //add command to intake + } + .turn(Math.toRadians(-80.0)) // + .addTemporalMarker() { + //add command to bring slides in + //add command to bring arm up + //add command to put slides out + //add command to turn belt servo + } + .addDisplacementMarker { + //add command to outtake + } + .splineToSplineHeading(Pose2d(36.0, 12.0, Math.toRadians(-180.0)), Math.toRadians(-90.0)) + .addDisplacementMarker { + //add command to bring arm up + //add command to bring slides out + } + + + + + .build() + } + var img: Image? = null + try { + img = ImageIO.read(File("C://Users//arava//Downloads//field.png/")) + } catch (_ : IOException) { + + } + + if (img != null) { + meepMeep.setBackground(img) + .setDarkMode(true) + .setBackgroundAlpha(0.95f) + .addEntity(myBot) + .start() + } + } +} \ No newline at end of file diff --git a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAutoLeft.kt b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAutoLeft.kt index c80e7bf..c12c9cb 100644 --- a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAutoLeft.kt +++ b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAutoLeft.kt @@ -20,15 +20,88 @@ object BlueAutoLeft { .setConstraints(30.0, 30.0, Math.toRadians(170.0), Math.toRadians(170.0), 13.94) .followTrajectorySequence { drive -> drive.trajectorySequenceBuilder(Pose2d(-12.0, 62.0, (270.0).toRadians())) - .turn((90.0).toRadians()) - .forward(60.0) - .waitSeconds(4.0) + .addTemporalMarker(0.1,) { + //add the command for the arm to go up + //add command for belt servo to go backwards + //add the command to add the slides go up + } + .splineToLinearHeading(Pose2d(55.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) + .addDisplacementMarker { + //add command for rollers servo to outtake + } + .turn(Math.toRadians(30.0)) // + .addTemporalMarker(6.0) { + //add command for arm to go down + //add command for slides to go out + //add command to belt servo to go at [x] angle + } + .addDisplacementMarker { + //add command to intake + } + .turn(Math.toRadians(-30.0)) // + .addTemporalMarker() { + //add command to bring slides in + //add command to bring arm up + //add command to put slides out + //add command to turn belt servo + } + .addDisplacementMarker { + //add command to outtake + } + .turn(Math.toRadians(55.0)) // + .addTemporalMarker() { + //add command to bring slides in + //add command to bring arm down + //add command to put slides out + //add command to turn belt servo + } + .addDisplacementMarker { + //add command to intake + } + .turn(Math.toRadians(-55.0)) // + .addTemporalMarker() { + //add command to bring slides in + //add command to bring arm up + //add command to put slides out + //add command to turn belt servo + } + .addDisplacementMarker { + //add command to outtake + } + .turn(Math.toRadians(80.0)) // + .addTemporalMarker() { + //add command to bring slides in + //add command to bring arm down + //add command to put slides out + //add command to turn belt servo + } + .addDisplacementMarker { + //add command to intake + } + .turn(Math.toRadians(-80.0)) // + .addTemporalMarker() { + //add command to bring slides in + //add command to bring arm up + //add command to put slides out + //add command to turn belt servo + } + .addDisplacementMarker { + //add command to outtake + } + .splineToSplineHeading(Pose2d(36.0, 12.0, Math.toRadians(-180.0)), Math.toRadians(-90.0)) + .addDisplacementMarker { + //add command to bring arm up + //add command to bring slides out + } + + + .build() } var img: Image? = null try { - img = ImageIO.read(File("/Users/ishaanghaskadbi/Downloads/field.png")) + img = ImageIO.read(File("C://Users//arava//Downloads//field.png/")) } catch (_ : IOException) { } 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 index 79b1af2..2a731d9 100644 --- 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 @@ -121,7 +121,7 @@ class ManualFeedforwardTuner : LinearOpMode() { } companion object { - private var DISTANCE = 110.0 // in + 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) 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 index 9854198..cd63379 100644 --- 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 @@ -35,6 +35,7 @@ class StraightTest : LinearOpMode() { } 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/TurnTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TurnTest.kt index 10ec0f6..5345621 100644 --- 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 @@ -22,6 +22,7 @@ class TurnTest : LinearOpMode() { } companion object { - var ANGLE = 90.0 // deg + @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 a813808..a535e01 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 @@ -272,9 +272,11 @@ class DriveSubsystem @JvmOverloads constructor( override fun getExternalHeadingVelocity() = 0.0 companion object { - var TRANSLATIONAL_PID = PIDCoefficients(8.0, 0.0, 0.0) - var HEADING_PID = PIDCoefficients(8.0, 0.0, 0.0) - var LATERAL_MULTIPLIER = 1.0126582278481012658227848101266 + @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