From 29749041091d72fca174eccacbc0f407b77342cf Mon Sep 17 00:00:00 2001 From: carlos Date: Thu, 6 Feb 2025 16:50:13 -0800 Subject: [PATCH] Changes that covered the hang and wrist touching bar during auto --- .../ftc/teamcode/SpecimenAuto.java | 38 +++++++++++-------- .../ftc/teamcode/SpecimenAutoLineTest.java | 14 +++---- .../cometbots/paths/CometBotDriveV2.java | 2 +- .../subsystems/HangMotorSubsystem.java | 4 +- 4 files changed, 33 insertions(+), 25 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java index 95b2238..d25726a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java @@ -43,6 +43,7 @@ public class SpecimenAuto extends OpMode { wrist.InitAuto(); arm.initAuto(); + follower.setMaxPower(.45); @@ -50,42 +51,49 @@ public class SpecimenAuto extends OpMode { @Override public void loop() { + if(runtime != null){ + telemetry.addData("Runtime (seconds)", runtime.seconds()); + } telemetry.addData("state", state); switch(state) { case 0: + runtime.reset(); wrist.toSpecimenPrep(); arm.toSpecimenPrep(); - state = 1; - new SleepAction(5); + if(runtime.seconds() > 0.25){state = 1;} break; case 1: - state = 2; + //line 1 + if(runtime.seconds() > 3){state = 2;} break; case 2: - new SleepAction(5); - state = 3; + lift.toSpecimanHangHeight(); + if(runtime.seconds() > 3.75){state = 3;} break; case 3: - - new SleepAction(5); - state = 4; + wrist.toSpecimenHang(); + if(runtime.seconds() > 4){state = 4;} break; case 4: - - new SleepAction(5); - state = 5; + lift.toSpecimanReleaseHeight(); + if(runtime.seconds() > 4.5){state = 5;} break; case 5: - //specimen drop + claw.switchState(); + if(runtime.seconds() > 4.65){state = 6;} break; case 6: - //path 4 + lift.toFloorPosition(); + if(runtime.seconds() > 4.75){state = 7;} break; case 7: - //specimen pick up + arm.toParkPosition(); + wrist.toTravelPosition(); + //line 2 + if(runtime.seconds() > 0.){state = 8;} break; case 8: - //path 5 + if(runtime.seconds() > 0.25){state = 9;} break; case 9: //specimen drop diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java index fbed29f..1b9e7c7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java @@ -13,7 +13,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; -@Autonomous(name = "Specimen Test", group = "Competition") +@Autonomous(name = "Specimen Line Test", group = "Competition") public class SpecimenAutoLineTest extends OpMode { private Telemetry telemetryA; @@ -40,15 +40,15 @@ public class SpecimenAutoLineTest extends OpMode { new Point(39.500, 60.000, Point.CARTESIAN) ) ) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)) -/* .addPath( + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0)) + .addPath( // Line 2 new BezierLine( new Point(39.500, 60.000, Point.CARTESIAN), new Point(37.000, 60.000, Point.CARTESIAN) ) ) - .setConstantHeadingInterpolation(Math.toRadians(180)) + .setConstantHeadingInterpolation(Math.toRadians(0)) .addPath( // Line 3 new BezierCurve( @@ -58,7 +58,7 @@ public class SpecimenAutoLineTest extends OpMode { new Point(58.000, 23.500, Point.CARTESIAN) ) ) - .setConstantHeadingInterpolation(Math.toRadians(180)) + .setConstantHeadingInterpolation(Math.toRadians(0)) .addPath( // Line 4 new BezierLine( @@ -66,7 +66,7 @@ public class SpecimenAutoLineTest extends OpMode { new Point(14.000, 23.500, Point.CARTESIAN) ) ) - .setConstantHeadingInterpolation(Math.toRadians(180)) + .setConstantHeadingInterpolation(Math.toRadians(0)) .addPath( // Line 5 new BezierLine( @@ -155,7 +155,7 @@ public class SpecimenAutoLineTest extends OpMode { new Point(37.000, 68.000, Point.CARTESIAN) ) ) - .setConstantHeadingInterpolation(Math.toRadians(180))*/.build(); + .setConstantHeadingInterpolation(Math.toRadians(180)).build(); follower.followPath(path); telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java index 8269243..b64b577 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java @@ -118,7 +118,7 @@ public class CometBotDriveV2 extends OpMode { moveToPark(); break; case 99: - wrist.toTravelPosition(); + wrist.toFloorPosition(); break; default: telemetry.addLine("default"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HangMotorSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HangMotorSubsystem.java index 773d58d..54ac835 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HangMotorSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HangMotorSubsystem.java @@ -22,10 +22,10 @@ public class HangMotorSubsystem { } public void init(){ //init hook, set runmodes - hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + //hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); hang.setDirection(DcMotorSimple.Direction.FORWARD); hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - hang.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + hang.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); } public void hangRobot(){