From f18b63a94be83360f1bcb39ca2e2466b0bdfb877 Mon Sep 17 00:00:00 2001 From: joshua smith Date: Tue, 3 Dec 2024 12:27:48 -0500 Subject: [PATCH 1/4] fix driving mistake made in last branch --- .../ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java | 2 +- .../ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java index 95c2817..b9d6a33 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java @@ -16,7 +16,7 @@ public class ScoreArm extends QQMechanism{ public static final double TEST_SPEED = 0.55; - public static final int CLAW_RELEASE_OFFSET = 150; + public static final int CLAW_RELEASE_OFFSET = 250; DcMotor leftMotor; DcMotor rightMotor; TouchSensor limitSwitch; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java index 124030d..1a6d907 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java @@ -5,7 +5,7 @@ @Autonomous public class CloseParkAuto extends QQOpMode{ - private final int autodelay = 23; + private final int autodelay = 0; ElapsedTime elapsedTime = new ElapsedTime(); private double step = 0; From 0e6ff02d7bb297f86a66535783f8e98134225dbe Mon Sep 17 00:00:00 2001 From: joshua smith Date: Wed, 4 Dec 2024 17:32:23 -0500 Subject: [PATCH 2/4] arm score position is lower --- .../ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java | 4 ++-- .../ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java index b9d6a33..6c5f06e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java @@ -16,7 +16,7 @@ public class ScoreArm extends QQMechanism{ public static final double TEST_SPEED = 0.55; - public static final int CLAW_RELEASE_OFFSET = 250; + public static final int CLAW_RELEASE_OFFSET = 300; DcMotor leftMotor; DcMotor rightMotor; TouchSensor limitSwitch; @@ -36,7 +36,7 @@ public class ScoreArm extends QQMechanism{ static int INTAKE_POSITION = 0; static int SCORING_POSITION = 350; - static int PLACING_POSITION = 750; + static int PLACING_POSITION = 850; PIDFController pidfController = new PIDFController(kP,kI,kD,kF,max,min); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java index b0d69fd..971b0ff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java @@ -56,9 +56,9 @@ public void loop(){ robot.scoreArm.goToPlace(); robot.intakeArm.transfer(); }else if (gamepad1.dpad_up){ - robot.intakeSlides.manualPositionChange(MANUAL_CHANGE); + robot.scoreArm.manualPositionChange(MANUAL_CHANGE); }else if (gamepad1.dpad_down){ - robot.intakeSlides.manualPositionChange(-MANUAL_CHANGE); + robot.scoreArm.manualPositionChange(-MANUAL_CHANGE); } if (gamepad1.left_stick_button){ robot.controlHub.resetGyro(); From b65afe62bc7840031e39c70fcaa3cbca1b2ded43 Mon Sep 17 00:00:00 2001 From: joshua smith Date: Thu, 5 Dec 2024 21:49:05 -0500 Subject: [PATCH 3/4] preload + park auto works --- .../OpticalTrackingOdometrySensor.java | 2 +- .../ftc16072/Mechanisms/ScoreArm.java | 2 +- .../ftc16072/Mechanisms/ScoringClaw.java | 1 - .../ftc16072/OpModes/CloseParkAuto.java | 8 +-- .../ftc16072/OpModes/PreloadAuto.java | 60 +++++++++++++++++++ .../teamcode/ftc16072/Util/Navigation.java | 6 +- 6 files changed, 68 insertions(+), 11 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/OpticalTrackingOdometrySensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/OpticalTrackingOdometrySensor.java index e1dc9a6..1a4dcf9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/OpticalTrackingOdometrySensor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/OpticalTrackingOdometrySensor.java @@ -21,7 +21,7 @@ public void init(HardwareMap hwMap) { otos.setAngularUnit(AngleUnit.DEGREES); otos.setLinearUnit(DistanceUnit.INCH); otos.calibrateImu(); - SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(X_POSITION,0,270); + SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(X_POSITION,0,180); otos.setOffset(offset); otos.resetTracking(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java index 6c5f06e..97df901 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java @@ -16,7 +16,7 @@ public class ScoreArm extends QQMechanism{ public static final double TEST_SPEED = 0.55; - public static final int CLAW_RELEASE_OFFSET = 300; + public static final int CLAW_RELEASE_OFFSET = 275; DcMotor leftMotor; DcMotor rightMotor; TouchSensor limitSwitch; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java index 28282cf..e999f1d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java @@ -37,7 +37,6 @@ public void init(HardwareMap hwMap) { clawServo = hwMap.get(Servo.class, "claw_servo"); wristServo = hwMap.get(Servo.class, "wrist_servo"); colorSensor = hwMap.get(ColorRangeSensor.class, "scoreclaw_color"); - open(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java index 1a6d907..809bad7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java @@ -12,11 +12,13 @@ public class CloseParkAuto extends QQOpMode{ public void init(){ super.init(); robot.otos.setOtosPosition(13.5,-61.5,0); + robot.scoringClaw.close(); } public void start(){ elapsedTime.reset(); } public void loop(){ + super.loop(); if(step == 0){ if(elapsedTime.seconds() > autodelay){ step = 1; @@ -28,12 +30,8 @@ else if(step == 1){ step = 2; } }else if(step == 2){ - boolean doneDriving = nav.driveToPositionIN(62,-60,0); - if(doneDriving){ - step = 3; - }if(step == 3){ robot.scoreArm.goToIntake(); } } } -} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAuto.java new file mode 100644 index 0000000..930653e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAuto.java @@ -0,0 +1,60 @@ +package org.firstinspires.ftc.teamcode.ftc16072.OpModes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.util.ElapsedTime; + +@Autonomous +public class PreloadAuto extends QQOpMode{ + private final int autodelay = 0; + + ElapsedTime elapsedTime = new ElapsedTime(); + private double step = 0; + boolean isPlacing; + boolean chamberContactWasPressed; + int contactLostPos; + boolean clawWasClosed; + public void init(){ + super.init(); + robot.otos.setOtosPosition(7,-61.5,0); + robot.scoringClaw.close(); + robot.intakeClaw.wristFlat(); + } + public void start(){ + elapsedTime.reset(); + } + public void loop(){ + super.loop(); + if(robot.scoreArm.isChamberContacted()){ + robot.scoreArm.goToScoring(); + isPlacing = true; + }else if(!robot.scoreArm.isChamberContacted() && isPlacing && chamberContactWasPressed){ + contactLostPos = robot.scoreArm.getCurrentPos(); + }else if(isPlacing && robot.scoreArm.isTimeToReleaseClaw(contactLostPos)){ + robot.scoringClaw.open(); + isPlacing = false; + } + + + if(step == 0){ + robot.scoreArm.goToPlace(); + if(elapsedTime.seconds() > autodelay){ + step = 1; + } + }else if(step == 1){ + boolean isDoneDriving = nav.driveToPositionIN(38, -61.5,0); + if (isDoneDriving){ + step = 2; + } + }else if(step == 2){ + boolean isDoneDriving = nav.driveToPositionIN(14,-109.5,0); + if (isDoneDriving){ + step = 3; + } + } + chamberContactWasPressed = robot.scoreArm.isChamberContacted(); + clawWasClosed = robot.scoringClaw.isClawClosed(); + } +} + + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java index 23db775..aa9683d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java @@ -7,9 +7,9 @@ import org.firstinspires.ftc.teamcode.ftc16072.Robot; public class Navigation { - public static final double MAX_TRANSLATE = 1; + public static final double MAX_TRANSLATE = 0.3; public static final double MIN_TRANSLATE = -MAX_TRANSLATE; - public static final double MAX_ROTATE = 1; + public static final double MAX_ROTATE = 0.3; public static final double MIN_ROTATE = -MAX_ROTATE; Robot robot; @@ -23,7 +23,7 @@ public class Navigation { static double ROTATIONAL_KI = 0.000; static double ROTATIONAL_KD = 0.00; static double ROTATIONAL_KF = 0; - static double ROTATIONAL_TOLERANCE_THRESHOLD = 1; + static double ROTATIONAL_TOLERANCE_THRESHOLD = 3; PIDFController PIDx, PIDy, PIDh; From 731aaaccdea4fe330323ad56c6744d766a473354 Mon Sep 17 00:00:00 2001 From: joshua smith Date: Thu, 5 Dec 2024 22:01:04 -0500 Subject: [PATCH 4/4] preload + park auto works more consistently --- .../ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java | 2 +- .../ftc/teamcode/ftc16072/OpModes/PreloadAuto.java | 2 ++ .../firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java index 97df901..a5fe649 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java @@ -16,7 +16,7 @@ public class ScoreArm extends QQMechanism{ public static final double TEST_SPEED = 0.55; - public static final int CLAW_RELEASE_OFFSET = 275; + public static final int CLAW_RELEASE_OFFSET = 250; DcMotor leftMotor; DcMotor rightMotor; TouchSensor limitSwitch; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAuto.java index 930653e..e611535 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAuto.java @@ -50,6 +50,8 @@ public void loop(){ if (isDoneDriving){ step = 3; } + }else if (step == 3){ + robot.scoreArm.goToIntake(); } chamberContactWasPressed = robot.scoreArm.isChamberContacted(); clawWasClosed = robot.scoringClaw.isClawClosed(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java index aa9683d..00a6559 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java @@ -19,7 +19,7 @@ public class Navigation { static double TRANSLATIONAL_KF = 0; static double TRANSLATIONAL_TOLERANCE_THRESHOLD = 0.5; - static double ROTATIONAL_KP = 0.1; + static double ROTATIONAL_KP = 0.01; static double ROTATIONAL_KI = 0.000; static double ROTATIONAL_KD = 0.00; static double ROTATIONAL_KF = 0;