From aa496b8237728f44c0b8b141caa89660a9970fba Mon Sep 17 00:00:00 2001 From: carlos Date: Sun, 8 Dec 2024 12:48:30 -0800 Subject: [PATCH] Ollie's work committed --- .../firstinspires/ftc/teamcode/NetAuto.java | 64 ++++++++++++++++ .../ftc/teamcode/PedroConstants.java | 7 +- .../ftc/teamcode/subsystem/AutoLine1.java | 44 +++++++++++ .../ftc/teamcode/subsystem/AutoLine2.java | 40 ++++++++++ .../ftc/teamcode/subsystem/AutoLine3.java | 73 +++++++++++++++++++ 5 files changed, 225 insertions(+), 3 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine3.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java new file mode 100644 index 0000000..b705544 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java @@ -0,0 +1,64 @@ +package org.firstinspires.ftc.teamcode; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.SleepAction; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.subsystem.AutoLine1; +import org.firstinspires.ftc.teamcode.subsystem.AutoLine2; +import org.firstinspires.ftc.teamcode.subsystem.AutoLine3; + +@Autonomous(name = "BlueNetAuto", group = "Dev") +public class NetAuto extends OpMode { + + public Follower follower; + + public AutoLine1 myFirstPath = new AutoLine1(); + public AutoLine2 mySecondPath = new AutoLine2(); + public int pathState = 0; + @Override + public void init() { + follower = new Follower(hardwareMap); + follower.setMaxPower(0.65); + myFirstPath.moveToAutoLine1(follower); + } + + @Override + public void loop() { + follower.update(); + switch(pathState) { + case 0: + if (!follower.isBusy()) { + pathState = 1; + mySecondPath.moveToAutoLine2(follower); + } + case 1: + if (!follower.isBusy()) { + System.out.println("Finished"); + } + } +// switch(pathState) { +// case 0: +// if (!follower.isBusy()) { +// mySecondPath.moveToAutoLine2(follower); +// pathState = 1; +// } +// case 1: +// if (!follower.isBusy()) { +// pathState = 2; +// } +// case 2: +// // set path 3 +// // as if busy, if not, set path 4 and so on. +// System.out.print("we're at the end"); +// +// } + + + + follower.telemetryDebug(telemetry); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java index 06f9aa7..6e2db74 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java @@ -38,9 +38,10 @@ public class PedroConstants { = RevHubOrientationOnRobot.UsbFacingDirection.UP; // Robot encoders - public static final String LEFT_ENCODER = "encoder-left"; - public static final String RIGHT_ENCODER = "encoder-right"; - public static final String BACK_ENCODER = "encoder-back"; + // NOTE: Encoders are plugged into the same ports as motors hence the weird names + public static final String RIGHT_ENCODER = "back-right"; //0 + public static final String BACK_ENCODER = "front-right"; //1 + public static final String LEFT_ENCODER = "front-left"; //2 // Robot encoder direction public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java new file mode 100644 index 0000000..6ebc8f5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + + + +/* + AutoLine# - This file does something of a path...... + + */ +public class AutoLine1 { + + private PathChain pathChain; + private Pose autoLin1StartPose = new Pose(8,65); + + public void moveToAutoLine1(Follower robot) { + PathBuilder builder = new PathBuilder(); + builder + .addPath( + // Line 1 + new BezierLine( + new Point(8.000, 65.000, Point.CARTESIAN), + new Point(30.000, 72.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)); + pathChain = builder.build(); + robot.setStartingPose(autoLin1StartPose); + robot.followPath(pathChain); + } + +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java new file mode 100644 index 0000000..eb7382f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java @@ -0,0 +1,40 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + + +/* + AutoLine# - This file does something of a path...... + + */ +public class AutoLine2 { + private PathChain pathChain; + public void moveToAutoLine2(Follower robot) { + PathBuilder builder = new PathBuilder(); + builder + .addPath( + // Line 2 + new BezierCurve( + new Point(36.000, 72.000, Point.CARTESIAN), + new Point(24.000, 24.000, Point.CARTESIAN), + new Point(72.000, 36.000, Point.CARTESIAN), + new Point(56.000, 24.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)); + pathChain = builder.build(); + robot.followPath(pathChain); + } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine3.java new file mode 100644 index 0000000..2a7d8d1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine3.java @@ -0,0 +1,73 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + + +/* + AutoLine# - This file does something of a path...... + + */ +public class AutoLine3 implements Action { + + private Follower actionRobot; + private PathChain pathChain; + + private Pose startPose = new Pose(56,24); + + public AutoLine3(Follower robot) { + this.actionRobot = robot; + this.actionRobot.setStartingPose(startPose); + + PathBuilder builder = new PathBuilder(); + builder + /* .addPath( + // Line 1 + new BezierLine( + new Point(8.000, 65.000, Point.CARTESIAN), + new Point(30.000, 72.000, Point.CARTESIAN) +// This is the origional new Point(36.000, 72.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 2 + new BezierCurve( + new Point(36.000, 72.000, Point.CARTESIAN), + new Point(24.000, 24.000, Point.CARTESIAN), + new Point(72.000, 36.000, Point.CARTESIAN), + new Point(56.000, 24.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)) */ + .addPath( + // Line 3 + new BezierLine( + new Point(56.000, 24.000, Point.CARTESIAN), + new Point(18.000, 24.000, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation(); + + pathChain = builder.build(); + + this.actionRobot.followPath(this.pathChain); + } + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + this.actionRobot.update(); + return this.actionRobot.isBusy(); + } +} +