From 83da8e0de099a1b4d8855305279b26bc93788dd1 Mon Sep 17 00:00:00 2001 From: carlos Date: Tue, 5 Nov 2024 16:39:42 -0800 Subject: [PATCH 1/5] Open close of the gripper from "X" to "right_bumper" --- .../src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java index b6b46d2..304ca6c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java @@ -83,7 +83,7 @@ public class DevTeleop extends OpMode { } public void thePickup(ClawSubsystem claw) { - if (currentGamepad1.x && !previousGamepad1.x) { + if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) { claw.switchState(); } From 2a06f7e98d313f0e5703706e796a8246fb297dc8 Mon Sep 17 00:00:00 2001 From: Carlos Rivas Date: Tue, 5 Nov 2024 16:40:46 -0800 Subject: [PATCH 2/5] Added basic states for motor --- .../ftc/teamcode/subsystem/MotorsSubsystem.java | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java index 30fb764..e217312 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java @@ -18,7 +18,6 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; public class MotorsSubsystem { public HardwareMap hardwareMap; - public Telemetry telemetry; public DcMotor frontLeftMotor; @@ -26,6 +25,12 @@ public class MotorsSubsystem { public DcMotor frontRightMotor; public DcMotor backRightMotor; + public enum TravelState { + PARKED, BUCKET, SUBMARINE + } + + public TravelState travelState; + public double power; public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { @@ -114,4 +119,12 @@ public class MotorsSubsystem { this.telemetry.update(); } + public void setState(TravelState travelState) { + this.travelState = travelState; + } + + public TravelState getState() { + return this.travelState; + } + } From a55d1902d20370a293b7eb8f99d6137f0d29cb4a Mon Sep 17 00:00:00 2001 From: robotics2 Date: Tue, 5 Nov 2024 17:06:47 -0800 Subject: [PATCH 3/5] Asher's path code --- .../ftc/teamcode/AsherOrientBlue.java | 93 ++++++++++++ .../ftc/teamcode/AsherPathBlueV1.java | 133 ++++++++++++++++++ .../ftc/teamcode/PedroConstants.java | 1 + .../ftc/teamcode/configs/RobotConstants.java | 2 +- 4 files changed, 228 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherOrientBlue.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherPathBlueV1.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherOrientBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherOrientBlue.java new file mode 100644 index 0000000..f933253 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherOrientBlue.java @@ -0,0 +1,93 @@ +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +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.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + +/** + * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite + * a circle, but some Bezier curves that have control points set essentially in a square. However, + * it turns enough to tune your centripetal force correction and some of your heading. Some lag in + * heading is to be expected. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +@Config +@Autonomous(name = "AsherOrientBlue", group = "Autonomous Pathing Tuning") +public class AsherOrientBlue extends OpMode { + private Telemetry telemetryA; + + private Follower follower; + + private PathChain path; + + private final Pose startPose = new Pose(9.757, 84.983, 90); + + /** + * This initializes the Follower and creates the PathChain for the "circle". Additionally, this + * initializes the FTC Dashboard telemetry. + */ + @Override + public void init() { + follower = new Follower(hardwareMap); + + follower.setMaxPower(.4); + + follower.setStartingPose(startPose); + + path = follower.pathBuilder() + /* + * Only update this path + */ + .addPath( + // Line 1 + new BezierLine( + new Point(20.500, 7.800, Point.CARTESIAN), + new Point(20.500, 87.500, Point.CARTESIAN) + ) + ) + .addPath( + // Line 2 + new BezierLine( + new Point(20.500, 87.500, Point.CARTESIAN), + new Point(7.800, 87.500, Point.CARTESIAN) + ) + ) + + .setConstantHeadingInterpolation(Math.toRadians(90)).build(); + /* + * End of only update this path + */ + + follower.followPath(path); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.update(); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the FTC Dashboard. + */ + @Override + public void loop() { + follower.update(); + if (follower.atParametricEnd()) { + follower.followPath(path); + } + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherPathBlueV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherPathBlueV1.java new file mode 100644 index 0000000..18bddac --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AsherPathBlueV1.java @@ -0,0 +1,133 @@ +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +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.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + +/** + * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite + * a circle, but some Bezier curves that have control points set essentially in a square. However, + * it turns enough to tune your centripetal force correction and some of your heading. Some lag in + * heading is to be expected. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +@Config +@Autonomous(name = "AsherPathBlueV1", group = "Autonomous Pathing Tuning") +public class AsherPathBlueV1 extends OpMode { + private Telemetry telemetryA; + + private Follower follower; + + private PathChain path; + + private final Pose startPose = new Pose(9.757, 84.983, 90); + + /** + * This initializes the Follower and creates the PathChain for the "circle". Additionally, this + * initializes the FTC Dashboard telemetry. + */ + @Override + public void init() { + follower = new Follower(hardwareMap); + + follower.setMaxPower(.4); + + follower.setStartingPose(startPose); + + path = follower.pathBuilder() + /* + * Only update this path + */ + .addPath( + // Line 1 + new BezierCurve( + new Point(7.800, 87.5, Point.CARTESIAN), + new Point(19.000, 116.000, Point.CARTESIAN), + new Point(93.000, 118.000, Point.CARTESIAN), + new Point(45.000, 115.000, Point.CARTESIAN) + ) + ) + .addPath( + // Line 2 + new BezierLine( + new Point(45.000, 115.000, Point.CARTESIAN), + new Point(14.000, 126.000, Point.CARTESIAN) + ) + ) + .addPath( + // Line 3 + new BezierCurve( + new Point(14.000, 126.000, Point.CARTESIAN), + new Point(43.000, 112.500, Point.CARTESIAN), + new Point(64.000, 92.000, Point.CARTESIAN), + new Point(77.000, 117.000, Point.CARTESIAN) + ) + ) + .addPath( + // Line 4 + new BezierLine( + new Point(77.000, 117.000, Point.CARTESIAN), + new Point(20.000, 135.000, Point.CARTESIAN) + ) + ) + .addPath( + // Line 5 + new BezierCurve( + new Point(20.000, 135.000, Point.CARTESIAN), + new Point(113.000, 95.000, Point.CARTESIAN), + new Point(69.000, 135.000, Point.CARTESIAN) + ) + ) + .addPath( + // Line 6 + new BezierLine( + new Point(69.000, 135.000, Point.CARTESIAN), + new Point(20.500, 135.000, Point.CARTESIAN) + ) + ) + .addPath( + // Line 7 + new BezierCurve( + new Point(20.500, 135.000, Point.CARTESIAN), + new Point(101.500, 95.500, Point.CARTESIAN), + new Point(72.500, 95.500, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)).build(); + /* + * End of only update this path + */ + + follower.followPath(path); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.update(); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the FTC Dashboard. + */ + @Override + public void loop() { + follower.update(); + if (follower.atParametricEnd()) { + follower.followPath(path); + } + follower.telemetryDebug(telemetryA); + } +} 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 0f098df..9699cf6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java @@ -14,6 +14,7 @@ public class PedroConstants { // Robot motor configurations + public static final String BRAIN_ROT = "Sikidi rizz 360 no teleop tf2 mama mia 2cool 4skool yasyasy yasyasyasyasyasyasyaysy ohio yes heh me is moar skeebeedee than u walked and got tripped on by your aunt my very educaded mother just served us nine what? just kydinfoiwfowefwofwioefoiejfeoiwjfomdsklfnslefknesfklnkfenfenkfeknfenkfeknfenkefnk"; public static final String FRONT_LEFT_MOTOR = "Drive front lt"; public static final String BACK_LEFT_MOTOR = "Drive back lt"; public static final String FRONT_RIGHT_MOTOR = "Drive front rt"; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java index 755de44..4341b8e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java @@ -14,7 +14,7 @@ public class RobotConstants { public static double wristFloor = 0.625; public static double wristBucket = 0.215; public static int liftToFloorPos = 0; - public static int liftToPoolPos = 500; + public static int liftToPoolPos = 650; public static int liftToLowBucketPos = 2250; public static int liftToHighBucketPos = 3850; public static double liftPower = .45; From 2c1f0d6c57daf8cfcba7702e5f8926a86f17a7ed Mon Sep 17 00:00:00 2001 From: carlos Date: Tue, 5 Nov 2024 17:20:43 -0800 Subject: [PATCH 4/5] Commit "working" code? --- .../firstinspires/ftc/teamcode/DevTeleop.java | 10 ++++++++++ .../ftc/teamcode/configs/RobotConstants.java | 4 ++-- .../ftc/teamcode/subsystem/LiftSubsystem.java | 17 +++++++++-------- 3 files changed, 21 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java index 304ca6c..8288f4e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java @@ -113,6 +113,14 @@ public class DevTeleop extends OpMode { wrist.bucketWrist(); } } + + public void theTravel(LiftSubsystem lift, ArmSubsystem arm, WristSubsystem wrist){ + if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down){ + lift.toFloor(); + arm.bucketArm(); + wrist.floorWrist(); + } + } @Override public void loop() { previousGamepad1.copy(currentGamepad1); @@ -126,6 +134,7 @@ public class DevTeleop extends OpMode { theLift(arm, wrist); theLowBucketScore(lift, wrist, arm); theHighBucketScore(lift, wrist, arm); + theTravel(lift, arm, wrist); double max; @@ -164,6 +173,7 @@ public class DevTeleop extends OpMode { // Show the elapsed game time and wheel power. telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + telemetry.addData("Current Lift Position", lift.getPosition()); telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java index 755de44..d56cf6b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java @@ -13,8 +13,8 @@ public class RobotConstants { public static double wristFloor = 0.625; public static double wristBucket = 0.215; - public static int liftToFloorPos = 0; - public static int liftToPoolPos = 500; + public static int liftToFloorPos = 20; + public static int liftToFloatPos = 150; public static int liftToLowBucketPos = 2250; public static int liftToHighBucketPos = 3850; public static double liftPower = .45; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java index ac895b5..ed54f00 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java @@ -1,9 +1,10 @@ package org.firstinspires.ftc.teamcode.subsystem; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloatPos; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorPos; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToPoolPos; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -17,7 +18,7 @@ public class LiftSubsystem { public RunAction toFloor, toLowBucket, toHighBucket; public enum LiftState { - FLOOR, LOW_BUCKET, HIGH_BUCKET, POOL + FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT } private LiftState liftState; @@ -36,8 +37,8 @@ public class LiftSubsystem { public void switchState() { if (this.liftState == LiftState.FLOOR) { - this.toPool(); - } else if (this.liftState == LiftState.POOL) { + this.toFloor(); + } else if (this.liftState == LiftState.FLOAT) { this.toLowBucket(); } else if (this.liftState == LiftState.LOW_BUCKET) { this.toHighBucket(); @@ -47,13 +48,13 @@ public class LiftSubsystem { } public void toFloor() { - this.setTarget(liftToPoolPos); + this.setTarget(liftToFloorPos); this.setState(LiftState.FLOOR); } - public void toPool() { - this.setTarget(liftToPoolPos); - this.setState(LiftState.POOL); + public void toFloat() { + this.setTarget(liftToFloatPos); + this.setState(LiftState.FLOAT); } public void toLowBucket() { From e5a429c6aea6086312c3ee366529ff74a8048558 Mon Sep 17 00:00:00 2001 From: carlos Date: Thu, 7 Nov 2024 15:38:33 -0800 Subject: [PATCH 5/5] Changing arm controls to be more intuitive --- .../firstinspires/ftc/teamcode/DevTeleop.java | 27 ++++++++++--------- 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java index 8288f4e..3548d41 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java @@ -45,7 +45,7 @@ public class DevTeleop extends OpMode { public DcMotor frontRightMotor; public DcMotor backRightMotor; - private double MAX_POWER = .6; + private double MAX_POWER = .45; @Override public void init() { claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); @@ -74,22 +74,22 @@ public class DevTeleop extends OpMode { } public void theDrop(ArmSubsystem arm, WristSubsystem wrist) { - - if (currentGamepad1.a && !previousGamepad1.a) { +//pick up + if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) { wrist.floorWrist(); arm.engageArm(); } } public void thePickup(ClawSubsystem claw) { - - if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) { +//claw open close + if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) { claw.switchState(); } } - public void theLift(ArmSubsystem arm, WristSubsystem wrist) { + /* public void theLift(ArmSubsystem arm, WristSubsystem wrist) { if (currentGamepad1.b && !previousGamepad1.b) { arm.parkArm(); @@ -97,17 +97,19 @@ public class DevTeleop extends OpMode { } } - +*/ public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) { - if (currentGamepad2.y && !previousGamepad2.y) { +//low bucket + if (currentGamepad2.a && !previousGamepad2.a) { lift.toLowBucket(); arm.bucketArm(); wrist.bucketWrist(); } } - public void theHighBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) { - if (currentGamepad2.a && !previousGamepad2.a) { + public void theHighBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) { +//high basket + if (currentGamepad2.b && !previousGamepad2.b) { lift.toHighBucket(); arm.bucketArm(); wrist.bucketWrist(); @@ -115,7 +117,8 @@ public class DevTeleop extends OpMode { } public void theTravel(LiftSubsystem lift, ArmSubsystem arm, WristSubsystem wrist){ - if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down){ +// + if (currentGamepad2.dpad_right && !previousGamepad2.dpad_right){ lift.toFloor(); arm.bucketArm(); wrist.floorWrist(); @@ -131,7 +134,7 @@ public class DevTeleop extends OpMode { theDrop(arm, wrist); thePickup(claw); - theLift(arm, wrist); + // theLift(arm, wrist); theLowBucketScore(lift, wrist, arm); theHighBucketScore(lift, wrist, arm); theTravel(lift, arm, wrist);