From 8336705662b752fd3a54d11ae45298d9227bd37e Mon Sep 17 00:00:00 2001 From: Khosraw Azizi Date: Sat, 5 Nov 2022 15:38:45 -0500 Subject: [PATCH] created red and blue substation parking behavior --- ...toBlueSubstationParkingBehaviorBottom.java | 55 +++++++++++++++++++ .../AutoBlueSubstationParkingBehaviorTop.java | 55 +++++++++++++++++++ ...utoRedSubstationParkingBehaviorBottom.java | 55 +++++++++++++++++++ .../AutoRedSubstationParkingBehaviorTop.java | 55 +++++++++++++++++++ 4 files changed, 220 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueSubstationParkingBehaviorBottom.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueSubstationParkingBehaviorTop.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedSubstationParkingBehaviorBottom.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedSubstationParkingBehaviorTop.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueSubstationParkingBehaviorBottom.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueSubstationParkingBehaviorBottom.java new file mode 100644 index 0000000..b2b50a9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueSubstationParkingBehaviorBottom.java @@ -0,0 +1,55 @@ +package org.firstinspires.ftc.teamcode.createdcode.enhancedautos; + + 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.qualcomm.robotcore.eventloop.opmode.Autonomous; + import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +@Autonomous +public class AutoBlueSubstationParkingBehaviorBottom extends LinearOpMode { + SampleMecanumDrive mecanumDrive; + + @Override + public void runOpMode() throws InterruptedException { + mecanumDrive = new SampleMecanumDrive(hardwareMap); + + + waitForStart(); + + Trajectory firstMove = makeTrajectories( + new Pose2d(new Vector2d(-34, 60), 0), + new int[]{-34}, + new int[]{40} + ); + + mecanumDrive.followTrajectory(firstMove); + + double time = getRuntime() + 5; + /* wait for 5 seconds to allow color sensor to settle */ + while (true) { + if (!(getRuntime() < time)) break; // needed to stop android studio from yelling + } + + // init move to default terminal + Trajectory endpos = makeTrajectories(firstMove.end(), + new int[]{-34, 1}, + new int[]{60, 60} + ); + + mecanumDrive.followTrajectory(endpos); + } + + private Trajectory makeTrajectories(Pose2d startPos, int[] x, int[] y) { + assert x.length == y.length; + TrajectoryBuilder builder = mecanumDrive.trajectoryBuilder(startPos); + + for (int i = 0; i < x.length; i++) { + builder.splineTo(new Vector2d(x[i], y[i]), 0); + } + + return builder.build(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueSubstationParkingBehaviorTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueSubstationParkingBehaviorTop.java new file mode 100644 index 0000000..52f7a45 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueSubstationParkingBehaviorTop.java @@ -0,0 +1,55 @@ +package org.firstinspires.ftc.teamcode.createdcode.enhancedautos; + + 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.qualcomm.robotcore.eventloop.opmode.Autonomous; + import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +@Autonomous +public class AutoBlueSubstationParkingBehaviorTop extends LinearOpMode { + SampleMecanumDrive mecanumDrive; + + @Override + public void runOpMode() throws InterruptedException { + mecanumDrive = new SampleMecanumDrive(hardwareMap); + + + waitForStart(); + + Trajectory firstMove = makeTrajectories( + new Pose2d(new Vector2d(36, 60), 0), + new int[]{36}, + new int[]{40} + ); + + mecanumDrive.followTrajectory(firstMove); + + double time = getRuntime() + 5; + /* wait for 5 seconds to allow color sensor to settle */ + while (true) { + if (!(getRuntime() < time)) break; // needed to stop android studio from yelling + } + + // init move to default terminal + Trajectory endpos = makeTrajectories(firstMove.end(), + new int[]{36, 1}, + new int[]{60, 60} + ); + + mecanumDrive.followTrajectory(endpos); + } + + private Trajectory makeTrajectories(Pose2d startPos, int[] x, int[] y) { + assert x.length == y.length; + TrajectoryBuilder builder = mecanumDrive.trajectoryBuilder(startPos); + + for (int i = 0; i < x.length; i++) { + builder.splineTo(new Vector2d(x[i], y[i]), 0); + } + + return builder.build(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedSubstationParkingBehaviorBottom.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedSubstationParkingBehaviorBottom.java new file mode 100644 index 0000000..5cf1e3d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedSubstationParkingBehaviorBottom.java @@ -0,0 +1,55 @@ +package org.firstinspires.ftc.teamcode.createdcode.enhancedautos; + + 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.qualcomm.robotcore.eventloop.opmode.Autonomous; + import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +@Autonomous +public class AutoRedSubstationParkingBehaviorBottom extends LinearOpMode { + SampleMecanumDrive mecanumDrive; + + @Override + public void runOpMode() throws InterruptedException { + mecanumDrive = new SampleMecanumDrive(hardwareMap); + + + waitForStart(); + + Trajectory firstMove = makeTrajectories( + new Pose2d(new Vector2d(-34, -60), 0), + new int[]{-34}, + new int[]{-40} + ); + + mecanumDrive.followTrajectory(firstMove); + + double time = getRuntime() + 5; + /* wait for 5 seconds to allow color sensor to settle */ + while (true) { + if (!(getRuntime() < time)) break; // needed to stop android studio from yelling + } + + // init move to default terminal + Trajectory endpos = makeTrajectories(firstMove.end(), + new int[]{-34, 1}, + new int[]{-60, -60} + ); + + mecanumDrive.followTrajectory(endpos); + } + + private Trajectory makeTrajectories(Pose2d startPos, int[] x, int[] y) { + assert x.length == y.length; + TrajectoryBuilder builder = mecanumDrive.trajectoryBuilder(startPos); + + for (int i = 0; i < x.length; i++) { + builder.splineTo(new Vector2d(x[i], y[i]), 0); + } + + return builder.build(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedSubstationParkingBehaviorTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedSubstationParkingBehaviorTop.java new file mode 100644 index 0000000..24736bc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedSubstationParkingBehaviorTop.java @@ -0,0 +1,55 @@ +package org.firstinspires.ftc.teamcode.createdcode.enhancedautos; + + 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.qualcomm.robotcore.eventloop.opmode.Autonomous; + import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +@Autonomous +public class AutoRedSubstationParkingBehaviorTop extends LinearOpMode { + SampleMecanumDrive mecanumDrive; + + @Override + public void runOpMode() throws InterruptedException { + mecanumDrive = new SampleMecanumDrive(hardwareMap); + + + waitForStart(); + + Trajectory firstMove = makeTrajectories( + new Pose2d(new Vector2d(36, -60), 0), + new int[]{36}, + new int[]{-40} + ); + + mecanumDrive.followTrajectory(firstMove); + + double time = getRuntime() + 5; + /* wait for 5 seconds to allow color sensor to settle */ + while (true) { + if (!(getRuntime() < time)) break; // needed to stop android studio from yelling + } + + // init move to default terminal + Trajectory endpos = makeTrajectories(firstMove.end(), + new int[]{36, 1}, + new int[]{-60, -60} + ); + + mecanumDrive.followTrajectory(endpos); + } + + private Trajectory makeTrajectories(Pose2d startPos, int[] x, int[] y) { + assert x.length == y.length; + TrajectoryBuilder builder = mecanumDrive.trajectoryBuilder(startPos); + + for (int i = 0; i < x.length; i++) { + builder.splineTo(new Vector2d(x[i], y[i]), 0); + } + + return builder.build(); + } +}