From 1dfff89e3e09d9c5e5ce0f43a1d744fa0e9c8458 Mon Sep 17 00:00:00 2001 From: Annika Date: Fri, 29 Nov 2024 19:21:29 -0700 Subject: [PATCH 001/129] Move practice bot from practice drive train to regular drive train. --- .../ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java | 6 ++++++ .../java/org/firstinspires/ftc/teamcode/PracticeBot.java | 8 ++++---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java index 53e4df7..7217f9d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.Mechanisms; +import android.os.DropBoxManager; + import com.arcrobotics.ftclib.controller.PIDController; import com.arcrobotics.ftclib.drivebase.MecanumDrive; import com.arcrobotics.ftclib.hardware.SimpleServo; @@ -50,6 +52,8 @@ public class PracticeDriveTrain2025 private boolean autoEnabled = false; + + HardwareMap hwMap; private double currentSpeed = 0; private double currentXTarget = 0; @@ -70,6 +74,7 @@ public PracticeDriveTrain2025(HardwareMap hwMap) driveBase = new MecanumDrive(leftFrontDrive,rightFrontDrive,leftBackDrive,rightBackDrive); } + public void init() { headingControl = new PIDController(0.02, 0.002, 0.000 ); headingControl.setTolerance(5);// was 1 .. @@ -208,6 +213,7 @@ public void printTelemetry(Telemetry telemetry) { String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Position", data); + //telemetry.addData("elevator","%5.2f",elevator.getCurrentPosition()); if(autoEnabled) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java index 9cfb57c..0e6015d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java @@ -11,7 +11,7 @@ import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; //import org.firstinspires.ftc.teamcode.Mechanisms.Claw; import org.firstinspires.ftc.teamcode.Mechanisms.GoBildaPinpointDriver; -import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; import java.util.Locale; @@ -21,7 +21,7 @@ @TeleOp(name = "PracticeBot") public class PracticeBot extends OpMode { - private PracticeDriveTrain2025 driveTrain; + private DriveTrain2025 driveTrain; public GamepadEx driver = null; public GamepadEx operator = null; private final ElapsedTime runtime = new ElapsedTime(); @@ -34,12 +34,12 @@ public void init() { ((CAITelemetry)telemetry).setDashboardEnabled(true); telemetry.addData("Status", "Initializing"); telemetry.update(); - driveTrain = new PracticeDriveTrain2025(hardwareMap); + driveTrain = new DriveTrain2025(hardwareMap); // Drive Train.init(); driveTrain.init(); - odometer.resetPosAndIMU(); + driveTrain.odometer.resetPosAndIMU(); telemetry.addData("Status", "Initialized"); telemetry.update(); From c406efe65f2d1bdb5bfc639a62cca72093a39d4d Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 29 Nov 2024 19:33:41 -0700 Subject: [PATCH 002/129] Revert "preset tweaks" This reverts commit bd0f579dae578b6f476fc69410ced98e39401eb2. --- .../ftc/teamcode/Mechanisms/Constants.java | 2 +- .../ftc/teamcode/Mechanisms/Elevator.java | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index dd57eb3..b4d35b1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -48,7 +48,7 @@ public static final int ELEVATOR_LOW_BAR= 350; // nov 22 public static final int ELEVATOR_HIGH_BAR = 660; - public static final int ELEVATOR_HIGH_BASKET = 1300; // or 1260? + public static final int ELEVATOR_HIGH_BASKET = 1260; public static final int ELEVATOR_MAX = 1220; public static final int ELEVATOR_MIN = 0; public static final double ELEVATOR_SPEED_FACTOR = 0.7; // 1.0 Old elevator and 0.3 for new elevator diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java index 598b2b3..a399609 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java @@ -74,16 +74,16 @@ public void setElevatorState(int position) { setpoint = Constants.ELEVATOR_LOW_BAR; break; - case 3: //Case for deposit specimen high bar - this.gotoPreSet(Constants.ELEVATOR_HIGH_BAR); - setpoint = Constants.ELEVATOR_HIGH_BAR; - break; - - case 4: //Case for deposit specimen low bar + case 3: //Case for deposit specimen low bar this.gotoPreSet(Constants.ELEVATOR_LOW_BASKET); setpoint = Constants.ELEVATOR_LOW_BASKET; break; + case 4: //Case for deposit specimen high bar + this.gotoPreSet(Constants.ELEVATOR_HIGH_BAR); + setpoint = Constants.ELEVATOR_HIGH_BAR; + break; + case 5: //Case for deposit sample in low bucket this.gotoPreSet(Constants.ELEVATOR_HIGH_BASKET); setpoint = Constants.ELEVATOR_HIGH_BASKET; From ea44bd8325ecc065b24a21492f96f4c74ae89f1c Mon Sep 17 00:00:00 2001 From: Annika Date: Fri, 29 Nov 2024 19:39:22 -0700 Subject: [PATCH 003/129] Reverted previous commit. --- .../java/org/firstinspires/ftc/teamcode/PracticeBot.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java index 0e6015d..9cfb57c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java @@ -11,7 +11,7 @@ import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; //import org.firstinspires.ftc.teamcode.Mechanisms.Claw; import org.firstinspires.ftc.teamcode.Mechanisms.GoBildaPinpointDriver; -import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; import java.util.Locale; @@ -21,7 +21,7 @@ @TeleOp(name = "PracticeBot") public class PracticeBot extends OpMode { - private DriveTrain2025 driveTrain; + private PracticeDriveTrain2025 driveTrain; public GamepadEx driver = null; public GamepadEx operator = null; private final ElapsedTime runtime = new ElapsedTime(); @@ -34,12 +34,12 @@ public void init() { ((CAITelemetry)telemetry).setDashboardEnabled(true); telemetry.addData("Status", "Initializing"); telemetry.update(); - driveTrain = new DriveTrain2025(hardwareMap); + driveTrain = new PracticeDriveTrain2025(hardwareMap); // Drive Train.init(); driveTrain.init(); - driveTrain.odometer.resetPosAndIMU(); + odometer.resetPosAndIMU(); telemetry.addData("Status", "Initialized"); telemetry.update(); From 13dc6417ab2d136afd344785f2d6548ad31ed6ae Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 29 Nov 2024 20:00:04 -0700 Subject: [PATCH 004/129] Changed odometer to drivetrain.odometer --- .../main/java/org/firstinspires/ftc/teamcode/PracticeBot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java index 9cfb57c..02e1596 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java @@ -39,7 +39,7 @@ public void init() { // Drive Train.init(); driveTrain.init(); - odometer.resetPosAndIMU(); + driveTrain.odometer.resetPosAndIMU(); telemetry.addData("Status", "Initialized"); telemetry.update(); From ed098c89d5d73d2715bd6c7ba0d651f72fc64a82 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 29 Nov 2024 23:16:09 -0700 Subject: [PATCH 005/129] RedSimple is going to be a simple park Moved RedPark to Park, since the course is symmetric --- .../teamcode/Auto/{RedPark.java => Park.java} | 4 ++-- .../ftc/teamcode/Auto/RedSimple.java | 17 ++++++++++++++--- 2 files changed, 16 insertions(+), 5 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/{RedPark.java => Park.java} (98%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedPark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Park.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedPark.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Park.java index 1e94d86..177ac83 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedPark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Park.java @@ -4,8 +4,8 @@ import org.firstinspires.ftc.teamcode.Mechanisms.Constants; -@Autonomous(name = "AutoRedPark_Test") -public class RedPark extends BaseAutoPractice { +@Autonomous(name = "AutoPark_Test") +public class Park extends BaseAutoPractice { private int currentStep; //Location location = location.DEPOSIT_SPECIMEN; //waypoint definition diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedSimple.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedSimple.java index 52c64d8..b278be7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedSimple.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedSimple.java @@ -8,10 +8,21 @@ @Autonomous (name = "AutoRedSimple") public class RedSimple extends BaseAutoPractice{ + private int currentStep; + BaseAutoPractice.WayPoint[] wayPoints; public void init(){ + wayPoints = new WayPoint[12]; - - }//inti - }//everything + wayPoints[0].x = 0.0;//initializing + wayPoints[0].y = 0.0; + wayPoints[0].facing = Constants.NORTH; + }//init +@Override +public void loop(){ + telemetry.addData("Sequence Step", currentStep); + driveTrain.odometer.update(); + driveTrain.driveTo(1.0,wayPoints[0].x,wayPoints[0].y,wayPoints[0].facing); + } // loop +}//everything From d4d8900f97688ce9502dc60865519d9cd902308e Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sat, 30 Nov 2024 09:20:57 -0700 Subject: [PATCH 006/129] Put in a simple x 10 to test auto --- .../Auto/{RedSimple.java => PracticeRedSimple.java} | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/{RedSimple.java => PracticeRedSimple.java} (78%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedSimple.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeRedSimple.java similarity index 78% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedSimple.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeRedSimple.java index b278be7..1b25b86 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedSimple.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeRedSimple.java @@ -1,20 +1,19 @@ package org.firstinspires.ftc.teamcode.Auto; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; //import com.qualcomm.robotcore.eventloop.opmode.teleop; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; @Autonomous (name = "AutoRedSimple") -public class RedSimple extends BaseAutoPractice{ - private int currentStep; +public class PracticeRedSimple extends BaseAutoPractice{ + private int currentStep = -1; BaseAutoPractice.WayPoint[] wayPoints; public void init(){ wayPoints = new WayPoint[12]; - wayPoints[0].x = 0.0;//initializing + wayPoints[0].x = 10.0;//initializing wayPoints[0].y = 0.0; wayPoints[0].facing = Constants.NORTH; }//init From 8f198ea5240c8cc53d6a1bc8731a37d3af3ac332 Mon Sep 17 00:00:00 2001 From: Annika Date: Sat, 30 Nov 2024 12:40:41 -0700 Subject: [PATCH 007/129] Got PractiveRedSimple to park. Using a very simple drive for 150 count in a loop. Created CompPark to be the auto park for competition. It has not been tested yet. Added needed code to have BaseAutoComp to be link BaseAutoPractice. --- .../ftc/teamcode/Auto/BaseAutoComp.java | 2 +- .../ftc/teamcode/Auto/CompPark.java | 39 ++++++++++++++++++ .../ftc/teamcode/Auto/PracticeRedSimple.java | 41 +++++++++++++++---- .../Mechanisms/PracticeDriveTrain2025.java | 2 + 4 files changed, 76 insertions(+), 8 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java index 24256f5..8fdcde8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java @@ -30,7 +30,7 @@ public void start() { } public void init(){ telemetry = new CAITelemetry(telemetry); - ((CAITelemetry)telemetry).setDashboardEnabled(true); + ((CAITelemetry)telemetry).setDashboardEnabled(false); telemetry.addData("Status", "Initializing"); telemetry.update(); driveTrain = new DriveTrain2025(hardwareMap); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark.java new file mode 100644 index 0000000..7dd0d9a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark.java @@ -0,0 +1,39 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; + +import java.util.Locale; + +@Autonomous(name = "CompPark") +public class CompPark extends BaseAutoComp{ + + private DriveTrain2025 driveTrain; + private double speed; + private int counter = 0; + + public void init(){ + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + }//init + @Override + public void loop(){ + telemetry.addData("Sequence Step", counter); + driveTrain.odometer.update(); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + driveTrain.odometer.update(); + if (counter <= 150){ + driveTrain.drive(0.0,0.3); + }else { + driveTrain.drive(0.0,0.0); + } + counter = counter + 1; + driveTrain.loop(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeRedSimple.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeRedSimple.java index 1b25b86..eccef6d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeRedSimple.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeRedSimple.java @@ -3,25 +3,52 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; //import com.qualcomm.robotcore.eventloop.opmode.teleop; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; + +import java.util.Locale; @Autonomous (name = "AutoRedSimple") public class PracticeRedSimple extends BaseAutoPractice{ - private int currentStep = -1; + private PracticeDriveTrain2025 driveTrain; + public int currentStep = -1; + private double speed; + private int counter = 0; BaseAutoPractice.WayPoint[] wayPoints; public void init(){ - wayPoints = new WayPoint[12]; + driveTrain = new PracticeDriveTrain2025(hardwareMap); + driveTrain.init(); - wayPoints[0].x = 10.0;//initializing - wayPoints[0].y = 0.0; - wayPoints[0].facing = Constants.NORTH; +// driveTrain.odometer.resetPosAndIMU(); +// this.wayPoints = new WayPoint[12]; +// +// this.wayPoints[0].x = 10.0;//initializing +// this.wayPoints[0].y = 0.0; +// this.wayPoints[0].facing = Constants.NORTH; }//init @Override public void loop(){ - telemetry.addData("Sequence Step", currentStep); + telemetry.addData("Sequence Step", counter); + driveTrain.odometer.update(); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); driveTrain.odometer.update(); - driveTrain.driveTo(1.0,wayPoints[0].x,wayPoints[0].y,wayPoints[0].facing); + if (counter <= 150){ + driveTrain.drive(0.0,0.3); + speed = 0.1; + }else { + driveTrain.drive(0.0,0.0); + speed = 0.0; + } + //driveTrain.drive(0.0,0.1); + counter = counter + 1; +// driveTrain.driveTo(speed,0.0, 5.0, Constants.NORTH); + driveTrain.loop(); } // loop }//everything diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java index 7217f9d..1f4bc33 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java @@ -82,6 +82,7 @@ public void init() { yControl = new PIDController(0.17, 0.0, 0.00); + // redundant as default is brake mode leftBackDrive.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); rightBackDrive.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); @@ -167,6 +168,7 @@ public void driveTo(double speed, double xDist, double yDist,double facing) { xControl.setSetPoint(currentXTarget); yControl.setSetPoint(currentYTarget); + } public void drive(double forwardSpeed, double strafeSpeed) { From b1e184780d123d82def1d29075a79512182398c9 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 6 Dec 2024 16:07:50 -0700 Subject: [PATCH 008/129] Turned off the dashboard for the competition --- .../src/main/java/org/firstinspires/ftc/teamcode/CompBot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java index bb017eb..08303f7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java @@ -87,7 +87,7 @@ public class CompBot extends OpMode { @Override public void init() { telemetry = new CAITelemetry(telemetry); - ((CAITelemetry) telemetry).setDashboardEnabled(true); + ((CAITelemetry) telemetry).setDashboardEnabled(false); telemetry.addData("Status", "Initializing"); telemetry.update(); driveTrain = new DriveTrain2025(hardwareMap); From beec3d32054f4c6b534b5804e0d3680191597eb6 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 6 Dec 2024 18:40:45 -0700 Subject: [PATCH 009/129] Modified auto to have one park CompPark. Commented out other autonomous modes. --- .../{CompPark.java => CompPark_Blue.java} | 6 ++- .../ftc/teamcode/Auto/CompPark_Red.java | 39 +++++++++++++++++++ .../firstinspires/ftc/teamcode/Auto/Park.java | 2 +- .../ftc/teamcode/Auto/PracticeRedSimple.java | 2 +- .../ftc/teamcode/Auto/RedShort.java | 2 +- 5 files changed, 46 insertions(+), 5 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/{CompPark.java => CompPark_Blue.java} (93%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Blue.java similarity index 93% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Blue.java index 7dd0d9a..adb6878 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Blue.java @@ -9,8 +9,8 @@ import java.util.Locale; -@Autonomous(name = "CompPark") -public class CompPark extends BaseAutoComp{ +//@Autonomous(name = "CompPark_Blue") +public class CompPark_Blue extends BaseAutoComp{ private DriveTrain2025 driveTrain; private double speed; @@ -37,3 +37,5 @@ public void loop(){ driveTrain.loop(); } } + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java new file mode 100644 index 0000000..39bb559 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java @@ -0,0 +1,39 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; + +import java.util.Locale; + +@Autonomous(name = "CompPark") +public class CompPark_Red extends BaseAutoComp{ + + private DriveTrain2025 driveTrain; + private double speed; + private int counter = 0; + + public void init(){ + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + }//init + @Override + public void loop(){ + telemetry.addData("Sequence Step", counter); + driveTrain.odometer.update(); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + driveTrain.odometer.update(); + if (counter <= 150){ + driveTrain.drive(0.0,-0.3); + }else { + driveTrain.drive(0.0,0.0); + } + counter = counter + 1; + driveTrain.loop(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Park.java index 177ac83..f95d04d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Park.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Park.java @@ -4,7 +4,7 @@ import org.firstinspires.ftc.teamcode.Mechanisms.Constants; -@Autonomous(name = "AutoPark_Test") +//@Autonomous(name = "AutoPark_Test") public class Park extends BaseAutoPractice { private int currentStep; //Location location = location.DEPOSIT_SPECIMEN; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeRedSimple.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeRedSimple.java index eccef6d..2f07fda 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeRedSimple.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeRedSimple.java @@ -11,7 +11,7 @@ import java.util.Locale; -@Autonomous (name = "AutoRedSimple") +//@Autonomous (name = "AutoRedSimple") public class PracticeRedSimple extends BaseAutoPractice{ private PracticeDriveTrain2025 driveTrain; public int currentStep = -1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedShort.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedShort.java index bd4cd63..f354350 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedShort.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedShort.java @@ -4,7 +4,7 @@ import org.firstinspires.ftc.teamcode.Mechanisms.Constants; -@Autonomous(name = "AutoRedParkShort_Test") +//@Autonomous(name = "AutoRedParkShort_Test") public class RedShort extends BaseAutoPractice { private int currentStep; //Location location = location.DEPOSIT_SPECIMEN; From 23df47e0ad33b8126d91a2253a18206467ba308f Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 6 Dec 2024 19:12:25 -0700 Subject: [PATCH 010/129] Added inits to have power in servros --- .../org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java index 39bb559..6ac38f5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java @@ -5,7 +5,9 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; import java.util.Locale; @@ -19,6 +21,10 @@ public class CompPark_Red extends BaseAutoComp{ public void init(){ driveTrain = new DriveTrain2025(hardwareMap); driveTrain.init(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + elbow.init(); + elbow.setElbowState(0); }//init @Override public void loop(){ From 2d8ec3581790bdadd19203196b3653fe01054d37 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 13 Dec 2024 23:27:56 -0700 Subject: [PATCH 011/129] Added AUTO_X_DISTANCE_ERROR Added AUTO_Y_DISTANCE_ERROR These are both used for auto. --- .../org/firstinspires/ftc/teamcode/Mechanisms/Constants.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index b4d35b1..df31581 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -86,4 +86,7 @@ public static int ODOMETER_COMPETITION_MODE = 8; public static double ODOMETER_X_OFFSET = -145.0; public static double ODOMETER_Y_OFFSET = 270.0; + // auto constants + public static double AUTO_X_DISTANCE_ERROR = 10; + public static double AUTO_Y_DISTANCE_ERROR = 10; } From a65fe425deaa0a91664d4219b16da834e728c2aa Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 13 Dec 2024 23:28:21 -0700 Subject: [PATCH 012/129] Removed this file, because it is not needed. --- .../ftc/teamcode/Auto/CompPark_Blue.java | 41 ------------------- 1 file changed, 41 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Blue.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Blue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Blue.java deleted file mode 100644 index adb6878..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Blue.java +++ /dev/null @@ -1,41 +0,0 @@ -package org.firstinspires.ftc.teamcode.Auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; -import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; - -import java.util.Locale; - -//@Autonomous(name = "CompPark_Blue") -public class CompPark_Blue extends BaseAutoComp{ - - private DriveTrain2025 driveTrain; - private double speed; - private int counter = 0; - - public void init(){ - driveTrain = new DriveTrain2025(hardwareMap); - driveTrain.init(); - }//init - @Override - public void loop(){ - telemetry.addData("Sequence Step", counter); - driveTrain.odometer.update(); - Pose2D pos = driveTrain.odometer.getPosition(); - String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); - telemetry.addData("Position", data); - driveTrain.odometer.update(); - if (counter <= 150){ - driveTrain.drive(0.0,0.3); - }else { - driveTrain.drive(0.0,0.0); - } - counter = counter + 1; - driveTrain.loop(); - } -} - - From bde6aed8de91f26e80d0fd2e820ae5b87aba31d1 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 13 Dec 2024 23:29:49 -0700 Subject: [PATCH 013/129] Added the Constants.DASHBOARD_ENABLED to the setDashboard enabled. This will make it easier to change the code from practice to comp easier and make allow us to be compliant with the rules. --- .../ftc/teamcode/Auto/BaseAutoPractice.java | 27 +++++++------------ 1 file changed, 9 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java index bd98b20..8cfe5a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java @@ -4,6 +4,7 @@ import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; @@ -11,27 +12,25 @@ //@Autonomous -public class BaseAutoPractice extends OpMode{ +public class BaseAutoPractice extends OpMode { String state = "START"; public PracticeDriveTrain2025 driveTrain; -// public Elevator elevator; -// public Claw claw; -// public Elbow elbow; public class WayPoint { public double x; public double y; public double facing; } - @Override + + @Override public void start() { state = "START"; - } - public void init(){ + + public void init() { telemetry = new CAITelemetry(telemetry); - ((CAITelemetry)telemetry).setDashboardEnabled(true); + ((CAITelemetry) telemetry).setDashboardEnabled(Constants.DASHBOARD_ENABLED); telemetry.addData("Status", "Initializing"); telemetry.update(); driveTrain = new PracticeDriveTrain2025(hardwareMap); @@ -39,19 +38,11 @@ public void init(){ driveTrain.init(); telemetry.addData("Status", "Initialized"); telemetry.update(); - -// elevator = new Elevator(hardwareMap); -// claw = new Claw(hardwareMap); -// elbow = new Elbow(hardwareMap); -// elevator.init();// delete if init in elevator can be private - - // this.elbowPosition = 0; - } @Override public void loop() { telemetry.addData("State", state); - - } } +} // End of Class + From fe3ff5625ac9623204dc1020d5e8dfb7a9b13307 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 13 Dec 2024 23:30:26 -0700 Subject: [PATCH 014/129] Working on added needed functionality to be able to practice auto mode. --- ...acticeRedSimple.java => PracticePark.java} | 35 ++++++++++--------- 1 file changed, 18 insertions(+), 17 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/{PracticeRedSimple.java => PracticePark.java} (65%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeRedSimple.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java similarity index 65% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeRedSimple.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java index 2f07fda..5d3e717 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeRedSimple.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java @@ -1,29 +1,26 @@ package org.firstinspires.ftc.teamcode.Auto; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -//import com.qualcomm.robotcore.eventloop.opmode.teleop; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; -import org.firstinspires.ftc.teamcode.Mechanisms.Constants; import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; import java.util.Locale; -//@Autonomous (name = "AutoRedSimple") -public class PracticeRedSimple extends BaseAutoPractice{ +@Autonomous (name = "PracticePark") +public class PracticePark extends BaseAutoPractice{ private PracticeDriveTrain2025 driveTrain; public int currentStep = -1; - private double speed; + // private double speed; private int counter = 0; BaseAutoPractice.WayPoint[] wayPoints; public void init(){ driveTrain = new PracticeDriveTrain2025(hardwareMap); driveTrain.init(); - -// driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.resetPosAndIMU(); // this.wayPoints = new WayPoint[12]; // // this.wayPoints[0].x = 10.0;//initializing @@ -37,18 +34,22 @@ public void loop(){ Pose2D pos = driveTrain.odometer.getPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + // telemetry.addData("heading:"."%.2f",heading); driveTrain.odometer.update(); - if (counter <= 150){ - driveTrain.drive(0.0,0.3); - speed = 0.1; - }else { - driveTrain.drive(0.0,0.0); - speed = 0.0; - } - //driveTrain.drive(0.0,0.1); - counter = counter + 1; + driveTrain.auto_drive_x(0.4,100); +// if (counter <= 150){ +// driveTrain.drive(0.0,0.3); +// speed = 0.1; +// }else { +// driveTrain.drive(0.0,0.0); +// speed = 0.0; +// } +// //driveTrain.drive(0.0,0.1); +// counter = counter + 1; // driveTrain.driveTo(speed,0.0, 5.0, Constants.NORTH); - driveTrain.loop(); +// driveTrain.loop(); } // loop }//everything From 8d9feae6e8013d7ccd34a5ce58cd63217760a921 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 13 Dec 2024 23:32:06 -0700 Subject: [PATCH 015/129] Added auto_goto_xy, auto_drive_x and auto_drive_y for auto mode. These may be taken out, since the main mode uses thread functions and not non-thread functions for activity. --- .../Mechanisms/PracticeDriveTrain2025.java | 36 +++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java index 1f4bc33..fe2660e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java @@ -94,7 +94,7 @@ public void init() { leftFrontDrive.setInverted(true); leftBackDrive.setInverted(true); - // odometer initializing -- this should go into PracticeDriveTrain2025?? + // odometer initializing odometer = hwMap.get(GoBildaPinpointDriver.class,"xy-cord"); odometer.setOffsets(Constants.ODOMETER_X_OFFSET,Constants.ODOMETER_Y_OFFSET); odometer.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD); @@ -144,6 +144,7 @@ public void loop(){ headingCorrection,// turn heading,// current heading in degrees false); + } public boolean atTarget() { // Pythagorean theorem to get the distance from target as a number. @@ -179,14 +180,45 @@ public void drive(double forwardSpeed, double strafeSpeed) { ySpeed = -forwardSpeed; } + // These functions will control where we are going + public void auto_goto_xy(double speed, double x_distance, double y_distance) + { + Pose2D pos = this.odometer.getPosition(); + this.auto_drive_x(speed,x_distance); + // this.auto_drive_y(speed,y_distance); + } + public void auto_drive_x(double speed,double distance) { + double current_position; + double final_position; + + current_position = this.getXPosition(); + final_position = current_position + distance; + // calculate direction + // positive is right + // negative is left + if (distance < 0.0) { + speed = -speed; + } + while (Math.abs(final_position - current_position) <= Constants.AUTO_X_DISTANCE_ERROR) { + current_position = this.getXPosition(); + xSpeed = speed; + } + } + public void auto_drive_y(double speed,double distance) + { + + } + public double getXPosition() { // Convert xPod into actual direction based on current heading - double xPos=0.0; + double xPos = 0.0; + odometer.update(); Pose2D pos = odometer.getPosition(); xPos = pos.getX(DistanceUnit.MM); return xPos; } public double getYPosition() { // Convert yPod into actual direction based on current heading double yPos = 0.0; + odometer.update(); Pose2D pos = odometer.getPosition(); yPos = pos.getY(DistanceUnit.MM); return yPos; From 82ac8bf0a51db0f168dc4705ff0a7a46608bf7cf Mon Sep 17 00:00:00 2001 From: Annika Date: Sat, 14 Dec 2024 10:42:44 -0700 Subject: [PATCH 016/129] Changed elevator switch statement for preset posiitions, got rid of two positions (low bucket and bar) --- .../ftc/teamcode/Mechanisms/Elevator.java | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java index a399609..c17b1c3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java @@ -58,33 +58,23 @@ public Elevator(HardwareMap hwMap) { // Set the elevator state public void setElevatorState(int position) { switch (position) { - case 0: //Case for start position + case 0: //Case for start position/pickup sample this.gotoPreSet(Constants.ELEVATOR_START); setpoint = Constants.ELEVATOR_START; //setpoint = target_height; // DEBUG: get from FTCDashboard break; - case 1: //Case for pickup sample + case 1: //Case for pickup specimen this.gotoPreSet(Constants.ELEVATOR_GROUND); setpoint = Constants.ELEVATOR_GROUND; break; - case 2: //Case for pickup specimen - this.gotoPreSet(Constants.ELEVATOR_LOW_BAR); - setpoint = Constants.ELEVATOR_LOW_BAR; - break; - - case 3: //Case for deposit specimen low bar + case 2: //Case for deposit specimen high bar this.gotoPreSet(Constants.ELEVATOR_LOW_BASKET); setpoint = Constants.ELEVATOR_LOW_BASKET; break; - case 4: //Case for deposit specimen high bar - this.gotoPreSet(Constants.ELEVATOR_HIGH_BAR); - setpoint = Constants.ELEVATOR_HIGH_BAR; - break; - - case 5: //Case for deposit sample in low bucket + case 3: //Case for deposit sample in high bucket this.gotoPreSet(Constants.ELEVATOR_HIGH_BASKET); setpoint = Constants.ELEVATOR_HIGH_BASKET; break; From c0542549b694f16b5df4f17ee389f387b1f7f90c Mon Sep 17 00:00:00 2001 From: Annika Date: Sat, 14 Dec 2024 12:51:57 -0700 Subject: [PATCH 017/129] Renamed the three elevator constants --- .../ftc/teamcode/Mechanisms/Constants.java | 8 ++++---- .../ftc/teamcode/Mechanisms/Elevator.java | 10 +++++----- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index df31581..fb605be 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -42,11 +42,11 @@ public static final double RF_OPEN = 0.2 ; // open position*/ // for elevator moves public static final int ELEVATOR_START = 0; - public static final int ELEVATOR_GROUND = 100; - public static final int ELEVATOR_LOW_BASKET = 850; // nov 22 + public static final int ELEVATOR_PICKUP_SPECIMEN = 100; + public static final int ELEVATOR_HIGH_BAR = 850; // nov 22 - public static final int ELEVATOR_LOW_BAR= 350; // nov 22 - public static final int ELEVATOR_HIGH_BAR = 660; + // public static final int ELEVATOR_LOW_BAR= 350; // nov 22 + // public static final int ELEVATOR_HIGH_BAR = 660; public static final int ELEVATOR_HIGH_BASKET = 1260; public static final int ELEVATOR_MAX = 1220; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java index c17b1c3..e97fec8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java @@ -65,16 +65,16 @@ public void setElevatorState(int position) { break; case 1: //Case for pickup specimen - this.gotoPreSet(Constants.ELEVATOR_GROUND); - setpoint = Constants.ELEVATOR_GROUND; + this.gotoPreSet(Constants.ELEVATOR_PICKUP_SPECIMEN); + setpoint = Constants.ELEVATOR_PICKUP_SPECIMEN; break; case 2: //Case for deposit specimen high bar - this.gotoPreSet(Constants.ELEVATOR_LOW_BASKET); - setpoint = Constants.ELEVATOR_LOW_BASKET; + this.gotoPreSet(Constants.ELEVATOR_HIGH_BAR); + setpoint = Constants.ELEVATOR_HIGH_BAR; break; - case 3: //Case for deposit sample in high bucket + case 3: //Case for deposit sample in high basket this.gotoPreSet(Constants.ELEVATOR_HIGH_BASKET); setpoint = Constants.ELEVATOR_HIGH_BASKET; break; From 7e6b9c9999066cab1eafcd17c034d21d89b9a888 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sat, 14 Dec 2024 12:57:59 -0700 Subject: [PATCH 018/129] Added need constants for auto: - AUTO_X_DISTANCE_ERROR - AUTO_Y_DISTANCE_ERROR --- .../org/firstinspires/ftc/teamcode/Mechanisms/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index df31581..26ff6dd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -87,6 +87,6 @@ public static double ODOMETER_X_OFFSET = -145.0; public static double ODOMETER_Y_OFFSET = 270.0; // auto constants - public static double AUTO_X_DISTANCE_ERROR = 10; - public static double AUTO_Y_DISTANCE_ERROR = 10; + public static double AUTO_X_DISTANCE_ERROR = 5; + public static double AUTO_Y_DISTANCE_ERROR = 5; } From 26828a59e237c89ab16d6009a32c1b2af027c4b8 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sat, 14 Dec 2024 12:59:38 -0700 Subject: [PATCH 019/129] Added a switch to control the steps or waypoints Added an inner switch for X and Y motions --- .../ftc/teamcode/Auto/PracticePark.java | 115 ++++++++++++++++-- 1 file changed, 108 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java index 5d3e717..c5e7add 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java @@ -5,6 +5,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; import java.util.Locale; @@ -12,15 +13,48 @@ @Autonomous (name = "PracticePark") public class PracticePark extends BaseAutoPractice{ private PracticeDriveTrain2025 driveTrain; - public int currentStep = -1; - // private double speed; + public int step = -1; + private double speed; private int counter = 0; BaseAutoPractice.WayPoint[] wayPoints; + // auto driving data + private double current_x, current_y; + private double start_x,start_y; + private double dest_x,dest_y,set_dest; + private double x_speed,y_speed; + private boolean at_x,at_y,set_x,set_y; + enum WorkingStep{ + X, + Y + } + WorkingStep currentStep; public void init(){ driveTrain = new PracticeDriveTrain2025(hardwareMap); driveTrain.init(); driveTrain.odometer.resetPosAndIMU(); + this.speed = 0.0; + this.at_x = false; + this.at_y = false; + this.step = 1; + this.currentStep = WorkingStep.X; + this.x_speed = 0.5; + this.y_speed = 0.5; + this.wayPoints = new WayPoint[4]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 50.0; + this.wayPoints[0].y = 50.0; + this.wayPoints[0].facing = 0.0; + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 0.0; + this.wayPoints[1].y = 0.0; + this.wayPoints[1].facing = 0.0; + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 0.0; + this.wayPoints[2].y = 0.0; + this.wayPoints[2].facing = 0.0; + // this.dest_x = 50.0;//initializing + // this.wayPoints = new WayPoint[12]; // // this.wayPoints[0].x = 10.0;//initializing @@ -29,16 +63,83 @@ public void init(){ }//init @Override public void loop(){ - telemetry.addData("Sequence Step", counter); + telemetry.addData("Sequence Step", this.step); + // Update x y coords driveTrain.odometer.update(); Pose2D pos = driveTrain.odometer.getPosition(); + current_x = driveTrain.getXPosition(); + current_y = driveTrain.getYPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Position", data); data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Function Position", data); - // telemetry.addData("heading:"."%.2f",heading); - driveTrain.odometer.update(); - driveTrain.auto_drive_x(0.4,100); + telemetry.addData("Speed x","%.2f",this.x_speed); + telemetry.addData("Speed y","%.2f",this.y_speed); + telemetry.addData("heading:","%.2f",driveTrain.heading); +// telemetry.addData("Dest x:","%.2f",this.dest_x); +// telemetry.addData("Dest y:","%.2f",this.dest_y); + telemetry.addData("Distance to X","%.2f",(Math.abs(this.wayPoints[this.step-1].x - this.current_x))); + telemetry.addData("Distance to Y","%.2f",Math.abs(this.wayPoints[this.step-1].y - this.current_y)); + switch (this.step){ + case 1: + switch (currentStep){ + case X: + if ((Math.abs(this.wayPoints[this.step-1].x) - Math.abs(current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) + { + driveTrain.drive(0,this.x_speed); + } + else + { + driveTrain.drive(0.0,0.0); + currentStep = WorkingStep.Y; + } + break; + case Y: + if ((Math.abs(this.wayPoints[this.step-1].y) - Math.abs(current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) + { + driveTrain.drive(this.y_speed,0.0); + } + else + { + driveTrain.drive(0.0,0.0); + currentStep = WorkingStep.X; + this.step += 1; + } + } + break; + case 2: + switch (currentStep){ + case X: + if ((Math.abs(this.wayPoints[this.step-1].x - this.current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) + { + this.x_speed = this.x_speed * Math.abs(this.wayPoints[this.step-1].x - this.current_x) / (this.wayPoints[this.step-1].x - this.current_x); + driveTrain.drive(0,this.x_speed); + } + else + { + driveTrain.drive(0.0,0.0); + currentStep = WorkingStep.Y; + } + break; + case Y: + if ((Math.abs(this.wayPoints[this.step-1].y - this.current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) + { + this.y_speed = this.y_speed * Math.abs(this.wayPoints[this.step-1].y - this.current_y) / (this.wayPoints[this.step-1].y - this.current_y); + driveTrain.drive(this.y_speed,0.0); + } + else + { + this.at_y = true; + driveTrain.drive(0.0,0.0); + currentStep = WorkingStep.X; + this.step += 1; + } + } + default: + break; + } + // driveTrain.odometer.update(); + // if (counter <= 150){ // driveTrain.drive(0.0,0.3); // speed = 0.1; @@ -49,7 +150,7 @@ public void loop(){ // //driveTrain.drive(0.0,0.1); // counter = counter + 1; // driveTrain.driveTo(speed,0.0, 5.0, Constants.NORTH); -// driveTrain.loop(); + driveTrain.loop(); } // loop }//everything From 584630d9b5900eed26c16e09eab57fb60b905192 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sat, 14 Dec 2024 13:01:51 -0700 Subject: [PATCH 020/129] moved function calls to Practice Park --- .../ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java index fe2660e..dd0cd59 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java @@ -211,14 +211,14 @@ public void auto_drive_y(double speed,double distance) public double getXPosition() { // Convert xPod into actual direction based on current heading double xPos = 0.0; - odometer.update(); + //odometer.update(); Pose2D pos = odometer.getPosition(); xPos = pos.getX(DistanceUnit.MM); return xPos; } public double getYPosition() { // Convert yPod into actual direction based on current heading double yPos = 0.0; - odometer.update(); + //odometer.update(); Pose2D pos = odometer.getPosition(); yPos = pos.getY(DistanceUnit.MM); return yPos; From b805cb3298306a77df0c308319d0c1728c07b28f Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 20 Dec 2024 19:44:29 -0700 Subject: [PATCH 021/129] Got the square to work --- .../ftc/teamcode/Auto/PracticePark.java | 43 +++++++++++-------- 1 file changed, 24 insertions(+), 19 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java index c5e7add..e63d94c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java @@ -41,17 +41,17 @@ public void init(){ this.x_speed = 0.5; this.y_speed = 0.5; this.wayPoints = new WayPoint[4]; - this.wayPoints[0] = new WayPoint(); - this.wayPoints[0].x = 50.0; - this.wayPoints[0].y = 50.0; - this.wayPoints[0].facing = 0.0; +// this.wayPoints[0] = new WayPoint(); +// this.wayPoints[0].x = 50.0; +// this.wayPoints[0].y = 50.0; +// this.wayPoints[0].facing = 0.0; this.wayPoints[1] = new WayPoint(); - this.wayPoints[1].x = 0.0; - this.wayPoints[1].y = 0.0; - this.wayPoints[1].facing = 0.0; + this.wayPoints[1].x = 50.0; + this.wayPoints[1].y = 50.0; + this.wayPoints[1].facing = Constants.NORTH; this.wayPoints[2] = new WayPoint(); - this.wayPoints[2].x = 0.0; - this.wayPoints[2].y = 0.0; + this.wayPoints[2].x = -50.0; + this.wayPoints[2].y = -50.0; this.wayPoints[2].facing = 0.0; // this.dest_x = 50.0;//initializing @@ -78,13 +78,18 @@ public void loop(){ telemetry.addData("heading:","%.2f",driveTrain.heading); // telemetry.addData("Dest x:","%.2f",this.dest_x); // telemetry.addData("Dest y:","%.2f",this.dest_y); - telemetry.addData("Distance to X","%.2f",(Math.abs(this.wayPoints[this.step-1].x - this.current_x))); - telemetry.addData("Distance to Y","%.2f",Math.abs(this.wayPoints[this.step-1].y - this.current_y)); + telemetry.addData("Distance to X","%.2f",(Math.abs(this.wayPoints[this.step].x - this.current_x))); + telemetry.addData("Distance to Y","%.2f",Math.abs(this.wayPoints[this.step].y - this.current_y)); switch (this.step){ case 1: +// driveTrain.driveTo(0.5,this.wayPoints[this.step].x,this.wayPoints[this.step].y,this.wayPoints[this.step].facing); +// if (driveTrain.atTarget()) +// { +// driveTrain.stop(); +// } switch (currentStep){ case X: - if ((Math.abs(this.wayPoints[this.step-1].x) - Math.abs(current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) + if ((Math.abs(this.wayPoints[this.step].x) - Math.abs(current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) { driveTrain.drive(0,this.x_speed); } @@ -95,7 +100,7 @@ public void loop(){ } break; case Y: - if ((Math.abs(this.wayPoints[this.step-1].y) - Math.abs(current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) + if ((Math.abs(this.wayPoints[this.step].y) - Math.abs(current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) { driveTrain.drive(this.y_speed,0.0); } @@ -110,10 +115,10 @@ public void loop(){ case 2: switch (currentStep){ case X: - if ((Math.abs(this.wayPoints[this.step-1].x - this.current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) + if ((Math.abs(this.wayPoints[this.step].x - this.current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) { - this.x_speed = this.x_speed * Math.abs(this.wayPoints[this.step-1].x - this.current_x) / (this.wayPoints[this.step-1].x - this.current_x); - driveTrain.drive(0,this.x_speed); + // this.x_speed = this.x_speed * Math.abs(this.wayPoints[this.step].x - this.current_x) / (this.wayPoints[this.step].x - this.current_x); + driveTrain.drive(0,-this.x_speed); } else { @@ -122,10 +127,10 @@ public void loop(){ } break; case Y: - if ((Math.abs(this.wayPoints[this.step-1].y - this.current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) + if ((Math.abs(this.wayPoints[this.step].y - this.current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) { - this.y_speed = this.y_speed * Math.abs(this.wayPoints[this.step-1].y - this.current_y) / (this.wayPoints[this.step-1].y - this.current_y); - driveTrain.drive(this.y_speed,0.0); + // this.y_speed = this.y_speed * Math.abs(this.wayPoints[this.step].y - this.current_y) / (this.wayPoints[this.step].y - this.current_y); + driveTrain.drive(-this.y_speed,0.0); } else { From 995aef643503ce0c2b3c8b8e93e947ea11cdae25 Mon Sep 17 00:00:00 2001 From: Kitten-kitten Date: Fri, 20 Dec 2024 20:18:04 -0700 Subject: [PATCH 022/129] craeted new auto for specimens --- .../ftc/teamcode/Auto/PracticeSpecimen.java | 161 ++++++++++++++++++ 1 file changed, 161 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java new file mode 100644 index 0000000..d11e156 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java @@ -0,0 +1,161 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; + +import java.util.Locale; + +@Autonomous(name = "PracticeSpecimen") +public class PracticeSpecimen extends BaseAutoPractice{ + private PracticeDriveTrain2025 driveTrain; + public int step = -1; + private double speed; + private int counter = 0; + BaseAutoPractice.WayPoint[] wayPoints; + // auto driving data + private double current_x, current_y; + private double start_x,start_y; + private double dest_x,dest_y,set_dest; + private double x_speed,y_speed; + private boolean at_x,at_y,set_x,set_y; + enum WorkingStep{ + X, + Y + } + WorkingStep currentStep; + + public void init(){ + driveTrain = new PracticeDriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + this.speed = 0.0; + this.at_x = false; + this.at_y = false; + this.step = 1; + this.currentStep = WorkingStep.X; + this.x_speed = 0.5; + this.y_speed = 0.5; + this.wayPoints = new WayPoint[4]; +// this.wayPoints[0] = new WayPoint(); +// this.wayPoints[0].x = 50.0; +// this.wayPoints[0].y = 50.0; +// this.wayPoints[0].facing = 0.0; + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 50.0; + this.wayPoints[1].y = 50.0; + this.wayPoints[1].facing = Constants.NORTH; + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = -50.0; + this.wayPoints[2].y = -50.0; + this.wayPoints[2].facing = 0.0; + // this.dest_x = 50.0;//initializing + +// this.wayPoints = new WayPoint[12]; +// +// this.wayPoints[0].x = 10.0;//initializing +// this.wayPoints[0].y = 0.0; +// this.wayPoints[0].facing = Constants.NORTH; + }//init + @Override + public void loop(){ + telemetry.addData("Sequence Step", this.step); + // Update x y coords + driveTrain.odometer.update(); + Pose2D pos = driveTrain.odometer.getPosition(); + current_x = driveTrain.getXPosition(); + current_y = driveTrain.getYPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Speed x","%.2f",this.x_speed); + telemetry.addData("Speed y","%.2f",this.y_speed); + telemetry.addData("heading:","%.2f",driveTrain.heading); +// telemetry.addData("Dest x:","%.2f",this.dest_x); +// telemetry.addData("Dest y:","%.2f",this.dest_y); + telemetry.addData("Distance to X","%.2f",(Math.abs(this.wayPoints[this.step].x - this.current_x))); + telemetry.addData("Distance to Y","%.2f",Math.abs(this.wayPoints[this.step].y - this.current_y)); + //starting waypoint navigation + switch (this.step){ + // drive 2 inches form submersible + case 1: + switch (currentStep){ + case X: + if ((Math.abs(this.wayPoints[this.step].x) - Math.abs(current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) + { + driveTrain.drive(0,this.x_speed); + } + else + { + driveTrain.drive(0.0,0.0); + currentStep = WorkingStep.Y; + } + break; + case Y: + if ((Math.abs(this.wayPoints[this.step].y) - Math.abs(current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) + { + driveTrain.drive(this.y_speed,0.0); + } + else + { + driveTrain.drive(0.0,0.0); + currentStep = WorkingStep.X; + this.step += 1; + } + } + break; + // putting specimen on the high bar + + case 2: + //raise elevator + switch (currentStep){ + case X: + if ((Math.abs(this.wayPoints[this.step].x - this.current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) + { + // this.x_speed = this.x_speed * Math.abs(this.wayPoints[this.step].x - this.current_x) / (this.wayPoints[this.step].x - this.current_x); + driveTrain.drive(0,-this.x_speed); + } + else + { + driveTrain.drive(0.0,0.0); + currentStep = WorkingStep.Y; + } + break; + case Y: + if ((Math.abs(this.wayPoints[this.step].y - this.current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) + { + // this.y_speed = this.y_speed * Math.abs(this.wayPoints[this.step].y - this.current_y) / (this.wayPoints[this.step].y - this.current_y); + driveTrain.drive(-this.y_speed,0.0); + } + else + { + this.at_y = true; + driveTrain.drive(0.0,0.0); + currentStep = WorkingStep.X; + this.step += 1; + } + } + default: + break; + } + // driveTrain.odometer.update(); + +// if (counter <= 150){ +// driveTrain.drive(0.0,0.3); +// speed = 0.1; +// }else { +// driveTrain.drive(0.0,0.0); +// speed = 0.0; +// } +// //driveTrain.drive(0.0,0.1); +// counter = counter + 1; +// driveTrain.driveTo(speed,0.0, 5.0, Constants.NORTH); + driveTrain.loop(); + } // loop +}//everything + From 06513f9753054ba92a23ac2e7d907f77f87fab09 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 20 Dec 2024 20:29:34 -0700 Subject: [PATCH 023/129] Cleaned up code with comments Removed code that wasn't being used --- .../ftc/teamcode/Auto/PracticeSpecimen.java | 29 ++----------------- 1 file changed, 3 insertions(+), 26 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java index d11e156..4a765c1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java @@ -41,10 +41,6 @@ public void init(){ this.x_speed = 0.5; this.y_speed = 0.5; this.wayPoints = new WayPoint[4]; -// this.wayPoints[0] = new WayPoint(); -// this.wayPoints[0].x = 50.0; -// this.wayPoints[0].y = 50.0; -// this.wayPoints[0].facing = 0.0; this.wayPoints[1] = new WayPoint(); this.wayPoints[1].x = 50.0; this.wayPoints[1].y = 50.0; @@ -53,19 +49,13 @@ public void init(){ this.wayPoints[2].x = -50.0; this.wayPoints[2].y = -50.0; this.wayPoints[2].facing = 0.0; - // this.dest_x = 50.0;//initializing - -// this.wayPoints = new WayPoint[12]; -// -// this.wayPoints[0].x = 10.0;//initializing -// this.wayPoints[0].y = 0.0; -// this.wayPoints[0].facing = Constants.NORTH; }//init @Override public void loop(){ - telemetry.addData("Sequence Step", this.step); // Update x y coords driveTrain.odometer.update(); + // Send information to the display + telemetry.addData("Sequence Step", this.step); Pose2D pos = driveTrain.odometer.getPosition(); current_x = driveTrain.getXPosition(); current_y = driveTrain.getYPosition(); @@ -76,8 +66,6 @@ public void loop(){ telemetry.addData("Speed x","%.2f",this.x_speed); telemetry.addData("Speed y","%.2f",this.y_speed); telemetry.addData("heading:","%.2f",driveTrain.heading); -// telemetry.addData("Dest x:","%.2f",this.dest_x); -// telemetry.addData("Dest y:","%.2f",this.dest_y); telemetry.addData("Distance to X","%.2f",(Math.abs(this.wayPoints[this.step].x - this.current_x))); telemetry.addData("Distance to Y","%.2f",Math.abs(this.wayPoints[this.step].y - this.current_y)); //starting waypoint navigation @@ -143,18 +131,7 @@ public void loop(){ default: break; } - // driveTrain.odometer.update(); - -// if (counter <= 150){ -// driveTrain.drive(0.0,0.3); -// speed = 0.1; -// }else { -// driveTrain.drive(0.0,0.0); -// speed = 0.0; -// } -// //driveTrain.drive(0.0,0.1); -// counter = counter + 1; -// driveTrain.driveTo(speed,0.0, 5.0, Constants.NORTH); + // Done Processing Waypoints driveTrain.loop(); } // loop }//everything From 149e66fbed16927bd2d71fb46f6e9a458ae7e061 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 20 Dec 2024 20:30:34 -0700 Subject: [PATCH 024/129] Commented out code that wasn't being used. --- .../ftc/teamcode/Mechanisms/DriveTrain2025.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index cd08440..e92fd65 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -141,11 +141,11 @@ public void loop(){ false); } - /* public boolean atTarget() { // Pythagorean theorem to get the distance from target as a number. - double xError = getXPosition() - currentXTarget; - double yError = getYPosition() - currentYTarget; - return !autoEnabled || Math.sqrt(xError * xError + yError * yError) < Constants.DRIVE_PID_ERROR; - }*/ +// public boolean atTarget() { // Pythagorean theorem to get the distance from target as a number. +// double xError = getXPosition() - currentXTarget; +// double yError = getYPosition() - currentYTarget; +// return !autoEnabled || Math.sqrt(xError * xError + yError * yError) < Constants.DRIVE_PID_ERROR; +// } //============== Move in new Direction ========== public void setDirection(double newHeading) { From cb8b5ba9275fe27ceefaa5b03f53ef78308439a5 Mon Sep 17 00:00:00 2001 From: Kitten-kitten Date: Sat, 21 Dec 2024 09:59:41 -0700 Subject: [PATCH 025/129] created new auto for specimens --- .../firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java index 4a765c1..d35df71 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java @@ -38,15 +38,15 @@ public void init(){ this.at_y = false; this.step = 1; this.currentStep = WorkingStep.X; - this.x_speed = 0.5; - this.y_speed = 0.5; + this.x_speed = 0.3; + this.y_speed = 0.3; this.wayPoints = new WayPoint[4]; this.wayPoints[1] = new WayPoint(); - this.wayPoints[1].x = 50.0; + this.wayPoints[1].x = 0.0; this.wayPoints[1].y = 50.0; this.wayPoints[1].facing = Constants.NORTH; this.wayPoints[2] = new WayPoint(); - this.wayPoints[2].x = -50.0; + this.wayPoints[2].x = 0.0; this.wayPoints[2].y = -50.0; this.wayPoints[2].facing = 0.0; }//init From 6b5b75484ffba704185e5b090e644c319f289481 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sun, 29 Dec 2024 23:46:52 -0700 Subject: [PATCH 026/129] Added Test Waypoints for George's Test. --- .../firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java index 8cfe5a8..7d5958e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java @@ -23,6 +23,13 @@ public class WayPoint { public double facing; } + public class GeorgeWayPoint { + public double x; + public double x_speed = 0.0; + public double y; + public double y_speed = 0.0; + public double facing; + } @Override public void start() { state = "START"; From 8f338ae92a4852ecb3b3346da923afc83238d805 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sun, 29 Dec 2024 23:47:19 -0700 Subject: [PATCH 027/129] Cleaned up formatting --- .../ftc/teamcode/Auto/PracticeSpecimen.java | 29 ++++++++++++------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java index d35df71..7206f77 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java @@ -16,12 +16,13 @@ public class PracticeSpecimen extends BaseAutoPractice{ public int step = -1; private double speed; private int counter = 0; + private int total_waypoints; BaseAutoPractice.WayPoint[] wayPoints; // auto driving data private double current_x, current_y; private double start_x,start_y; private double dest_x,dest_y,set_dest; - private double x_speed,y_speed; + private double x_speed,y_speed,x_step_speed,y_step_speed; private boolean at_x,at_y,set_x,set_y; enum WorkingStep{ X, @@ -37,17 +38,20 @@ public void init(){ this.at_x = false; this.at_y = false; this.step = 1; + this.total_waypoints = 2; this.currentStep = WorkingStep.X; this.x_speed = 0.3; this.y_speed = 0.3; - this.wayPoints = new WayPoint[4]; + this.x_step_speed = this.x_speed; + this.y_step_speed = this.y_speed; + this.wayPoints = new WayPoint[this.total_waypoints+1]; this.wayPoints[1] = new WayPoint(); this.wayPoints[1].x = 0.0; - this.wayPoints[1].y = 50.0; + this.wayPoints[1].y = -500.0; this.wayPoints[1].facing = Constants.NORTH; this.wayPoints[2] = new WayPoint(); this.wayPoints[2].x = 0.0; - this.wayPoints[2].y = -50.0; + this.wayPoints[2].y = -650.0; this.wayPoints[2].facing = 0.0; }//init @Override @@ -66,8 +70,13 @@ public void loop(){ telemetry.addData("Speed x","%.2f",this.x_speed); telemetry.addData("Speed y","%.2f",this.y_speed); telemetry.addData("heading:","%.2f",driveTrain.heading); - telemetry.addData("Distance to X","%.2f",(Math.abs(this.wayPoints[this.step].x - this.current_x))); - telemetry.addData("Distance to Y","%.2f",Math.abs(this.wayPoints[this.step].y - this.current_y)); +// telemetry.addData("Distance to X","%.2f",(Math.abs(this.wayPoints[this.step].x - this.current_x))); + // telemetry.addData("Distance to Y","%.2f",Math.abs(this.wayPoints[this.step].y - this.current_y)); + // Set speed + if (this.step <= this.total_waypoints) { + this.x_step_speed = (this.wayPoints[this.step].x <= 0) ? -this.x_speed : this.x_speed; + this.y_step_speed = (this.wayPoints[this.step].y <= 0) ? -this.y_speed : this.y_speed; + } //starting waypoint navigation switch (this.step){ // drive 2 inches form submersible @@ -76,7 +85,7 @@ public void loop(){ case X: if ((Math.abs(this.wayPoints[this.step].x) - Math.abs(current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) { - driveTrain.drive(0,this.x_speed); + driveTrain.drive(0,this.x_step_speed); } else { @@ -87,7 +96,7 @@ public void loop(){ case Y: if ((Math.abs(this.wayPoints[this.step].y) - Math.abs(current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) { - driveTrain.drive(this.y_speed,0.0); + driveTrain.drive(this.y_step_speed,0.0); } else { @@ -106,7 +115,7 @@ public void loop(){ if ((Math.abs(this.wayPoints[this.step].x - this.current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) { // this.x_speed = this.x_speed * Math.abs(this.wayPoints[this.step].x - this.current_x) / (this.wayPoints[this.step].x - this.current_x); - driveTrain.drive(0,-this.x_speed); + driveTrain.drive(0,this.x_step_speed); } else { @@ -118,7 +127,7 @@ public void loop(){ if ((Math.abs(this.wayPoints[this.step].y - this.current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) { // this.y_speed = this.y_speed * Math.abs(this.wayPoints[this.step].y - this.current_y) / (this.wayPoints[this.step].y - this.current_y); - driveTrain.drive(-this.y_speed,0.0); + driveTrain.drive(this.y_step_speed,0.0); } else { From f6292dab113850cfe4d6fab02fb74fd2a8e9d7a1 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 3 Jan 2025 14:06:45 -0700 Subject: [PATCH 028/129] Corrected the get y position to return the correct value. --- .../ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java index dd0cd59..21bfc54 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java @@ -221,7 +221,7 @@ public double getYPosition() { // Convert yPod into actual direction based on cu //odometer.update(); Pose2D pos = odometer.getPosition(); yPos = pos.getY(DistanceUnit.MM); - return yPos; + return -yPos; } From b739bf0758fb10a2099cd7e7b7816b9e41000ae5 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 3 Jan 2025 14:07:24 -0700 Subject: [PATCH 029/129] Modified to use for gathering x y coordinates --- .../ftc/teamcode/Auto/PracticePark.java | 119 +++++++++--------- 1 file changed, 57 insertions(+), 62 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java index e63d94c..20b8142 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java @@ -80,69 +80,64 @@ public void loop(){ // telemetry.addData("Dest y:","%.2f",this.dest_y); telemetry.addData("Distance to X","%.2f",(Math.abs(this.wayPoints[this.step].x - this.current_x))); telemetry.addData("Distance to Y","%.2f",Math.abs(this.wayPoints[this.step].y - this.current_y)); - switch (this.step){ - case 1: -// driveTrain.driveTo(0.5,this.wayPoints[this.step].x,this.wayPoints[this.step].y,this.wayPoints[this.step].facing); -// if (driveTrain.atTarget()) -// { -// driveTrain.stop(); +// switch (this.step){ +// case 1: +// switch (currentStep){ +// case X: +// if ((Math.abs(this.wayPoints[this.step].x) - Math.abs(current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) +// { +// driveTrain.drive(0,this.x_speed); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// currentStep = WorkingStep.Y; +// } +// break; +// case Y: +// if ((Math.abs(this.wayPoints[this.step].y) - Math.abs(current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) +// { +// driveTrain.drive(this.y_speed,0.0); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// currentStep = WorkingStep.X; +// this.step += 1; +// } // } - switch (currentStep){ - case X: - if ((Math.abs(this.wayPoints[this.step].x) - Math.abs(current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) - { - driveTrain.drive(0,this.x_speed); - } - else - { - driveTrain.drive(0.0,0.0); - currentStep = WorkingStep.Y; - } - break; - case Y: - if ((Math.abs(this.wayPoints[this.step].y) - Math.abs(current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) - { - driveTrain.drive(this.y_speed,0.0); - } - else - { - driveTrain.drive(0.0,0.0); - currentStep = WorkingStep.X; - this.step += 1; - } - } - break; - case 2: - switch (currentStep){ - case X: - if ((Math.abs(this.wayPoints[this.step].x - this.current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) - { - // this.x_speed = this.x_speed * Math.abs(this.wayPoints[this.step].x - this.current_x) / (this.wayPoints[this.step].x - this.current_x); - driveTrain.drive(0,-this.x_speed); - } - else - { - driveTrain.drive(0.0,0.0); - currentStep = WorkingStep.Y; - } - break; - case Y: - if ((Math.abs(this.wayPoints[this.step].y - this.current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) - { - // this.y_speed = this.y_speed * Math.abs(this.wayPoints[this.step].y - this.current_y) / (this.wayPoints[this.step].y - this.current_y); - driveTrain.drive(-this.y_speed,0.0); - } - else - { - this.at_y = true; - driveTrain.drive(0.0,0.0); - currentStep = WorkingStep.X; - this.step += 1; - } - } - default: - break; - } +// break; +// case 2: +// switch (currentStep){ +// case X: +// if ((Math.abs(this.wayPoints[this.step].x - this.current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) +// { +// // this.x_speed = this.x_speed * Math.abs(this.wayPoints[this.step].x - this.current_x) / (this.wayPoints[this.step].x - this.current_x); +// driveTrain.drive(0,-this.x_speed); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// currentStep = WorkingStep.Y; +// } +// break; +// case Y: +// if ((Math.abs(this.wayPoints[this.step].y - this.current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) +// { +// // this.y_speed = this.y_speed * Math.abs(this.wayPoints[this.step].y - this.current_y) / (this.wayPoints[this.step].y - this.current_y); +// driveTrain.drive(-this.y_speed,0.0); +// } +// else +// { +// this.at_y = true; +// driveTrain.drive(0.0,0.0); +// currentStep = WorkingStep.X; +// this.step += 1; +// } +// } +// default: +// break; +// } // driveTrain.odometer.update(); // if (counter <= 150){ From 11187dd4ea8c909894b49de0a5e9d427c1cb0428 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 3 Jan 2025 14:08:02 -0700 Subject: [PATCH 030/129] Modified getYPosition to return the correct signed value. --- .../ftc/teamcode/Mechanisms/DriveTrain2025.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index e92fd65..74f4549 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -12,6 +12,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Auto.PracticeSpecimen; public class @@ -183,8 +184,10 @@ public double getXPosition() { // Convert xPod into actual direction based on cu public double getYPosition() { // Convert yPod into actual direction based on current heading double yPos = 0.0; Pose2D pos = odometer.getPosition(); - yPos = pos.getY(DistanceUnit.MM); - return yPos; + // The following is needed because of an error in the driver + yPos = pos.getY(DistanceUnit.MM) / 10; + // Added negative sign to match drive control + return -yPos; } public void stop() { @@ -193,6 +196,7 @@ public void stop() { driveBase.stop(); autoEnabled = false; } + public void printTelemetry(Telemetry telemetry) { //telemetry.addData("actual heading:", "%5.2f", imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); telemetry.addData("saved heading:", "%5.2f", heading); From ff16720aca79eb9e56d9f13007bed83f18ed7f51 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 3 Jan 2025 14:08:27 -0700 Subject: [PATCH 031/129] Created this file for code testing. --- .../ftc/teamcode/Auto/GeorgeTest.java | 245 ++++++++++++++++++ 1 file changed, 245 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java new file mode 100644 index 0000000..c58adee --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java @@ -0,0 +1,245 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; + +import java.util.Locale; + +@Autonomous(name = "GeorgeTest") +public class GeorgeTest extends BaseAutoPractice{ + private PracticeDriveTrain2025 driveTrain; + enum WorkingStep{ + X, + Y + } + PracticeSpecimen.WorkingStep currentStep; + BaseAutoPractice.GeorgeWayPoint[] wayPoints; + private int current_step; + private int total_waypoints = 10; + + public void init() { + driveTrain = new PracticeDriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + this.current_step = 1; + // Waypoint Setup + this.wayPoints = new GeorgeWayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new GeorgeWayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + this.wayPoints[1] = new GeorgeWayPoint(); + this.wayPoints[1].x = 0.0; + this.wayPoints[1].y = -200.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + this.wayPoints[2] = new GeorgeWayPoint(); + this.wayPoints[2].x = 40.0; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].y = -200.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = 0.0; + this.wayPoints[3] = new GeorgeWayPoint(); + this.wayPoints[3].x = 0.0; + this.wayPoints[3].x_speed = 0.3; + this.wayPoints[3].y = -400.0; + this.wayPoints[3].y_speed = 0.5; + this.wayPoints[3].facing = 0.0; + this.wayPoints[4] = new GeorgeWayPoint(); + this.wayPoints[4].x = 0.0; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].y = -400.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].facing = 0.0; + this.wayPoints[5] = new GeorgeWayPoint(); + this.wayPoints[5].x = 0.0; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].y = -500.0; + this.wayPoints[5].y_speed = 0.4; + this.wayPoints[5].facing = 0.0; + this.wayPoints[6] = new GeorgeWayPoint(); + this.wayPoints[6].x = 0.0; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].y = -400.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].facing = 0.0; + this.wayPoints[7] = new GeorgeWayPoint(); + this.wayPoints[7].x = 0.0; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].y = 0.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].facing = 0.0; + } // init + + @Override + public void loop(){ + driveTrain.odometer.update(); + + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } + +// telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); +// telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); +// telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); +// telemetry.addData("Speed x","%.2f",this.x_speed); +// telemetry.addData("Speed y","%.2f",this.y_speed); + telemetry.addData("heading:","%.2f",driveTrain.heading); + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 2: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 3: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 4: + driveTrain.setDirection(Constants.WEST); + this.current_step++; + break; + case 5: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 6: + driveTrain.setDirection(Constants.EAST); + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 7: + driveTrain.setDirection(Constants.NORTH); + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + default: + break; + } + // Done Processing Waypoints + driveTrain.loop(); + } + + public boolean at_xy(GeorgeWayPoint previousWaypoint, GeorgeWayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + + public boolean at_x(double previousX, double currentX) + { + if (currentX - previousX >= 0.0) + { + return driveTrain.getXPosition() >= currentX; + } + else { + return driveTrain.getXPosition() <= currentX; + } + } + + public boolean at_y(double previousY, double currentY) + { + if (currentY - previousY >= 0.0) + { + return driveTrain.getYPosition() >= currentY; + } + else { + return driveTrain.getYPosition() <= currentY; + } + } + + public boolean goto_xy(GeorgeWayPoint previousWaypoint, GeorgeWayPoint currentWaypoint) { + // Check to see if you are at x. If yes, then process y; + // Process x direction + if (! this.at_x(previousWaypoint.x,currentWaypoint.x) ) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(0, currentWaypoint.x_speed); + } + else { + this.driveTrain.drive(0,-currentWaypoint.x_speed); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + // Process y direction + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(currentWaypoint.y_speed, 0.0); + } + else { + this.driveTrain.drive(-currentWaypoint.y_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; + } +} From 78c2816d4b988ea69599ea6c9993e09516a5a0d3 Mon Sep 17 00:00:00 2001 From: Annika Date: Fri, 3 Jan 2025 17:28:49 -0700 Subject: [PATCH 032/129] Updated code from George Test for moving in auto --- .../ftc/teamcode/Auto/PracticeSpecimen.java | 264 +++++++++++------- 1 file changed, 167 insertions(+), 97 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java index 7206f77..e5d5af6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java @@ -13,135 +13,205 @@ @Autonomous(name = "PracticeSpecimen") public class PracticeSpecimen extends BaseAutoPractice{ private PracticeDriveTrain2025 driveTrain; - public int step = -1; - private double speed; - private int counter = 0; - private int total_waypoints; + private int current_step; + private int total_waypoints = 10; BaseAutoPractice.WayPoint[] wayPoints; - // auto driving data - private double current_x, current_y; - private double start_x,start_y; - private double dest_x,dest_y,set_dest; - private double x_speed,y_speed,x_step_speed,y_step_speed; - private boolean at_x,at_y,set_x,set_y; - enum WorkingStep{ - X, - Y - } - WorkingStep currentStep; public void init(){ driveTrain = new PracticeDriveTrain2025(hardwareMap); driveTrain.init(); driveTrain.odometer.resetPosAndIMU(); - this.speed = 0.0; - this.at_x = false; - this.at_y = false; - this.step = 1; - this.total_waypoints = 2; - this.currentStep = WorkingStep.X; - this.x_speed = 0.3; - this.y_speed = 0.3; - this.x_step_speed = this.x_speed; - this.y_step_speed = this.y_speed; - this.wayPoints = new WayPoint[this.total_waypoints+1]; + this.current_step = 1; + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; this.wayPoints[1] = new WayPoint(); this.wayPoints[1].x = 0.0; - this.wayPoints[1].y = -500.0; + this.wayPoints[1].y = -582.0; + this.wayPoints[1].y_speed = 0.3; this.wayPoints[1].facing = Constants.NORTH; this.wayPoints[2] = new WayPoint(); this.wayPoints[2].x = 0.0; - this.wayPoints[2].y = -650.0; - this.wayPoints[2].facing = 0.0; + this.wayPoints[2].y = -746.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.NORTH; + this.wayPoints[3] = new WayPoint(); + this.wayPoints[3].x = 0.0; + this.wayPoints[3].y = -560.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].facing = Constants.NORTH; + }//init @Override public void loop(){ - // Update x y coords driveTrain.odometer.update(); + // Send information to the display - telemetry.addData("Sequence Step", this.step); + telemetry.addData("Sequence Step", this.current_step); Pose2D pos = driveTrain.odometer.getPosition(); - current_x = driveTrain.getXPosition(); - current_y = driveTrain.getYPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Position", data); data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Function Position", data); - telemetry.addData("Speed x","%.2f",this.x_speed); - telemetry.addData("Speed y","%.2f",this.y_speed); - telemetry.addData("heading:","%.2f",driveTrain.heading); -// telemetry.addData("Distance to X","%.2f",(Math.abs(this.wayPoints[this.step].x - this.current_x))); - // telemetry.addData("Distance to Y","%.2f",Math.abs(this.wayPoints[this.step].y - this.current_y)); - // Set speed - if (this.step <= this.total_waypoints) { - this.x_step_speed = (this.wayPoints[this.step].x <= 0) ? -this.x_speed : this.x_speed; - this.y_step_speed = (this.wayPoints[this.step].y <= 0) ? -this.y_speed : this.y_speed; + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } + else { + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); } - //starting waypoint navigation - switch (this.step){ + + telemetry.addData("heading:","%.2f",driveTrain.heading); + + //starting waypoint navigation + switch (this.current_step) { // drive 2 inches form submersible case 1: - switch (currentStep){ - case X: - if ((Math.abs(this.wayPoints[this.step].x) - Math.abs(current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) - { - driveTrain.drive(0,this.x_step_speed); - } - else - { - driveTrain.drive(0.0,0.0); - currentStep = WorkingStep.Y; - } - break; - case Y: - if ((Math.abs(this.wayPoints[this.step].y) - Math.abs(current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) - { - driveTrain.drive(this.y_step_speed,0.0); - } - else - { - driveTrain.drive(0.0,0.0); - currentStep = WorkingStep.X; - this.step += 1; - } + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; } break; - // putting specimen on the high bar - case 2: - //raise elevator - switch (currentStep){ - case X: - if ((Math.abs(this.wayPoints[this.step].x - this.current_x)) >= Constants.AUTO_X_DISTANCE_ERROR) - { - // this.x_speed = this.x_speed * Math.abs(this.wayPoints[this.step].x - this.current_x) / (this.wayPoints[this.step].x - this.current_x); - driveTrain.drive(0,this.x_step_speed); - } - else - { - driveTrain.drive(0.0,0.0); - currentStep = WorkingStep.Y; - } - break; - case Y: - if ((Math.abs(this.wayPoints[this.step].y - this.current_y)) >= Constants.AUTO_Y_DISTANCE_ERROR) - { - // this.y_speed = this.y_speed * Math.abs(this.wayPoints[this.step].y - this.current_y) / (this.wayPoints[this.step].y - this.current_y); - driveTrain.drive(this.y_step_speed,0.0); - } - else - { - this.at_y = true; - driveTrain.drive(0.0,0.0); - currentStep = WorkingStep.X; - this.step += 1; - } + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 3: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; +// case 4: +// driveTrain.setDirection(Constants.WEST); +// this.current_step++; +// break; +// case 5: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 6: +// driveTrain.setDirection(Constants.EAST); +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 7: +// driveTrain.setDirection(Constants.NORTH); +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; default: break; } // Done Processing Waypoints driveTrain.loop(); - } // loop + } + + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + + public boolean at_x(double previousX, double currentX) + { + if (currentX - previousX >= 0.0) + { + return driveTrain.getXPosition() >= currentX; + } + else { + return driveTrain.getXPosition() <= currentX; + } + } + + public boolean at_y(double previousY, double currentY) + { + if (currentY - previousY >= 0.0) + { + return driveTrain.getYPosition() >= currentY; + } + else { + return driveTrain.getYPosition() <= currentY; + } + } + + public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + // Check to see if you are at x. If yes, then process y; + // Process x direction + if (! this.at_x(previousWaypoint.x,currentWaypoint.x) ) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(0, currentWaypoint.x_speed); + } + else { + this.driveTrain.drive(0,-currentWaypoint.x_speed); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + // Process y direction + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(currentWaypoint.y_speed, 0.0); + } + else { + this.driveTrain.drive(-currentWaypoint.y_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; + } + }//everything From 639b8ac21c04d0ad77aceb8b526cc7f06f5f4919 Mon Sep 17 00:00:00 2001 From: Annika Date: Fri, 3 Jan 2025 17:29:18 -0700 Subject: [PATCH 033/129] Changed the waypoint class to include x and y speed --- .../firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java index 7d5958e..41ee3a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java @@ -17,13 +17,8 @@ public class BaseAutoPractice extends OpMode { public PracticeDriveTrain2025 driveTrain; - public class WayPoint { - public double x; - public double y; - public double facing; - } - public class GeorgeWayPoint { + public class WayPoint { public double x; public double x_speed = 0.0; public double y; From 7f815227dc38fbb9ca10714cea7d51db3b2c1a44 Mon Sep 17 00:00:00 2001 From: Annika Date: Fri, 3 Jan 2025 18:00:59 -0700 Subject: [PATCH 034/129] Added more waypoints --- .../ftc/teamcode/Auto/PracticeSpecimen.java | 81 ++++++++++++------- 1 file changed, 54 insertions(+), 27 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java index e5d5af6..666be9d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java @@ -33,16 +33,43 @@ public void init(){ this.wayPoints[1].y = -582.0; this.wayPoints[1].y_speed = 0.3; this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 this.wayPoints[2] = new WayPoint(); this.wayPoints[2].x = 0.0; this.wayPoints[2].y = -746.0; this.wayPoints[2].y_speed = 0.3; this.wayPoints[2].facing = Constants.NORTH; + //Done Waypoint 2 this.wayPoints[3] = new WayPoint(); this.wayPoints[3].x = 0.0; this.wayPoints[3].y = -560.0; this.wayPoints[3].y_speed = 0.3; this.wayPoints[3].facing = Constants.NORTH; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint(); + this.wayPoints[4].x = 1118.0; + this.wayPoints[4].y = -560.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.NORTH; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint(); + this.wayPoints[5].x = 1118.0; + this.wayPoints[5].y = -560.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.SOUTH; + //Done Waypoint 5 + //Waypoint for rotating + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = 1118.0; + this.wayPoints[6].y = -148.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.SOUTH; + //Done waypoint 6 + + }//init @Override @@ -107,33 +134,33 @@ public void loop(){ this.current_step++; } break; -// case 4: -// driveTrain.setDirection(Constants.WEST); -// this.current_step++; -// break; -// case 5: -// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) -// { -// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); -// } -// else -// { -// driveTrain.drive(0.0,0.0); -// this.current_step++; -// } -// break; -// case 6: -// driveTrain.setDirection(Constants.EAST); -// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) -// { -// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); -// } -// else -// { -// driveTrain.drive(0.0,0.0); -// this.current_step++; -// } -// break; + case 4: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 5: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 6: + driveTrain.setDirection(Constants.EAST); + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; // case 7: // driveTrain.setDirection(Constants.NORTH); // if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) From 5a71d543e698c4c09f80a66798b594fa1fe57ce2 Mon Sep 17 00:00:00 2001 From: Annika Date: Fri, 3 Jan 2025 18:33:54 -0700 Subject: [PATCH 035/129] Added more waypoints --- .../ftc/teamcode/Auto/PracticeSpecimen.java | 30 +++++++++++-------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java index 666be9d..7643a1c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java @@ -68,6 +68,12 @@ public void init(){ this.wayPoints[6].x_speed = 0.3; this.wayPoints[6].facing = Constants.SOUTH; //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = -126.0; + this.wayPoints[7].y = -148.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.SOUTH; @@ -150,7 +156,6 @@ public void loop(){ this.current_step++; break; case 6: - driveTrain.setDirection(Constants.EAST); if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); @@ -161,18 +166,17 @@ public void loop(){ this.current_step++; } break; -// case 7: -// driveTrain.setDirection(Constants.NORTH); -// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) -// { -// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); -// } -// else -// { -// driveTrain.drive(0.0,0.0); -// this.current_step++; -// } -// break; + case 7: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; default: break; } From d9aeabbce25f9d412869cf33401a846c6d4ad6e0 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 3 Jan 2025 20:36:17 -0700 Subject: [PATCH 036/129] Changed from GeorgeWaypoint to Waypoint --- .../ftc/teamcode/Auto/GeorgeTest.java | 31 ++++++++----------- 1 file changed, 13 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java index c58adee..82698fa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java @@ -17,8 +17,8 @@ enum WorkingStep{ X, Y } - PracticeSpecimen.WorkingStep currentStep; - BaseAutoPractice.GeorgeWayPoint[] wayPoints; + + BaseAutoPractice.WayPoint[] wayPoints; private int current_step; private int total_waypoints = 10; @@ -28,47 +28,47 @@ public void init() { driveTrain.odometer.resetPosAndIMU(); this.current_step = 1; // Waypoint Setup - this.wayPoints = new GeorgeWayPoint[this.total_waypoints + 1]; - this.wayPoints[0] = new GeorgeWayPoint(); + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); this.wayPoints[0].x = 0.0; this.wayPoints[0].y = 0.0; this.wayPoints[0].facing = Constants.NORTH; - this.wayPoints[1] = new GeorgeWayPoint(); + this.wayPoints[1] = new WayPoint(); this.wayPoints[1].x = 0.0; this.wayPoints[1].y = -200.0; this.wayPoints[1].y_speed = 0.3; this.wayPoints[1].facing = Constants.NORTH; - this.wayPoints[2] = new GeorgeWayPoint(); + this.wayPoints[2] = new WayPoint(); this.wayPoints[2].x = 40.0; this.wayPoints[2].x_speed = 0.3; this.wayPoints[2].y = -200.0; this.wayPoints[2].y_speed = 0.3; this.wayPoints[2].facing = 0.0; - this.wayPoints[3] = new GeorgeWayPoint(); + this.wayPoints[3] = new WayPoint(); this.wayPoints[3].x = 0.0; this.wayPoints[3].x_speed = 0.3; this.wayPoints[3].y = -400.0; this.wayPoints[3].y_speed = 0.5; this.wayPoints[3].facing = 0.0; - this.wayPoints[4] = new GeorgeWayPoint(); + this.wayPoints[4] = new WayPoint(); this.wayPoints[4].x = 0.0; this.wayPoints[4].x_speed = 0.3; this.wayPoints[4].y = -400.0; this.wayPoints[4].y_speed = 0.3; this.wayPoints[4].facing = 0.0; - this.wayPoints[5] = new GeorgeWayPoint(); + this.wayPoints[5] = new WayPoint(); this.wayPoints[5].x = 0.0; this.wayPoints[5].x_speed = 0.3; this.wayPoints[5].y = -500.0; this.wayPoints[5].y_speed = 0.4; this.wayPoints[5].facing = 0.0; - this.wayPoints[6] = new GeorgeWayPoint(); + this.wayPoints[6] = new WayPoint(); this.wayPoints[6].x = 0.0; this.wayPoints[6].x_speed = 0.3; this.wayPoints[6].y = -400.0; this.wayPoints[6].y_speed = 0.3; this.wayPoints[6].facing = 0.0; - this.wayPoints[7] = new GeorgeWayPoint(); + this.wayPoints[7] = new WayPoint(); this.wayPoints[7].x = 0.0; this.wayPoints[7].x_speed = 0.3; this.wayPoints[7].y = 0.0; @@ -94,11 +94,6 @@ public void loop(){ telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); } -// telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); -// telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); -// telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); -// telemetry.addData("Speed x","%.2f",this.x_speed); -// telemetry.addData("Speed y","%.2f",this.y_speed); telemetry.addData("heading:","%.2f",driveTrain.heading); //starting waypoint navigation @@ -183,7 +178,7 @@ public void loop(){ driveTrain.loop(); } - public boolean at_xy(GeorgeWayPoint previousWaypoint, GeorgeWayPoint currentWaypoint) + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); } @@ -210,7 +205,7 @@ public boolean at_y(double previousY, double currentY) } } - public boolean goto_xy(GeorgeWayPoint previousWaypoint, GeorgeWayPoint currentWaypoint) { + public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { // Check to see if you are at x. If yes, then process y; // Process x direction if (! this.at_x(previousWaypoint.x,currentWaypoint.x) ) { From a60d8d118a966243cb1abedf71d92d6fba8c753c Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 3 Jan 2025 20:36:42 -0700 Subject: [PATCH 037/129] Modified to get reading for x y coord for auto --- .../java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java | 1 + 1 file changed, 1 insertion(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java index 20b8142..4976b74 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java @@ -80,6 +80,7 @@ public void loop(){ // telemetry.addData("Dest y:","%.2f",this.dest_y); telemetry.addData("Distance to X","%.2f",(Math.abs(this.wayPoints[this.step].x - this.current_x))); telemetry.addData("Distance to Y","%.2f",Math.abs(this.wayPoints[this.step].y - this.current_y)); +// driveTrain.setDirection(Constants.SOUTH); // switch (this.step){ // case 1: // switch (currentStep){ From eb05d051e32171930c948ae49ec54bf976c23f32 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 3 Jan 2025 20:37:16 -0700 Subject: [PATCH 038/129] All of the waypoints are finished. Need to move to compbot for additional testing. --- .../ftc/teamcode/Auto/PracticeSpecimen.java | 168 +++++++++++++++++- 1 file changed, 160 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java index 7643a1c..c2f166a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java @@ -14,7 +14,7 @@ public class PracticeSpecimen extends BaseAutoPractice{ private PracticeDriveTrain2025 driveTrain; private int current_step; - private int total_waypoints = 10; + private int total_waypoints = 20; BaseAutoPractice.WayPoint[] wayPoints; public void init(){ @@ -63,20 +63,80 @@ public void init(){ //Waypoint for rotating this.wayPoints[6] = new WayPoint(); this.wayPoints[6].x = 1118.0; - this.wayPoints[6].y = -148.0; + this.wayPoints[6].y = 200.0; this.wayPoints[6].y_speed = 0.3; this.wayPoints[6].x_speed = 0.3; this.wayPoints[6].facing = Constants.SOUTH; //Done waypoint 6 this.wayPoints[7] = new WayPoint(); - this.wayPoints[7].x = -126.0; - this.wayPoints[7].y = -148.0; + this.wayPoints[7].x = 1118.0; + this.wayPoints[7].y = 200.0; this.wayPoints[7].y_speed = 0.3; this.wayPoints[7].x_speed = 0.3; - this.wayPoints[7].facing = Constants.SOUTH; - - - + this.wayPoints[7].facing = Constants.NORTH; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = 0.0; + this.wayPoints[8].y = 0.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = 0.0; + this.wayPoints[9].y = -582.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = -720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + //Waypoint + this.wayPoints[11] = new WayPoint(); + this.wayPoints[11].x = 0.0; + this.wayPoints[11].y = -540.0; + this.wayPoints[11].y_speed = 0.3; + this.wayPoints[11].x_speed = 0.3; + this.wayPoints[11].facing = Constants.NORTH; + //Waypoint + this.wayPoints[12] = new WayPoint(); + this.wayPoints[12].x = 640.0; + this.wayPoints[12].y = -540.0; + this.wayPoints[12].y_speed = 0.3; + this.wayPoints[12].x_speed = 0.3; + this.wayPoints[12].facing = Constants.NORTH; + //Waypoint + this.wayPoints[13] = new WayPoint(); + this.wayPoints[13].x = 640.0; + this.wayPoints[13].y = -1350.0; + this.wayPoints[13].y_speed = 0.3; + this.wayPoints[13].x_speed = 0.3; + this.wayPoints[13].facing = Constants.NORTH; + //Waypoint + this.wayPoints[14] = new WayPoint(); + this.wayPoints[14].x = 990.0; + this.wayPoints[14].y = -200.0; + this.wayPoints[14].y_speed = 0.4; + this.wayPoints[14].x_speed = 0.3; + this.wayPoints[14].facing = Constants.NORTH; + //Waypoint + this.wayPoints[15] = new WayPoint(); + this.wayPoints[15].x = 990.0; + this.wayPoints[15].y = -1350.0; + this.wayPoints[15].y_speed = 0.4; + this.wayPoints[15].x_speed = 0.3; + this.wayPoints[15].facing = Constants.NORTH; + //Waypoint + this.wayPoints[16] = new WayPoint(); + this.wayPoints[16].x = 1220.0; + this.wayPoints[16].y = -200.0; + this.wayPoints[16].y_speed = 0.4; + this.wayPoints[16].x_speed = 0.3; + this.wayPoints[16].facing = Constants.NORTH; }//init @Override public void loop(){ @@ -167,6 +227,98 @@ public void loop(){ } break; case 7: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 8: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 9: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 10: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 11: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 12: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 13: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 14: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 15: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 16: if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); From ae2e8729d08fb5037774a175829d3ec82d3b8424 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 3 Jan 2025 23:01:34 -0700 Subject: [PATCH 039/129] Added PracticeBasket for Practice Bot for the auto Basket task. --- .../ftc/teamcode/Auto/PracticeBasket.java | 278 ++++++++++++++++++ 1 file changed, 278 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java new file mode 100644 index 0000000..9f844a1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java @@ -0,0 +1,278 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; + +import java.util.Locale; + +@Autonomous(name = "PracticeBasket") +public class PracticeBasket extends BaseAutoPractice{ + private PracticeDriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + BaseAutoPractice.WayPoint[] wayPoints; + + public void init(){ + driveTrain = new PracticeDriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + this.current_step = 1; + //Waypoint setup + this.wayPoints = new BaseAutoPractice.WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new BaseAutoPractice.WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new BaseAutoPractice.WayPoint(); + this.wayPoints[1].x = 0.0; + this.wayPoints[1].y = -450.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new BaseAutoPractice.WayPoint(); + this.wayPoints[2].x = 0.0; + this.wayPoints[2].y = -450.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.SOUTH_WEST; + //Done Waypoint 2 + this.wayPoints[3] = new BaseAutoPractice.WayPoint(); + this.wayPoints[3].x = -270.0; + this.wayPoints[3].y = -250.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].facing = Constants.SOUTH_WEST; + //Done Waypoint 3 + this.wayPoints[4] = new BaseAutoPractice.WayPoint(); + this.wayPoints[4].x = -270.0; + this.wayPoints[4].y = -250.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.NORTH; + //Done Waypoint 4 + this.wayPoints[5] = new BaseAutoPractice.WayPoint(); + this.wayPoints[5].x = -445.0; + this.wayPoints[5].y = -530.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.NORTH; + //Done Waypoint 5 + //Waypoint for rotating + this.wayPoints[6] = new BaseAutoPractice.WayPoint(); + this.wayPoints[6].x = -445.0; + this.wayPoints[6].y = -530.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.SOUTH_WEST; + //Done waypoint 6 + this.wayPoints[7] = new BaseAutoPractice.WayPoint(); + this.wayPoints[7].x = -270.0; + this.wayPoints[7].y = -250.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[8] = new BaseAutoPractice.WayPoint(); + this.wayPoints[8].x = -270.0; + this.wayPoints[8].y = -530.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new BaseAutoPractice.WayPoint(); + this.wayPoints[9].x = -695.0; + this.wayPoints[9].y = -530.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new BaseAutoPractice.WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = -720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + + }//init + @Override + public void loop(){ + driveTrain.odometer.update(); + + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } + else { + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + } + telemetry.addData("heading:","%.2f",driveTrain.heading); + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 2: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 3: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 4: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 5: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 6: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 7: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 8: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 9: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 10: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + default: + break; + } + // Done Processing Waypoints + driveTrain.loop(); + } + + public boolean at_xy(BaseAutoPractice.WayPoint previousWaypoint, BaseAutoPractice.WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + + public boolean at_x(double previousX, double currentX) + { + if (currentX - previousX >= 0.0) + { + return driveTrain.getXPosition() >= currentX; + } + else { + return driveTrain.getXPosition() <= currentX; + } + } + + public boolean at_y(double previousY, double currentY) + { + if (currentY - previousY >= 0.0) + { + return driveTrain.getYPosition() >= currentY; + } + else { + return driveTrain.getYPosition() <= currentY; + } + } + + public boolean goto_xy(BaseAutoPractice.WayPoint previousWaypoint, BaseAutoPractice.WayPoint currentWaypoint) { + // Check to see if you are at x. If yes, then process y; + // Process x direction + if (! this.at_x(previousWaypoint.x,currentWaypoint.x) ) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(0, currentWaypoint.x_speed); + } + else { + this.driveTrain.drive(0,-currentWaypoint.x_speed); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + // Process y direction + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(currentWaypoint.y_speed, 0.0); + } + else { + this.driveTrain.drive(-currentWaypoint.y_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; + } + +} From 9989d2a0979be045ba907a1ce9a52659e1eb20b6 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 10 Jan 2025 17:33:33 -0700 Subject: [PATCH 040/129] Added PracticeBasket for Practice Bot for the auto Basket task. --- .../java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java | 1 + 1 file changed, 1 insertion(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java index 9f844a1..1cd9cb6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java @@ -97,6 +97,7 @@ public void init(){ this.wayPoints[10].x_speed = 0.3; this.wayPoints[10].facing = Constants.NORTH; + }//init @Override public void loop(){ From 4ae041b9bfad1f2250c20cc60b37de3eb057824f Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 10 Jan 2025 17:33:56 -0700 Subject: [PATCH 041/129] Added code from AutoPrac for completeness --- .../java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java index 8fdcde8..db541b9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java @@ -20,7 +20,9 @@ public class BaseAutoComp extends OpMode{ public class WayPoint { public double x; + public double x_speed = 0.0; public double y; + public double y_speed = 0.0; public double facing; } @Override From a8ad803b31baf271aef2e817cfe2d4398e69543b Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 10 Jan 2025 17:34:20 -0700 Subject: [PATCH 042/129] Created class for comp specimen --- .../ftc/teamcode/Auto/CompSpecimen.java | 406 ++++++++++++++++++ 1 file changed, 406 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompSpecimen.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompSpecimen.java new file mode 100644 index 0000000..bd279ce --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompSpecimen.java @@ -0,0 +1,406 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; + +import java.util.Locale; + +@Autonomous(name = "CompSpecimen") +public class CompSpecimen extends BaseAutoComp{ + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + WayPoint[] wayPoints; + + public void init(){ + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + elbow.init(); + elbow.setElbowState(0); + this.current_step = 1; + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 0.0; + this.wayPoints[1].y = -582.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 0.0; + this.wayPoints[2].y = -746.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.NORTH; + //Done Waypoint 2 + this.wayPoints[3] = new WayPoint(); + this.wayPoints[3].x = 0.0; + this.wayPoints[3].y = -560.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].facing = Constants.NORTH; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint(); + this.wayPoints[4].x = 1118.0; + this.wayPoints[4].y = -560.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.NORTH; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint(); + this.wayPoints[5].x = 1118.0; + this.wayPoints[5].y = -560.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.SOUTH; + //Done Waypoint 5 + //Waypoint for rotating + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = 1118.0; + this.wayPoints[6].y = 200.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.SOUTH; + //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = 1118.0; + this.wayPoints[7].y = 200.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.NORTH; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = 0.0; + this.wayPoints[8].y = 0.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = 0.0; + this.wayPoints[9].y = -582.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = -720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + //Waypoint + this.wayPoints[11] = new WayPoint(); + this.wayPoints[11].x = 0.0; + this.wayPoints[11].y = -540.0; + this.wayPoints[11].y_speed = 0.3; + this.wayPoints[11].x_speed = 0.3; + this.wayPoints[11].facing = Constants.NORTH; + //Waypoint + this.wayPoints[12] = new WayPoint(); + this.wayPoints[12].x = 640.0; + this.wayPoints[12].y = -540.0; + this.wayPoints[12].y_speed = 0.3; + this.wayPoints[12].x_speed = 0.3; + this.wayPoints[12].facing = Constants.NORTH; + //Waypoint + this.wayPoints[13] = new WayPoint(); + this.wayPoints[13].x = 640.0; + this.wayPoints[13].y = -1350.0; + this.wayPoints[13].y_speed = 0.3; + this.wayPoints[13].x_speed = 0.3; + this.wayPoints[13].facing = Constants.NORTH; + //Waypoint + this.wayPoints[14] = new WayPoint(); + this.wayPoints[14].x = 990.0; + this.wayPoints[14].y = -200.0; + this.wayPoints[14].y_speed = 0.4; + this.wayPoints[14].x_speed = 0.3; + this.wayPoints[14].facing = Constants.NORTH; + //Waypoint + this.wayPoints[15] = new WayPoint(); + this.wayPoints[15].x = 990.0; + this.wayPoints[15].y = -1350.0; + this.wayPoints[15].y_speed = 0.4; + this.wayPoints[15].x_speed = 0.3; + this.wayPoints[15].facing = Constants.NORTH; + //Waypoint + this.wayPoints[16] = new WayPoint(); + this.wayPoints[16].x = 1220.0; + this.wayPoints[16].y = -200.0; + this.wayPoints[16].y_speed = 0.4; + this.wayPoints[16].x_speed = 0.3; + this.wayPoints[16].facing = Constants.NORTH; + }//init + @Override + public void loop(){ + driveTrain.odometer.update(); + + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } + else { + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + } + + telemetry.addData("heading:","%.2f",driveTrain.heading); + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 2: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 3: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 4: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 5: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 6: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 7: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 8: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 9: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 10: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 11: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 12: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 13: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 14: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 15: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 16: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + default: + break; + } + // Done Processing Waypoints + driveTrain.loop(); + } + + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + + public boolean at_x(double previousX, double currentX) + { + if (currentX - previousX >= 0.0) + { + return driveTrain.getXPosition() >= currentX; + } + else { + return driveTrain.getXPosition() <= currentX; + } + } + + public boolean at_y(double previousY, double currentY) + { + if (currentY - previousY >= 0.0) + { + return driveTrain.getYPosition() >= currentY; + } + else { + return driveTrain.getYPosition() <= currentY; + } + } + + public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + // Check to see if you are at x. If yes, then process y; + // Process x direction + if (! this.at_x(previousWaypoint.x,currentWaypoint.x) ) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(0, currentWaypoint.x_speed); + } + else { + this.driveTrain.drive(0,-currentWaypoint.x_speed); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + // Process y direction + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(currentWaypoint.y_speed, 0.0); + } + else { + this.driveTrain.drive(-currentWaypoint.y_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; + } + +}//everything + From 9e03ec5b1dd54b2056a80750afec1c25d1444479 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 10 Jan 2025 20:52:37 -0700 Subject: [PATCH 043/129] Made changes to equal practice bot --- .../org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java index db541b9..ddef1a0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java @@ -20,9 +20,9 @@ public class BaseAutoComp extends OpMode{ public class WayPoint { public double x; - public double x_speed = 0.0; + public double x_speed = 0.2; public double y; - public double y_speed = 0.0; + public double y_speed = 0.2; public double facing; } @Override From 241ec2099ac1f08244d78cc320f9fd0d154eee43 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 10 Jan 2025 20:52:58 -0700 Subject: [PATCH 044/129] Remove engine commands, so we can use this for measurements --- .../ftc/teamcode/Auto/CompPark_Red.java | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java index 6ac38f5..dfff7bc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java @@ -21,6 +21,7 @@ public class CompPark_Red extends BaseAutoComp{ public void init(){ driveTrain = new DriveTrain2025(hardwareMap); driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); claw = new Claw(hardwareMap); elbow = new Elbow(hardwareMap); elbow.init(); @@ -34,12 +35,12 @@ public void loop(){ String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Position", data); driveTrain.odometer.update(); - if (counter <= 150){ - driveTrain.drive(0.0,-0.3); - }else { - driveTrain.drive(0.0,0.0); - } - counter = counter + 1; +// if (counter <= 150){ +// driveTrain.drive(0.0,-0.3); +// }else { +// driveTrain.drive(0.0,0.0); +// } +// counter = counter + 1; driveTrain.loop(); } } \ No newline at end of file From b625349c0a05693bd77ac4242655e1eb0f400c5a Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 10 Jan 2025 20:53:22 -0700 Subject: [PATCH 045/129] Working at added elevator, elbow, and claw commands. --- .../ftc/teamcode/Auto/CompSpecimen.java | 50 ++++++++++++------- 1 file changed, 32 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompSpecimen.java index bd279ce..faa0c7f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompSpecimen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompSpecimen.java @@ -9,6 +9,7 @@ import org.firstinspires.ftc.teamcode.Mechanisms.Constants; import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; import java.util.Locale; @@ -18,6 +19,8 @@ public class CompSpecimen extends BaseAutoComp{ private int current_step; private int total_waypoints = 20; WayPoint[] wayPoints; + double startTime; + boolean timeSet; public void init(){ driveTrain = new DriveTrain2025(hardwareMap); @@ -27,6 +30,9 @@ public void init(){ elbow = new Elbow(hardwareMap); elbow.init(); elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); this.current_step = 1; //Waypoint setup this.wayPoints = new WayPoint[this.total_waypoints + 1]; @@ -36,14 +42,15 @@ public void init(){ this.wayPoints[0].facing = Constants.NORTH; this.wayPoints[1] = new WayPoint(); this.wayPoints[1].x = 0.0; - this.wayPoints[1].y = -582.0; - this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].y = -400.0; + this.wayPoints[1].y_speed = 0.2; this.wayPoints[1].facing = Constants.NORTH; //Done Waypoint 1 this.wayPoints[2] = new WayPoint(); this.wayPoints[2].x = 0.0; - this.wayPoints[2].y = -746.0; + this.wayPoints[2].y = -737.0; this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].x_speed = 0.3; this.wayPoints[2].facing = Constants.NORTH; //Done Waypoint 2 this.wayPoints[3] = new WayPoint(); @@ -53,14 +60,14 @@ public void init(){ this.wayPoints[3].facing = Constants.NORTH; //Done Waypoint 3 this.wayPoints[4] = new WayPoint(); - this.wayPoints[4].x = 1118.0; + this.wayPoints[4].x = -870.0; this.wayPoints[4].y = -560.0; this.wayPoints[4].y_speed = 0.3; this.wayPoints[4].x_speed = 0.3; this.wayPoints[4].facing = Constants.NORTH; //Done Waypoint 4 this.wayPoints[5] = new WayPoint(); - this.wayPoints[5].x = 1118.0; + this.wayPoints[5].x = -1118.0; this.wayPoints[5].y = -560.0; this.wayPoints[5].y_speed = 0.3; this.wayPoints[5].x_speed = 0.3; @@ -68,14 +75,14 @@ public void init(){ //Done Waypoint 5 //Waypoint for rotating this.wayPoints[6] = new WayPoint(); - this.wayPoints[6].x = 1118.0; + this.wayPoints[6].x = -1118.0; this.wayPoints[6].y = 200.0; this.wayPoints[6].y_speed = 0.3; this.wayPoints[6].x_speed = 0.3; this.wayPoints[6].facing = Constants.SOUTH; //Done waypoint 6 this.wayPoints[7] = new WayPoint(); - this.wayPoints[7].x = 1118.0; + this.wayPoints[7].x = -1118.0; this.wayPoints[7].y = 200.0; this.wayPoints[7].y_speed = 0.3; this.wayPoints[7].x_speed = 0.3; @@ -110,35 +117,35 @@ public void init(){ this.wayPoints[11].facing = Constants.NORTH; //Waypoint this.wayPoints[12] = new WayPoint(); - this.wayPoints[12].x = 640.0; + this.wayPoints[12].x = -640.0; this.wayPoints[12].y = -540.0; this.wayPoints[12].y_speed = 0.3; this.wayPoints[12].x_speed = 0.3; this.wayPoints[12].facing = Constants.NORTH; //Waypoint this.wayPoints[13] = new WayPoint(); - this.wayPoints[13].x = 640.0; + this.wayPoints[13].x = -640.0; this.wayPoints[13].y = -1350.0; this.wayPoints[13].y_speed = 0.3; this.wayPoints[13].x_speed = 0.3; this.wayPoints[13].facing = Constants.NORTH; //Waypoint this.wayPoints[14] = new WayPoint(); - this.wayPoints[14].x = 990.0; + this.wayPoints[14].x = -990.0; this.wayPoints[14].y = -200.0; this.wayPoints[14].y_speed = 0.4; this.wayPoints[14].x_speed = 0.3; this.wayPoints[14].facing = Constants.NORTH; //Waypoint this.wayPoints[15] = new WayPoint(); - this.wayPoints[15].x = 990.0; + this.wayPoints[15].x = -990.0; this.wayPoints[15].y = -1350.0; this.wayPoints[15].y_speed = 0.4; this.wayPoints[15].x_speed = 0.3; this.wayPoints[15].facing = Constants.NORTH; //Waypoint this.wayPoints[16] = new WayPoint(); - this.wayPoints[16].x = 1220.0; + this.wayPoints[16].x = -1220.0; this.wayPoints[16].y = -200.0; this.wayPoints[16].y_speed = 0.4; this.wayPoints[16].x_speed = 0.3; @@ -181,10 +188,12 @@ public void loop(){ else { driveTrain.drive(0.0,0.0); + this.current_step++; } break; case 2: + elevator.setElevatorState(2); if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); @@ -351,10 +360,10 @@ public boolean at_x(double previousX, double currentX) { if (currentX - previousX >= 0.0) { - return driveTrain.getXPosition() >= currentX; + return driveTrain.getXPosition() <= currentX; } else { - return driveTrain.getXPosition() <= currentX; + return driveTrain.getXPosition() >= currentX; } } @@ -374,10 +383,10 @@ public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { // Process x direction if (! this.at_x(previousWaypoint.x,currentWaypoint.x) ) { if (currentWaypoint.x >= previousWaypoint.x) { - this.driveTrain.drive(0, currentWaypoint.x_speed); + this.driveTrain.drive(0, -currentWaypoint.x_speed); } else { - this.driveTrain.drive(0,-currentWaypoint.x_speed); + this.driveTrain.drive(0,currentWaypoint.x_speed); } return false; } @@ -388,10 +397,10 @@ public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { // Process y direction if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { if (currentWaypoint.y >= previousWaypoint.y) { - this.driveTrain.drive(currentWaypoint.y_speed, 0.0); + this.driveTrain.drive(-currentWaypoint.y_speed, 0.0); } else { - this.driveTrain.drive(-currentWaypoint.y_speed, 0.0); + this.driveTrain.drive(currentWaypoint.y_speed, 0.0); } return false; } @@ -402,5 +411,10 @@ public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { return true; } + public void set_time() + { + this.startTime = time; + this.timeSet = true; + } }//everything From 727a1a8495eaa8fd410468b7d296e5c9301de562 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 10 Jan 2025 20:53:37 -0700 Subject: [PATCH 046/129] Tweeting for correct waypoints --- .../java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java | 1 + 1 file changed, 1 insertion(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java index 1cd9cb6..ac0ec45 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java @@ -45,6 +45,7 @@ public void init(){ this.wayPoints[3].x = -270.0; this.wayPoints[3].y = -250.0; this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].x_speed = 0.3; this.wayPoints[3].facing = Constants.SOUTH_WEST; //Done Waypoint 3 this.wayPoints[4] = new BaseAutoPractice.WayPoint(); From 0a795acbaa9a8c8eb11c2aa9216b16667cd4b934 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 10 Jan 2025 20:53:58 -0700 Subject: [PATCH 047/129] Added tweets for auto mode. --- .../firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index 74f4549..3d55e32 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -185,9 +185,9 @@ public double getYPosition() { // Convert yPod into actual direction based on cu double yPos = 0.0; Pose2D pos = odometer.getPosition(); // The following is needed because of an error in the driver - yPos = pos.getY(DistanceUnit.MM) / 10; + yPos = pos.getY(DistanceUnit.MM); // Added negative sign to match drive control - return -yPos; + return yPos; } public void stop() { From aeeae967540e2f22b7c37c7be6099b848995edba Mon Sep 17 00:00:00 2001 From: Annika Date: Sat, 11 Jan 2025 10:34:13 -0700 Subject: [PATCH 048/129] Added another constant to accomodate for the the practice being able to pick from the wall --- .../ftc/teamcode/Mechanisms/Constants.java | 7 +++++-- .../firstinspires/ftc/teamcode/Mechanisms/Elbow.java | 11 +++++++---- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index b7f11f9..8dc4ff9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -66,11 +66,14 @@ public static final double ELBOW_BOTTOM = 0.3; public static final double ELBOW_START_POSITION = 0.0; // public static final double ELBOW_PICKUP_SAMPLE = 1.0; - public static final double ELBOW_PICKUP_SPECIMEN = 0.3; - public static final double ELBOW_DEPOSIT_SPECIMEN_LOW_BAR = 0.4; + public static final double ELBOW_PICKUP_SPECIMEN = 0.4; + //Pickup specimen from the wall for CompBot (Tortuga) + public static final double ELBOW_PICKUP_SPECIMEN_WALL = 0.5; + //Pickup specimen from the wall for PracticeBot (Esio Trot) // public static final double ELBOW_DEPOSIT_SPECIMEN_HIGH_BAR = 0.5; public static final double ELBOW_DEPOSIT_SAMPLE_LOW_BUCKET = 0.6; public static final double ELBOW_DEPOSIT_SAMPLE_HIGH_BUCKET = 0.75; + public static int ELBOW_MAX = 3; public static int ELBOW_MIN = 0; // diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java index 050db73..6e56f9d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java @@ -35,16 +35,19 @@ public void setElbowState(int position) { // case 0: //Case for pickup specimen // armServo.setPosition(Constants.ELBOW_PICKUP_SPECIMEN); // break; - case 1: //Case for deposit specimen low bar - armServo.setPosition(Constants.ELBOW_DEPOSIT_SPECIMEN_LOW_BAR); + case 1: //Case for pickup specimen for practice bot + armServo.setPosition(Constants.ELBOW_PICKUP_SPECIMEN); + break; + case 2: //Case for pickup specimen for comp bot + armServo.setPosition(Constants.ELBOW_PICKUP_SPECIMEN_WALL); break; // case 4: //Case for deposit specimen high bar // armServo.setPosition(Constants.ELBOW_DEPOSIT_SPECIMEN_HIGH_BAR); // break; - case 2: //Case for deposit sample on low bucket + case 3: //Case for deposit sample on low bucket armServo.setPosition(Constants.ELBOW_DEPOSIT_SAMPLE_LOW_BUCKET); break; - case 3: //Case for deposit sample on high bucket + case 4: //Case for deposit sample on high bucket armServo.setPosition(Constants.ELBOW_DEPOSIT_SAMPLE_HIGH_BUCKET); break; } From 82800344f2f55481cfe3a99ecbb19cee2f1a704d Mon Sep 17 00:00:00 2001 From: Kitten-kitten Date: Sat, 11 Jan 2025 12:43:35 -0700 Subject: [PATCH 049/129] Added another constant so both robots can pickup specimens from the wall. Merged two preset positions to pickup samples. Added another case statment --- .../ftc/teamcode/Auto/PracticePark.java | 2 +- .../java/org/firstinspires/ftc/teamcode/CompBot.java | 3 ++- .../ftc/teamcode/Mechanisms/Constants.java | 4 ++-- .../firstinspires/ftc/teamcode/Mechanisms/Elbow.java | 12 ++++++------ 4 files changed, 11 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java index 4976b74..7209962 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java @@ -10,7 +10,7 @@ import java.util.Locale; -@Autonomous (name = "PracticePark") +@Autonomous (name = "PracticePark")// public class PracticePark extends BaseAutoPractice{ private PracticeDriveTrain2025 driveTrain; public int step = -1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java index 08303f7..6d77351 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java @@ -55,7 +55,8 @@ @Config // need to use dashboard to change PID gains; comment out for competition @TeleOp(name = "CompBot") -public class CompBot extends OpMode { +public class +CompBot extends OpMode { private final ElapsedTime runtime = new ElapsedTime(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index 8dc4ff9..c842d41 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -71,8 +71,8 @@ public static final double ELBOW_PICKUP_SPECIMEN_WALL = 0.5; //Pickup specimen from the wall for PracticeBot (Esio Trot) // public static final double ELBOW_DEPOSIT_SPECIMEN_HIGH_BAR = 0.5; - public static final double ELBOW_DEPOSIT_SAMPLE_LOW_BUCKET = 0.6; - public static final double ELBOW_DEPOSIT_SAMPLE_HIGH_BUCKET = 0.75; + //public static final double ELBOW_DEPOSIT_SAMPLE_LOW_BUCKET = 0.6; + public static final double ELBOW_DEPOSIT_SAMPLE_HIGH_BUCKET = 0.67; public static int ELBOW_MAX = 3; public static int ELBOW_MIN = 0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java index 6e56f9d..8bbf3bc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java @@ -39,15 +39,15 @@ public void setElbowState(int position) { armServo.setPosition(Constants.ELBOW_PICKUP_SPECIMEN); break; case 2: //Case for pickup specimen for comp bot - armServo.setPosition(Constants.ELBOW_PICKUP_SPECIMEN_WALL); - break; + armServo.setPosition(Constants.ELBOW_PICKUP_SPECIMEN_WALL); + break; // case 4: //Case for deposit specimen high bar // armServo.setPosition(Constants.ELBOW_DEPOSIT_SPECIMEN_HIGH_BAR); // break; - case 3: //Case for deposit sample on low bucket - armServo.setPosition(Constants.ELBOW_DEPOSIT_SAMPLE_LOW_BUCKET); - break; - case 4: //Case for deposit sample on high bucket + // case 2: //Case for deposit sample on low bucket + // armServo.setPosition(Constants.ELBOW_DEPOSIT_SAMPLE_LOW_BUCKET); + //break; + case 3: //Case for deposit sample on high bucket armServo.setPosition(Constants.ELBOW_DEPOSIT_SAMPLE_HIGH_BUCKET); break; } From f49dc9704d2a65b8631c5fa8a6a22190240b0cd3 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 17 Jan 2025 20:46:57 -0700 Subject: [PATCH 050/129] Added for the Compbot basket for auto --- .../ftc/teamcode/Auto/CompBasket.java | 293 ++++++++++++++++++ 1 file changed, 293 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java new file mode 100644 index 0000000..a313992 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -0,0 +1,293 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; +import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; + +import java.util.Locale; + +@Autonomous(name = "CompBasket") +public class CompBasket extends BaseAutoComp{ + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + WayPoint[] wayPoints; + + public void init(){ + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + elbow.init(); + elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 0.0; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = -450.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 0.0; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].y = -450.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.SOUTH_WEST; + //Done Waypoint 2 + this.wayPoints[3] = new WayPoint(); + this.wayPoints[3].x = -270.0; + this.wayPoints[3].y = -250.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].x_speed = 0.3; + this.wayPoints[3].facing = Constants.SOUTH_WEST; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint(); + this.wayPoints[4].x = -270.0; + this.wayPoints[4].y = -250.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.NORTH; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint(); + this.wayPoints[5].x = -445.0; + this.wayPoints[5].y = -530.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.NORTH; + //Done Waypoint 5 + //Waypoint for rotating + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = -445.0; + this.wayPoints[6].y = -530.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.SOUTH_WEST; + //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = -270.0; + this.wayPoints[7].y = -250.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = -270.0; + this.wayPoints[8].y = -530.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = -695.0; + this.wayPoints[9].y = -530.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = -720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + + + }//init + @Override + public void loop(){ + driveTrain.odometer.update(); + + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } + else { + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + } + telemetry.addData("heading:","%.2f",driveTrain.heading); + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 2: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 3: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 4: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 5: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 6: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 7: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 8: + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; + case 9: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 10: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + default: + break; + } + // Done Processing Waypoints + driveTrain.loop(); + } + + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + + public boolean at_x(double previousX, double currentX) + { + if (currentX - previousX >= 0.0) + { + return driveTrain.getXPosition() >= currentX; + } + else { + return driveTrain.getXPosition() <= currentX; + } + } + + public boolean at_y(double previousY, double currentY) + { + if (currentY - previousY >= 0.0) + { + return driveTrain.getYPosition() >= currentY; + } + else { + return driveTrain.getYPosition() <= currentY; + } + } + + public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + // Check to see if you are at x. If yes, then process y; + // Process x direction + if (! this.at_x(previousWaypoint.x,currentWaypoint.x) ) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(0, currentWaypoint.x_speed); + } + else { + this.driveTrain.drive(0,-currentWaypoint.x_speed); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + // Process y direction + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(currentWaypoint.y_speed, 0.0); + } + else { + this.driveTrain.drive(-currentWaypoint.y_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; + } + +} From af04b8dba69363049ed6cad2c04d4d3db3b69bb4 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 17 Jan 2025 20:47:21 -0700 Subject: [PATCH 051/129] Modified default speed setting for x and y --- .../org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java index c2f166a..280cbbc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java @@ -5,7 +5,10 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; import java.util.Locale; @@ -22,6 +25,7 @@ public void init(){ driveTrain.init(); driveTrain.odometer.resetPosAndIMU(); this.current_step = 1; + //Waypoint setup this.wayPoints = new WayPoint[this.total_waypoints + 1]; this.wayPoints[0] = new WayPoint(); From 890326b8a9ca85168080c001ad29a8761f5432f6 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 17 Jan 2025 20:47:50 -0700 Subject: [PATCH 052/129] Added comments for drivetrain setting --- .../org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java index 41ee3a9..17f9655 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java @@ -20,9 +20,9 @@ public class BaseAutoPractice extends OpMode { public class WayPoint { public double x; - public double x_speed = 0.0; + public double x_speed = 0.2; public double y; - public double y_speed = 0.0; + public double y_speed = 0.2; public double facing; } @Override From caf76f2a36dede5c21b5c65a090c8115a92898af Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 17 Jan 2025 23:48:46 -0700 Subject: [PATCH 053/129] Fixed formatting of class line. --- .../java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java index 8bbf3bc..4a363bb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java @@ -5,9 +5,7 @@ //import com.qualcomm.robotcore.hardware.Servo; -public class - -Elbow { +public class Elbow { private SimpleServo armServo; private HardwareMap hwMap; From 5148a01b00fd41afe3ca9dce2dcbaeb25d5afe73 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sat, 18 Jan 2025 00:04:38 -0700 Subject: [PATCH 054/129] Added the - sign in the getYposition function to match practice drive train. --- .../firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index 3d55e32..4597b41 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -187,7 +187,7 @@ public double getYPosition() { // Convert yPod into actual direction based on cu // The following is needed because of an error in the driver yPos = pos.getY(DistanceUnit.MM); // Added negative sign to match drive control - return yPos; + return -yPos; } public void stop() { From c43a0f49bf84778b4d4a6688624f3bb0c195cf58 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sat, 18 Jan 2025 00:05:06 -0700 Subject: [PATCH 055/129] Add some comments that were in DriveTrain, but not in PracticeDriveTrain --- .../ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java index 21bfc54..0eb1ecc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java @@ -220,7 +220,9 @@ public double getYPosition() { // Convert yPod into actual direction based on cu double yPos = 0.0; //odometer.update(); Pose2D pos = odometer.getPosition(); + // The following is needed because of an error in the driver yPos = pos.getY(DistanceUnit.MM); + // Added negative sign to match drive control return -yPos; } From 935332a0e2a6a5c32f5969334acbc504752e174e Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sat, 18 Jan 2025 00:21:31 -0700 Subject: [PATCH 056/129] - added elevator command in case 3 - added claw open when advancing to next step - added elevator moving command in case 4. Moving command is the height that we want the elevator to be when we are driving. I am using the constant.start for that now - added claw close when advancing to next step --- .../firstinspires/ftc/teamcode/Auto/CompBasket.java | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java index a313992..da33d54 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -27,8 +27,8 @@ public void init(){ driveTrain.odometer.resetPosAndIMU(); claw = new Claw(hardwareMap); elbow = new Elbow(hardwareMap); - elbow.init(); - elbow.setElbowState(0); + this.elbow.init(); + this.elbow.setElbowState(0); elevator = new Elevator(hardwareMap); this.elevator.init(); this.elevator.setElevatorState(0); @@ -157,6 +157,7 @@ public void loop(){ this.current_step++; break; case 3: +// elevator.setElevatorState(Constants.ELEVATOR_HIGH_BASKET); if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); @@ -164,10 +165,13 @@ public void loop(){ else { driveTrain.drive(0.0,0.0); +// claw.open(); this.current_step++; } break; case 4: + // elevator.setElevatorState(Constants.ELEVATOR_START); + // claw.close(); driveTrain.setDirection(this.wayPoints[this.current_step].facing); this.current_step++; break; @@ -262,10 +266,10 @@ public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { // Process x direction if (! this.at_x(previousWaypoint.x,currentWaypoint.x) ) { if (currentWaypoint.x >= previousWaypoint.x) { - this.driveTrain.drive(0, currentWaypoint.x_speed); + this.driveTrain.drive(0, -currentWaypoint.x_speed); } else { - this.driveTrain.drive(0,-currentWaypoint.x_speed); + this.driveTrain.drive(0,currentWaypoint.x_speed); } return false; } From a00d2483bea531da0fcd498b6766a66570db875c Mon Sep 17 00:00:00 2001 From: Annika Date: Sat, 18 Jan 2025 11:15:11 -0700 Subject: [PATCH 057/129] Defined claw and elevator in all practice classes --- .../ftc/teamcode/Auto/BaseAutoPractice.java | 5 ++++- .../ftc/teamcode/Auto/CompBasket.java | 7 +++--- .../ftc/teamcode/Auto/PracticeBasket.java | 22 +++++++++++++++++-- .../ftc/teamcode/Auto/PracticePark.java | 2 ++ .../ftc/teamcode/Mechanisms/Elevator.java | 3 ++- .../Mechanisms/PracticeDriveTrain2025.java | 15 +++++++++++++ 6 files changed, 47 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java index 17f9655..9770295 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java @@ -16,7 +16,10 @@ public class BaseAutoPractice extends OpMode { String state = "START"; public PracticeDriveTrain2025 driveTrain; - + public DriveTrain2025 PracticeDriveTrain2025; + public Elevator elevator; + public Claw claw; + public Elbow elbow; public class WayPoint { public double x; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java index da33d54..1f1b014 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -12,6 +12,7 @@ import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; +import java.net.NoRouteToHostException; import java.util.Locale; @Autonomous(name = "CompBasket") @@ -43,7 +44,7 @@ public void init(){ this.wayPoints[1] = new WayPoint(); this.wayPoints[1].x = 0.0; this.wayPoints[1].x_speed = 0.3; - this.wayPoints[1].y = -450.0; + this.wayPoints[1].y = -525.0; this.wayPoints[1].y_speed = 0.3; this.wayPoints[1].facing = Constants.NORTH; //Done Waypoint 1 @@ -157,7 +158,7 @@ public void loop(){ this.current_step++; break; case 3: -// elevator.setElevatorState(Constants.ELEVATOR_HIGH_BASKET); + elevator.setElevatorState(Constants.ELEVATOR_HIGH_BASKET); if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); @@ -165,7 +166,7 @@ public void loop(){ else { driveTrain.drive(0.0,0.0); -// claw.open(); + // claw.open(); this.current_step++; } break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java index ac0ec45..afce4fe 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java @@ -5,8 +5,12 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; import java.util.Locale; @@ -21,8 +25,20 @@ public void init(){ driveTrain = new PracticeDriveTrain2025(hardwareMap); driveTrain.init(); driveTrain.odometer.resetPosAndIMU(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); this.current_step = 1; //Waypoint setup + //Practice bot setup: (Based on heading facing North/Forward) + // -Y = +North + // +X = +East + // +Y = +South + // -X = +West this.wayPoints = new BaseAutoPractice.WayPoint[this.total_waypoints + 1]; this.wayPoints[0] = new BaseAutoPractice.WayPoint(); this.wayPoints[0].x = 0.0; @@ -31,13 +47,13 @@ public void init(){ // WayPoint 1 this.wayPoints[1] = new BaseAutoPractice.WayPoint(); this.wayPoints[1].x = 0.0; - this.wayPoints[1].y = -450.0; + this.wayPoints[1].y = -525.0; this.wayPoints[1].y_speed = 0.3; this.wayPoints[1].facing = Constants.NORTH; //Done Waypoint 1 this.wayPoints[2] = new BaseAutoPractice.WayPoint(); this.wayPoints[2].x = 0.0; - this.wayPoints[2].y = -450.0; + this.wayPoints[2].y = -525.0; this.wayPoints[2].y_speed = 0.3; this.wayPoints[2].facing = Constants.SOUTH_WEST; //Done Waypoint 2 @@ -116,6 +132,7 @@ public void loop(){ telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } else { telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); @@ -144,6 +161,7 @@ public void loop(){ this.current_step++; break; case 3: + elevator.setElevatorState(Constants.ELEVATOR_HIGH_BASKET); if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java index 7209962..74b3f4a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java @@ -80,6 +80,8 @@ public void loop(){ // telemetry.addData("Dest y:","%.2f",this.dest_y); telemetry.addData("Distance to X","%.2f",(Math.abs(this.wayPoints[this.step].x - this.current_x))); telemetry.addData("Distance to Y","%.2f",Math.abs(this.wayPoints[this.step].y - this.current_y)); + + // driveTrain.setDirection(Constants.SOUTH); // switch (this.step){ // case 1: diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java index e97fec8..7c00e34 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java @@ -11,7 +11,8 @@ @Config public class Elevator { static Motor elevatorMotor = null; - public static int target_height; + public static + int target_height; private HardwareMap hwMap; //public boolean manualElevator = false; private int elevatorPosition; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java index 0eb1ecc..f48fcdb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java @@ -39,6 +39,9 @@ public class PracticeDriveTrain2025 private final Motor rightBackDrive; MecanumDrive driveBase; + //servos + private final SimpleServo claw; + private final SimpleServo elbow; public GoBildaPinpointDriver odometer; private PIDController headingControl = null; @@ -72,6 +75,13 @@ public PracticeDriveTrain2025(HardwareMap hwMap) leftBackDrive = new Motor(hwMap, "left_back_drive"); // 2 rightBackDrive = new Motor(hwMap, "right_back_drive"); // 3 + //elevator = new Motor(hwMap,"elevator"); + claw = new SimpleServo(hwMap,"claw",0,1); // expansion hub port 0 + elbow = new SimpleServo(hwMap,"elbow",0,1); // expansion hub port 1 + + //elevator.resetEncoder(); + + driveBase = new MecanumDrive(leftFrontDrive,rightFrontDrive,leftBackDrive,rightBackDrive); } @@ -241,6 +251,11 @@ public void printTelemetry(Telemetry telemetry) { telemetry.addData("heading correction:", headingCorrection); telemetry.addData("X Distance inches:", "%5.2f", getXPosition()); telemetry.addData("Y Distance inches:", "%5.2f", getYPosition()); + + + odometer.update(); + odometer.update(); + telemetry.addData("Auto DriveTo Enabled", autoEnabled); // Odometer Output odometer.update(); From 4001ac59a7b72b56a3632c5231236d016a561bde Mon Sep 17 00:00:00 2001 From: Annika Date: Sat, 18 Jan 2025 11:49:57 -0700 Subject: [PATCH 058/129] changed waypoints positions --- .../ftc/teamcode/Auto/PracticeBasket.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java index afce4fe..0916643 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java @@ -35,10 +35,10 @@ public void init(){ this.current_step = 1; //Waypoint setup //Practice bot setup: (Based on heading facing North/Forward) - // -Y = +North - // +X = +East - // +Y = +South - // -X = +West + // +Y = +North + // -X = +East + // -Y = +South + // +X = +West this.wayPoints = new BaseAutoPractice.WayPoint[this.total_waypoints + 1]; this.wayPoints[0] = new BaseAutoPractice.WayPoint(); this.wayPoints[0].x = 0.0; @@ -47,19 +47,19 @@ public void init(){ // WayPoint 1 this.wayPoints[1] = new BaseAutoPractice.WayPoint(); this.wayPoints[1].x = 0.0; - this.wayPoints[1].y = -525.0; + this.wayPoints[1].y = 525.0; this.wayPoints[1].y_speed = 0.3; this.wayPoints[1].facing = Constants.NORTH; //Done Waypoint 1 this.wayPoints[2] = new BaseAutoPractice.WayPoint(); this.wayPoints[2].x = 0.0; - this.wayPoints[2].y = -525.0; + this.wayPoints[2].y = 525.0; this.wayPoints[2].y_speed = 0.3; this.wayPoints[2].facing = Constants.SOUTH_WEST; //Done Waypoint 2 this.wayPoints[3] = new BaseAutoPractice.WayPoint(); - this.wayPoints[3].x = -270.0; - this.wayPoints[3].y = -250.0; + this.wayPoints[3].x = 43.0; + this.wayPoints[3].y = 367.0; this.wayPoints[3].y_speed = 0.3; this.wayPoints[3].x_speed = 0.3; this.wayPoints[3].facing = Constants.SOUTH_WEST; From af83eb3b4029faa335857abb753ba682f0e9284a Mon Sep 17 00:00:00 2001 From: Annika Date: Sat, 18 Jan 2025 13:07:07 -0700 Subject: [PATCH 059/129] Moved all the inversion for the odometry, after the pods were mounted correctly. --- .../org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java | 2 +- .../org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java | 2 +- .../firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java | 4 ++-- .../ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java index 9770295..6759ebb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java @@ -16,7 +16,7 @@ public class BaseAutoPractice extends OpMode { String state = "START"; public PracticeDriveTrain2025 driveTrain; - public DriveTrain2025 PracticeDriveTrain2025; + // public DriveTrain2025 PracticeDriveTrain2025; public Elevator elevator; public Claw claw; public Elbow elbow; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java index 0916643..037b8d0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java @@ -267,7 +267,7 @@ public boolean goto_xy(BaseAutoPractice.WayPoint previousWaypoint, BaseAutoPract // Process x direction if (! this.at_x(previousWaypoint.x,currentWaypoint.x) ) { if (currentWaypoint.x >= previousWaypoint.x) { - this.driveTrain.drive(0, currentWaypoint.x_speed); + this.driveTrain.drive(0,currentWaypoint.x_speed); } else { this.driveTrain.drive(0,-currentWaypoint.x_speed); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index 4597b41..58ddaa7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -170,7 +170,7 @@ public void driveTo(double speed, double xDist, double yDist, double facing) { public void drive(double forwardSpeed, double strafeSpeed) { // tell ftclib its inputs strafeSpeed,forwardSpeed,turn,heading // turn and heading are managed in loop with the heading control PID - // inverting strafe speed to correct directions + // inverting strafe speed to correct directions, inverting no longer required for telemtry pdos have been turned to right position xSpeed = -strafeSpeed; ySpeed = forwardSpeed; } @@ -187,7 +187,7 @@ public double getYPosition() { // Convert yPod into actual direction based on cu // The following is needed because of an error in the driver yPos = pos.getY(DistanceUnit.MM); // Added negative sign to match drive control - return -yPos; + return yPos; } public void stop() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java index f48fcdb..4b35b7f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java @@ -187,7 +187,7 @@ public void drive(double forwardSpeed, double strafeSpeed) { // turn and heading are managed in loop with the heading control PID // inverting strafe speed to correct directions xSpeed = strafeSpeed; - ySpeed = -forwardSpeed; + ySpeed = forwardSpeed; } // These functions will control where we are going @@ -233,7 +233,7 @@ public double getYPosition() { // Convert yPod into actual direction based on cu // The following is needed because of an error in the driver yPos = pos.getY(DistanceUnit.MM); // Added negative sign to match drive control - return -yPos; + return yPos; } From e2aae15c33e2aa1063cd87cbda8f4da3e9957fc6 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 24 Jan 2025 17:19:06 -0700 Subject: [PATCH 060/129] - added elevator command in case 3 - added claw open when advancing to next step --- .../ftc/teamcode/Auto/GeorgeTest.java | 154 ++++++++++-------- 1 file changed, 88 insertions(+), 66 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java index 82698fa..ca9c6e5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java @@ -5,28 +5,48 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; import java.util.Locale; @Autonomous(name = "GeorgeTest") -public class GeorgeTest extends BaseAutoPractice{ - private PracticeDriveTrain2025 driveTrain; +public class GeorgeTest extends BaseAutoComp{ + private DriveTrain2025 driveTrain; enum WorkingStep{ X, Y } - BaseAutoPractice.WayPoint[] wayPoints; + BaseAutoComp.WayPoint[] wayPoints; private int current_step; private int total_waypoints = 10; public void init() { - driveTrain = new PracticeDriveTrain2025(hardwareMap); + driveTrain = new DriveTrain2025(hardwareMap); driveTrain.init(); driveTrain.odometer.resetPosAndIMU(); this.current_step = 1; + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + // Display Settings + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + + telemetry.addData("heading:","%.2f",driveTrain.heading); // Waypoint Setup this.wayPoints = new WayPoint[this.total_waypoints + 1]; this.wayPoints[0] = new WayPoint(); @@ -34,8 +54,8 @@ public void init() { this.wayPoints[0].y = 0.0; this.wayPoints[0].facing = Constants.NORTH; this.wayPoints[1] = new WayPoint(); - this.wayPoints[1].x = 0.0; - this.wayPoints[1].y = -200.0; + this.wayPoints[1].x = 100.0; + this.wayPoints[1].y = 0.0; this.wayPoints[1].y_speed = 0.3; this.wayPoints[1].facing = Constants.NORTH; this.wayPoints[2] = new WayPoint(); @@ -110,67 +130,69 @@ public void loop(){ this.current_step++; } break; +// case 2: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 3: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; case 2: - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) - { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); - } - else - { - driveTrain.drive(0.0,0.0); - this.current_step++; - } - break; - case 3: - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) - { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); - } - else - { - driveTrain.drive(0.0,0.0); - this.current_step++; - } - break; - case 4: driveTrain.setDirection(Constants.WEST); + this.claw.openClaw(); + elevator.setElevatorState(Constants.ELEVATOR_HIGH_BASKET); this.current_step++; break; - case 5: - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) - { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); - } - else - { - driveTrain.drive(0.0,0.0); - this.current_step++; - } - break; - case 6: - driveTrain.setDirection(Constants.EAST); - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) - { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); - } - else - { - driveTrain.drive(0.0,0.0); - this.current_step++; - } - break; - case 7: - driveTrain.setDirection(Constants.NORTH); - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) - { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); - } - else - { - driveTrain.drive(0.0,0.0); - this.current_step++; - } - break; +// case 5: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 6: +// driveTrain.setDirection(Constants.EAST); +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 7: +// driveTrain.setDirection(Constants.NORTH); +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; default: break; } @@ -178,7 +200,7 @@ public void loop(){ driveTrain.loop(); } - public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + public boolean at_xy(BaseAutoComp.WayPoint previousWaypoint, BaseAutoComp.WayPoint currentWaypoint) { return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); } @@ -187,10 +209,10 @@ public boolean at_x(double previousX, double currentX) { if (currentX - previousX >= 0.0) { - return driveTrain.getXPosition() >= currentX; + return driveTrain.getXPosition() <= currentX; } else { - return driveTrain.getXPosition() <= currentX; + return driveTrain.getXPosition() >= currentX; } } From b750594831d316ed6dd9c804504cf94d8ca52dbd Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 24 Jan 2025 20:57:07 -0700 Subject: [PATCH 061/129] add an init_loop to reset the odometer - added a display to say initialized --- .../ftc/teamcode/Auto/CompBasket.java | 202 +++++++++++------- 1 file changed, 119 insertions(+), 83 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java index 1f1b014..9f632ea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -22,10 +22,26 @@ public class CompBasket extends BaseAutoComp{ private int total_waypoints = 20; WayPoint[] wayPoints; + public void init_loop(){ + driveTrain.odometer.update(); + driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // This gets called when we have zero out and ready to hit play. + if (driveTrain.getXPosition() <= 0.01 && driveTrain.getYPosition() <= 0.01) { + telemetry.addData("Initialized",""); + } + } public void init(){ driveTrain = new DriveTrain2025(hardwareMap); driveTrain.init(); driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); claw = new Claw(hardwareMap); elbow = new Elbow(hardwareMap); this.elbow.init(); @@ -34,6 +50,14 @@ public void init(){ this.elevator.init(); this.elevator.setElevatorState(0); this.current_step = 1; + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); //Waypoint setup this.wayPoints = new WayPoint[this.total_waypoints + 1]; this.wayPoints[0] = new WayPoint(); @@ -44,70 +68,70 @@ public void init(){ this.wayPoints[1] = new WayPoint(); this.wayPoints[1].x = 0.0; this.wayPoints[1].x_speed = 0.3; - this.wayPoints[1].y = -525.0; + this.wayPoints[1].y = 400.0; this.wayPoints[1].y_speed = 0.3; this.wayPoints[1].facing = Constants.NORTH; //Done Waypoint 1 this.wayPoints[2] = new WayPoint(); - this.wayPoints[2].x = 0.0; + this.wayPoints[2].x = -130.0; this.wayPoints[2].x_speed = 0.3; - this.wayPoints[2].y = -450.0; + this.wayPoints[2].y = -290.0; this.wayPoints[2].y_speed = 0.3; this.wayPoints[2].facing = Constants.SOUTH_WEST; //Done Waypoint 2 this.wayPoints[3] = new WayPoint(); - this.wayPoints[3].x = -270.0; - this.wayPoints[3].y = -250.0; + this.wayPoints[3].x = 270.0; + this.wayPoints[3].y = 250.0; this.wayPoints[3].y_speed = 0.3; this.wayPoints[3].x_speed = 0.3; this.wayPoints[3].facing = Constants.SOUTH_WEST; //Done Waypoint 3 this.wayPoints[4] = new WayPoint(); - this.wayPoints[4].x = -270.0; - this.wayPoints[4].y = -250.0; + this.wayPoints[4].x = 270.0; + this.wayPoints[4].y = 250.0; this.wayPoints[4].y_speed = 0.3; this.wayPoints[4].x_speed = 0.3; this.wayPoints[4].facing = Constants.NORTH; //Done Waypoint 4 this.wayPoints[5] = new WayPoint(); - this.wayPoints[5].x = -445.0; - this.wayPoints[5].y = -530.0; + this.wayPoints[5].x = 445.0; + this.wayPoints[5].y = 530.0; this.wayPoints[5].y_speed = 0.3; this.wayPoints[5].x_speed = 0.3; this.wayPoints[5].facing = Constants.NORTH; //Done Waypoint 5 //Waypoint for rotating this.wayPoints[6] = new WayPoint(); - this.wayPoints[6].x = -445.0; - this.wayPoints[6].y = -530.0; + this.wayPoints[6].x = 445.0; + this.wayPoints[6].y = 530.0; this.wayPoints[6].y_speed = 0.3; this.wayPoints[6].x_speed = 0.3; this.wayPoints[6].facing = Constants.SOUTH_WEST; //Done waypoint 6 this.wayPoints[7] = new WayPoint(); - this.wayPoints[7].x = -270.0; - this.wayPoints[7].y = -250.0; + this.wayPoints[7].x = 270.0; + this.wayPoints[7].y = 250.0; this.wayPoints[7].y_speed = 0.3; this.wayPoints[7].x_speed = 0.3; this.wayPoints[7].facing = Constants.SOUTH_WEST; // Waypoint this.wayPoints[8] = new WayPoint(); - this.wayPoints[8].x = -270.0; - this.wayPoints[8].y = -530.0; + this.wayPoints[8].x = 270.0; + this.wayPoints[8].y = 530.0; this.wayPoints[8].y_speed = 0.3; this.wayPoints[8].x_speed = 0.3; this.wayPoints[8].facing = Constants.NORTH; // Waypoint this.wayPoints[9] = new WayPoint(); - this.wayPoints[9].x = -695.0; - this.wayPoints[9].y = -530.0; + this.wayPoints[9].x = 695.0; + this.wayPoints[9].y = 530.0; this.wayPoints[9].y_speed = 0.3; this.wayPoints[9].x_speed = 0.3; this.wayPoints[9].facing = Constants.NORTH; // Waypoint this.wayPoints[10] = new WayPoint(); this.wayPoints[10].x = 0.0; - this.wayPoints[10].y = -720.0; + this.wayPoints[10].y = 720.0; this.wayPoints[10].y_speed = 0.3; this.wayPoints[10].x_speed = 0.3; this.wayPoints[10].facing = Constants.NORTH; @@ -138,7 +162,7 @@ public void loop(){ telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); } telemetry.addData("heading:","%.2f",driveTrain.heading); - + telemetry.addData("Elevator State:",elevator.getState()); //starting waypoint navigation switch (this.current_step) { // drive 2 inches form submersible @@ -155,43 +179,7 @@ public void loop(){ break; case 2: driveTrain.setDirection(this.wayPoints[this.current_step].facing); - this.current_step++; - break; - case 3: - elevator.setElevatorState(Constants.ELEVATOR_HIGH_BASKET); - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) - { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); - } - else - { - driveTrain.drive(0.0,0.0); - // claw.open(); - this.current_step++; - } - break; - case 4: - // elevator.setElevatorState(Constants.ELEVATOR_START); - // claw.close(); - driveTrain.setDirection(this.wayPoints[this.current_step].facing); - this.current_step++; - break; - case 5: - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) - { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); - } - else - { - driveTrain.drive(0.0,0.0); - this.current_step++; - } - break; - case 6: - driveTrain.setDirection(this.wayPoints[this.current_step].facing); - this.current_step++; - break; - case 7: + elevator.setElevatorState(3); if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); @@ -199,40 +187,88 @@ public void loop(){ else { driveTrain.drive(0.0,0.0); + this.claw.openClaw(); this.current_step++; } - break; - case 8: - driveTrain.setDirection(this.wayPoints[this.current_step].facing); this.current_step++; break; - case 9: - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) - { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); - } - else - { - driveTrain.drive(0.0,0.0); - this.current_step++; - } - break; - case 10: - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) - { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); - } - else - { - driveTrain.drive(0.0,0.0); - this.current_step++; - } - break; +// case 3: +// elevator.setElevatorState(3); +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.claw.openClaw(); +// this.current_step++; +// } +// break; +// case 4: +// elevator.setElevatorState(Constants.ELEVATOR_START); +// this.claw.closeClaw(); +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 5: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 6: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 7: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 8: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 9: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 10: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; default: break; } // Done Processing Waypoints driveTrain.loop(); + this.elevator.loop(); } public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) @@ -267,10 +303,10 @@ public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { // Process x direction if (! this.at_x(previousWaypoint.x,currentWaypoint.x) ) { if (currentWaypoint.x >= previousWaypoint.x) { - this.driveTrain.drive(0, -currentWaypoint.x_speed); + this.driveTrain.drive(0, currentWaypoint.x_speed); } else { - this.driveTrain.drive(0,currentWaypoint.x_speed); + this.driveTrain.drive(0,-currentWaypoint.x_speed); } return false; } From d8fdd348adc365f9b33f80b39c6917cd661fa589 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 24 Jan 2025 20:58:40 -0700 Subject: [PATCH 062/129] add comments --- .../java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java index 9f632ea..a369f1a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -178,6 +178,7 @@ public void loop(){ } break; case 2: + // Rotating and moving the elevator before moving driveTrain.setDirection(this.wayPoints[this.current_step].facing); elevator.setElevatorState(3); if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) @@ -267,6 +268,7 @@ public void loop(){ break; } // Done Processing Waypoints + // Process loops driveTrain.loop(); this.elevator.loop(); } From 14e2694221700f9bd08186d19d86a030b9a2236c Mon Sep 17 00:00:00 2001 From: bruce Date: Sat, 25 Jan 2025 11:17:18 -0700 Subject: [PATCH 063/129] updaeetd the offsets --- .../java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java | 2 +- .../org/firstinspires/ftc/teamcode/Mechanisms/Constants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java index a369f1a..d2c4a3b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -33,7 +33,7 @@ public void init_loop(){ telemetry.addData("Function Position", data); telemetry.addData("Elevator State:",elevator.getState()); // This gets called when we have zero out and ready to hit play. - if (driveTrain.getXPosition() <= 0.01 && driveTrain.getYPosition() <= 0.01) { + if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { telemetry.addData("Initialized",""); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index c842d41..3311113 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -87,7 +87,7 @@ //odometer constants public static int ODOMETER_PRACTICE_MODE = 1; public static int ODOMETER_COMPETITION_MODE = 8; - public static double ODOMETER_X_OFFSET = -145.0; + public static double ODOMETER_X_OFFSET = -212.0; public static double ODOMETER_Y_OFFSET = 270.0; // auto constants public static double AUTO_X_DISTANCE_ERROR = 5; From b62ff8b1ebbea1c0257b31649f5986618fd058bc Mon Sep 17 00:00:00 2001 From: Kitten-kitten Date: Sat, 25 Jan 2025 13:08:00 -0700 Subject: [PATCH 064/129] Changed values of waypoints --- .../ftc/teamcode/Auto/CompBasket.java | 40 ++++++++++--------- 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java index d2c4a3b..4e72017 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -68,20 +68,20 @@ public void init(){ this.wayPoints[1] = new WayPoint(); this.wayPoints[1].x = 0.0; this.wayPoints[1].x_speed = 0.3; - this.wayPoints[1].y = 400.0; + this.wayPoints[1].y = 711.0; this.wayPoints[1].y_speed = 0.3; this.wayPoints[1].facing = Constants.NORTH; //Done Waypoint 1 this.wayPoints[2] = new WayPoint(); - this.wayPoints[2].x = -130.0; + this.wayPoints[2].x = -293.0; this.wayPoints[2].x_speed = 0.3; - this.wayPoints[2].y = -290.0; + this.wayPoints[2].y = -494.0; this.wayPoints[2].y_speed = 0.3; this.wayPoints[2].facing = Constants.SOUTH_WEST; //Done Waypoint 2 this.wayPoints[3] = new WayPoint(); - this.wayPoints[3].x = 270.0; - this.wayPoints[3].y = 250.0; + this.wayPoints[3].x = -80.0; + this.wayPoints[3].y = -180.0; this.wayPoints[3].y_speed = 0.3; this.wayPoints[3].x_speed = 0.3; this.wayPoints[3].facing = Constants.SOUTH_WEST; @@ -180,6 +180,20 @@ public void loop(){ case 2: // Rotating and moving the elevator before moving driveTrain.setDirection(this.wayPoints[this.current_step].facing); + elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + // this.claw.openClaw(); + this.current_step++; + } + this.current_step++;// needed?? + break; + case 3: elevator.setElevatorState(3); if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) { @@ -191,21 +205,7 @@ public void loop(){ this.claw.openClaw(); this.current_step++; } - this.current_step++; break; -// case 3: -// elevator.setElevatorState(3); -// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) -// { -// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); -// } -// else -// { -// driveTrain.drive(0.0,0.0); -// this.claw.openClaw(); -// this.current_step++; -// } -// break; // case 4: // elevator.setElevatorState(Constants.ELEVATOR_START); // this.claw.closeClaw(); @@ -268,6 +268,8 @@ public void loop(){ break; } // Done Processing Waypoints + this.driveTrain.drive(0.0,0.0); + // Process loops driveTrain.loop(); this.elevator.loop(); From 9e915a3e17f3bf0bae3a9a93cce72df1e11fb03f Mon Sep 17 00:00:00 2001 From: bruce Date: Wed, 29 Jan 2025 20:43:07 -0700 Subject: [PATCH 065/129] created compBasket_CBW for testing; corrected Pinpoint directions. still getting elevator move even though that case is commented out! --- .../ftc/teamcode/Auto/CompBasket.java | 2 +- .../ftc/teamcode/Auto/CompBasket_CBW.java | 335 ++++++++++++++++++ .../ftc/teamcode/Auto/PracticeBasket.java | 4 +- .../ftc/teamcode/Mechanisms/Constants.java | 6 +- .../teamcode/Mechanisms/DriveTrain2025.java | 6 +- .../ftc/teamcode/Mechanisms/Elevator.java | 1 + 6 files changed, 345 insertions(+), 9 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java index d2c4a3b..7ce6f5c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -268,7 +268,7 @@ public void loop(){ break; } // Done Processing Waypoints - // Process loops + // Process loop driveTrain.loop(); this.elevator.loop(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java new file mode 100644 index 0000000..3dec16e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java @@ -0,0 +1,335 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; + +import java.util.Locale; + +@Autonomous(name = "CompBasket_cbw") +public class CompBasket_CBW extends BaseAutoComp{ + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + WayPoint[] wayPoints; + + public void init_loop(){ + driveTrain.odometer.update(); + driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // This gets called when we have zero out and ready to hit play. + if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { + telemetry.addData("Initialized",""); + } + } + public void init(){ + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 640.5; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = 0.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 640.5; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].y = 100.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.NORTH; + //Done Waypoint 2 + this.wayPoints[3] = new WayPoint(); + this.wayPoints[3].x = 640.0; + this.wayPoints[3].y = 100.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].x_speed = 0.3; + this.wayPoints[3].facing = Constants.SOUTH_WEST; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint(); + this.wayPoints[4].x = 145.0; + this.wayPoints[4].y = 556.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.SOUTH_WEST; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint(); + this.wayPoints[5].x = 445.0; + this.wayPoints[5].y = 530.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.NORTH; + //Done Waypoint 5 + //Waypoint for rotating + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = 445.0; + this.wayPoints[6].y = 530.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.SOUTH_WEST; + //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = 270.0; + this.wayPoints[7].y = 250.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = 270.0; + this.wayPoints[8].y = 530.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = 695.0; + this.wayPoints[9].y = 530.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = 720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + + + }//init + @Override + public void loop(){ + driveTrain.odometer.update(); + + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } + else { + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + } + telemetry.addData("heading:","%.2f",driveTrain.heading); + telemetry.addData("Elevator State:",elevator.getState()); + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 2: + // strafe to line up with basketr + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.claw.openClaw(); + this.current_step++; + } + this.current_step++; + break; + case 3://turn to SW + // elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + // this.claw.openClaw(); + this.current_step++; + } + break; + case 4:// drive to basket at 135 heading + + + + + //elevator.setElevatorState(Constants.ELEVATOR_START); + // this.claw.closeClaw(); + //driveTrain.setDirection(this.wayPoints[this.current_step].facing); + this.current_step++; + break; +// case 5: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 6: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 7: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 8: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 9: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 10: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; + default: + break; + } + // Done Processing Waypoints + // Process loop + driveTrain.loop(); + this.elevator.loop(); + } + + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + + public boolean at_x(double previousX, double currentX) + { + if (currentX - previousX >= 0.0) + { + return driveTrain.getXPosition() >= currentX; + } + else { + return driveTrain.getXPosition() <= currentX; + } + } + + public boolean at_y(double previousY, double currentY) + { + if (currentY - previousY >= 0.0) + { + return driveTrain.getYPosition() >= currentY; + } + else { + return driveTrain.getYPosition() <= currentY; + } + } + + public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + // Check to see if you are at y. If yes, then process x; + // Process y direction + if (! this.at_y(previousWaypoint.x,currentWaypoint.y) ) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(0, currentWaypoint.y_speed); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + // Process x direction + if (!this.at_x(previousWaypoint.x,currentWaypoint.x)) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(currentWaypoint.x_speed, 0.0); + } + else { + this.driveTrain.drive(-currentWaypoint.x_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java index 037b8d0..a13e6fc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java @@ -36,9 +36,9 @@ public void init(){ //Waypoint setup //Practice bot setup: (Based on heading facing North/Forward) // +Y = +North - // -X = +East + // -X = +West // -Y = +South - // +X = +West + // +X = +East this.wayPoints = new BaseAutoPractice.WayPoint[this.total_waypoints + 1]; this.wayPoints[0] = new BaseAutoPractice.WayPoint(); this.wayPoints[0].x = 0.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index 3311113..086fa40 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -9,7 +9,7 @@ public static final boolean JUST_TESTING=true; // will be used to insert temporary test code public static final double TICKS_TO_INCHES = Math.PI * 48 / (25.4 * 2000); // for use in Odometry - public static final double SPEED_RATIO = 1.0; // Use this to slow down robot + public static final double SPEED_RATIO = 0.3; // Use this to slow down robot public static final double TURN_RATIO = 1.0; // use this to slow turn rate public static final double DRIVE_PID_ERROR = 1.5; // inches @@ -87,8 +87,8 @@ //odometer constants public static int ODOMETER_PRACTICE_MODE = 1; public static int ODOMETER_COMPETITION_MODE = 8; - public static double ODOMETER_X_OFFSET = -212.0; - public static double ODOMETER_Y_OFFSET = 270.0; + public static double ODOMETER_X_OFFSET = 0;// uppdated after contact with gobilda support + public static double ODOMETER_Y_OFFSET = 21.0; // auto constants public static double AUTO_X_DISTANCE_ERROR = 5; public static double AUTO_Y_DISTANCE_ERROR = 5; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index 58ddaa7..80e5677 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -179,15 +179,15 @@ public double getXPosition() { // Convert xPod into actual direction based on cu double xPos = 0.0; Pose2D pos = odometer.getPosition(); xPos = pos.getX(DistanceUnit.MM); - return xPos; + return -xPos;// pod mounted backwards } public double getYPosition() { // Convert yPod into actual direction based on current heading double yPos = 0.0; Pose2D pos = odometer.getPosition(); // The following is needed because of an error in the driver yPos = pos.getY(DistanceUnit.MM); - // Added negative sign to match drive control - return yPos; + + return -yPos;// pod mounted backwards } public void stop() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java index 7c00e34..e0a40da 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java @@ -93,6 +93,7 @@ public void move(double speed) {// this method is for manual elevator moves elevatorMotor.setRunMode(Motor.RunMode.RawPower); elevatorSpeed = speed * Constants.ELEVATOR_SPEED_FACTOR; elevatorMotor.set(elevatorSpeed); + // if (getHeight() >= Constants.ELEVATOR_MIN & getHeight() <= Constants.ELEVATOR_MAX) { // elevatorMotor.set(elevatorSpeed); // } else if (getHeight() >= Constants.ELEVATOR_MAX & speed < 0.0) { From 26804df7b307e675d5b8a7a8d18acfbcbee1c3e1 Mon Sep 17 00:00:00 2001 From: bruce Date: Thu, 30 Jan 2025 14:20:43 -0700 Subject: [PATCH 066/129] updated case 3 --- .../ftc/teamcode/Auto/CompBasket_CBW.java | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java index 3dec16e..b53b163 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java @@ -178,7 +178,7 @@ public void loop(){ case 2: // strafe to line up with basketr driveTrain.setDirection(this.wayPoints[this.current_step].facing); - elevator.setElevatorState(3); + // elevator.setElevatorState(3); if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); @@ -186,13 +186,13 @@ public void loop(){ else { driveTrain.drive(0.0,0.0); - this.claw.openClaw(); + // this.claw.openClaw(); this.current_step++; } this.current_step++; break; case 3://turn to SW - // elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); @@ -200,19 +200,21 @@ public void loop(){ else { driveTrain.drive(0.0,0.0); - // this.claw.openClaw(); + elevator.setElevatorState(3); this.current_step++; } break; case 4:// drive to basket at 135 heading + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else { + driveTrain.drive(0.0, 0.0); - - - //elevator.setElevatorState(Constants.ELEVATOR_START); - // this.claw.closeClaw(); - //driveTrain.setDirection(this.wayPoints[this.current_step].facing); - this.current_step++; + this.current_step++; + } break; // case 5: // if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) From 19234a33ac21ce2a455d36bc9f252349102b1083 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 31 Jan 2025 10:09:43 -0700 Subject: [PATCH 067/129] found an x that should have been a y in goto_xy --- .../org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java index b53b163..d313784 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java @@ -307,7 +307,7 @@ public boolean at_y(double previousY, double currentY) public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { // Check to see if you are at y. If yes, then process x; // Process y direction - if (! this.at_y(previousWaypoint.x,currentWaypoint.y) ) { + if (! this.at_y(previousWaypoint.y,currentWaypoint.y) ) { if (currentWaypoint.y >= previousWaypoint.y) { this.driveTrain.drive(0, currentWaypoint.y_speed); } From 8eaa01638c33fc78cfb92ee5fba6cdfa3c465681 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 31 Jan 2025 10:13:27 -0700 Subject: [PATCH 068/129] Format change. No code change. --- .../src/main/java/org/firstinspires/ftc/teamcode/CompBot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java index 6d77351..a781eb1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java @@ -215,7 +215,7 @@ public void loop() { Pose2D pos = driveTrain.odometer.getPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Position", data); -elevator.printTelemetry(telemetry); + elevator.printTelemetry(telemetry); driveTrain.printTelemetry(telemetry);// correct usage?? } } From e0dc0330fc5e43c32a5d6532d1101145315e6440 Mon Sep 17 00:00:00 2001 From: bruce Date: Fri, 31 Jan 2025 15:51:10 -0700 Subject: [PATCH 069/129] increasedd speed --- .../org/firstinspires/ftc/teamcode/Mechanisms/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index 086fa40..2405b85 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -9,7 +9,7 @@ public static final boolean JUST_TESTING=true; // will be used to insert temporary test code public static final double TICKS_TO_INCHES = Math.PI * 48 / (25.4 * 2000); // for use in Odometry - public static final double SPEED_RATIO = 0.3; // Use this to slow down robot + public static final double SPEED_RATIO = 0.7; // Use this to slow down robot public static final double TURN_RATIO = 1.0; // use this to slow turn rate public static final double DRIVE_PID_ERROR = 1.5; // inches From 2361e1a60c0e104c6de3fb114429e98453ff51f7 Mon Sep 17 00:00:00 2001 From: bruce Date: Fri, 31 Jan 2025 17:35:22 -0700 Subject: [PATCH 070/129] working on new auto --- .../ftc/teamcode/Auto/CompBasket_CBW.java | 49 +++++++++---------- .../firstinspires/ftc/teamcode/CompBot.java | 2 +- .../ftc/teamcode/Mechanisms/Constants.java | 8 +-- .../ftc/teamcode/Mechanisms/Elevator.java | 2 +- 4 files changed, 30 insertions(+), 31 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java index d313784..6536db1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java @@ -5,6 +5,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; import org.firstinspires.ftc.teamcode.Mechanisms.Claw; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; @@ -56,6 +57,8 @@ public void init(){ data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Function Position", data); telemetry.addData("Elevator State:",elevator.getState()); + + //Waypoint setup this.wayPoints = new WayPoint[this.total_waypoints + 1]; this.wayPoints[0] = new WayPoint(); @@ -85,8 +88,8 @@ public void init(){ this.wayPoints[3].facing = Constants.SOUTH_WEST; //Done Waypoint 3 this.wayPoints[4] = new WayPoint(); - this.wayPoints[4].x = 145.0; - this.wayPoints[4].y = 556.0; + this.wayPoints[4].x = 295.0; + this.wayPoints[4].y = 606.0; this.wayPoints[4].y_speed = 0.3; this.wayPoints[4].x_speed = 0.3; this.wayPoints[4].facing = Constants.SOUTH_WEST; @@ -142,6 +145,7 @@ public void loop(){ // Send information to the display telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Position", data); @@ -177,7 +181,7 @@ public void loop(){ break; case 2: // strafe to line up with basketr - driveTrain.setDirection(this.wayPoints[this.current_step].facing); + // elevator.setElevatorState(3); if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) { @@ -189,10 +193,10 @@ public void loop(){ // this.claw.openClaw(); this.current_step++; } - this.current_step++; + // this.current_step++; break; case 3://turn to SW - + driveTrain.setDirection(this.wayPoints[this.current_step].facing); if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); @@ -269,14 +273,22 @@ public void loop(){ // } // break; default: + elevator.setElevatorState(3); break; } // Done Processing Waypoints // Process loop + //elevator.setElevatorState(3); driveTrain.loop(); this.elevator.loop(); } + @Override + public void stop() { + elevator.setElevatorState(0); + super.stop(); + } + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); @@ -284,39 +296,26 @@ public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) public boolean at_x(double previousX, double currentX) { - if (currentX - previousX >= 0.0) - { - return driveTrain.getXPosition() >= currentX; - } - else { - return driveTrain.getXPosition() <= currentX; - } + return Math.abs(currentX - driveTrain.getXPosition()) <= Constants.AUTO_X_DISTANCE_ERROR; } public boolean at_y(double previousY, double currentY) { - if (currentY - previousY >= 0.0) - { - return driveTrain.getYPosition() >= currentY; - } - else { - return driveTrain.getYPosition() <= currentY; - } + return Math.abs(currentY - driveTrain.getYPosition()) <= Constants.AUTO_Y_DISTANCE_ERROR; } public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { // Check to see if you are at y. If yes, then process x; // Process y direction - if (! this.at_y(previousWaypoint.y,currentWaypoint.y) ) { + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { if (currentWaypoint.y >= previousWaypoint.y) { - this.driveTrain.drive(0, currentWaypoint.y_speed); + this.driveTrain.drive(0.0, currentWaypoint.y_speed); + } + else { + this.driveTrain.drive(0.0,-currentWaypoint.y_speed); } return false; } - else - { - this.driveTrain.drive(0.0,0.0); - } // Process x direction if (!this.at_x(previousWaypoint.x,currentWaypoint.x)) { if (currentWaypoint.x >= previousWaypoint.x) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java index 6d77351..341a7d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java @@ -102,7 +102,7 @@ public void init() { elevator.init(); this.elbowPosition = 0; // comment this out for competition and ensure it happens in auto code - driveTrain.odometer.resetPosAndIMU(); + // driveTrain.odometer.resetPosAndIMU(); // comment out with Auto telemetry.addData("Status", "Initialized"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index 2405b85..af17db1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -48,8 +48,8 @@ // public static final int ELEVATOR_LOW_BAR= 350; // nov 22 // public static final int ELEVATOR_HIGH_BAR = 660; - public static final int ELEVATOR_HIGH_BASKET = 1260; - public static final int ELEVATOR_MAX = 1220; + public static final int ELEVATOR_HIGH_BASKET = 1270; + public static final int ELEVATOR_MAX = 1270; public static final int ELEVATOR_MIN = 0; public static final double ELEVATOR_SPEED_FACTOR = 0.7; // 1.0 Old elevator and 0.3 for new elevator public static final double ELEVATOR_POSITION_TOLERANCE = 15.0; @@ -90,6 +90,6 @@ public static double ODOMETER_X_OFFSET = 0;// uppdated after contact with gobilda support public static double ODOMETER_Y_OFFSET = 21.0; // auto constants - public static double AUTO_X_DISTANCE_ERROR = 5; - public static double AUTO_Y_DISTANCE_ERROR = 5; + public static double AUTO_X_DISTANCE_ERROR = 50; + public static double AUTO_Y_DISTANCE_ERROR = 50; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java index e0a40da..a674d85 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java @@ -30,7 +30,7 @@ public void init() { } public void start() { - elevatorMotor.resetEncoder(); + // elevatorMotor.resetEncoder(); } public void loop() { From 09686500840615b704c4fec28161f931374fdf59 Mon Sep 17 00:00:00 2001 From: Annika Date: Fri, 31 Jan 2025 18:29:06 -0700 Subject: [PATCH 071/129] Created a new auto park using new system. --- .../ftc/teamcode/Auto/New_Park.java | 195 ++++++++++++++++++ 1 file changed, 195 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java new file mode 100644 index 0000000..2314430 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java @@ -0,0 +1,195 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; + +import java.util.Locale; + +@Autonomous(name = "NewPark") +public class New_Park extends BaseAutoComp{ + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + WayPoint[] wayPoints; + + public void init_loop(){ + driveTrain.odometer.update(); + driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // This gets called when we have zero out and ready to hit play. + if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { + telemetry.addData("Initialized",""); + } + } + public void init(){ + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + + + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 165.5; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = 0.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 165.5; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].y = -1208.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.NORTH; + //Done Waypoint 2 + }//init + @Override + public void loop(){ + driveTrain.odometer.update(); + + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } + else { + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + } + telemetry.addData("heading:","%.2f",driveTrain.heading); + telemetry.addData("Elevator State:",elevator.getState()); + //starting waypoint navigation + switch (this.current_step) { + case 1: // Move an inch from the wall + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 2: // Move over to Park + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + // this.claw.openClaw(); + this.current_step++; + } + // this.current_step++; + break; + default: + break; + } + // Done Processing Waypoints + // Process loop + //elevator.setElevatorState(3); + driveTrain.loop(); + this.elevator.loop(); + } + + @Override + public void stop() { + elevator.setElevatorState(0); + super.stop(); + } + + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + + public boolean at_x(double previousX, double currentX) + { + return Math.abs(currentX - driveTrain.getXPosition()) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double previousY, double currentY) + { + return Math.abs(currentY - driveTrain.getYPosition()) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + // Check to see if you are at y. If yes, then process x; + // Process y direction + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(0.0, currentWaypoint.y_speed); + } + else { + this.driveTrain.drive(0.0,-currentWaypoint.y_speed); + } + return false; + } + // Process x direction + if (!this.at_x(previousWaypoint.x,currentWaypoint.x)) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(currentWaypoint.x_speed, 0.0); + } + else { + this.driveTrain.drive(-currentWaypoint.x_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; + } + +} From f2d3fff31684a9d35a4a2df651979915c979b443 Mon Sep 17 00:00:00 2001 From: Annika Date: Fri, 31 Jan 2025 18:30:26 -0700 Subject: [PATCH 072/129] Created a new auto park using new system. --- .../ftc/teamcode/Auto/CompPark_Red.java | 156 +++++++++++++++-- .../ftc/teamcode/Auto/PracticePark.java | 159 ++++++++++++------ 2 files changed, 253 insertions(+), 62 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java index dfff7bc..113d64a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java @@ -6,19 +6,32 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; import java.util.Locale; -@Autonomous(name = "CompPark") -public class CompPark_Red extends BaseAutoComp{ +@Autonomous(name = "CompParkRed") +public class CompPark_Red extends BaseAutoComp { private DriveTrain2025 driveTrain; private double speed; private int counter = 0; + private double current_x, current_y; + private double x_speed,y_speed; + private int total_waypoints = 20; + WayPoint[] wayPoints; + public int step = 1; + private int current_step; + + // auto driving data + private double start_x, start_y; + private double dest_x, dest_y, set_dest; + private boolean at_x, at_y, set_x, set_y; + private boolean at_xy; - public void init(){ + public void init() { driveTrain = new DriveTrain2025(hardwareMap); driveTrain.init(); driveTrain.odometer.resetPosAndIMU(); @@ -26,21 +39,140 @@ public void init(){ elbow = new Elbow(hardwareMap); elbow.init(); elbow.setElbowState(0); + + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 165.0; + this.wayPoints[1].x_speed = 0.50; + this.wayPoints[1].y = 0.0; + this.wayPoints[1].y_speed = 0.50; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 165.0; + this.wayPoints[2].x_speed = 0.5; + this.wayPoints[2].y =-1208.0; + this.wayPoints[2].y_speed = 0.5; + this.wayPoints[2].facing = Constants.NORTH; + //Done Waypoint 2 + + }//init + @Override - public void loop(){ + public void loop() { + + driveTrain.odometer.update(); telemetry.addData("Sequence Step", counter); driveTrain.odometer.update(); Pose2D pos = driveTrain.odometer.getPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Position", data); driveTrain.odometer.update(); -// if (counter <= 150){ -// driveTrain.drive(0.0,-0.3); -// }else { -// driveTrain.drive(0.0,0.0); -// } -// counter = counter + 1; - driveTrain.loop(); + if (counter <= 150){ + driveTrain.drive(0.0,0.3); + }else { + driveTrain.drive(0.0,0.0); + } + counter = counter + 1; + current_x = driveTrain.getXPosition(); + current_y = driveTrain.getYPosition(); + String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Speed x","%.2f",this.x_speed); + telemetry.addData("Speed y","%.2f",this.y_speed); + telemetry.addData("heading:","%.2f",driveTrain.heading); +// telemetry.addData("Dest x:","%.2f",this.dest_x); +// telemetry.addData("Dest y:","%.2f",this.dest_y);t + //telemetry.addData("Distance to X","%.2f",(Math.abs(this.wayPoints[this.step].x - this.current_x))); + //telemetry.addData("Distance to Y","%.2f",Math.abs(this.wayPoints[this.step].y - this.current_y)); + if (this.wayPoints[this.current_step] != null){ + //telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + //telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + //telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } + +//cases + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + break; + default: + break; + case 2: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + + + } + } + + + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + + public boolean at_x(double previousX, double currentX) + { + return Math.abs(currentX - driveTrain.getXPosition()) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double previousY, double currentY) + { + return Math.abs(currentY - driveTrain.getYPosition()) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + // Check to see if you are at y. If yes, then process x; + // Process y direction + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(0.0, currentWaypoint.y_speed); + } + else { + this.driveTrain.drive(0.0,-currentWaypoint.y_speed); + } + return false; + } + // Process x direction + if (!this.at_x(previousWaypoint.x,currentWaypoint.x)) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(currentWaypoint.x_speed, 0.0); + } + else { + this.driveTrain.drive(-currentWaypoint.x_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java index 74b3f4a..5c50c89 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java @@ -5,81 +5,132 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; import java.util.Locale; @Autonomous (name = "PracticePark")// -public class PracticePark extends BaseAutoPractice{ +public class PracticePark extends BaseAutoPractice { private PracticeDriveTrain2025 driveTrain; public int step = -1; - private double speed; private int counter = 0; + private int current_step; BaseAutoPractice.WayPoint[] wayPoints; // auto driving data private double current_x, current_y; - private double start_x,start_y; - private double dest_x,dest_y,set_dest; - private double x_speed,y_speed; - private boolean at_x,at_y,set_x,set_y; - enum WorkingStep{ + private double speed; + private double start_x, start_y; + private double dest_x, dest_y, set_dest; + private double x_speed, y_speed; + private boolean at_x, at_y, set_x, set_y; + private boolean at_xy; + + enum WorkingStep { X, Y } + WorkingStep currentStep; - public void init(){ - driveTrain = new PracticeDriveTrain2025(hardwareMap); - driveTrain.init(); - driveTrain.odometer.resetPosAndIMU(); - this.speed = 0.0; - this.at_x = false; - this.at_y = false; - this.step = 1; - this.currentStep = WorkingStep.X; - this.x_speed = 0.5; - this.y_speed = 0.5; - this.wayPoints = new WayPoint[4]; + public void init() { + driveTrain = new PracticeDriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + + driveTrain = new PracticeDriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + this.speed = 0.0; + this.at_x = false; + this.at_y = false; + this.at_xy = false; + + this.step = 1; + this.currentStep = WorkingStep.X; + this.x_speed = 0.5; + this.y_speed = 0.5; + this.wayPoints = new WayPoint[4]; // this.wayPoints[0] = new WayPoint(); // this.wayPoints[0].x = 50.0; // this.wayPoints[0].y = 50.0; // this.wayPoints[0].facing = 0.0; - this.wayPoints[1] = new WayPoint(); - this.wayPoints[1].x = 50.0; - this.wayPoints[1].y = 50.0; - this.wayPoints[1].facing = Constants.NORTH; - this.wayPoints[2] = new WayPoint(); - this.wayPoints[2].x = -50.0; - this.wayPoints[2].y = -50.0; - this.wayPoints[2].facing = 0.0; - // this.dest_x = 50.0;//initializing + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 50.0; + this.wayPoints[1].y = 50.0; + this.wayPoints[1].facing = Constants.NORTH; + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = -50.0; + this.wayPoints[2].y = -50.0; + this.wayPoints[2].facing = 0.0; + + // this.dest_x = 50.0;//initializing // this.wayPoints = new WayPoint[12]; // // this.wayPoints[0].x = 10.0;//initializing // this.wayPoints[0].y = 0.0; // this.wayPoints[0].facing = Constants.NORTH; - }//init -@Override -public void loop(){ - telemetry.addData("Sequence Step", this.step); - // Update x y coords - driveTrain.odometer.update(); - Pose2D pos = driveTrain.odometer.getPosition(); - current_x = driveTrain.getXPosition(); - current_y = driveTrain.getYPosition(); - String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); - telemetry.addData("Position", data); - data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); - telemetry.addData("Function Position", data); - telemetry.addData("Speed x","%.2f",this.x_speed); - telemetry.addData("Speed y","%.2f",this.y_speed); - telemetry.addData("heading:","%.2f",driveTrain.heading); + }//init + + @Override + public void loop() { + telemetry.addData("Sequence Step", this.step); + // Update x y coords + driveTrain.odometer.update(); + Pose2D pos = driveTrain.odometer.getPosition(); + current_x = driveTrain.getXPosition(); + current_y = driveTrain.getYPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Speed x", "%.2f", this.x_speed); + telemetry.addData("Speed y", "%.2f", this.y_speed); + telemetry.addData("heading:", "%.2f", driveTrain.heading); // telemetry.addData("Dest x:","%.2f",this.dest_x); // telemetry.addData("Dest y:","%.2f",this.dest_y); - telemetry.addData("Distance to X","%.2f",(Math.abs(this.wayPoints[this.step].x - this.current_x))); - telemetry.addData("Distance to Y","%.2f",Math.abs(this.wayPoints[this.step].y - this.current_y)); + telemetry.addData("Distance to X", "%.2f", (Math.abs(this.wayPoints[this.step].x - this.current_x))); + telemetry.addData("Distance to Y", "%.2f", Math.abs(this.wayPoints[this.step].y - this.current_y)); + telemetry.addData("Function Position", data); + + + //cases + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + break; + default: + break; + case 2: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; // driveTrain.setDirection(Constants.SOUTH); @@ -141,7 +192,7 @@ public void loop(){ // default: // break; // } - // driveTrain.odometer.update(); + // driveTrain.odometer.update(); // if (counter <= 150){ // driveTrain.drive(0.0,0.3); @@ -153,7 +204,15 @@ public void loop(){ // //driveTrain.drive(0.0,0.1); // counter = counter + 1; // driveTrain.driveTo(speed,0.0, 5.0, Constants.NORTH); - driveTrain.loop(); - } // loop -}//everything + } // loop + }//everything + + private void goto_xy(WayPoint wayPoint, WayPoint wayPoint1) { + } + + private boolean at_xy(WayPoint wayPoint, WayPoint wayPoint1) { + return false; + } +} + From 1a3c11fcf23db34a3dfcdba2a6e1d6032f8408ce Mon Sep 17 00:00:00 2001 From: bruce Date: Fri, 31 Jan 2025 20:39:58 -0700 Subject: [PATCH 073/129] New park auto sort of working, --- .../ftc/teamcode/Auto/CompBasket.java | 2 +- .../ftc/teamcode/Auto/CompBasket_CBW.java | 18 +++++++++--------- .../ftc/teamcode/Auto/CompPark_Red.java | 2 +- .../ftc/teamcode/Auto/CompSpecimen.java | 2 +- .../ftc/teamcode/Auto/GeorgeTest.java | 2 +- .../ftc/teamcode/Auto/New_Park.java | 19 +++++++++---------- .../ftc/teamcode/Auto/PracticeBasket.java | 2 +- .../ftc/teamcode/Auto/PracticePark.java | 2 +- .../ftc/teamcode/Auto/PracticeSpecimen.java | 2 +- .../firstinspires/ftc/teamcode/CompBot.java | 2 +- .../ftc/teamcode/Mechanisms/Constants.java | 6 +++--- .../SensorGoBildaPinpointExample.java | 2 +- .../ftc/teamcode/PracticeBot.java | 2 +- 13 files changed, 31 insertions(+), 32 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java index 4e72017..d37e0c0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -15,7 +15,7 @@ import java.net.NoRouteToHostException; import java.util.Locale; -@Autonomous(name = "CompBasket") +//@Autonomous(name = "CompBasket") public class CompBasket extends BaseAutoComp{ private DriveTrain2025 driveTrain; private int current_step; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java index 6536db1..400b750 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java @@ -14,7 +14,7 @@ import java.util.Locale; -@Autonomous(name = "CompBasket_cbw") +//@Autonomous(name = "CompBasket_cbw") public class CompBasket_CBW extends BaseAutoComp{ private DriveTrain2025 driveTrain; private int current_step; @@ -89,7 +89,7 @@ public void init(){ //Done Waypoint 3 this.wayPoints[4] = new WayPoint(); this.wayPoints[4].x = 295.0; - this.wayPoints[4].y = 606.0; + this.wayPoints[4].y = 506.0; this.wayPoints[4].y_speed = 0.3; this.wayPoints[4].x_speed = 0.3; this.wayPoints[4].facing = Constants.SOUTH_WEST; @@ -283,11 +283,11 @@ public void loop(){ this.elevator.loop(); } - @Override - public void stop() { - elevator.setElevatorState(0); - super.stop(); - } +// @Override +// public void stop() { +// elevator.setElevatorState(0); +// super.stop(); +// } public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { @@ -296,12 +296,12 @@ public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) public boolean at_x(double previousX, double currentX) { - return Math.abs(currentX - driveTrain.getXPosition()) <= Constants.AUTO_X_DISTANCE_ERROR; + return (Math.abs(currentX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; } public boolean at_y(double previousY, double currentY) { - return Math.abs(currentY - driveTrain.getYPosition()) <= Constants.AUTO_Y_DISTANCE_ERROR; + return (Math.abs(currentY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; } public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java index 113d64a..4e3977f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java @@ -12,7 +12,7 @@ import java.util.Locale; -@Autonomous(name = "CompParkRed") +//@Autonomous(name = "CompParkRed") public class CompPark_Red extends BaseAutoComp { private DriveTrain2025 driveTrain; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompSpecimen.java index faa0c7f..51bfabf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompSpecimen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompSpecimen.java @@ -13,7 +13,7 @@ import java.util.Locale; -@Autonomous(name = "CompSpecimen") +//@Autonomous(name = "CompSpecimen") public class CompSpecimen extends BaseAutoComp{ private DriveTrain2025 driveTrain; private int current_step; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java index ca9c6e5..fc954aa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java @@ -14,7 +14,7 @@ import java.util.Locale; -@Autonomous(name = "GeorgeTest") +//@Autonomous(name = "GeorgeTest") public class GeorgeTest extends BaseAutoComp{ private DriveTrain2025 driveTrain; enum WorkingStep{ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java index 2314430..41e9e4d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java @@ -66,16 +66,16 @@ public void init(){ this.wayPoints[0].facing = Constants.NORTH; // WayPoint 1 this.wayPoints[1] = new WayPoint(); - this.wayPoints[1].x = 165.5; + this.wayPoints[1].x = 25.0; this.wayPoints[1].x_speed = 0.3; this.wayPoints[1].y = 0.0; - this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].y_speed = 0.0; this.wayPoints[1].facing = Constants.NORTH; //Done Waypoint 1 this.wayPoints[2] = new WayPoint(); - this.wayPoints[2].x = 165.5; - this.wayPoints[2].x_speed = 0.3; - this.wayPoints[2].y = -1208.0; + this.wayPoints[2].x = 25.0; + this.wayPoints[2].x_speed = 0.0; + this.wayPoints[2].y = -1108.0; this.wayPoints[2].y_speed = 0.3; this.wayPoints[2].facing = Constants.NORTH; //Done Waypoint 2 @@ -93,7 +93,7 @@ public void loop(){ data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Function Position", data); if (this.wayPoints[this.current_step] != null){ - telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint",at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); @@ -133,6 +133,7 @@ public void loop(){ // this.current_step++; break; default: + driveTrain.drive(0.0,0.0); break; } // Done Processing Waypoints @@ -147,20 +148,18 @@ public void stop() { elevator.setElevatorState(0); super.stop(); } - public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); } - public boolean at_x(double previousX, double currentX) { - return Math.abs(currentX - driveTrain.getXPosition()) <= Constants.AUTO_X_DISTANCE_ERROR; + return (Math.abs(currentX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; } public boolean at_y(double previousY, double currentY) { - return Math.abs(currentY - driveTrain.getYPosition()) <= Constants.AUTO_Y_DISTANCE_ERROR; + return (Math.abs(currentY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; } public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java index a13e6fc..ec826bc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java @@ -14,7 +14,7 @@ import java.util.Locale; -@Autonomous(name = "PracticeBasket") +//@Autonomous(name = "PracticeBasket") public class PracticeBasket extends BaseAutoPractice{ private PracticeDriveTrain2025 driveTrain; private int current_step; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java index 5c50c89..7d81125 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java @@ -13,7 +13,7 @@ import java.util.Locale; -@Autonomous (name = "PracticePark")// +//@Autonomous (name = "PracticePark")// public class PracticePark extends BaseAutoPractice { private PracticeDriveTrain2025 driveTrain; public int step = -1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java index 280cbbc..7547538 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java @@ -13,7 +13,7 @@ import java.util.Locale; -@Autonomous(name = "PracticeSpecimen") +//@Autonomous(name = "PracticeSpecimen") public class PracticeSpecimen extends BaseAutoPractice{ private PracticeDriveTrain2025 driveTrain; private int current_step; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java index 341a7d6..a20e42a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java @@ -102,7 +102,7 @@ public void init() { elevator.init(); this.elbowPosition = 0; // comment this out for competition and ensure it happens in auto code - // driveTrain.odometer.resetPosAndIMU(); // comment out with Auto + driveTrain.odometer.resetPosAndIMU(); // comment out with Auto telemetry.addData("Status", "Initialized"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index af17db1..69092b5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -9,7 +9,7 @@ public static final boolean JUST_TESTING=true; // will be used to insert temporary test code public static final double TICKS_TO_INCHES = Math.PI * 48 / (25.4 * 2000); // for use in Odometry - public static final double SPEED_RATIO = 0.7; // Use this to slow down robot + public static final double SPEED_RATIO = 1.0; // Use this to slow down robot public static final double TURN_RATIO = 1.0; // use this to slow turn rate public static final double DRIVE_PID_ERROR = 1.5; // inches @@ -90,6 +90,6 @@ public static double ODOMETER_X_OFFSET = 0;// uppdated after contact with gobilda support public static double ODOMETER_Y_OFFSET = 21.0; // auto constants - public static double AUTO_X_DISTANCE_ERROR = 50; - public static double AUTO_Y_DISTANCE_ERROR = 50; + public static double AUTO_X_DISTANCE_ERROR = 10; + public static double AUTO_Y_DISTANCE_ERROR = 10; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/SensorGoBildaPinpointExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/SensorGoBildaPinpointExample.java index 7f49357..1fefa21 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/SensorGoBildaPinpointExample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/SensorGoBildaPinpointExample.java @@ -57,7 +57,7 @@ -Ethan Doak */ -@TeleOp(name="PinPoint Odometry Reading", group="Linear OpMode") +//@TeleOp(name="PinPoint Odometry Reading", group="Linear OpMode") //@Disabled public class diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java index 02e1596..2ab113e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBot.java @@ -18,7 +18,7 @@ -@TeleOp(name = "PracticeBot") +//@TeleOp(name = "PracticeBot") public class PracticeBot extends OpMode { private PracticeDriveTrain2025 driveTrain; From 8d06f73b873411b56b46e51b6ba02894167595c4 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sun, 2 Feb 2025 11:34:10 -0700 Subject: [PATCH 074/129] Working on added web telemetry. --- .../org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java index fc954aa..7010a18 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java @@ -5,6 +5,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; import org.firstinspires.ftc.teamcode.Mechanisms.Claw; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; @@ -36,8 +37,8 @@ public void init() { this.elbow.init(); this.elbow.setElbowState(0); elevator = new Elevator(hardwareMap); - this.elevator.init(); - this.elevator.setElevatorState(0); + elevator.init(); + elevator.setElevatorState(0); // Display Settings telemetry.addData("Sequence Step", this.current_step); Pose2D pos = driveTrain.odometer.getPosition(); @@ -115,6 +116,7 @@ public void loop(){ } telemetry.addData("heading:","%.2f",driveTrain.heading); + telemetry.addData("Elevator State:",elevator.getState()); //starting waypoint navigation switch (this.current_step) { @@ -155,7 +157,7 @@ public void loop(){ case 2: driveTrain.setDirection(Constants.WEST); this.claw.openClaw(); - elevator.setElevatorState(Constants.ELEVATOR_HIGH_BASKET); + elevator.setElevatorState(3); this.current_step++; break; // case 5: @@ -198,6 +200,7 @@ public void loop(){ } // Done Processing Waypoints driveTrain.loop(); + elevator.loop(); } public boolean at_xy(BaseAutoComp.WayPoint previousWaypoint, BaseAutoComp.WayPoint currentWaypoint) From 82a3f5762b3feb961c25bf3063e9d0725cf12d42 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sun, 2 Feb 2025 11:35:23 -0700 Subject: [PATCH 075/129] Created a Do Nothing auto, so the initalization to TeleOp will work correctly. This auto initalizes everything and stays in the the init loop until the end. --- .../ftc/teamcode/Auto/Do_Nothing.java | 166 ++++++++++++++++++ 1 file changed, 166 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Do_Nothing.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Do_Nothing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Do_Nothing.java new file mode 100644 index 0000000..0eca10e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Do_Nothing.java @@ -0,0 +1,166 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; + +import java.util.Locale; + +@Autonomous(name = "DoNothing") +public class Do_Nothing extends BaseAutoComp{ + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + WayPoint[] wayPoints; + + public void init_loop(){ + driveTrain.odometer.update(); + driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); +// String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); +// telemetry.addData("Position", data); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // This gets called when we have zero out and ready to hit play. + if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { + telemetry.addData("Initialized",""); + } + } + public void init(){ + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + + + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 25.0; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = 0.0; + this.wayPoints[1].y_speed = 0.0; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 25.0; + this.wayPoints[2].x_speed = 0.0; + this.wayPoints[2].y = -1108.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.NORTH; + //Done Waypoint 2 + }//init + @Override + public void loop(){ + driveTrain.odometer.update(); + + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + + Pose2D pos = driveTrain.odometer.getPosition(); +// String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); +// telemetry.addData("Position", data); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + } + else { + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + } + telemetry.addData("heading:","%.2f",driveTrain.heading); + telemetry.addData("Elevator State:",elevator.getState()); + + } + + @Override + public void stop() { + elevator.setElevatorState(0); + super.stop(); + } + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + public boolean at_x(double previousX, double currentX) + { + double error; + error = (Math.abs(currentX) - Math.abs(driveTrain.getXPosition())); + telemetry.addData("X Error:",error); + return (Math.abs(currentX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double previousY, double currentY) + { + double error; + error = (Math.abs(currentY) - Math.abs(driveTrain.getYPosition())); + telemetry.addData("Y Error:",error); + return (Math.abs(currentY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + // Check to see if you are at y. If yes, then process x; + // Process y direction + if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { + if (currentWaypoint.y >= previousWaypoint.y) { + this.driveTrain.drive(0.0, currentWaypoint.y_speed); + } + else { + this.driveTrain.drive(0.0,-currentWaypoint.y_speed); + } + return false; + } + // Process x direction + if (!this.at_x(previousWaypoint.x,currentWaypoint.x)) { + if (currentWaypoint.x >= previousWaypoint.x) { + this.driveTrain.drive(currentWaypoint.x_speed, 0.0); + } + else { + this.driveTrain.drive(-currentWaypoint.x_speed, 0.0); + } + return false; + } + else + { + this.driveTrain.drive(0.0,0.0); + } + return true; + } + +} From f5009d0e50391a723be3f5f9dcc41e0ddb86b139 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sun, 2 Feb 2025 11:36:21 -0700 Subject: [PATCH 076/129] Modified the elevator height for high bar. --- .../org/firstinspires/ftc/teamcode/Mechanisms/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index 69092b5..b8a0e63 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -43,7 +43,7 @@ // for elevator moves public static final int ELEVATOR_START = 0; public static final int ELEVATOR_PICKUP_SPECIMEN = 100; - public static final int ELEVATOR_HIGH_BAR = 850; // nov 22 + public static final int ELEVATOR_HIGH_BAR = 700; // nov 22 // public static final int ELEVATOR_LOW_BAR= 350; // nov 22 // public static final int ELEVATOR_HIGH_BAR = 660; From fd4a2908b61b02a55da8e428440acead2192a5b9 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sun, 2 Feb 2025 11:36:37 -0700 Subject: [PATCH 077/129] Modified the elevator height for high bar. --- .../org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java index a674d85..b88091e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java @@ -57,7 +57,7 @@ public Elevator(HardwareMap hwMap) { //====================== METHODS =================== // Set the elevator state - public void setElevatorState(int position) { + public int setElevatorState(int position) { switch (position) { case 0: //Case for start position/pickup sample this.gotoPreSet(Constants.ELEVATOR_START); @@ -84,6 +84,7 @@ public void setElevatorState(int position) { // elevatorMotor.stopMotor(); } this.elevatorPosition = position; + return position; } @@ -124,7 +125,7 @@ public void gotoPreSet (int setpoint) { } public int getState() { - return this.elevatorPosition; + return setpoint; } public void printTelemetry(Telemetry telem){ From 58b8e1b9f7ef0f36073c0e948574d98cbda12a2a Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sun, 2 Feb 2025 11:38:01 -0700 Subject: [PATCH 078/129] Put in new logic for at_x and at_y. We were missing a couple of tests that won't provide the correct return value. --- .../ftc/teamcode/Auto/CompBasket_CBW.java | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java index 400b750..c7effa7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java @@ -14,7 +14,7 @@ import java.util.Locale; -//@Autonomous(name = "CompBasket_cbw") +@Autonomous(name = "CompBasket_cbw") public class CompBasket_CBW extends BaseAutoComp{ private DriveTrain2025 driveTrain; private int current_step; @@ -33,7 +33,7 @@ public void init_loop(){ telemetry.addData("Elevator State:",elevator.getState()); // This gets called when we have zero out and ready to hit play. if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { - telemetry.addData("Initialized",""); + telemetry.addData("Initialized","Done"); } } public void init(){ @@ -296,12 +296,18 @@ public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) public boolean at_x(double previousX, double currentX) { - return (Math.abs(currentX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + double error; + error = Math.abs(Math.abs(currentX) - Math.abs(driveTrain.getXPosition())); + telemetry.addData("X Error:",error); + return Math.abs((Math.abs(currentX) - Math.abs(driveTrain.getXPosition()))) <= Constants.AUTO_X_DISTANCE_ERROR; } public boolean at_y(double previousY, double currentY) { - return (Math.abs(currentY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + double error; + error = Math.abs(Math.abs(currentY) - Math.abs(driveTrain.getYPosition())); + telemetry.addData("Y Error:",error); + return Math.abs((Math.abs(currentY) - Math.abs(driveTrain.getYPosition()))) <= Constants.AUTO_Y_DISTANCE_ERROR; } public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { From 77234125898e280df392856d47727562c9aa4149 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sun, 2 Feb 2025 11:38:36 -0700 Subject: [PATCH 079/129] Added code to output the error value for at_x and at_y. --- .../main/java/org/firstinspires/ftc/teamcode/CompBot.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java index e50cd72..8a94c23 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java @@ -199,7 +199,11 @@ public void loop() { { claw.switchClaw(); } - + // Emergency Reset + if((driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.0) && (driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.0)) + { + driveTrain.odometer.resetPosAndIMU(); + } // Telemetry Output telemetry.addData("Direction Stick X:","%5.2f",driver.getRightX()); telemetry.addData("Direction Stick Y:","%5.2f",driver.getRightY()); From 481941821138fac189283350705f9e1564fa7eaa Mon Sep 17 00:00:00 2001 From: gnazarey Date: Mon, 3 Feb 2025 00:29:20 -0700 Subject: [PATCH 080/129] Added code to send to dashboard --- .../ftc/teamcode/Auto/GeorgeTest.java | 23 ++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java index 7010a18..e5b368d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java @@ -26,7 +26,21 @@ enum WorkingStep{ BaseAutoComp.WayPoint[] wayPoints; private int current_step; private int total_waypoints = 10; + CAITelemetry dashboardtelemetry; + public void init_loop() { + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("heading:","%.2f",driveTrain.heading); + dashboardtelemetry.addData("X Position",driveTrain.getXPosition()); + dashboardtelemetry.addData("Y Position",driveTrain.getYPosition()); + dashboardtelemetry.addData("Heading",driveTrain.heading); + dashboardtelemetry.update(); + } public void init() { driveTrain = new DriveTrain2025(hardwareMap); driveTrain.init(); @@ -40,14 +54,21 @@ public void init() { elevator.init(); elevator.setElevatorState(0); // Display Settings + telemetry.addData("Sequence Step", this.current_step); Pose2D pos = driveTrain.odometer.getPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Position", data); data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Function Position", data); - telemetry.addData("heading:","%.2f",driveTrain.heading); + dashboardtelemetry.setDashboardEnabled(true); + dashboardtelemetry.addData("X Position",driveTrain.getXPosition()); + dashboardtelemetry.addData("Y Position",driveTrain.getYPosition()); + dashboardtelemetry.addData("Heading",driveTrain.heading); + dashboardtelemetry.update(); + + // Waypoint Setup this.wayPoints = new WayPoint[this.total_waypoints + 1]; this.wayPoints[0] = new WayPoint(); From 8340dc49476192d28d536b98c67c5bc47f70cdf0 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Mon, 3 Feb 2025 11:14:04 -0700 Subject: [PATCH 081/129] Added code to send to dashboard --- .../ftc/teamcode/Auto/CompBasket_CBW.java | 24 ++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java index c7effa7..3deee2e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java @@ -12,6 +12,9 @@ import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; + import java.util.Locale; @Autonomous(name = "CompBasket_cbw") @@ -20,6 +23,7 @@ public class CompBasket_CBW extends BaseAutoComp{ private int current_step; private int total_waypoints = 20; WayPoint[] wayPoints; + TelemetryPacket pack; public void init_loop(){ driveTrain.odometer.update(); @@ -35,6 +39,11 @@ public void init_loop(){ if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { telemetry.addData("Initialized","Done"); } + // Dashboard Telemetry + // ftc-dashboard telemetry + pack.put("heading", pos.getHeading(AngleUnit.DEGREES)); + pack.put("X distance:", driveTrain.getXPosition()); + pack.put("Y distance:", driveTrain.getYPosition()); } public void init(){ driveTrain = new DriveTrain2025(hardwareMap); @@ -57,7 +66,8 @@ public void init(){ data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Function Position", data); telemetry.addData("Elevator State:",elevator.getState()); - + // Dashboard Setup + pack = new TelemetryPacket(); //Waypoint setup this.wayPoints = new WayPoint[this.total_waypoints + 1]; @@ -156,15 +166,27 @@ public void loop(){ telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + // ftc-dashboard telemetry + pack.put("At Waypoint", this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + pack.put("At Waypoint X", this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + pack.put("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); } else { telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + // ftc-dashboard telemetry + pack.put("At Waypoint", this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + pack.put("At Waypoint X", this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + pack.put("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); } telemetry.addData("heading:","%.2f",driveTrain.heading); telemetry.addData("Elevator State:",elevator.getState()); + // ftc-dashboard telemetry + pack.put("heading", pos.getHeading(AngleUnit.DEGREES)); + pack.put("X distance:", driveTrain.getXPosition()); + pack.put("Y distance:", driveTrain.getYPosition()); //starting waypoint navigation switch (this.current_step) { // drive 2 inches form submersible From 9ed5bf544328e71d3f4890d1d8091af284d7f5a5 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Mon, 3 Feb 2025 11:14:43 -0700 Subject: [PATCH 082/129] Added code to send to dashboard --- .../java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java | 1 + 1 file changed, 1 insertion(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java index e5b368d..6d85014 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java @@ -36,6 +36,7 @@ public void init_loop() { data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Function Position", data); telemetry.addData("heading:","%.2f",driveTrain.heading); + dashboardtelemetry.addData("X Position",driveTrain.getXPosition()); dashboardtelemetry.addData("Y Position",driveTrain.getYPosition()); dashboardtelemetry.addData("Heading",driveTrain.heading); From 6bb2e7364ff3efa08eca1feb623abbe316d5b565 Mon Sep 17 00:00:00 2001 From: bruce Date: Fri, 7 Feb 2025 18:18:45 -0700 Subject: [PATCH 083/129] added testGoTo --- .../ftc/teamcode/Auto/CompBasket_CBW.java | 14 +- .../ftc/teamcode/Mechanisms/Constants.java | 1 + .../teamcode/Mechanisms/DriveTrain2025.java | 1 + .../ftc/teamcode/Test_GoTo_bruce.java | 350 ++++++++++++++++++ 4 files changed, 356 insertions(+), 10 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test_GoTo_bruce.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java index 3deee2e..f1b4eb1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java @@ -17,7 +17,7 @@ import java.util.Locale; -@Autonomous(name = "CompBasket_cbw") +//@Autonomous(name = "CompBasket_cbw") public class CompBasket_CBW extends BaseAutoComp{ private DriveTrain2025 driveTrain; private int current_step; @@ -37,7 +37,7 @@ public void init_loop(){ telemetry.addData("Elevator State:",elevator.getState()); // This gets called when we have zero out and ready to hit play. if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { - telemetry.addData("Initialized","Done"); + telemetry.addData("Initialized",""); } // Dashboard Telemetry // ftc-dashboard telemetry @@ -318,18 +318,12 @@ public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) public boolean at_x(double previousX, double currentX) { - double error; - error = Math.abs(Math.abs(currentX) - Math.abs(driveTrain.getXPosition())); - telemetry.addData("X Error:",error); - return Math.abs((Math.abs(currentX) - Math.abs(driveTrain.getXPosition()))) <= Constants.AUTO_X_DISTANCE_ERROR; + return (Math.abs(currentX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; } public boolean at_y(double previousY, double currentY) { - double error; - error = Math.abs(Math.abs(currentY) - Math.abs(driveTrain.getYPosition())); - telemetry.addData("Y Error:",error); - return Math.abs((Math.abs(currentY) - Math.abs(driveTrain.getYPosition()))) <= Constants.AUTO_Y_DISTANCE_ERROR; + return (Math.abs(currentY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; } public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index b8a0e63..bc93840 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -79,6 +79,7 @@ // // Tolerance Section public static final double JOYSTICK_TOLERANCE = 0.5; + public static final double AT_XY_TOLERANCE = 5; // Bruce:sept30: change these for use in elevator?kk // public static final double ARM_RADIANS_PER_TICK = Math.PI/244.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index 80e5677..f4f3aaa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -165,6 +165,7 @@ public void driveTo(double speed, double xDist, double yDist, double facing) { xControl.setSetPoint(currentXTarget); yControl.setSetPoint(currentYTarget); + } public void drive(double forwardSpeed, double strafeSpeed) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test_GoTo_bruce.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test_GoTo_bruce.java new file mode 100644 index 0000000..3fc4e4a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test_GoTo_bruce.java @@ -0,0 +1,350 @@ +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.arcrobotics.ftclib.controller.PIDController; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; + +import org.firstinspires.ftc.teamcode.Auto.BaseAutoComp; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; + +import java.util.Locale; + +@Autonomous(name = "testGOTO") +public class Test_GoTo_bruce extends BaseAutoComp { + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + private PIDController headingControl = null; + private PIDController xControl = null; + private PIDController yControl = null; + WayPoint[] wayPoints; + + + public void init_loop() { + driveTrain.odometer.update(); + driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:", elevator.getState()); + + // This gets called when we have zero out and ready to hit play. + if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { + telemetry.addData("Initialized", "Done"); + } + } + + public void init() { + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + + xControl = new PIDController(0.17, 0.0, 0.00);//FOR AUTO? + yControl = new PIDController(0.17, 0.0, 0.00);//For AUTO + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:", elevator.getState()); + + + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 640.5; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = 0.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 640.5; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].y = 100.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.NORTH; + //Done Waypoint 2 + this.wayPoints[3] = new WayPoint(); + this.wayPoints[3].x = 640.0; + this.wayPoints[3].y = 100.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].x_speed = 0.3; + this.wayPoints[3].facing = Constants.SOUTH_WEST; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint(); + this.wayPoints[4].x = 295.0; + this.wayPoints[4].y = 506.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.SOUTH_WEST; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint(); + this.wayPoints[5].x = 445.0; + this.wayPoints[5].y = 530.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.NORTH; + //Done Waypoint 5 + //Waypoint for rotating + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = 445.0; + this.wayPoints[6].y = 530.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.SOUTH_WEST; + //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = 270.0; + this.wayPoints[7].y = 250.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = 270.0; + this.wayPoints[8].y = 530.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = 695.0; + this.wayPoints[9].y = 530.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = 720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + + + }//init + + @Override + public void loop() { + driveTrain.odometer.update(); + TelemetryPacket pack = new TelemetryPacket(); + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null) { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step - 1].x, this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step - 1].y, this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step].x, this.wayPoints[this.current_step].y); + } else { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step - 2], this.wayPoints[this.current_step - 1])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step - 2].x, this.wayPoints[this.current_step - 1].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step - 2].y, this.wayPoints[this.current_step - 1].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step - 1].x, this.wayPoints[this.current_step - 1].y); + } + telemetry.addData("heading:", "%.2f", driveTrain.heading); + telemetry.addData("Elevator State:", elevator.getState()); + + // data to dashboard--------------------------------------COMMENT OUT FOR COMPETITION + pack.put("heading",driveTrain.heading); + pack.put ("X",driveTrain.getXPosition()); + pack.put ("y:",driveTrain.getYPosition()); + + FtcDashboard.getInstance().sendTelemetryPacket(pack); + + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } + if (!at_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step]))// if not at waypoint continue + { + goto_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + } + break; + case 2: + // strafe to line up with basketr + + // elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + // this.claw.openClaw(); + this.current_step++; + } + // this.current_step++; + break; + case 3://turn to SW + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + elevator.setElevatorState(3); + this.current_step++; + } + break; + case 4:// drive to basket at 135 heading + + if (!this.at_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step - 1], this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + + this.current_step++; + } + break; +// case 5: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 6: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 7: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 8: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 9: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 10: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; + default: + elevator.setElevatorState(3); + break; + } + // Done Processing Waypoints + // Process loop + //elevator.setElevatorState(3); + driveTrain.loop(); + this.elevator.loop(); + } + + + @Override + public void stop() { + elevator.setElevatorState(0); + super.stop(); + } + + + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + double locationX = driveTrain.getXPosition(); + double locationY = driveTrain.getYPosition(); + double errorX = Math.abs(locationX - currentWaypoint.x); + double errorY = Math.abs(locationY - currentWaypoint.y); + // pack.put("Xerror:",errorX); + // pack.put("Yerror:",errorY); + //return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + return Math.sqrt((errorX * errorX) + (errorY * errorY)) <= Constants.AT_XY_TOLERANCE; + + + } + + public boolean at_x(double previousX, double currentX) { + double error; + error = Math.abs(Math.abs(currentX) - Math.abs(driveTrain.getXPosition())); + telemetry.addData("X Error:", error); + return Math.abs((Math.abs(currentX) - Math.abs(driveTrain.getXPosition()))) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double previousY, double currentY) { + double error; + error = Math.abs(Math.abs(currentY) - Math.abs(driveTrain.getYPosition())); + telemetry.addData("Y Error:", error); + return Math.abs((Math.abs(currentY) - Math.abs(driveTrain.getYPosition()))) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public void goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + + xControl.setSetPoint(currentWaypoint.x);// PID setpoint is new X + yControl.setSetPoint(currentWaypoint.y);// PID setpoint is new Y + // now move robot using PID for x & y position targets + driveTrain.drive(xControl.calculate(driveTrain.getXPosition()), yControl.calculate(driveTrain.getYPosition())); + + }// end of goto_xy +} \ No newline at end of file From 243329a6321bec1b1a28ee0531a0f571fb597f97 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 7 Feb 2025 19:26:40 -0700 Subject: [PATCH 084/129] Added code to send to dashboard --- .../ftc/teamcode/Auto/CompBasket_CBW.java | 39 +++++++++++-------- .../ftc/teamcode/Auto/GeorgeTest.java | 9 ++++- 2 files changed, 30 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java index f1b4eb1..dd5bd62 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java @@ -17,13 +17,14 @@ import java.util.Locale; -//@Autonomous(name = "CompBasket_cbw") +@Autonomous(name = "CompBasket_cbw") public class CompBasket_CBW extends BaseAutoComp{ private DriveTrain2025 driveTrain; private int current_step; private int total_waypoints = 20; WayPoint[] wayPoints; TelemetryPacket pack; + CAITelemetry dashboardtelemetry; public void init_loop(){ driveTrain.odometer.update(); @@ -41,9 +42,10 @@ public void init_loop(){ } // Dashboard Telemetry // ftc-dashboard telemetry - pack.put("heading", pos.getHeading(AngleUnit.DEGREES)); - pack.put("X distance:", driveTrain.getXPosition()); - pack.put("Y distance:", driveTrain.getYPosition()); + dashboardtelemetry.addData("X Position",driveTrain.getXPosition()); + dashboardtelemetry.addData("Y Position",driveTrain.getYPosition()); + dashboardtelemetry.addData("Heading",pos.getHeading(AngleUnit.DEGREES)); + dashboardtelemetry.update(); } public void init(){ driveTrain = new DriveTrain2025(hardwareMap); @@ -67,8 +69,8 @@ public void init(){ telemetry.addData("Function Position", data); telemetry.addData("Elevator State:",elevator.getState()); // Dashboard Setup - pack = new TelemetryPacket(); - +// pack = new TelemetryPacket(); + dashboardtelemetry = new CAITelemetry(telemetry); //Waypoint setup this.wayPoints = new WayPoint[this.total_waypoints + 1]; this.wayPoints[0] = new WayPoint(); @@ -98,7 +100,7 @@ public void init(){ this.wayPoints[3].facing = Constants.SOUTH_WEST; //Done Waypoint 3 this.wayPoints[4] = new WayPoint(); - this.wayPoints[4].x = 295.0; + this.wayPoints[4].x = 195.0; this.wayPoints[4].y = 506.0; this.wayPoints[4].y_speed = 0.3; this.wayPoints[4].x_speed = 0.3; @@ -167,9 +169,9 @@ public void loop(){ telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); // ftc-dashboard telemetry - pack.put("At Waypoint", this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); - pack.put("At Waypoint X", this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); - pack.put("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + dashboardtelemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + dashboardtelemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + dashboardtelemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); } else { telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); @@ -177,16 +179,21 @@ public void loop(){ telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); // ftc-dashboard telemetry - pack.put("At Waypoint", this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); - pack.put("At Waypoint X", this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); - pack.put("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + dashboardtelemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + dashboardtelemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + dashboardtelemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); } telemetry.addData("heading:","%.2f",driveTrain.heading); telemetry.addData("Elevator State:",elevator.getState()); // ftc-dashboard telemetry - pack.put("heading", pos.getHeading(AngleUnit.DEGREES)); - pack.put("X distance:", driveTrain.getXPosition()); - pack.put("Y distance:", driveTrain.getYPosition()); +// pack.put("heading", pos.getHeading(AngleUnit.DEGREES)); +// pack.put("X distance:", driveTrain.getXPosition()); +// pack.put("Y distance:", driveTrain.getYPosition()); + // dashboard info + dashboardtelemetry.addData("X Position",driveTrain.getXPosition()); + dashboardtelemetry.addData("Y Position",driveTrain.getYPosition()); + dashboardtelemetry.addData("Heading",pos.getHeading(AngleUnit.DEGREES)); + dashboardtelemetry.update(); //starting waypoint navigation switch (this.current_step) { // drive 2 inches form submersible diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java index 6d85014..d349456 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java @@ -15,7 +15,7 @@ import java.util.Locale; -//@Autonomous(name = "GeorgeTest") +@Autonomous(name = "GeorgeTest") public class GeorgeTest extends BaseAutoComp{ private DriveTrain2025 driveTrain; enum WorkingStep{ @@ -54,6 +54,7 @@ public void init() { elevator = new Elevator(hardwareMap); elevator.init(); elevator.setElevatorState(0); + dashboardtelemetry = new CAITelemetry(telemetry); // Display Settings telemetry.addData("Sequence Step", this.current_step); @@ -139,7 +140,11 @@ public void loop(){ telemetry.addData("heading:","%.2f",driveTrain.heading); telemetry.addData("Elevator State:",elevator.getState()); - + // dashboard info + dashboardtelemetry.addData("X Position",driveTrain.getXPosition()); + dashboardtelemetry.addData("Y Position",driveTrain.getYPosition()); + dashboardtelemetry.addData("Heading",driveTrain.heading); + dashboardtelemetry.update(); //starting waypoint navigation switch (this.current_step) { // drive 2 inches form submersible From b0771f35f9e6f41006fa5c7192e9bb338c483143 Mon Sep 17 00:00:00 2001 From: bruce Date: Fri, 7 Feb 2025 19:39:38 -0700 Subject: [PATCH 085/129] added testGoTo --- .../ftc/teamcode/Auto/gotoXY_test.java | 351 ++++++++++++++++++ .../ftc/teamcode/Mechanisms/Constants.java | 1 + .../teamcode/Mechanisms/DriveTrain2025.java | 6 +- .../ftc/teamcode/Test_GoTo_bruce.java | 2 +- 4 files changed, 356 insertions(+), 4 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java new file mode 100644 index 0000000..6ae8279 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java @@ -0,0 +1,351 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; + +import java.util.Locale; + +@Autonomous(name = "testGOTO") +public class gotoXY_test extends BaseAutoComp{ + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + WayPoint[] wayPoints; + TelemetryPacket pack; + CAITelemetry dashboardtelemtry; + + public void init_loop(){ + driveTrain.odometer.update(); + driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // This gets called when we have zero out and ready to hit play. + if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { + telemetry.addData("Initialized",""); + } + // Dashboard Telemetry + // ftc-dashboard telemetry + pack.put("heading", pos.getHeading(AngleUnit.DEGREES)); + pack.put("X distance:", driveTrain.getXPosition()); + pack.put("Y distance:", driveTrain.getYPosition()); + } + public void init(){ + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + dashboardtelemtry = new CAITelemetry(telemetry); + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // Dashboard Setup + pack = new TelemetryPacket(); + + //Waypoint setup + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 640.5; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = 0.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 640.5; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].y = 100.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.NORTH; + //Done Waypoint 2 + this.wayPoints[3] = new WayPoint(); + this.wayPoints[3].x = 640.0; + this.wayPoints[3].y = 100.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].x_speed = 0.3; + this.wayPoints[3].facing = Constants.SOUTH_WEST; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint(); + this.wayPoints[4].x = 295.0; + this.wayPoints[4].y = 506.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.SOUTH_WEST; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint(); + this.wayPoints[5].x = 445.0; + this.wayPoints[5].y = 530.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.NORTH; + //Done Waypoint 5 + //Waypoint for rotating + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = 445.0; + this.wayPoints[6].y = 530.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.SOUTH_WEST; + //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = 270.0; + this.wayPoints[7].y = 250.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = 270.0; + this.wayPoints[8].y = 530.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = 695.0; + this.wayPoints[9].y = 530.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = 720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + + + }//init + @Override + public void loop(){ + driveTrain.odometer.update(); +dashboardtelemtry.update(); + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null){ + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); + // ftc-dashboard telemetry + pack.put("At Waypoint", this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); + pack.put("At Waypoint X", this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); + pack.put("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + } + else { + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + // ftc-dashboard telemetry + pack.put("At Waypoint", this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); + pack.put("At Waypoint X", this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); + pack.put("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + } + telemetry.addData("heading:","%.2f",driveTrain.heading); + telemetry.addData("Elevator State:",elevator.getState()); + // ftc-dashboard telemetry + pack.put("heading", pos.getHeading(AngleUnit.DEGREES)); + pack.put("X distance:", driveTrain.getXPosition()); + pack.put("Y distance:", driveTrain.getYPosition()); + + + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 2: + // strafe to line up with basketr + + // elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + // this.claw.openClaw(); + this.current_step++; + } + // this.current_step++; + break; + case 3://turn to SW + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0,0.0); + elevator.setElevatorState(3); + this.current_step++; + } + break; + case 4:// drive to basket at 135 heading + + if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + } + else { + driveTrain.drive(0.0, 0.0); + + this.current_step++; + } + break; +// case 5: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 6: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 7: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 8: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 9: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 10: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; + default: + elevator.setElevatorState(3); + break; + } + // Done Processing Waypoints + // Process loop + //elevator.setElevatorState(3); + driveTrain.loop(); + this.elevator.loop(); + } + +// @Override +// public void stop() { +// elevator.setElevatorState(0); +// super.stop(); +// } + + public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + { + return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + } + + public boolean at_x(double previousX, double currentX) + { + return (Math.abs(currentX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double previousY, double currentY) + { + return (Math.abs(currentY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public void goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + + double errorX = Math.abs(currentWaypoint.x - driveTrain.getXPosition()); + double errorY = Math.abs(currentWaypoint.y - driveTrain.getYPosition()); + driveTrain.xControl.setSetPoint(currentWaypoint.x); + driveTrain.yControl.setSetPoint(currentWaypoint.y); + driveTrain.drive(driveTrain.xControl.calculate(),driveTrain.yControl.calculate()); + + double totalError = Math.sqrt(errorX*errorX + errorY*errorY); + // pack.put("error:",totalError); + dashboardtelemtry.addData("Error",totalError); + +// if ( totalError <= Constants.AT_XY_TOLERANCE) +// { return true;} +// else +// {return false;} + return; + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index bc93840..5067e1e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -81,6 +81,7 @@ public static final double JOYSTICK_TOLERANCE = 0.5; public static final double AT_XY_TOLERANCE = 5; + // Bruce:sept30: change these for use in elevator?kk // public static final double ARM_RADIANS_PER_TICK = Math.PI/244.0; // public static final double ARM_TICKS_PER_90DEG = 113; // CONFIRM #TICKS PER 90. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index f4f3aaa..b6bdb60 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -32,9 +32,9 @@ MecanumDrive driveBase; public GoBildaPinpointDriver odometer; - private PIDController headingControl = null; - private PIDController xControl = null; - private PIDController yControl = null; + public PIDController headingControl = null; + public PIDController xControl = null; + public PIDController yControl = null; public double headingCorrection = 0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test_GoTo_bruce.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test_GoTo_bruce.java index 3fc4e4a..f1de0ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test_GoTo_bruce.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test_GoTo_bruce.java @@ -19,7 +19,7 @@ import java.util.Locale; -@Autonomous(name = "testGOTO") +@Autonomous(name = "testGOTO_old") public class Test_GoTo_bruce extends BaseAutoComp { private DriveTrain2025 driveTrain; private int current_step; From c83e0d504a2e4447630c4b534beccd60d8d574f3 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 7 Feb 2025 20:56:31 -0700 Subject: [PATCH 086/129] Change 10 to 10.0 --- .../firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index f4f3aaa..15e156b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -78,7 +78,7 @@ public DriveTrain2025(HardwareMap hwMap) } public void init() { headingControl = new PIDController(0.02, 0.002, 0.000 ); - headingControl.setTolerance(10);// was 3 increased to see if affects spinnning ..cbw + headingControl.setTolerance(10.0);// was 3 increased to see if affects spinnning ..cbw xControl = new PIDController(0.17, 0.0, 0.00);//FOR AUTO? yControl = new PIDController(0.17, 0.0, 0.00);//For AUTO From 3ca63d918195e48fa77c428f2ea5e6b38e2380ce Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 7 Feb 2025 20:56:53 -0700 Subject: [PATCH 087/129] Changed Waypoint values --- .../ftc/teamcode/Auto/CompBasket_CBW.java | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java index dd5bd62..2221e90 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java @@ -100,8 +100,8 @@ public void init(){ this.wayPoints[3].facing = Constants.SOUTH_WEST; //Done Waypoint 3 this.wayPoints[4] = new WayPoint(); - this.wayPoints[4].x = 195.0; - this.wayPoints[4].y = 506.0; + this.wayPoints[4].x = 100.0; + this.wayPoints[4].y = 450.0; this.wayPoints[4].y_speed = 0.3; this.wayPoints[4].x_speed = 0.3; this.wayPoints[4].facing = Constants.SOUTH_WEST; @@ -157,6 +157,7 @@ public void loop(){ // Send information to the display telemetry.addData("Sequence Step", this.current_step); + dashboardtelemetry.addData("Sequence Step", this.current_step); Pose2D pos = driveTrain.odometer.getPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); @@ -172,6 +173,7 @@ public void loop(){ dashboardtelemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); dashboardtelemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); dashboardtelemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + dashboardtelemetry.addData("Facing",this.wayPoints[this.current_step].facing); } else { telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); @@ -182,6 +184,7 @@ public void loop(){ dashboardtelemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); dashboardtelemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); dashboardtelemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + dashboardtelemetry.addData("Facing",this.wayPoints[this.current_step-1].facing); } telemetry.addData("heading:","%.2f",driveTrain.heading); telemetry.addData("Elevator State:",elevator.getState()); @@ -190,9 +193,9 @@ public void loop(){ // pack.put("X distance:", driveTrain.getXPosition()); // pack.put("Y distance:", driveTrain.getYPosition()); // dashboard info - dashboardtelemetry.addData("X Position",driveTrain.getXPosition()); - dashboardtelemetry.addData("Y Position",driveTrain.getYPosition()); - dashboardtelemetry.addData("Heading",pos.getHeading(AngleUnit.DEGREES)); +// dashboardtelemetry.addData("X Position",driveTrain.getXPosition()); +// dashboardtelemetry.addData("Y Position",driveTrain.getYPosition()); +// dashboardtelemetry.addData("Heading",pos.getHeading(AngleUnit.DEGREES)); dashboardtelemetry.update(); //starting waypoint navigation switch (this.current_step) { @@ -245,8 +248,7 @@ public void loop(){ } else { driveTrain.drive(0.0, 0.0); - - this.current_step++; +// this.current_step++; } break; // case 5: @@ -325,12 +327,12 @@ public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) public boolean at_x(double previousX, double currentX) { - return (Math.abs(currentX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + return Math.abs(Math.abs(currentX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; } public boolean at_y(double previousY, double currentY) { - return (Math.abs(currentY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + return Math.abs(Math.abs(currentY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; } public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { From f353d1e0f3edc825da165115c6e2bfb360b7531f Mon Sep 17 00:00:00 2001 From: bruce Date: Fri, 7 Feb 2025 23:03:47 -0700 Subject: [PATCH 088/129] testing gotoXY_test.java --- .../ftc/teamcode/Auto/gotoXY_test.java | 33 ++++++++++++------- .../ftc/teamcode/Mechanisms/Constants.java | 8 ++--- .../teamcode/Mechanisms/DriveTrain2025.java | 4 +-- 3 files changed, 28 insertions(+), 17 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java index 6ae8279..bac9212 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java @@ -40,9 +40,10 @@ public void init_loop(){ } // Dashboard Telemetry // ftc-dashboard telemetry - pack.put("heading", pos.getHeading(AngleUnit.DEGREES)); - pack.put("X distance:", driveTrain.getXPosition()); - pack.put("Y distance:", driveTrain.getYPosition()); + + dashboardtelemtry.addData("X",driveTrain.getXPosition()); + dashboardtelemtry.addData("Y",driveTrain.getYPosition()); + dashboardtelemtry.update(); } public void init(){ driveTrain = new DriveTrain2025(hardwareMap); @@ -200,8 +201,9 @@ public void loop(){ } else { + telemetry.addData("made to else",""); driveTrain.drive(0.0,0.0); - this.current_step++; + //this.current_step++; } break; case 2: @@ -314,9 +316,16 @@ public void loop(){ // super.stop(); // } - public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + public boolean at_xy(WayPoint previousWaypoint, WayPoint targetWaypoint) { - return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + //return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + double errorX = Math.abs(targetWaypoint.x - driveTrain.getXPosition()); + double errorY = Math.abs(targetWaypoint.y - driveTrain.getYPosition()); + double totalError = Math.sqrt(errorX*errorX + errorY*errorY); + dashboardtelemtry.addData("Error X",errorX); + dashboardtelemtry.addData("Error Y",errorY); + dashboardtelemtry.addData("Error",totalError); + return (errorX <= Constants.AT_XY_TOLERANCE && errorY<= Constants.AT_XY_TOLERANCE); } public boolean at_x(double previousX, double currentX) @@ -331,15 +340,17 @@ public boolean at_y(double previousY, double currentY) public void goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { - double errorX = Math.abs(currentWaypoint.x - driveTrain.getXPosition()); - double errorY = Math.abs(currentWaypoint.y - driveTrain.getYPosition()); + driveTrain.xControl.setSetPoint(currentWaypoint.x); driveTrain.yControl.setSetPoint(currentWaypoint.y); - driveTrain.drive(driveTrain.xControl.calculate(),driveTrain.yControl.calculate()); + driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition())*Constants.AUTO_DRIVE_SPEED,driveTrain.yControl.calculate(driveTrain.getYPosition())*Constants.AUTO_DRIVE_SPEED); + dashboardtelemtry.addData("X Calc",driveTrain.xControl.calculate()); + dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); + dashboardtelemtry.addData("in goto",""); +dashboardtelemtry.update(); - double totalError = Math.sqrt(errorX*errorX + errorY*errorY); // pack.put("error:",totalError); - dashboardtelemtry.addData("Error",totalError); + // if ( totalError <= Constants.AT_XY_TOLERANCE) // { return true;} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index 5067e1e..4ae22fa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -14,7 +14,7 @@ public static final double DRIVE_PID_ERROR = 1.5; // inches - public static final double AUTO_DRIVE_SPEED = 0.3; + public static final double AUTO_DRIVE_SPEED = 0.2; public static final double AUTO_STEP_DELAY = 2.0; @@ -79,7 +79,7 @@ // // Tolerance Section public static final double JOYSTICK_TOLERANCE = 0.5; - public static final double AT_XY_TOLERANCE = 5; + public static final double AT_XY_TOLERANCE = 55.0; // Bruce:sept30: change these for use in elevator?kk @@ -92,6 +92,6 @@ public static double ODOMETER_X_OFFSET = 0;// uppdated after contact with gobilda support public static double ODOMETER_Y_OFFSET = 21.0; // auto constants - public static double AUTO_X_DISTANCE_ERROR = 10; - public static double AUTO_Y_DISTANCE_ERROR = 10; + public static double AUTO_X_DISTANCE_ERROR = 25; + public static double AUTO_Y_DISTANCE_ERROR = 25; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index b6bdb60..0b86f8f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -79,8 +79,8 @@ public DriveTrain2025(HardwareMap hwMap) public void init() { headingControl = new PIDController(0.02, 0.002, 0.000 ); headingControl.setTolerance(10);// was 3 increased to see if affects spinnning ..cbw - xControl = new PIDController(0.17, 0.0, 0.00);//FOR AUTO? - yControl = new PIDController(0.17, 0.0, 0.00);//For AUTO + xControl = new PIDController(0.0075, 0.0, 0.00);//FOR AUTO? + yControl = new PIDController(0.0075, 0.0, 0.00);//For AUTO // redundant as default is brake mode leftBackDrive.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); From 1e21ebdfe06a32da2d34c0747767f27bd20146a5 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 7 Feb 2025 23:05:11 -0700 Subject: [PATCH 089/129] Cleaned up the formating and made sure that each double had .0 after it --- .../ftc/teamcode/Mechanisms/Constants.java | 70 +++++++++---------- 1 file changed, 35 insertions(+), 35 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index bc93840..b4d91c0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -1,9 +1,6 @@ package org.firstinspires.ftc.teamcode.Mechanisms; -public class - - -Constants { +public class Constants { public static final boolean DASHBOARD_ENABLED = true; // this needs to be false for competitions!! public static final boolean TELEMETRY_ENABLED = true; public static final boolean JUST_TESTING=true; // will be used to insert temporary test code @@ -18,29 +15,31 @@ public static final double AUTO_STEP_DELAY = 2.0; - public static double HEADING_ERROR = 5; // Degrees... this now matches PID tolerance - // ========== Define Drive constants. Make them public so they CAN be used by the calling OpMode + public static double HEADING_ERROR = 5.0; // Degrees... this now matches PID tolerance + +// ========== Define Drive constants. Make them public so they CAN be used by the calling OpMode - public static final double FORWARD = 0; // north - public static final double BACK = 180; // south - public static final double RIGHT = -90; // east - public static final double LEFT = 90; //west - public static final double NORTH = 0; - public static final double SOUTH = 180; - public static final double EAST = -90; - public static final double WEST = 90; - public static final double NORTH_EAST = -45; // North East - public static final double SOUTH_EAST = -135; // South East - public static final double SOUTH_WEST = 135; // South West - public static final double NORTH_WEST = 45; // North West + public static final double FORWARD = 0.0; // north + public static final double BACK = 180.0; // south + public static final double RIGHT = -90.0; // east + public static final double LEFT = 90.0; //west + public static final double NORTH = 0.0; + public static final double SOUTH = 180.0; + public static final double EAST = -90.0; + public static final double WEST = 90.0; + public static final double NORTH_EAST = -45.0; // North East + public static final double SOUTH_EAST = -135.0; // South East + public static final double SOUTH_WEST = 135.0; // South West + public static final double NORTH_WEST = 45.0; // North West - // SERVO CONSTANTS // finger servo closed position +// =========== SERVO CONSTANTS // finger servo closed position public static final double STEP = 0.05; // used for manual increment of wrist servo /*public static final double LF_CLOSED = 0.3 ;// finger servo closed position public static final double LF_OPEN = 0.7 ; // open position public static final double RF_CLOSED = 0.6 ; public static final double RF_OPEN = 0.2 ; // open position*/ -// for elevator moves + +// =========== Elevator Constants public static final int ELEVATOR_START = 0; public static final int ELEVATOR_PICKUP_SPECIMEN = 100; public static final int ELEVATOR_HIGH_BAR = 700; // nov 22 @@ -55,7 +54,7 @@ public static final double ELEVATOR_POSITION_TOLERANCE = 15.0; public static final double ELEVATOR_POSITION_COEFFICIENT = 0.05; public static final int ELEVATOR_STUCK_COUNT = 5; - //Claw constants +// =========== Claw constants ====================== public static boolean CLAW_OPEN = true; public static boolean CLAW_CLOSED = false; public static final double SERVO_CLAW_OPEN = 0.2 ; @@ -65,32 +64,33 @@ public static final int ELBOW_START = 0; public static final double ELBOW_BOTTOM = 0.3; public static final double ELBOW_START_POSITION = 0.0; -// public static final double ELBOW_PICKUP_SAMPLE = 1.0; + // public static final double ELBOW_PICKUP_SAMPLE = 1.0; public static final double ELBOW_PICKUP_SPECIMEN = 0.4; - //Pickup specimen from the wall for CompBot (Tortuga) + // Pickup specimen from the wall for CompBot (Tortuga) public static final double ELBOW_PICKUP_SPECIMEN_WALL = 0.5; - //Pickup specimen from the wall for PracticeBot (Esio Trot) -// public static final double ELBOW_DEPOSIT_SPECIMEN_HIGH_BAR = 0.5; - //public static final double ELBOW_DEPOSIT_SAMPLE_LOW_BUCKET = 0.6; + // Pickup specimen from the wall for PracticeBot (Esio Trot) + // public static final double ELBOW_DEPOSIT_SPECIMEN_HIGH_BAR = 0.5; + // public static final double ELBOW_DEPOSIT_SAMPLE_LOW_BUCKET = 0.6; public static final double ELBOW_DEPOSIT_SAMPLE_HIGH_BUCKET = 0.67; public static int ELBOW_MAX = 3; public static int ELBOW_MIN = 0; // -// Tolerance Section +// =========== Tolerance Section ====================== public static final double JOYSTICK_TOLERANCE = 0.5; - public static final double AT_XY_TOLERANCE = 5; + public static final double AT_XY_TOLERANCE = 5.0; + + // Bruce:sept30: change these for use in elevator? + // public static final double ARM_RADIANS_PER_TICK = Math.PI/244.0; + // public static final double ARM_TICKS_PER_90DEG = 113; // CONFIRM #TICKS PER 90. + // public static final double DRIVE_PID_TOLERANCE = 1;//testing changed from .5 to 1 - // Bruce:sept30: change these for use in elevator?kk -// public static final double ARM_RADIANS_PER_TICK = Math.PI/244.0; -// public static final double ARM_TICKS_PER_90DEG = 113; // CONFIRM #TICKS PER 90. - // public static final double DRIVE_PID_TOLERANCE = 1;//testing changed from .5 to 1 - //odometer constants +// =========== odometer constants ====================== public static int ODOMETER_PRACTICE_MODE = 1; public static int ODOMETER_COMPETITION_MODE = 8; - public static double ODOMETER_X_OFFSET = 0;// uppdated after contact with gobilda support + public static double ODOMETER_X_OFFSET = 0;// updated after contact with gobilda support public static double ODOMETER_Y_OFFSET = 21.0; - // auto constants +// =========== auto constants ====================== public static double AUTO_X_DISTANCE_ERROR = 10; public static double AUTO_Y_DISTANCE_ERROR = 10; } From 6e455c09c3dfac695bad3d15279146ffe57367ce Mon Sep 17 00:00:00 2001 From: bruce Date: Fri, 7 Feb 2025 23:07:08 -0700 Subject: [PATCH 090/129] testing gotoXY_test.java --- .../firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index 15e156b..9923aa6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -33,8 +33,8 @@ public GoBildaPinpointDriver odometer; private PIDController headingControl = null; - private PIDController xControl = null; - private PIDController yControl = null; + public PIDController xControl = null; + public PIDController yControl = null; public double headingCorrection = 0; From 9bb645d28ea86831622ec98ffd7645910791e04a Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 7 Feb 2025 23:17:40 -0700 Subject: [PATCH 091/129] Added some comments to the waypoint Removed passing in previous waypoint, since it was not being used. Changed the variable name from currentWaypoint to targetWaypoint for clarity --- .../ftc/teamcode/Auto/New_Park.java | 56 +++++++++---------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java index 41e9e4d..e643f30 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java @@ -32,7 +32,7 @@ public void init_loop(){ telemetry.addData("Elevator State:",elevator.getState()); // This gets called when we have zero out and ready to hit play. if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { - telemetry.addData("Initialized",""); + telemetry.addData("Initialized:","Done"); } } public void init(){ @@ -71,7 +71,8 @@ public void init(){ this.wayPoints[1].y = 0.0; this.wayPoints[1].y_speed = 0.0; this.wayPoints[1].facing = Constants.NORTH; - //Done Waypoint 1 + // Done Waypoint 1 + // WayPoint 2 this.wayPoints[2] = new WayPoint(); this.wayPoints[2].x = 25.0; this.wayPoints[2].x_speed = 0.0; @@ -93,15 +94,15 @@ public void loop(){ data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Function Position", data); if (this.wayPoints[this.current_step] != null){ - telemetry.addData("At Waypoint",at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); - telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); - telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("At Waypoint",at_xy(this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step].y)); telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); } else { - telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); - telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); - telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y)); telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); } telemetry.addData("heading:","%.2f",driveTrain.heading); @@ -109,9 +110,9 @@ public void loop(){ //starting waypoint navigation switch (this.current_step) { case 1: // Move an inch from the wall - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + if (!this.at_xy(this.wayPoints[this.current_step])) { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + this.goto_xy(this.wayPoints[this.current_step]); } else { @@ -120,14 +121,13 @@ public void loop(){ } break; case 2: // Move over to Park - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + if (!this.at_xy(this.wayPoints[this.current_step])) { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + this.goto_xy(this.wayPoints[this.current_step]); } else { driveTrain.drive(0.0,0.0); - // this.claw.openClaw(); this.current_step++; } // this.current_step++; @@ -148,39 +148,39 @@ public void stop() { elevator.setElevatorState(0); super.stop(); } - public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + public boolean at_xy(WayPoint currentWaypoint) { - return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + return this.at_x(currentWaypoint.x) && this.at_y(currentWaypoint.y); } - public boolean at_x(double previousX, double currentX) + public boolean at_x(double targetX) { - return (Math.abs(currentX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + return (Math.abs(targetX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; } - public boolean at_y(double previousY, double currentY) + public boolean at_y(double targetY) { - return (Math.abs(currentY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + return (Math.abs(targetY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; } - public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + public boolean goto_xy(WayPoint targetWaypoint) { // Check to see if you are at y. If yes, then process x; // Process y direction - if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { - if (currentWaypoint.y >= previousWaypoint.y) { - this.driveTrain.drive(0.0, currentWaypoint.y_speed); + if (!this.at_y(targetWaypoint.y)) { + if (targetWaypoint.y >= driveTrain.getYPosition()) { + this.driveTrain.drive(0.0, targetWaypoint.y_speed); } else { - this.driveTrain.drive(0.0,-currentWaypoint.y_speed); + this.driveTrain.drive(0.0,-targetWaypoint.y_speed); } return false; } // Process x direction - if (!this.at_x(previousWaypoint.x,currentWaypoint.x)) { - if (currentWaypoint.x >= previousWaypoint.x) { - this.driveTrain.drive(currentWaypoint.x_speed, 0.0); + if (!this.at_x(targetWaypoint.x)) { + if (targetWaypoint.x >= driveTrain.getXPosition()) { + this.driveTrain.drive(targetWaypoint.x_speed, 0.0); } else { - this.driveTrain.drive(-currentWaypoint.x_speed, 0.0); + this.driveTrain.drive(-targetWaypoint.x_speed, 0.0); } return false; } From 11680eb5289995eb1cad23631997cadd0a7b30b1 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 7 Feb 2025 23:44:58 -0700 Subject: [PATCH 092/129] Removed passing in previous waypoint, since it was not being used. Changed the variable name from currentWaypoint to targetWaypoint for clarity --- .../ftc/teamcode/Auto/gotoXY_test.java | 56 +++++++++---------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java index bac9212..4688e36 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java @@ -163,24 +163,24 @@ public void loop(){ data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Function Position", data); if (this.wayPoints[this.current_step] != null){ - telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); - telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); - telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step].y)); telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); // ftc-dashboard telemetry - pack.put("At Waypoint", this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); - pack.put("At Waypoint X", this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); - pack.put("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + pack.put("At Waypoint", this.at_xy(this.wayPoints[this.current_step])); + pack.put("At Waypoint X", this.at_x(this.wayPoints[this.current_step].x)); + pack.put("At Waypoint Y", this.at_y(this.wayPoints[this.current_step].y)); } else { - telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); - telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); - telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y)); telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); // ftc-dashboard telemetry - pack.put("At Waypoint", this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); - pack.put("At Waypoint X", this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); - pack.put("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + pack.put("At Waypoint", this.at_xy(this.wayPoints[this.current_step-1])); + pack.put("At Waypoint X", this.at_x(this.wayPoints[this.current_step-1].x)); + pack.put("At Waypoint Y", this.at_y(this.wayPoints[this.current_step-1].y)); } telemetry.addData("heading:","%.2f",driveTrain.heading); telemetry.addData("Elevator State:",elevator.getState()); @@ -195,9 +195,9 @@ public void loop(){ switch (this.current_step) { // drive 2 inches form submersible case 1: - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + if (!this.at_xy(this.wayPoints[this.current_step])) { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + this.goto_xy(this.wayPoints[this.current_step]); } else { @@ -210,9 +210,9 @@ public void loop(){ // strafe to line up with basketr // elevator.setElevatorState(3); - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + if (!this.at_xy(this.wayPoints[this.current_step])) { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + this.goto_xy(this.wayPoints[this.current_step]); } else { @@ -224,9 +224,9 @@ public void loop(){ break; case 3://turn to SW driveTrain.setDirection(this.wayPoints[this.current_step].facing); - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + if (!this.at_xy(this.wayPoints[this.current_step])) { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + this.goto_xy(this.wayPoints[this.current_step]); } else { @@ -237,9 +237,9 @@ public void loop(){ break; case 4:// drive to basket at 135 heading - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + if (!this.at_xy(this.wayPoints[this.current_step])) { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + this.goto_xy(this.wayPoints[this.current_step]); } else { driveTrain.drive(0.0, 0.0); @@ -316,7 +316,7 @@ public void loop(){ // super.stop(); // } - public boolean at_xy(WayPoint previousWaypoint, WayPoint targetWaypoint) + public boolean at_xy(WayPoint targetWaypoint) { //return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); double errorX = Math.abs(targetWaypoint.x - driveTrain.getXPosition()); @@ -328,21 +328,21 @@ public boolean at_xy(WayPoint previousWaypoint, WayPoint targetWaypoint) return (errorX <= Constants.AT_XY_TOLERANCE && errorY<= Constants.AT_XY_TOLERANCE); } - public boolean at_x(double previousX, double currentX) + public boolean at_x(double targetX) { - return (Math.abs(currentX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + return (Math.abs(targetX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; } - public boolean at_y(double previousY, double currentY) + public boolean at_y(double targetY) { - return (Math.abs(currentY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + return (Math.abs(targetY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; } - public void goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { + public void goto_xy(WayPoint targetWaypoint) { - driveTrain.xControl.setSetPoint(currentWaypoint.x); - driveTrain.yControl.setSetPoint(currentWaypoint.y); + driveTrain.xControl.setSetPoint(targetWaypoint.x); + driveTrain.yControl.setSetPoint(targetWaypoint.y); driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition())*Constants.AUTO_DRIVE_SPEED,driveTrain.yControl.calculate(driveTrain.getYPosition())*Constants.AUTO_DRIVE_SPEED); dashboardtelemtry.addData("X Calc",driveTrain.xControl.calculate()); dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); From 26d1bd1cdadad0e55ce3e1b54032f4aa00518b40 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 7 Feb 2025 23:49:03 -0700 Subject: [PATCH 093/129] Added the configuration option to DriveTrain, so modification can be made from the web dashboard --- .../ftc/teamcode/Mechanisms/DriveTrain2025.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index 9923aa6..efd87f0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.Mechanisms; +import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.controller.PIDController; import com.arcrobotics.ftclib.drivebase.MecanumDrive; import com.arcrobotics.ftclib.hardware.SimpleServo; @@ -14,9 +15,8 @@ import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; import org.firstinspires.ftc.teamcode.Auto.PracticeSpecimen; -public class - -DriveTrain2025 { +@Config +public class DriveTrain2025 { // Driving Motors private final Motor leftFrontDrive; From bcb43ef162d8a58bef32c25dcb69ce43ed721a10 Mon Sep 17 00:00:00 2001 From: bruce Date: Sat, 8 Feb 2025 11:47:01 -0700 Subject: [PATCH 094/129] testing gotoxy: added tbleshooting messages, gains changed. --- .../ftc/teamcode/Auto/gotoXY_test.java | 18 +++++++++++------- .../firstinspires/ftc/teamcode/CompBot.java | 4 ++-- .../ftc/teamcode/Mechanisms/Constants.java | 3 +++ .../teamcode/Mechanisms/DriveTrain2025.java | 4 ++-- 4 files changed, 18 insertions(+), 11 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java index 4688e36..c309d66 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java @@ -197,12 +197,15 @@ public void loop(){ case 1: if (!this.at_xy(this.wayPoints[this.current_step])) { + dashboardtelemtry.addData("in CASE 1!!!!!",""); + dashboardtelemtry.update(); this.goto_xy(this.wayPoints[this.current_step]); } else { telemetry.addData("made to else",""); - driveTrain.drive(0.0,0.0); + //driveTrain.drive(0.0,0.0); + stop(); //this.current_step++; } break; @@ -310,11 +313,11 @@ public void loop(){ this.elevator.loop(); } -// @Override -// public void stop() { -// elevator.setElevatorState(0); -// super.stop(); -// } + @Override + public void stop() { + elevator.setElevatorState(0); + super.stop(); + } public boolean at_xy(WayPoint targetWaypoint) { @@ -325,7 +328,8 @@ public boolean at_xy(WayPoint targetWaypoint) dashboardtelemtry.addData("Error X",errorX); dashboardtelemtry.addData("Error Y",errorY); dashboardtelemtry.addData("Error",totalError); - return (errorX <= Constants.AT_XY_TOLERANCE && errorY<= Constants.AT_XY_TOLERANCE); + return (totalError <= Constants.AT_XY_TOLERANCE); + // return (errorX <= Constants.AT_XY_TOLERANCE && errorY <= Constants.AT_XY_TOLERANCE); } public boolean at_x(double targetX) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java index 8a94c23..253c8b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java @@ -97,12 +97,12 @@ public void init() { boolean manualElevator = false; elevator = new Elevator(hardwareMap);// already in DriveTrain2025 - driveTrain.init(); + driveTrain.init(); // commented out ,done in Auto elbow.init(); elevator.init(); this.elbowPosition = 0; // comment this out for competition and ensure it happens in auto code - driveTrain.odometer.resetPosAndIMU(); // comment out with Auto + driveTrain.odometer.resetPosAndIMU(); // comment out with Auto telemetry.addData("Status", "Initialized"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index 4ae22fa..35a61c3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -92,6 +92,9 @@ public static double ODOMETER_X_OFFSET = 0;// uppdated after contact with gobilda support public static double ODOMETER_Y_OFFSET = 21.0; // auto constants + public static double XPID_Kp = 0.007; + public static double YPID_Kp = 0.007; + public static double AUTO_X_DISTANCE_ERROR = 25; public static double AUTO_Y_DISTANCE_ERROR = 25; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index efd87f0..d09567a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -79,8 +79,8 @@ public DriveTrain2025(HardwareMap hwMap) public void init() { headingControl = new PIDController(0.02, 0.002, 0.000 ); headingControl.setTolerance(10.0);// was 3 increased to see if affects spinnning ..cbw - xControl = new PIDController(0.17, 0.0, 0.00);//FOR AUTO? - yControl = new PIDController(0.17, 0.0, 0.00);//For AUTO + xControl = new PIDController(Constants.XPID_Kp, 0.0, 0.00);//FOR AUTO? + yControl = new PIDController(Constants.YPID_Kp, 0.0, 0.00);//For AUTO // redundant as default is brake mode leftBackDrive.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); From f95226cf8efdd5300eeb3bea24054a602d759409 Mon Sep 17 00:00:00 2001 From: bruce Date: Sat, 8 Feb 2025 13:22:46 -0700 Subject: [PATCH 095/129] fixed turning: added "correction = 0 if within tolerance into drive train" --- .../ftc/teamcode/Auto/New_Park.java | 3 ++- .../ftc/teamcode/Auto/gotoXY_test.java | 25 +++++++++++-------- .../teamcode/Mechanisms/DriveTrain2025.java | 4 +++ 3 files changed, 20 insertions(+), 12 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java index e643f30..e0a4bb0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java @@ -76,7 +76,7 @@ public void init(){ this.wayPoints[2] = new WayPoint(); this.wayPoints[2].x = 25.0; this.wayPoints[2].x_speed = 0.0; - this.wayPoints[2].y = -1108.0; + this.wayPoints[2].y = -2108.0;// cbw changed from 1108 this.wayPoints[2].y_speed = 0.3; this.wayPoints[2].facing = Constants.NORTH; //Done Waypoint 2 @@ -139,6 +139,7 @@ public void loop(){ // Done Processing Waypoints // Process loop //elevator.setElevatorState(3); + stop();// put here for testing bw driveTrain.loop(); this.elevator.loop(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java index c309d66..49b13c0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java @@ -80,7 +80,7 @@ public void init(){ this.wayPoints[1] = new WayPoint(); this.wayPoints[1].x = 640.5; this.wayPoints[1].x_speed = 0.3; - this.wayPoints[1].y = 0.0; + this.wayPoints[1].y = 200.0; this.wayPoints[1].y_speed = 0.3; this.wayPoints[1].facing = Constants.NORTH; //Done Waypoint 1 @@ -197,15 +197,17 @@ public void loop(){ case 1: if (!this.at_xy(this.wayPoints[this.current_step])) { - dashboardtelemtry.addData("in CASE 1!!!!!",""); - dashboardtelemtry.update(); + //dashboardtelemtry.addData("in CASE 1!!!!!",""); + // dashboardtelemtry.update(); this.goto_xy(this.wayPoints[this.current_step]); } else { - telemetry.addData("made to else",""); - //driveTrain.drive(0.0,0.0); - stop(); + + dashboardtelemtry.addData("made to else",""); + dashboardtelemtry.update(); + driveTrain.drive(0.0,0.0); + stop();// redundant?? //this.current_step++; } break; @@ -327,9 +329,10 @@ public boolean at_xy(WayPoint targetWaypoint) double totalError = Math.sqrt(errorX*errorX + errorY*errorY); dashboardtelemtry.addData("Error X",errorX); dashboardtelemtry.addData("Error Y",errorY); - dashboardtelemtry.addData("Error",totalError); - return (totalError <= Constants.AT_XY_TOLERANCE); - // return (errorX <= Constants.AT_XY_TOLERANCE && errorY <= Constants.AT_XY_TOLERANCE); + dashboardtelemtry.addData("heading:",driveTrain.heading); + // dashboardtelemtry.addData("Error",totalError); + // return (totalError <= Constants.AT_XY_TOLERANCE); + return (errorX <= Constants.AT_XY_TOLERANCE && errorY <= Constants.AT_XY_TOLERANCE); } public boolean at_x(double targetX) @@ -348,8 +351,8 @@ public void goto_xy(WayPoint targetWaypoint) { driveTrain.xControl.setSetPoint(targetWaypoint.x); driveTrain.yControl.setSetPoint(targetWaypoint.y); driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition())*Constants.AUTO_DRIVE_SPEED,driveTrain.yControl.calculate(driveTrain.getYPosition())*Constants.AUTO_DRIVE_SPEED); - dashboardtelemtry.addData("X Calc",driveTrain.xControl.calculate()); - dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); + //dashboardtelemtry.addData("X Calc",driveTrain.xControl.calculate()); + // dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); dashboardtelemtry.addData("in goto",""); dashboardtelemtry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index d09567a..e8787c9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -125,6 +125,10 @@ public void loop(){ // PID controller for heading headingControl.setSetPoint(headingSetPoint); headingCorrection = -headingControl.calculate(heading);// confirm if (-) is needed. + if (headingCorrection <= Constants.HEADING_ERROR) + { headingCorrection = 0;} + + // need to send 0 correction if 'Happy" // test this code in Practice bot first /*if(autoEnabled && !atTarget()) { double xTargetSpeed = -xControl.calculate(getXPosition()); From f2ec357dd5149a55e2fdfa44ffca904dbcfe6df4 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sun, 9 Feb 2025 09:27:23 -0700 Subject: [PATCH 096/129] Implemented the new way of going to waypoints --- .../ftc/teamcode/Auto/New_Park.java | 58 +++++++++++-------- 1 file changed, 33 insertions(+), 25 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java index e0a4bb0..f135bb0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java @@ -163,33 +163,41 @@ public boolean at_y(double targetY) return (Math.abs(targetY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; } - public boolean goto_xy(WayPoint targetWaypoint) { + public void goto_xy(WayPoint targetWaypoint) { + // New Way to get there + driveTrain.xControl.setSetPoint(targetWaypoint.x); + driveTrain.yControl.setSetPoint(targetWaypoint.y); + driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition())*Constants.AUTO_DRIVE_SPEED,driveTrain.yControl.calculate(driveTrain.getYPosition())*Constants.AUTO_DRIVE_SPEED); + + // New Way End + // Old Way Start // Check to see if you are at y. If yes, then process x; // Process y direction - if (!this.at_y(targetWaypoint.y)) { - if (targetWaypoint.y >= driveTrain.getYPosition()) { - this.driveTrain.drive(0.0, targetWaypoint.y_speed); - } - else { - this.driveTrain.drive(0.0,-targetWaypoint.y_speed); - } - return false; - } - // Process x direction - if (!this.at_x(targetWaypoint.x)) { - if (targetWaypoint.x >= driveTrain.getXPosition()) { - this.driveTrain.drive(targetWaypoint.x_speed, 0.0); - } - else { - this.driveTrain.drive(-targetWaypoint.x_speed, 0.0); - } - return false; - } - else - { - this.driveTrain.drive(0.0,0.0); - } - return true; +// if (!this.at_y(targetWaypoint.y)) { +// if (targetWaypoint.y >= driveTrain.getYPosition()) { +// this.driveTrain.drive(0.0, targetWaypoint.y_speed); +// } +// else { +// this.driveTrain.drive(0.0,-targetWaypoint.y_speed); +// } +// return false; +// } +// // Process x direction +// if (!this.at_x(targetWaypoint.x)) { +// if (targetWaypoint.x >= driveTrain.getXPosition()) { +// this.driveTrain.drive(targetWaypoint.x_speed, 0.0); +// } +// else { +// this.driveTrain.drive(-targetWaypoint.x_speed, 0.0); +// } +// return false; +// } +// else +// { +// this.driveTrain.drive(0.0,0.0); +// } +// return true; + // End of Old Way } } From 841bcaf76d607343dda3487a43f95df2ede59688 Mon Sep 17 00:00:00 2001 From: bruce Date: Fri, 14 Feb 2025 16:34:57 -0700 Subject: [PATCH 097/129] corrected new park --- .../org/firstinspires/ftc/teamcode/Auto/New_Park.java | 2 +- .../ftc/teamcode/Mechanisms/DriveTrain2025.java | 9 +-------- 2 files changed, 2 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java index f135bb0..b3540fd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java @@ -76,7 +76,7 @@ public void init(){ this.wayPoints[2] = new WayPoint(); this.wayPoints[2].x = 25.0; this.wayPoints[2].x_speed = 0.0; - this.wayPoints[2].y = -2108.0;// cbw changed from 1108 + this.wayPoints[2].y = -1108.0; this.wayPoints[2].y_speed = 0.3; this.wayPoints[2].facing = Constants.NORTH; //Done Waypoint 2 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index e8787c9..9fb173a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -129,14 +129,7 @@ public void loop(){ { headingCorrection = 0;} // need to send 0 correction if 'Happy" -// test this code in Practice bot first - /*if(autoEnabled && !atTarget()) { - double xTargetSpeed = -xControl.calculate(getXPosition()); - double yTargetSpeed = -yControl.calculate(getYPosition()); - double targetSpeed = currentSpeed;// Math.sqrt(yTargetSpeed * yTargetSpeed + xTargetSpeed * xTargetSpeed); - xSpeed = xTargetSpeed * targetSpeed; - ySpeed = yTargetSpeed * targetSpeed; - }*/ + driveBase.driveFieldCentric( xSpeed,// strafe From 6911cf990d06e27b0add42c2c042ba32aea0b9f8 Mon Sep 17 00:00:00 2001 From: bruce Date: Fri, 14 Feb 2025 17:04:55 -0700 Subject: [PATCH 098/129] tuned gotoxy --- .../firstinspires/ftc/teamcode/Mechanisms/Constants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index 35a61c3..1602ae5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -14,7 +14,7 @@ public static final double DRIVE_PID_ERROR = 1.5; // inches - public static final double AUTO_DRIVE_SPEED = 0.2; + public static final double AUTO_DRIVE_SPEED = 0.6; public static final double AUTO_STEP_DELAY = 2.0; @@ -79,7 +79,7 @@ // // Tolerance Section public static final double JOYSTICK_TOLERANCE = 0.5; - public static final double AT_XY_TOLERANCE = 55.0; + public static final double AT_XY_TOLERANCE = 25.0; // Bruce:sept30: change these for use in elevator?kk @@ -92,8 +92,8 @@ public static double ODOMETER_X_OFFSET = 0;// uppdated after contact with gobilda support public static double ODOMETER_Y_OFFSET = 21.0; // auto constants - public static double XPID_Kp = 0.007; - public static double YPID_Kp = 0.007; + public static double XPID_Kp = 0.005; + public static double YPID_Kp = 0.005; public static double AUTO_X_DISTANCE_ERROR = 25; public static double AUTO_Y_DISTANCE_ERROR = 25; From d1d3522125003ff14d74f300f12edefc75355cb1 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 14 Feb 2025 17:22:18 -0700 Subject: [PATCH 099/129] Implemented the new way of going to waypoints --- .../ftc/teamcode/Auto/CompBasket.java | 119 ++++++++---------- 1 file changed, 49 insertions(+), 70 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java index d37e0c0..9b60604 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -5,6 +5,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; import org.firstinspires.ftc.teamcode.Mechanisms.Claw; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; @@ -15,12 +16,13 @@ import java.net.NoRouteToHostException; import java.util.Locale; -//@Autonomous(name = "CompBasket") +@Autonomous(name = "CompBasket") public class CompBasket extends BaseAutoComp{ private DriveTrain2025 driveTrain; private int current_step; private int total_waypoints = 20; WayPoint[] wayPoints; + CAITelemetry dashboardtelemtry; public void init_loop(){ driveTrain.odometer.update(); @@ -50,6 +52,7 @@ public void init(){ this.elevator.init(); this.elevator.setElevatorState(0); this.current_step = 1; + dashboardtelemtry = new CAITelemetry(telemetry); // Display Setup telemetry.addData("Sequence Step", this.current_step); Pose2D pos = driveTrain.odometer.getPosition(); @@ -66,29 +69,29 @@ public void init(){ this.wayPoints[0].facing = Constants.NORTH; // WayPoint 1 this.wayPoints[1] = new WayPoint(); - this.wayPoints[1].x = 0.0; + this.wayPoints[1].x = 711.0; this.wayPoints[1].x_speed = 0.3; - this.wayPoints[1].y = 711.0; + this.wayPoints[1].y = 0.0; this.wayPoints[1].y_speed = 0.3; this.wayPoints[1].facing = Constants.NORTH; //Done Waypoint 1 this.wayPoints[2] = new WayPoint(); - this.wayPoints[2].x = -293.0; + this.wayPoints[2].x = 494.0; this.wayPoints[2].x_speed = 0.3; - this.wayPoints[2].y = -494.0; + this.wayPoints[2].y = 293.0; this.wayPoints[2].y_speed = 0.3; this.wayPoints[2].facing = Constants.SOUTH_WEST; //Done Waypoint 2 this.wayPoints[3] = new WayPoint(); - this.wayPoints[3].x = -80.0; - this.wayPoints[3].y = -180.0; + this.wayPoints[3].x = 180.0; + this.wayPoints[3].y = 80.0; this.wayPoints[3].y_speed = 0.3; this.wayPoints[3].x_speed = 0.3; this.wayPoints[3].facing = Constants.SOUTH_WEST; //Done Waypoint 3 this.wayPoints[4] = new WayPoint(); - this.wayPoints[4].x = 270.0; - this.wayPoints[4].y = 250.0; + this.wayPoints[4].x = 250.0; + this.wayPoints[4].y = 270.0; this.wayPoints[4].y_speed = 0.3; this.wayPoints[4].x_speed = 0.3; this.wayPoints[4].facing = Constants.NORTH; @@ -150,15 +153,15 @@ public void loop(){ data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Function Position", data); if (this.wayPoints[this.current_step] != null){ - telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])); - telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step].x)); - telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y,this.wayPoints[this.current_step].y)); + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step].y)); telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); } else { - telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-2],this.wayPoints[this.current_step-1])); - telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-2].x,this.wayPoints[this.current_step-1].x)); - telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); + telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1])); + telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x)); + telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y)); telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); } telemetry.addData("heading:","%.2f",driveTrain.heading); @@ -167,9 +170,9 @@ public void loop(){ switch (this.current_step) { // drive 2 inches form submersible case 1: - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + if (!this.at_xy(this.wayPoints[this.current_step])) { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + this.goto_xy(this.wayPoints[this.current_step]); } else { @@ -181,9 +184,9 @@ public void loop(){ // Rotating and moving the elevator before moving driveTrain.setDirection(this.wayPoints[this.current_step].facing); elevator.setElevatorState(3); - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + if (!this.at_xy(this.wayPoints[this.current_step])) { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + this.goto_xy(this.wayPoints[this.current_step]); } else { @@ -195,9 +198,9 @@ public void loop(){ break; case 3: elevator.setElevatorState(3); - if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) + if (!this.at_xy(this.wayPoints[this.current_step])) { - this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); + this.goto_xy(this.wayPoints[this.current_step]); } else { @@ -275,64 +278,40 @@ public void loop(){ this.elevator.loop(); } - public boolean at_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) + public boolean at_xy(WayPoint targetWaypoint) { - return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + //return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + double errorX = Math.abs(targetWaypoint.x - driveTrain.getXPosition()); + double errorY = Math.abs(targetWaypoint.y - driveTrain.getYPosition()); + double totalError = Math.sqrt(errorX*errorX + errorY*errorY); + dashboardtelemtry.addData("Error X",errorX); + dashboardtelemtry.addData("Error Y",errorY); + dashboardtelemtry.addData("heading:",driveTrain.heading); + // dashboardtelemtry.addData("Error",totalError); + // return (totalError <= Constants.AT_XY_TOLERANCE); + return (errorX <= Constants.AT_XY_TOLERANCE && errorY <= Constants.AT_XY_TOLERANCE); } - public boolean at_x(double previousX, double currentX) + public boolean at_x(double targetX) { - if (currentX - previousX >= 0.0) - { - return driveTrain.getXPosition() >= currentX; - } - else { - return driveTrain.getXPosition() <= currentX; - } + return (Math.abs(targetX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; } - public boolean at_y(double previousY, double currentY) + public boolean at_y(double targetY) { - if (currentY - previousY >= 0.0) - { - return driveTrain.getYPosition() >= currentY; - } - else { - return driveTrain.getYPosition() <= currentY; - } + return (Math.abs(targetY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; } - public boolean goto_xy(WayPoint previousWaypoint, WayPoint currentWaypoint) { - // Check to see if you are at x. If yes, then process y; - // Process x direction - if (! this.at_x(previousWaypoint.x,currentWaypoint.x) ) { - if (currentWaypoint.x >= previousWaypoint.x) { - this.driveTrain.drive(0, currentWaypoint.x_speed); - } - else { - this.driveTrain.drive(0,-currentWaypoint.x_speed); - } - return false; - } - else - { - this.driveTrain.drive(0.0,0.0); - } - // Process y direction - if (!this.at_y(previousWaypoint.y,currentWaypoint.y)) { - if (currentWaypoint.y >= previousWaypoint.y) { - this.driveTrain.drive(currentWaypoint.y_speed, 0.0); - } - else { - this.driveTrain.drive(-currentWaypoint.y_speed, 0.0); - } - return false; - } - else - { - this.driveTrain.drive(0.0,0.0); - } - return true; + public void goto_xy(WayPoint targetWaypoint) { + + + driveTrain.xControl.setSetPoint(targetWaypoint.x); + driveTrain.yControl.setSetPoint(targetWaypoint.y); + driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition())*Constants.AUTO_DRIVE_SPEED,driveTrain.yControl.calculate(driveTrain.getYPosition())*Constants.AUTO_DRIVE_SPEED); + //dashboardtelemtry.addData("X Calc",driveTrain.xControl.calculate()); + // dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); + dashboardtelemtry.addData("in goto",""); + dashboardtelemtry.update(); } } From cfa1ac562c85bb900a15763188041424f526037c Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 14 Feb 2025 19:14:08 -0700 Subject: [PATCH 100/129] Implemented the new way of going to waypoints --- .../ftc/teamcode/Auto/CompBasket.java | 170 ++++++++++-------- .../ftc/teamcode/Auto/New_Park.java | 3 + .../ftc/teamcode/Auto/gotoXY_test.java | 18 +- .../firstinspires/ftc/teamcode/CompBot.java | 1 + .../ftc/teamcode/Mechanisms/Constants.java | 2 +- .../teamcode/Mechanisms/DriveTrain2025.java | 6 +- 6 files changed, 113 insertions(+), 87 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java index 9b60604..9762a4d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -17,7 +17,7 @@ import java.util.Locale; @Autonomous(name = "CompBasket") -public class CompBasket extends BaseAutoComp{ +public class CompBasket extends BaseAutoComp { private DriveTrain2025 driveTrain; private int current_step; private int total_waypoints = 20; @@ -35,11 +35,11 @@ public void init_loop(){ telemetry.addData("Function Position", data); telemetry.addData("Elevator State:",elevator.getState()); // This gets called when we have zero out and ready to hit play. - if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { - telemetry.addData("Initialized",""); - } - } - public void init(){ +// if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { +// telemetry.addData("Initialized",""); +// } + } + public void init() { driveTrain = new DriveTrain2025(hardwareMap); driveTrain.init(); driveTrain.odometer.resetPosAndIMU(); @@ -60,7 +60,7 @@ public void init(){ telemetry.addData("Position", data); data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Function Position", data); - telemetry.addData("Elevator State:",elevator.getState()); + telemetry.addData("Elevator State:", elevator.getState()); //Waypoint setup this.wayPoints = new WayPoint[this.total_waypoints + 1]; this.wayPoints[0] = new WayPoint(); @@ -69,16 +69,16 @@ public void init(){ this.wayPoints[0].facing = Constants.NORTH; // WayPoint 1 this.wayPoints[1] = new WayPoint(); - this.wayPoints[1].x = 711.0; + this.wayPoints[1].x = 500.0; this.wayPoints[1].x_speed = 0.3; - this.wayPoints[1].y = 0.0; + this.wayPoints[1].y = 1.0; this.wayPoints[1].y_speed = 0.3; this.wayPoints[1].facing = Constants.NORTH; //Done Waypoint 1 this.wayPoints[2] = new WayPoint(); - this.wayPoints[2].x = 494.0; + this.wayPoints[2].x = 500.0; this.wayPoints[2].x_speed = 0.3; - this.wayPoints[2].y = 293.0; + this.wayPoints[2].y = 400.0; this.wayPoints[2].y_speed = 0.3; this.wayPoints[2].facing = Constants.SOUTH_WEST; //Done Waypoint 2 @@ -141,80 +141,89 @@ public void init(){ }//init + @Override - public void loop(){ + public void loop() { driveTrain.odometer.update(); - + dashboardtelemtry.update(); // Send information to the display telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Position", data); data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Function Position", data); - if (this.wayPoints[this.current_step] != null){ - telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step])); - telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step].x)); - telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step].y)); - telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step].x,this.wayPoints[this.current_step].y); - } - else { - telemetry.addData("At Waypoint",this.at_xy(this.wayPoints[this.current_step-1])); - telemetry.addData("At Waypoint X",this.at_x(this.wayPoints[this.current_step-1].x)); - telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-1].y)); - telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); + if (this.wayPoints[this.current_step] != null) { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step].x, this.wayPoints[this.current_step].y); + // ftc-dashboard telemetry + + } else { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step - 1])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step - 1].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step - 1].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step - 1].x, this.wayPoints[this.current_step - 1].y); + // ftc-dashboard telemetry + } - telemetry.addData("heading:","%.2f",driveTrain.heading); - telemetry.addData("Elevator State:",elevator.getState()); + telemetry.addData("heading:", "%.2f", driveTrain.heading); + telemetry.addData("Elevator State:", elevator.getState()); + // ftc-dashboard telemetry + dashboardtelemtry.addData("Step:",this.current_step); + + //starting waypoint navigation switch (this.current_step) { // drive 2 inches form submersible case 1: - if (!this.at_xy(this.wayPoints[this.current_step])) - { + if (!this.at_xy(this.wayPoints[this.current_step])) { + //dashboardtelemtry.addData("in CASE 1!!!!!",""); + // dashboardtelemtry.update(); this.goto_xy(this.wayPoints[this.current_step]); - } - else - { - driveTrain.drive(0.0,0.0); + } else { + + dashboardtelemtry.addData("made to else", ""); + dashboardtelemtry.update(); + driveTrain.drive(0.0, 0.0); this.current_step++; } break; case 2: - // Rotating and moving the elevator before moving - driveTrain.setDirection(this.wayPoints[this.current_step].facing); + // strafe to line up with basketr + elevator.setElevatorState(3); - if (!this.at_xy(this.wayPoints[this.current_step])) - { + if (!this.at_xy(this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step]); - } - else - { - driveTrain.drive(0.0,0.0); - // this.claw.openClaw(); + } else { + driveTrain.drive(0.0, 0.0); + // this.claw.openClaw(); this.current_step++; } - this.current_step++;// needed?? + break; - case 3: - elevator.setElevatorState(3); - if (!this.at_xy(this.wayPoints[this.current_step])) - { + case 3://turn to SW + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + elevator.setElevatorState(3); + this.current_step++; } - else - { - driveTrain.drive(0.0,0.0); - this.claw.openClaw(); + break; + case 4:// drive to basket at 135 heading + + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + this.current_step++; } break; -// case 4: -// elevator.setElevatorState(Constants.ELEVATOR_START); -// this.claw.closeClaw(); -// driveTrain.setDirection(this.wayPoints[this.current_step].facing); -// this.current_step++; -// break; // case 5: // if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) // { @@ -268,37 +277,40 @@ public void loop(){ // } // break; default: + elevator.setElevatorState(3); break; } // Done Processing Waypoints - this.driveTrain.drive(0.0,0.0); - - // Process loops + // Process loop + //elevator.setElevatorState(3); driveTrain.loop(); this.elevator.loop(); } - public boolean at_xy(WayPoint targetWaypoint) - { + @Override + public void stop() { + elevator.setElevatorState(0); + super.stop(); + } + + public boolean at_xy(WayPoint targetWaypoint) { //return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); double errorX = Math.abs(targetWaypoint.x - driveTrain.getXPosition()); double errorY = Math.abs(targetWaypoint.y - driveTrain.getYPosition()); - double totalError = Math.sqrt(errorX*errorX + errorY*errorY); - dashboardtelemtry.addData("Error X",errorX); - dashboardtelemtry.addData("Error Y",errorY); - dashboardtelemtry.addData("heading:",driveTrain.heading); + double totalError = Math.sqrt(errorX * errorX + errorY * errorY); + dashboardtelemtry.addData("Error X", errorX); + dashboardtelemtry.addData("Error Y", errorY); + dashboardtelemtry.addData("heading:", driveTrain.heading); // dashboardtelemtry.addData("Error",totalError); // return (totalError <= Constants.AT_XY_TOLERANCE); return (errorX <= Constants.AT_XY_TOLERANCE && errorY <= Constants.AT_XY_TOLERANCE); } - public boolean at_x(double targetX) - { + public boolean at_x(double targetX) { return (Math.abs(targetX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; } - public boolean at_y(double targetY) - { + public boolean at_y(double targetY) { return (Math.abs(targetY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; } @@ -307,11 +319,19 @@ public void goto_xy(WayPoint targetWaypoint) { driveTrain.xControl.setSetPoint(targetWaypoint.x); driveTrain.yControl.setSetPoint(targetWaypoint.y); - driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition())*Constants.AUTO_DRIVE_SPEED,driveTrain.yControl.calculate(driveTrain.getYPosition())*Constants.AUTO_DRIVE_SPEED); - //dashboardtelemtry.addData("X Calc",driveTrain.xControl.calculate()); - // dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); - dashboardtelemtry.addData("in goto",""); + driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition()) * Constants.AUTO_DRIVE_SPEED, driveTrain.yControl.calculate(driveTrain.getYPosition()) * Constants.AUTO_DRIVE_SPEED); +// dashboardtelemtry.addData("X Calc", driveTrain.xControl.calculate()); +// dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); +// dashboardtelemtry.addData("in goto", ""); dashboardtelemtry.update(); - } -} + // pack.put("error:",totalError); + + +// if ( totalError <= Constants.AT_XY_TOLERANCE) +// { return true;} +// else +// {return false;} + return; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java index b3540fd..f111a87 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java @@ -106,10 +106,13 @@ public void loop(){ telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); } telemetry.addData("heading:","%.2f",driveTrain.heading); + telemetry.addData("heading error:","%.2f",driveTrain.headingError); + telemetry.addData("heading correction:","%.2f",driveTrain.headingCorrection); telemetry.addData("Elevator State:",elevator.getState()); //starting waypoint navigation switch (this.current_step) { case 1: // Move an inch from the wall + driveTrain.setFacing(this.wayPoints[this.current_step].facing); if (!this.at_xy(this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step]); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java index 49b13c0..db45cf4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java @@ -78,22 +78,22 @@ public void init(){ this.wayPoints[0].facing = Constants.NORTH; // WayPoint 1 this.wayPoints[1] = new WayPoint(); - this.wayPoints[1].x = 640.5; + this.wayPoints[1].x = 711; this.wayPoints[1].x_speed = 0.3; - this.wayPoints[1].y = 200.0; + this.wayPoints[1].y = 0.0; this.wayPoints[1].y_speed = 0.3; this.wayPoints[1].facing = Constants.NORTH; //Done Waypoint 1 this.wayPoints[2] = new WayPoint(); - this.wayPoints[2].x = 640.5; + this.wayPoints[2].x = 494.0; this.wayPoints[2].x_speed = 0.3; - this.wayPoints[2].y = 100.0; + this.wayPoints[2].y = 293.0; this.wayPoints[2].y_speed = 0.3; this.wayPoints[2].facing = Constants.NORTH; //Done Waypoint 2 this.wayPoints[3] = new WayPoint(); - this.wayPoints[3].x = 640.0; - this.wayPoints[3].y = 100.0; + this.wayPoints[3].x = 180.0; + this.wayPoints[3].y = 80.0; this.wayPoints[3].y_speed = 0.3; this.wayPoints[3].x_speed = 0.3; this.wayPoints[3].facing = Constants.SOUTH_WEST; @@ -351,10 +351,10 @@ public void goto_xy(WayPoint targetWaypoint) { driveTrain.xControl.setSetPoint(targetWaypoint.x); driveTrain.yControl.setSetPoint(targetWaypoint.y); driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition())*Constants.AUTO_DRIVE_SPEED,driveTrain.yControl.calculate(driveTrain.getYPosition())*Constants.AUTO_DRIVE_SPEED); - //dashboardtelemtry.addData("X Calc",driveTrain.xControl.calculate()); - // dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); + dashboardtelemtry.addData("X Calc",driveTrain.xControl.calculate()); + dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); dashboardtelemtry.addData("in goto",""); -dashboardtelemtry.update(); + dashboardtelemtry.update(); // pack.put("error:",totalError); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java index 253c8b2..5a61895 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java @@ -212,6 +212,7 @@ public void loop() { telemetry.addData("Elevator State:","%d",elevatorPosition); telemetry.addData("Claw State:","%s",claw.printClaw()); telemetry.addData("Elbow Position:","%s",this.elbowPositionDescriptions[elbowPosition]); + telemetry.addData("Heading Correction",driveTrain.headingCorrection); // telemetry.addData("Claw Position",(); telemetry.addData("Elbow Position:","%2.4f",elbow.getElbowState()); //telemetry.addData("elevator manual/auto","%2.4f",elevator.manualElevator); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index 1602ae5..3f18326 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -18,7 +18,7 @@ public static final double AUTO_STEP_DELAY = 2.0; - public static double HEADING_ERROR = 5; // Degrees... this now matches PID tolerance + public static double HEADING_ERROR = 10.0; // Degrees... this now matches PID tolerance // ========== Define Drive constants. Make them public so they CAN be used by the calling OpMode public static final double FORWARD = 0; // north diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index 9fb173a..5766d71 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -49,6 +49,7 @@ public class DriveTrain2025 { private double currentYTarget = 0; double xSpeed = 0; double ySpeed = 0; + public double headingError; public DriveTrain2025(HardwareMap hwMap) { @@ -125,7 +126,8 @@ public void loop(){ // PID controller for heading headingControl.setSetPoint(headingSetPoint); headingCorrection = -headingControl.calculate(heading);// confirm if (-) is needed. - if (headingCorrection <= Constants.HEADING_ERROR) + headingError = Math.abs(headingSetPoint - heading); + if (headingError <= Constants.HEADING_ERROR) { headingCorrection = 0;} // need to send 0 correction if 'Happy" @@ -149,7 +151,7 @@ public void loop(){ public void setDirection(double newHeading) { headingSetPoint = newHeading; } - + public void setFacing(double newHeading) { headingSetPoint = newHeading; } public boolean onHeading() { return Math.abs(heading - headingSetPoint) < Constants.HEADING_ERROR; } From 5eeee36f212ce4c1ca8342eab94aa84718c9e89d Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 14 Feb 2025 19:30:12 -0700 Subject: [PATCH 101/129] Fix turning by checking if we are done with auto. If done, don't call the loop. --- .../org/firstinspires/ftc/teamcode/Auto/New_Park.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java index f111a87..5ca0372 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java @@ -17,6 +17,7 @@ public class New_Park extends BaseAutoComp{ private DriveTrain2025 driveTrain; private int current_step; + private boolean autodone = false; private int total_waypoints = 20; WayPoint[] wayPoints; @@ -137,14 +138,17 @@ public void loop(){ break; default: driveTrain.drive(0.0,0.0); + autodone = true; break; } // Done Processing Waypoints // Process loop //elevator.setElevatorState(3); stop();// put here for testing bw - driveTrain.loop(); - this.elevator.loop(); + if (!autodone) { + driveTrain.loop(); + this.elevator.loop(); + } } @Override From 54bbe9710df0b85cfa807103abe0f43b8a209adf Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 14 Feb 2025 20:52:00 -0700 Subject: [PATCH 102/129] Fix turning by checking if we are done with auto. If done, don't call the loop. --- .../ftc/teamcode/Auto/CompBasket.java | 22 ++++++++----------- .../ftc/teamcode/Auto/gotoXY_test.java | 9 +++----- .../ftc/teamcode/Mechanisms/Constants.java | 4 ++-- .../teamcode/Mechanisms/DriveTrain2025.java | 2 -- 4 files changed, 14 insertions(+), 23 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java index 9762a4d..3bb5f1d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -69,7 +69,7 @@ public void init() { this.wayPoints[0].facing = Constants.NORTH; // WayPoint 1 this.wayPoints[1] = new WayPoint(); - this.wayPoints[1].x = 500.0; + this.wayPoints[1].x = 400.0; this.wayPoints[1].x_speed = 0.3; this.wayPoints[1].y = 1.0; this.wayPoints[1].y_speed = 0.3; @@ -90,11 +90,11 @@ public void init() { this.wayPoints[3].facing = Constants.SOUTH_WEST; //Done Waypoint 3 this.wayPoints[4] = new WayPoint(); - this.wayPoints[4].x = 250.0; - this.wayPoints[4].y = 270.0; + this.wayPoints[4].x = -5.0; + this.wayPoints[4].y = 645.0; this.wayPoints[4].y_speed = 0.3; this.wayPoints[4].x_speed = 0.3; - this.wayPoints[4].facing = Constants.NORTH; + this.wayPoints[4].facing = Constants.SOUTH_WEST; //Done Waypoint 4 this.wayPoints[5] = new WayPoint(); this.wayPoints[5].x = 445.0; @@ -180,8 +180,6 @@ public void loop() { // drive 2 inches form submersible case 1: if (!this.at_xy(this.wayPoints[this.current_step])) { - //dashboardtelemtry.addData("in CASE 1!!!!!",""); - // dashboardtelemtry.update(); this.goto_xy(this.wayPoints[this.current_step]); } else { @@ -194,7 +192,6 @@ public void loop() { case 2: // strafe to line up with basketr - elevator.setElevatorState(3); if (!this.at_xy(this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step]); } else { @@ -202,7 +199,6 @@ public void loop() { // this.claw.openClaw(); this.current_step++; } - break; case 3://turn to SW driveTrain.setFacing(this.wayPoints[this.current_step].facing); @@ -215,12 +211,13 @@ public void loop() { } break; case 4:// drive to basket at 135 heading - + elevator.setElevatorState(3); if (!this.at_xy(this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step]); } else { driveTrain.drive(0.0, 0.0); - + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + this.claw.openClaw(); this.current_step++; } break; @@ -320,9 +317,8 @@ public void goto_xy(WayPoint targetWaypoint) { driveTrain.xControl.setSetPoint(targetWaypoint.x); driveTrain.yControl.setSetPoint(targetWaypoint.y); driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition()) * Constants.AUTO_DRIVE_SPEED, driveTrain.yControl.calculate(driveTrain.getYPosition()) * Constants.AUTO_DRIVE_SPEED); -// dashboardtelemtry.addData("X Calc", driveTrain.xControl.calculate()); -// dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); -// dashboardtelemtry.addData("in goto", ""); + dashboardtelemtry.addData("X Calc", driveTrain.xControl.calculate()); + dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); dashboardtelemtry.update(); // pack.put("error:",totalError); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java index db45cf4..0f13e14 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java @@ -203,12 +203,9 @@ public void loop(){ } else { - - dashboardtelemtry.addData("made to else",""); dashboardtelemtry.update(); driveTrain.drive(0.0,0.0); - stop();// redundant?? - //this.current_step++; + this.current_step++; } break; case 2: @@ -241,7 +238,7 @@ public void loop(){ } break; case 4:// drive to basket at 135 heading - + stop(); if (!this.at_xy(this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step]); @@ -353,7 +350,7 @@ public void goto_xy(WayPoint targetWaypoint) { driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition())*Constants.AUTO_DRIVE_SPEED,driveTrain.yControl.calculate(driveTrain.getYPosition())*Constants.AUTO_DRIVE_SPEED); dashboardtelemtry.addData("X Calc",driveTrain.xControl.calculate()); dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); - dashboardtelemtry.addData("in goto",""); + dashboardtelemtry.addData("Step", this.current_step); dashboardtelemtry.update(); // pack.put("error:",totalError); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index 3f18326..6220f1a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -14,7 +14,7 @@ public static final double DRIVE_PID_ERROR = 1.5; // inches - public static final double AUTO_DRIVE_SPEED = 0.6; + public static final double AUTO_DRIVE_SPEED = 0.5; public static final double AUTO_STEP_DELAY = 2.0; @@ -93,7 +93,7 @@ public static double ODOMETER_Y_OFFSET = 21.0; // auto constants public static double XPID_Kp = 0.005; - public static double YPID_Kp = 0.005; + public static double YPID_Kp = 0.006; public static double AUTO_X_DISTANCE_ERROR = 25; public static double AUTO_Y_DISTANCE_ERROR = 25; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index 5766d71..4e95506 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -127,8 +127,6 @@ public void loop(){ headingControl.setSetPoint(headingSetPoint); headingCorrection = -headingControl.calculate(heading);// confirm if (-) is needed. headingError = Math.abs(headingSetPoint - heading); - if (headingError <= Constants.HEADING_ERROR) - { headingCorrection = 0;} // need to send 0 correction if 'Happy" From 209e58c099a1c785cdd48ce15ba4ee0d38f93361 Mon Sep 17 00:00:00 2001 From: bruce Date: Sat, 15 Feb 2025 13:32:44 -0700 Subject: [PATCH 103/129] trouble shooting auto spin --- .../ftc/teamcode/Auto/CompBasket.java | 4 +- .../ftc/teamcode/Auto/Do_Nothing.java | 6 ++- .../ftc/teamcode/Auto/gotoXY_test.java | 31 ++++++++----- .../firstinspires/ftc/teamcode/CompBot.java | 2 +- .../ftc/teamcode/Mechanisms/Constants.java | 4 +- .../teamcode/Mechanisms/DriveTrain2025.java | 43 ++++++++++--------- .../Mechanisms/PracticeDriveTrain2025.java | 19 +------- 7 files changed, 55 insertions(+), 54 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java index 3bb5f1d..54bf826 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -294,10 +294,12 @@ public boolean at_xy(WayPoint targetWaypoint) { //return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); double errorX = Math.abs(targetWaypoint.x - driveTrain.getXPosition()); double errorY = Math.abs(targetWaypoint.y - driveTrain.getYPosition()); - double totalError = Math.sqrt(errorX * errorX + errorY * errorY); + //double totalError = Math.sqrt(errorX * errorX + errorY * errorY); dashboardtelemtry.addData("Error X", errorX); dashboardtelemtry.addData("Error Y", errorY); dashboardtelemtry.addData("heading:", driveTrain.heading); + dashboardtelemtry.addData("heading error",driveTrain.headingError); + dashboardtelemtry.addData("heading setpoint",driveTrain.headingSetPoint); // dashboardtelemtry.addData("Error",totalError); // return (totalError <= Constants.AT_XY_TOLERANCE); return (errorX <= Constants.AT_XY_TOLERANCE && errorY <= Constants.AT_XY_TOLERANCE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Do_Nothing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Do_Nothing.java index 0eca10e..691331c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Do_Nothing.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Do_Nothing.java @@ -88,6 +88,7 @@ public void loop(){ telemetry.addData("Sequence Step", this.current_step); Pose2D pos = driveTrain.odometer.getPosition(); + double local_heading = pos.getHeading(AngleUnit.DEGREES); // String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); // telemetry.addData("Position", data); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); @@ -104,10 +105,11 @@ public void loop(){ telemetry.addData("At Waypoint Y",this.at_y(this.wayPoints[this.current_step-2].y,this.wayPoints[this.current_step-1].y)); telemetry.addData("Current Waypoint","X: %.3f, Y %.3f",this.wayPoints[this.current_step-1].x,this.wayPoints[this.current_step-1].y); } - telemetry.addData("heading:","%.2f",driveTrain.heading); - telemetry.addData("Elevator State:",elevator.getState()); + telemetry.addData("Elevator State:",elevator.getState()); + telemetry.addData("heading:",local_heading); } + //end of loop @Override public void stop() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java index 0f13e14..f2e885d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java @@ -152,6 +152,7 @@ public void init(){ }//init @Override public void loop(){ + // driveTrain.loop(); driveTrain.odometer.update(); dashboardtelemtry.update(); // Send information to the display @@ -189,24 +190,33 @@ public void loop(){ pack.put("X distance:", driveTrain.getXPosition()); pack.put("Y distance:", driveTrain.getYPosition()); + dashboardtelemtry.addData("headingError", driveTrain.headingError); + dashboardtelemtry.update(); //starting waypoint navigation switch (this.current_step) { // drive 2 inches form submersible case 1: - if (!this.at_xy(this.wayPoints[this.current_step])) + driveTrain.setFacing(Constants.WEST); + if (!driveTrain.onHeading()) { - //dashboardtelemtry.addData("in CASE 1!!!!!",""); - // dashboardtelemtry.update(); - this.goto_xy(this.wayPoints[this.current_step]); - } - else - { - dashboardtelemtry.update(); - driveTrain.drive(0.0,0.0); - this.current_step++; + driveTrain.setFacing(Constants.WEST); } + +// if (!this.at_xy(this.wayPoints[this.current_step])) +// { +// //dashboardtelemtry.addData("in CASE 1!!!!!",""); +// // dashboardtelemtry.update(); +// this.goto_xy(this.wayPoints[this.current_step]); +// } +// else +// { +// dashboardtelemtry.update(); +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } + break; case 2: // strafe to line up with basketr @@ -351,6 +361,7 @@ public void goto_xy(WayPoint targetWaypoint) { dashboardtelemtry.addData("X Calc",driveTrain.xControl.calculate()); dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); dashboardtelemtry.addData("Step", this.current_step); + dashboardtelemtry.addData("headingError", driveTrain.headingError); dashboardtelemtry.update(); // pack.put("error:",totalError); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java index 5a61895..a23db3f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompBot.java @@ -200,7 +200,7 @@ public void loop() { claw.switchClaw(); } // Emergency Reset - if((driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.0) && (driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.0)) + if((driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.3) && (driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.3)) { driveTrain.odometer.resetPosAndIMU(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index 6220f1a..c4cf617 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -14,11 +14,11 @@ public static final double DRIVE_PID_ERROR = 1.5; // inches - public static final double AUTO_DRIVE_SPEED = 0.5; + public static final double AUTO_DRIVE_SPEED = 0.3; public static final double AUTO_STEP_DELAY = 2.0; - public static double HEADING_ERROR = 10.0; // Degrees... this now matches PID tolerance + public static double HEADING_ERROR_Tolerance = 15.0; // Degrees... this now matches PID tolerance // ========== Define Drive constants. Make them public so they CAN be used by the calling OpMode public static final double FORWARD = 0; // north diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index 4e95506..ea55f5c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -6,14 +6,12 @@ import com.arcrobotics.ftclib.drivebase.MecanumDrive; import com.arcrobotics.ftclib.hardware.SimpleServo; import com.arcrobotics.ftclib.hardware.motors.Motor; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; -import org.firstinspires.ftc.teamcode.Auto.PracticeSpecimen; @Config public class DriveTrain2025 { @@ -25,7 +23,6 @@ public class DriveTrain2025 { private final Motor rightBackDrive; - //servos private final SimpleServo claw; private final SimpleServo elbow; @@ -51,8 +48,7 @@ public class DriveTrain2025 { double ySpeed = 0; public double headingError; - public DriveTrain2025(HardwareMap hwMap) - { + public DriveTrain2025(HardwareMap hwMap) { this.hwMap = hwMap; // Define and Initialize Motors (note: need to use reference to actual OpMode). @@ -62,24 +58,24 @@ public DriveTrain2025(HardwareMap hwMap) leftBackDrive = new Motor(hwMap, "left_back_drive"); // 2 rightBackDrive = new Motor(hwMap, "right_back_drive"); // 3 //elevator = new Motor(hwMap,"elevator"); - claw = new SimpleServo(hwMap,"claw",0,1); // expansion hub port 0 - elbow = new SimpleServo(hwMap,"elbow",0,1); // expansion hub port 1 + claw = new SimpleServo(hwMap, "claw", 0, 1); // expansion hub port 0 + elbow = new SimpleServo(hwMap, "elbow", 0, 1); // expansion hub port 1 //elevator.resetEncoder(); - // xPod = new Motor(hwMap, "x-pod"); // 3 - // yPod = new Motor(hwMap, "y-pod"); // 2 - + // xPod = new Motor(hwMap, "x-pod"); // 3 + // yPod = new Motor(hwMap, "y-pod"); // 2 - driveBase = new MecanumDrive(leftFrontDrive,rightFrontDrive,leftBackDrive,rightBackDrive); + driveBase = new MecanumDrive(leftFrontDrive, rightFrontDrive, leftBackDrive, rightBackDrive); } + public void init() { - headingControl = new PIDController(0.02, 0.002, 0.000 ); - headingControl.setTolerance(10.0);// was 3 increased to see if affects spinnning ..cbw + headingControl = new PIDController(0.025, 0.0, 0.000); + headingControl.setTolerance(Constants.HEADING_ERROR_Tolerance);// was 3 increased to see if affects spinnning ..cbw xControl = new PIDController(Constants.XPID_Kp, 0.0, 0.00);//FOR AUTO? yControl = new PIDController(Constants.YPID_Kp, 0.0, 0.00);//For AUTO @@ -94,8 +90,8 @@ public void init() { leftFrontDrive.setInverted(true); leftBackDrive.setInverted(true); // odometer initializing -- this should go into PracticeDriveTrain2025?? - odometer = hwMap.get(GoBildaPinpointDriver.class,"xy-cord"); - odometer.setOffsets(Constants.ODOMETER_X_OFFSET,Constants.ODOMETER_Y_OFFSET); + odometer = hwMap.get(GoBildaPinpointDriver.class, "xy-cord"); + odometer.setOffsets(Constants.ODOMETER_X_OFFSET, Constants.ODOMETER_Y_OFFSET); odometer.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD); odometer.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); @@ -105,20 +101,20 @@ public void init() { public void start() { } - public void loop(){ + + public void loop() { //heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); odometer.update(); // needs to be called once per loop Pose2D pos = odometer.getPosition(); heading = pos.getHeading(AngleUnit.DEGREES); // Because it's a double, can't check for exactly 180, so we check if it's almost 180 in either direction. - if (Math.abs(Math.abs(headingSetPoint) - 180.0) < Constants.HEADING_ERROR) { + if (Math.abs(Math.abs(headingSetPoint) - 180.0) < Constants.HEADING_ERROR_Tolerance) { // "south" is special because it's around the 180/-180 toggle-point // Change set-point between 180/-180 depending on which is closer. if (heading < 0.0) { headingSetPoint = -180; - } - else { + } else { headingSetPoint = 180; } } @@ -127,6 +123,10 @@ public void loop(){ headingControl.setSetPoint(headingSetPoint); headingCorrection = -headingControl.calculate(heading);// confirm if (-) is needed. headingError = Math.abs(headingSetPoint - heading); +//temporary test code + if (headingError <= Constants.HEADING_ERROR_Tolerance) { + headingCorrection = 0; + } // need to send 0 correction if 'Happy" @@ -137,7 +137,7 @@ public void loop(){ headingCorrection,// turn heading,// heading false); - } + }// end of loop() // public boolean atTarget() { // Pythagorean theorem to get the distance from target as a number. // double xError = getXPosition() - currentXTarget; @@ -151,7 +151,8 @@ public void setDirection(double newHeading) { } public void setFacing(double newHeading) { headingSetPoint = newHeading; } public boolean onHeading() { - return Math.abs(heading - headingSetPoint) < Constants.HEADING_ERROR; + return Math.abs(heading - headingSetPoint) < Constants.HEADING_ERROR_Tolerance; + } public void driveTo(double speed, double xDist, double yDist, double facing) { autoEnabled = true; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java index 4b35b7f..61aa8d5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/PracticeDriveTrain2025.java @@ -1,31 +1,16 @@ package org.firstinspires.ftc.teamcode.Mechanisms; -import android.os.DropBoxManager; - import com.arcrobotics.ftclib.controller.PIDController; import com.arcrobotics.ftclib.drivebase.MecanumDrive; import com.arcrobotics.ftclib.hardware.SimpleServo; import com.arcrobotics.ftclib.hardware.motors.Motor; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import com.arcrobotics.ftclib.controller.PIDController; -import com.arcrobotics.ftclib.drivebase.MecanumDrive; -import com.arcrobotics.ftclib.hardware.SimpleServo; -import com.arcrobotics.ftclib.hardware.motors.Motor; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.IMU; - -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; -import org.firstinspires.ftc.teamcode.PracticeBot; import java.util.Locale; @@ -124,7 +109,7 @@ public void loop(){ //rightFrontDrive.setTargetDistance(1.0); // what is this doing?? and why?? // Because it's a double, can't check for exactly 180, so we check if it's almost 180 in either direction. - if (Math.abs(Math.abs(headingSetPoint) - 180.0) < Constants.HEADING_ERROR) { + if (Math.abs(Math.abs(headingSetPoint) - 180.0) < Constants.HEADING_ERROR_Tolerance) { // "south" is special because it's around the 180/-180 toggle-point // Change set-point between 180/-180 depending on which is closer. @@ -169,7 +154,7 @@ public void setDirection(double newHeading) { } public boolean onHeading() { - return Math.abs(heading - headingSetPoint) < Constants.HEADING_ERROR; + return Math.abs(heading - headingSetPoint) < Constants.HEADING_ERROR_Tolerance; } public void driveTo(double speed, double xDist, double yDist,double facing) { autoEnabled = true; From e2b975bdfd60aa1708c3f0be050b4c47bd0b0837 Mon Sep 17 00:00:00 2001 From: bruce Date: Sat, 15 Feb 2025 15:14:30 -0700 Subject: [PATCH 104/129] comp basket auto working on test chassis,need to tune for comp chassis. --- .../ftc/teamcode/Auto/CompBasket.java | 10 +- .../ftc/teamcode/Auto/gotoXY_test.java | 94 +++++++++++-------- .../ftc/teamcode/Mechanisms/Constants.java | 10 +- .../teamcode/Mechanisms/DriveTrain2025.java | 2 +- .../ftc/teamcode/Mechanisms/Elevator.java | 2 + 5 files changed, 66 insertions(+), 52 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java index 54bf826..b20cfd0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -26,7 +26,7 @@ public class CompBasket extends BaseAutoComp { public void init_loop(){ driveTrain.odometer.update(); - driveTrain.odometer.resetPosAndIMU(); + // driveTrain.odometer.resetPosAndIMU(); telemetry.addData("Sequence Step", this.current_step); Pose2D pos = driveTrain.odometer.getPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); @@ -83,15 +83,15 @@ public void init() { this.wayPoints[2].facing = Constants.SOUTH_WEST; //Done Waypoint 2 this.wayPoints[3] = new WayPoint(); - this.wayPoints[3].x = 180.0; - this.wayPoints[3].y = 80.0; + this.wayPoints[3].x = 110.0;// hitting the wall? + this.wayPoints[3].y = 90.0; this.wayPoints[3].y_speed = 0.3; this.wayPoints[3].x_speed = 0.3; this.wayPoints[3].facing = Constants.SOUTH_WEST; //Done Waypoint 3 this.wayPoints[4] = new WayPoint(); - this.wayPoints[4].x = -5.0; - this.wayPoints[4].y = 645.0; + this.wayPoints[4].x = 15.0; + this.wayPoints[4].y = 700.0; this.wayPoints[4].y_speed = 0.3; this.wayPoints[4].x_speed = 0.3; this.wayPoints[4].facing = Constants.SOUTH_WEST; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java index f2e885d..d96d737 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java @@ -26,7 +26,7 @@ public class gotoXY_test extends BaseAutoComp{ public void init_loop(){ driveTrain.odometer.update(); - driveTrain.odometer.resetPosAndIMU(); + // driveTrain.odometer.resetPosAndIMU(); telemetry.addData("Sequence Step", this.current_step); Pose2D pos = driveTrain.odometer.getPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); @@ -89,7 +89,7 @@ public void init(){ this.wayPoints[2].x_speed = 0.3; this.wayPoints[2].y = 293.0; this.wayPoints[2].y_speed = 0.3; - this.wayPoints[2].facing = Constants.NORTH; + this.wayPoints[2].facing = Constants.WEST; //Done Waypoint 2 this.wayPoints[3] = new WayPoint(); this.wayPoints[3].x = 180.0; @@ -198,10 +198,15 @@ public void loop(){ switch (this.current_step) { // drive 2 inches form submersible case 1: - driveTrain.setFacing(Constants.WEST); - if (!driveTrain.onHeading()) - { - driveTrain.setFacing(Constants.WEST); + driveTrain.setFacing(Constants.EAST); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + + dashboardtelemtry.addData("made to else", ""); + dashboardtelemtry.update(); + driveTrain.drive(0.0, 0.0); + // this.current_step++; } // if (!this.at_xy(this.wayPoints[this.current_step])) @@ -219,46 +224,53 @@ public void loop(){ break; case 2: - // strafe to line up with basketr - - // elevator.setElevatorState(3); - if (!this.at_xy(this.wayPoints[this.current_step])) - { + if (!this.at_xy(this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step]); - } - else - { - driveTrain.drive(0.0,0.0); - // this.claw.openClaw(); - this.current_step++; - } - // this.current_step++; - break; - case 3://turn to SW - driveTrain.setDirection(this.wayPoints[this.current_step].facing); - if (!this.at_xy(this.wayPoints[this.current_step])) - { - this.goto_xy(this.wayPoints[this.current_step]); - } - else - { - driveTrain.drive(0.0,0.0); - elevator.setElevatorState(3); - this.current_step++; - } - break; - case 4:// drive to basket at 135 heading - stop(); - if (!this.at_xy(this.wayPoints[this.current_step])) - { - this.goto_xy(this.wayPoints[this.current_step]); - } - else { + } else { driveTrain.drive(0.0, 0.0); - + // this.claw.openClaw(); this.current_step++; } + + // elevator.setElevatorState(3); +// if (!this.at_xy(this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// // this.claw.openClaw(); +// this.current_step++; break; + + // this.current_step++; + +// case 3://turn to SW +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// if (!this.at_xy(this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// elevator.setElevatorState(3); +// this.current_step++; +// } +// break; +// case 4:// drive to basket at 135 heading +// stop(); +// if (!this.at_xy(this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step]); +// } +// else { +// driveTrain.drive(0.0, 0.0); +// +// this.current_step++; +// } +// break; // case 5: // if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) // { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index c4cf617..4d03390 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -18,7 +18,7 @@ public static final double AUTO_STEP_DELAY = 2.0; - public static double HEADING_ERROR_Tolerance = 15.0; // Degrees... this now matches PID tolerance + public static double HEADING_ERROR_Tolerance = 5.0; // Degrees... this now matches PID tolerance // ========== Define Drive constants. Make them public so they CAN be used by the calling OpMode public static final double FORWARD = 0; // north @@ -92,9 +92,9 @@ public static double ODOMETER_X_OFFSET = 0;// uppdated after contact with gobilda support public static double ODOMETER_Y_OFFSET = 21.0; // auto constants - public static double XPID_Kp = 0.005; - public static double YPID_Kp = 0.006; + public static double XPID_Kp = 0.014; + public static double YPID_Kp = 0.014; - public static double AUTO_X_DISTANCE_ERROR = 25; - public static double AUTO_Y_DISTANCE_ERROR = 25; + public static double AUTO_X_DISTANCE_ERROR = 15.0; + public static double AUTO_Y_DISTANCE_ERROR = 15.0; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index ea55f5c..502c5e8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -74,7 +74,7 @@ public DriveTrain2025(HardwareMap hwMap) { } public void init() { - headingControl = new PIDController(0.025, 0.0, 0.000); + headingControl = new PIDController(0.01, 0.001, 0.000); headingControl.setTolerance(Constants.HEADING_ERROR_Tolerance);// was 3 increased to see if affects spinnning ..cbw xControl = new PIDController(Constants.XPID_Kp, 0.0, 0.00);//FOR AUTO? yControl = new PIDController(Constants.YPID_Kp, 0.0, 0.00);//For AUTO diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java index b88091e..be1ff9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java @@ -27,6 +27,8 @@ public class Elevator { public void init() { elevatorController = new PIDController(pid.p, pid.i, pid.d); elevatorMotor.setRunMode(Motor.RunMode.RawPower); + // added reset encoder + elevatorMotor.resetEncoder(); } public void start() { From 7762d9bf039f3b7d22de8a8336088911c802872d Mon Sep 17 00:00:00 2001 From: bruce Date: Sun, 16 Feb 2025 17:27:44 -0700 Subject: [PATCH 105/129] 1)clean up comments, put old Autos into folder2) added heading Kp to constants --- .../ftc/teamcode/Auto/CompBasket.java | 5 +-- .../ftc/teamcode/Auto/Do_Nothing.java | 3 +- .../ftc/teamcode/Auto/New_Park.java | 5 +-- .../ftc/teamcode/Mechanisms/Constants.java | 36 +++++++++---------- .../teamcode/Mechanisms/DriveTrain2025.java | 4 +-- .../ftc/teamcode/Mechanisms/Elevator.java | 1 - .../BaseAutoComp.java | 2 +- .../BaseAutoPractice.java | 3 +- .../CompBasket_CBW.java | 9 ++--- .../CompPark_Red.java | 4 +-- .../CompSpecimen.java | 4 +-- .../GeorgeTest.java | 7 ++-- .../{Auto => OLD_AutoPrograms}/Park.java | 4 +-- .../PracticeBasket.java | 7 ++-- .../PracticePark.java | 4 +-- .../PracticeSpecimen.java | 9 ++--- .../{Auto => OLD_AutoPrograms}/RedShort.java | 4 +-- .../gotoXY_test.java | 4 +-- .../ftc/teamcode/Test_GoTo_bruce.java | 3 +- 19 files changed, 46 insertions(+), 72 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => OLD_AutoPrograms}/BaseAutoComp.java (96%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => OLD_AutoPrograms}/BaseAutoPractice.java (93%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => OLD_AutoPrograms}/CompBasket_CBW.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => OLD_AutoPrograms}/CompPark_Red.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => OLD_AutoPrograms}/CompSpecimen.java (99%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => OLD_AutoPrograms}/GeorgeTest.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => OLD_AutoPrograms}/Park.java (97%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => OLD_AutoPrograms}/PracticeBasket.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => OLD_AutoPrograms}/PracticePark.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => OLD_AutoPrograms}/PracticeSpecimen.java (97%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => OLD_AutoPrograms}/RedShort.java (94%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => OLD_AutoPrograms}/gotoXY_test.java (99%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java index b20cfd0..0e8a0f8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -11,9 +11,8 @@ import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; -import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; -import java.net.NoRouteToHostException; import java.util.Locale; @Autonomous(name = "CompBasket") @@ -221,6 +220,8 @@ public void loop() { this.current_step++; } break; + + // need to add: back away from basket, lower elevator, drive to submerssable , raise arm to touch bar // case 5: // if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) // { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Do_Nothing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Do_Nothing.java index 691331c..31f38cb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Do_Nothing.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Do_Nothing.java @@ -10,11 +10,12 @@ import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; import java.util.Locale; @Autonomous(name = "DoNothing") -public class Do_Nothing extends BaseAutoComp{ +public class Do_Nothing extends BaseAutoComp { private DriveTrain2025 driveTrain; private int current_step; private int total_waypoints = 20; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java index 5ca0372..7d3f7e5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java @@ -10,11 +10,12 @@ import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; import java.util.Locale; -@Autonomous(name = "NewPark") -public class New_Park extends BaseAutoComp{ +@Autonomous(name = "NewPark_Right") +public class New_Park extends BaseAutoComp { private DriveTrain2025 driveTrain; private int current_step; private boolean autodone = false; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index 4d03390..92a8102 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -12,15 +12,14 @@ public static final double SPEED_RATIO = 1.0; // Use this to slow down robot public static final double TURN_RATIO = 1.0; // use this to slow turn rate - public static final double DRIVE_PID_ERROR = 1.5; // inches public static final double AUTO_DRIVE_SPEED = 0.3; public static final double AUTO_STEP_DELAY = 2.0; - public static double HEADING_ERROR_Tolerance = 5.0; // Degrees... this now matches PID tolerance - // ========== Define Drive constants. Make them public so they CAN be used by the calling OpMode + // + // ========== Define Drive constants. Make them public so they CAN be used by the calling OpMode public static final double FORWARD = 0; // north public static final double BACK = 180; // south public static final double RIGHT = -90; // east @@ -35,8 +34,8 @@ public static final double NORTH_WEST = 45; // North West // SERVO CONSTANTS // finger servo closed position - public static final double STEP = 0.05; // used for manual increment of wrist servo - /*public static final double LF_CLOSED = 0.3 ;// finger servo closed position + // public static final double STEP = 0.05; // used for manual increment of wrist servo + // public static final double LF_CLOSED = 0.3 ;// finger servo closed position public static final double LF_OPEN = 0.7 ; // open position public static final double RF_CLOSED = 0.6 ; public static final double RF_OPEN = 0.2 ; // open position*/ @@ -46,32 +45,28 @@ public static final int ELEVATOR_HIGH_BAR = 700; // nov 22 // public static final int ELEVATOR_LOW_BAR= 350; // nov 22 - // public static final int ELEVATOR_HIGH_BAR = 660; public static final int ELEVATOR_HIGH_BASKET = 1270; public static final int ELEVATOR_MAX = 1270; public static final int ELEVATOR_MIN = 0; public static final double ELEVATOR_SPEED_FACTOR = 0.7; // 1.0 Old elevator and 0.3 for new elevator - public static final double ELEVATOR_POSITION_TOLERANCE = 15.0; - public static final double ELEVATOR_POSITION_COEFFICIENT = 0.05; - public static final int ELEVATOR_STUCK_COUNT = 5; +// public static final double ELEVATOR_POSITION_TOLERANCE = 15.0; +// public static final double ELEVATOR_POSITION_COEFFICIENT = 0.05; +// public static final int ELEVATOR_STUCK_COUNT = 5; //Claw constants - public static boolean CLAW_OPEN = true; - public static boolean CLAW_CLOSED = false; +// public static boolean CLAW_OPEN = true; +// public static boolean CLAW_CLOSED = false; public static final double SERVO_CLAW_OPEN = 0.2 ; public static final double SERVO_CLAW_CLOSED = 0.8 ; // =========== for elbow movements ====================== public static final int ELBOW_START = 0; - public static final double ELBOW_BOTTOM = 0.3; + // public static final double ELBOW_BOTTOM = 0.3; public static final double ELBOW_START_POSITION = 0.0; // public static final double ELBOW_PICKUP_SAMPLE = 1.0; public static final double ELBOW_PICKUP_SPECIMEN = 0.4; //Pickup specimen from the wall for CompBot (Tortuga) public static final double ELBOW_PICKUP_SPECIMEN_WALL = 0.5; - //Pickup specimen from the wall for PracticeBot (Esio Trot) -// public static final double ELBOW_DEPOSIT_SPECIMEN_HIGH_BAR = 0.5; - //public static final double ELBOW_DEPOSIT_SAMPLE_LOW_BUCKET = 0.6; public static final double ELBOW_DEPOSIT_SAMPLE_HIGH_BUCKET = 0.67; public static int ELBOW_MAX = 3; @@ -80,8 +75,10 @@ // Tolerance Section public static final double JOYSTICK_TOLERANCE = 0.5; public static final double AT_XY_TOLERANCE = 25.0; - - + public static double AUTO_Y_DISTANCE_ERROR = 15.0; + public static double AUTO_X_DISTANCE_ERROR = 15.0; + public static final double DRIVE_PID_ERROR = 1.5; // inches?? Only used in PractiseDriveTrain?? + public static double HEADING_ERROR_Tolerance = 5.0; // Degrees... this now matches PID tolerance // Bruce:sept30: change these for use in elevator?kk // public static final double ARM_RADIANS_PER_TICK = Math.PI/244.0; // public static final double ARM_TICKS_PER_90DEG = 113; // CONFIRM #TICKS PER 90. @@ -94,7 +91,8 @@ // auto constants public static double XPID_Kp = 0.014; public static double YPID_Kp = 0.014; + public static double HEADING_Kp = 0.01; + + - public static double AUTO_X_DISTANCE_ERROR = 15.0; - public static double AUTO_Y_DISTANCE_ERROR = 15.0; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index 502c5e8..c09c1fc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -74,9 +74,9 @@ public DriveTrain2025(HardwareMap hwMap) { } public void init() { - headingControl = new PIDController(0.01, 0.001, 0.000); + headingControl = new PIDController(Constants.HEADING_Kp, 0.001, 0.000); headingControl.setTolerance(Constants.HEADING_ERROR_Tolerance);// was 3 increased to see if affects spinnning ..cbw - xControl = new PIDController(Constants.XPID_Kp, 0.0, 0.00);//FOR AUTO? + xControl = new PIDController(Constants.XPID_Kp, 0.0, 0.00);//FOR AUTO yControl = new PIDController(Constants.YPID_Kp, 0.0, 0.00);//For AUTO // redundant as default is brake mode diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java index be1ff9a..56a0254 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java @@ -11,7 +11,6 @@ @Config public class Elevator { static Motor elevatorMotor = null; - public static int target_height; private HardwareMap hwMap; //public boolean manualElevator = false; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/BaseAutoComp.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/BaseAutoComp.java index ddef1a0..d39c480 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoComp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/BaseAutoComp.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import com.qualcomm.robotcore.eventloop.opmode.OpMode; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/BaseAutoPractice.java similarity index 93% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/BaseAutoPractice.java index 6759ebb..55f0306 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/BaseAutoPractice.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/BaseAutoPractice.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import com.qualcomm.robotcore.eventloop.opmode.OpMode; @@ -7,7 +7,6 @@ import org.firstinspires.ftc.teamcode.Mechanisms.Constants; import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; -import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompBasket_CBW.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompBasket_CBW.java index 2221e90..e4a876e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_CBW.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompBasket_CBW.java @@ -1,6 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; @@ -12,13 +10,12 @@ import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; -import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import java.util.Locale; -@Autonomous(name = "CompBasket_cbw") -public class CompBasket_CBW extends BaseAutoComp{ +//@Autonomous(name = "CompBasket_cbw") +public class CompBasket_CBW extends BaseAutoComp { private DriveTrain2025 driveTrain; private int current_step; private int total_waypoints = 20; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompPark_Red.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompPark_Red.java index 4e3977f..872c196 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompPark_Red.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompPark_Red.java @@ -1,6 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompSpecimen.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompSpecimen.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompSpecimen.java index 51bfabf..2be52c5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompSpecimen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/CompSpecimen.java @@ -1,6 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/GeorgeTest.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/GeorgeTest.java index d349456..f1cd8f9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/GeorgeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/GeorgeTest.java @@ -1,6 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; @@ -11,11 +9,10 @@ import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; -import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; import java.util.Locale; -@Autonomous(name = "GeorgeTest") +//@Autonomous(name = "GeorgeTest") public class GeorgeTest extends BaseAutoComp{ private DriveTrain2025 driveTrain; enum WorkingStep{ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/Park.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Park.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/Park.java index f95d04d..4201b60 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Park.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/Park.java @@ -1,6 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticeBasket.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticeBasket.java index ec826bc..85986c4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticeBasket.java @@ -1,6 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; @@ -10,12 +8,11 @@ import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; -import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; import java.util.Locale; //@Autonomous(name = "PracticeBasket") -public class PracticeBasket extends BaseAutoPractice{ +public class PracticeBasket extends BaseAutoPractice { private PracticeDriveTrain2025 driveTrain; private int current_step; private int total_waypoints = 20; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticePark.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticePark.java index 7d81125..33ccf2e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticePark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticePark.java @@ -1,6 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticeSpecimen.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticeSpecimen.java index 7547538..e538131 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/PracticeSpecimen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/PracticeSpecimen.java @@ -1,20 +1,15 @@ -package org.firstinspires.ftc.teamcode.Auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; -import org.firstinspires.ftc.teamcode.Mechanisms.Claw; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; -import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; -import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; import java.util.Locale; //@Autonomous(name = "PracticeSpecimen") -public class PracticeSpecimen extends BaseAutoPractice{ +public class PracticeSpecimen extends BaseAutoPractice { private PracticeDriveTrain2025 driveTrain; private int current_step; private int total_waypoints = 20; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedShort.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/RedShort.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedShort.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/RedShort.java index f354350..b96b601 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/RedShort.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/RedShort.java @@ -1,6 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/gotoXY_test.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/gotoXY_test.java index d96d737..74ecf37 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/gotoXY_test.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/gotoXY_test.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto; +package org.firstinspires.ftc.teamcode.OLD_AutoPrograms; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -16,7 +16,7 @@ import java.util.Locale; @Autonomous(name = "testGOTO") -public class gotoXY_test extends BaseAutoComp{ +public class gotoXY_test extends BaseAutoComp { private DriveTrain2025 driveTrain; private int current_step; private int total_waypoints = 20; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test_GoTo_bruce.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test_GoTo_bruce.java index f1de0ba..2100160 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test_GoTo_bruce.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test_GoTo_bruce.java @@ -5,12 +5,11 @@ import com.arcrobotics.ftclib.controller.PIDController; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; -import org.firstinspires.ftc.teamcode.Auto.BaseAutoComp; +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; import org.firstinspires.ftc.teamcode.Mechanisms.Claw; import org.firstinspires.ftc.teamcode.Mechanisms.Constants; import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; From 91a3c0ad82db88493f18bae1407b37a8f3a67dac Mon Sep 17 00:00:00 2001 From: bruce Date: Fri, 21 Feb 2025 16:44:31 -0700 Subject: [PATCH 106/129] fine tuning waypoints for basked and adding parking --- .../ftc/teamcode/Auto/CompBasket_andPark.java | 341 ++++++++++++++++++ .../ftc/teamcode/Mechanisms/Constants.java | 6 +- 2 files changed, 344 insertions(+), 3 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_andPark.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_andPark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_andPark.java new file mode 100644 index 0000000..39121a8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_andPark.java @@ -0,0 +1,341 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; + +import java.util.Locale; + +@Autonomous(name = "CompBasket& Park") +public class CompBasket_andPark extends BaseAutoComp { + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + WayPoint[] wayPoints; + CAITelemetry dashboardtelemtry; + + public void init_loop(){ + driveTrain.odometer.update(); + // driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // This gets called when we have zero out and ready to hit play. +// if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { +// telemetry.addData("Initialized",""); +// } + } + public void init() { + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + dashboardtelemtry = new CAITelemetry(telemetry); + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:", elevator.getState()); + //Waypoint setup + // Add waypoints for parking + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 400.0; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = 1.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 500.0; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].y = 400.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.SOUTH_WEST; + //Done Waypoint 2 + this.wayPoints[3] = new WayPoint();// go and raise elevator + this.wayPoints[3].x = 110.0;// hitting the wall? + this.wayPoints[3].y = 90.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].x_speed = 0.3; + this.wayPoints[3].facing = Constants.SOUTH_WEST; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint();//open claw at basket + this.wayPoints[4].x = 25.0; + this.wayPoints[4].y = 675.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.SOUTH_WEST; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint();// small backup and drop elevator + this.wayPoints[5].x = 75.0; + this.wayPoints[5].y = 650.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.NORTH; + //Done Waypoint 5- deposit and elevator up + //Waypoint to get away from basket and drop elevator + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = 480.0; + this.wayPoints[6].y = 500.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.SOUTH_WEST; + //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = 270.0; + this.wayPoints[7].y = 250.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = 270.0; + this.wayPoints[8].y = 530.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = 695.0; + this.wayPoints[9].y = 530.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = 720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + + + }//init + + @Override + public void loop() { + driveTrain.odometer.update(); + dashboardtelemtry.update(); + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null) { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step].x, this.wayPoints[this.current_step].y); + // ftc-dashboard telemetry + + } else { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step - 1])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step - 1].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step - 1].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step - 1].x, this.wayPoints[this.current_step - 1].y); + // ftc-dashboard telemetry + + } + telemetry.addData("heading:", "%.2f", driveTrain.heading); + telemetry.addData("Elevator State:", elevator.getState()); + // ftc-dashboard telemetry + dashboardtelemtry.addData("Step:",this.current_step); + + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + + dashboardtelemtry.addData("made to else", ""); + dashboardtelemtry.update(); + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + break; + case 2: + // strafe to line up with basketr + + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + // this.claw.openClaw(); + this.current_step++; + } + break; + case 3://turn to SW + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + elevator.setElevatorState(3); + this.current_step++; + } + break; + case 4:// drive to basket at 135 heading + elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + this.claw.openClaw(); + this.current_step++; + } + break; + + // need to add: back away from basket, lower elevator, drive to submerssable , raise arm to touch bar + case 5: + if (!this.at_xy(this.wayPoints[current_step])) + { + this.goto_xy(this.wayPoints[current_step]); + } + else + { + elevator.setElevatorState(0); + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; +// case 6: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 7: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 8: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 9: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 10: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; + default: + elevator.setElevatorState(3); + break; + } + // Done Processing Waypoints + // Process loop + //elevator.setElevatorState(3); + driveTrain.loop(); + this.elevator.loop(); + } + + @Override + public void stop() { + elevator.setElevatorState(0); + super.stop(); + } + + public boolean at_xy(WayPoint targetWaypoint) { + //return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + double errorX = Math.abs(targetWaypoint.x - driveTrain.getXPosition()); + double errorY = Math.abs(targetWaypoint.y - driveTrain.getYPosition()); + //double totalError = Math.sqrt(errorX * errorX + errorY * errorY); + dashboardtelemtry.addData("Error X", errorX); + dashboardtelemtry.addData("Error Y", errorY); + dashboardtelemtry.addData("Y pos:", driveTrain.getYPosition()); + dashboardtelemtry.addData("X pos:", driveTrain.getXPosition()); + + dashboardtelemtry.addData("heading:", driveTrain.heading); + dashboardtelemtry.addData("heading error",driveTrain.headingError); + dashboardtelemtry.addData("heading setpoint",driveTrain.headingSetPoint); + // dashboardtelemtry.addData("Error",totalError); + // return (totalError <= Constants.AT_XY_TOLERANCE); + return (errorX <= Constants.AT_XY_TOLERANCE && errorY <= Constants.AT_XY_TOLERANCE); + } + + public boolean at_x(double targetX) { + return (Math.abs(targetX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double targetY) { + return (Math.abs(targetY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public void goto_xy(WayPoint targetWaypoint) { + + + driveTrain.xControl.setSetPoint(targetWaypoint.x); + driveTrain.yControl.setSetPoint(targetWaypoint.y); + driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition()) * Constants.AUTO_DRIVE_SPEED, driveTrain.yControl.calculate(driveTrain.getYPosition()) * Constants.AUTO_DRIVE_SPEED); + dashboardtelemtry.addData("X Calc", driveTrain.xControl.calculate()); + dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); + dashboardtelemtry.update(); + + // pack.put("error:",totalError); + + +// if ( totalError <= Constants.AT_XY_TOLERANCE) +// { return true;} +// else +// {return false;} + return; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index 92a8102..7237600 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -89,9 +89,9 @@ public static double ODOMETER_X_OFFSET = 0;// uppdated after contact with gobilda support public static double ODOMETER_Y_OFFSET = 21.0; // auto constants - public static double XPID_Kp = 0.014; - public static double YPID_Kp = 0.014; - public static double HEADING_Kp = 0.01; + public static double XPID_Kp = 0.018;// was 0.14 + public static double YPID_Kp = 0.018; + public static double HEADING_Kp = 0.015; // was 0.01 From e53dfe66ee3b7ce2d43b33a3d8a614950ccfe944 Mon Sep 17 00:00:00 2001 From: bruce Date: Fri, 21 Feb 2025 19:52:54 -0700 Subject: [PATCH 107/129] fine tuning waypoints for basket --- .../ftc/teamcode/Auto/CompBasket.java | 2 +- .../ftc/teamcode/Auto/CompBasket_andPark.java | 51 ++++++++++++++----- .../ftc/teamcode/Auto/New_Park.java | 3 +- .../ftc/teamcode/Mechanisms/Constants.java | 4 +- .../OLD_AutoPrograms/gotoXY_test.java | 2 +- 5 files changed, 43 insertions(+), 19 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java index 0e8a0f8..02b5ffd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket.java @@ -15,7 +15,7 @@ import java.util.Locale; -@Autonomous(name = "CompBasket") +//@Autonomous(name = "CompBasket") public class CompBasket extends BaseAutoComp { private DriveTrain2025 driveTrain; private int current_step; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_andPark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_andPark.java index 39121a8..b66bee4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_andPark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_andPark.java @@ -91,7 +91,7 @@ public void init() { //Done Waypoint 3 this.wayPoints[4] = new WayPoint();//open claw at basket this.wayPoints[4].x = 25.0; - this.wayPoints[4].y = 675.0; + this.wayPoints[4].y = 650.0; this.wayPoints[4].y_speed = 0.3; this.wayPoints[4].x_speed = 0.3; this.wayPoints[4].facing = Constants.SOUTH_WEST; @@ -105,11 +105,11 @@ public void init() { //Done Waypoint 5- deposit and elevator up //Waypoint to get away from basket and drop elevator this.wayPoints[6] = new WayPoint(); - this.wayPoints[6].x = 480.0; - this.wayPoints[6].y = 500.0; + this.wayPoints[6].x = 562.0; + this.wayPoints[6].y = 478.0; this.wayPoints[6].y_speed = 0.3; this.wayPoints[6].x_speed = 0.3; - this.wayPoints[6].facing = Constants.SOUTH_WEST; + this.wayPoints[6].facing = Constants.NORTH; //Done waypoint 6 this.wayPoints[7] = new WayPoint(); this.wayPoints[7].x = 270.0; @@ -183,8 +183,8 @@ public void loop() { this.goto_xy(this.wayPoints[this.current_step]); } else { - dashboardtelemtry.addData("made to else", ""); - dashboardtelemtry.update(); + //dashboardtelemtry.addData("made to else", ""); + //dashboardtelemtry.update(); driveTrain.drive(0.0, 0.0); this.current_step++; } @@ -202,16 +202,17 @@ public void loop() { break; case 3://turn to SW driveTrain.setFacing(this.wayPoints[this.current_step].facing); + elevator.setElevatorState(3); if (!this.at_xy(this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step]); } else { driveTrain.drive(0.0, 0.0); - elevator.setElevatorState(3); + //elevator.setElevatorState(3); this.current_step++; } break; case 4:// drive to basket at 135 heading - elevator.setElevatorState(3); + //elevator.setElevatorState(3); if (!this.at_xy(this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step]); } else { @@ -230,16 +231,37 @@ public void loop() { } else { + elevator.setElevatorState(0); + dashboardtelemtry.addData("drop elevator??", elevator.getState()); driveTrain.drive(0.0,0.0); this.current_step++; } break; -// case 6: -// driveTrain.setDirection(this.wayPoints[this.current_step].facing); -// this.current_step++; -// break; + case 6: + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[current_step])) + { + this.goto_xy(this.wayPoints[current_step]); + } + else { + elevator.setElevatorState(0); + this.elbow.setElbowState(3); + if (this.elbow.getElbowState() <= 0.6) + { + this.elbow.setElbowState(3); + } + else { + this.claw.closeClaw(); + dashboardtelemtry.addData("Elbow State:", elbow.getElbowState()); + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + } + break; // case 7: +// this.elbow.setElbowState(0); + // elevator.setElevatorState(3); // if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) // { // this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); @@ -277,14 +299,15 @@ public void loop() { // } // break; default: - elevator.setElevatorState(3); + //elevator.setElevatorState(3); + //driveTrain.setDirection(Constants.NORTH); break; } // Done Processing Waypoints // Process loop - //elevator.setElevatorState(3); driveTrain.loop(); this.elevator.loop(); + } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java index 7d3f7e5..c300dc7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/New_Park.java @@ -78,7 +78,7 @@ public void init(){ this.wayPoints[2] = new WayPoint(); this.wayPoints[2].x = 25.0; this.wayPoints[2].x_speed = 0.0; - this.wayPoints[2].y = -1108.0; + this.wayPoints[2].y = -1058.0; this.wayPoints[2].y_speed = 0.3; this.wayPoints[2].facing = Constants.NORTH; //Done Waypoint 2 @@ -138,6 +138,7 @@ public void loop(){ // this.current_step++; break; default: + driveTrain.setFacing(Constants.NORTH); driveTrain.drive(0.0,0.0); autodone = true; break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index 7237600..cbceff3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -46,8 +46,8 @@ // public static final int ELEVATOR_LOW_BAR= 350; // nov 22 - public static final int ELEVATOR_HIGH_BASKET = 1270; - public static final int ELEVATOR_MAX = 1270; + public static final int ELEVATOR_HIGH_BASKET = 1285; + public static final int ELEVATOR_MAX = 1300; public static final int ELEVATOR_MIN = 0; public static final double ELEVATOR_SPEED_FACTOR = 0.7; // 1.0 Old elevator and 0.3 for new elevator // public static final double ELEVATOR_POSITION_TOLERANCE = 15.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/gotoXY_test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/gotoXY_test.java index 74ecf37..7679ec6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/gotoXY_test.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OLD_AutoPrograms/gotoXY_test.java @@ -15,7 +15,7 @@ import java.util.Locale; -@Autonomous(name = "testGOTO") +//@Autonomous(name = "testGOTO") public class gotoXY_test extends BaseAutoComp { private DriveTrain2025 driveTrain; private int current_step; From b91dd5fcb5ec5889ab20fac9807454e83c2441e6 Mon Sep 17 00:00:00 2001 From: bruce Date: Sat, 22 Feb 2025 11:48:23 -0700 Subject: [PATCH 108/129] created new two basket auto --- .../ftc/teamcode/Auto/Compbasket_2High.java | 366 ++++++++++++++++++ 1 file changed, 366 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java new file mode 100644 index 0000000..36b9c72 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java @@ -0,0 +1,366 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; + +import java.util.Locale; + +@Autonomous(name = "CompBasket& Park") +public class Compbasket_2High extends BaseAutoComp { + private DriveTrain2025 driveTrain; + private int current_step; + private int total_waypoints = 20; + WayPoint[] wayPoints; + CAITelemetry dashboardtelemtry; + + public void init_loop(){ + driveTrain.odometer.update(); + // driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:",elevator.getState()); + // This gets called when we have zero out and ready to hit play. +// if (driveTrain.getXPosition() <= 0.1 && driveTrain.getYPosition() <= 0.1) { +// telemetry.addData("Initialized",""); +// } + } + public void init() { + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + claw = new Claw(hardwareMap); + elbow = new Elbow(hardwareMap); + this.elbow.init(); + this.elbow.setElbowState(0); + elevator = new Elevator(hardwareMap); + this.elevator.init(); + this.elevator.setElevatorState(0); + this.current_step = 1; + dashboardtelemtry = new CAITelemetry(telemetry); + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.addData("Elevator State:", elevator.getState()); + //Waypoint setup + // Add waypoints for parking + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].x = 0.0; + this.wayPoints[0].y = 0.0; + this.wayPoints[0].facing = Constants.NORTH; + // WayPoint 1 + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].x = 400.0; + this.wayPoints[1].x_speed = 0.3; + this.wayPoints[1].y = 1.0; + this.wayPoints[1].y_speed = 0.3; + this.wayPoints[1].facing = Constants.NORTH; + //Done Waypoint 1 + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].x = 500.0; + this.wayPoints[2].x_speed = 0.3; + this.wayPoints[2].y = 400.0; + this.wayPoints[2].y_speed = 0.3; + this.wayPoints[2].facing = Constants.SOUTH_WEST; + //Done Waypoint 2 + this.wayPoints[3] = new WayPoint();// go and raise elevator + this.wayPoints[3].x = 110.0;// hitting the wall? + this.wayPoints[3].y = 90.0; + this.wayPoints[3].y_speed = 0.3; + this.wayPoints[3].x_speed = 0.3; + this.wayPoints[3].facing = Constants.SOUTH_WEST; + //Done Waypoint 3 + this.wayPoints[4] = new WayPoint();//open claw at basket + this.wayPoints[4].x = 25.0; + this.wayPoints[4].y = 650.0; + this.wayPoints[4].y_speed = 0.3; + this.wayPoints[4].x_speed = 0.3; + this.wayPoints[4].facing = Constants.SOUTH_WEST; + //Done Waypoint 4 + this.wayPoints[5] = new WayPoint();// small backup and drop elevator + this.wayPoints[5].x = 75.0; + this.wayPoints[5].y = 650.0; + this.wayPoints[5].y_speed = 0.3; + this.wayPoints[5].x_speed = 0.3; + this.wayPoints[5].facing = Constants.NORTH; + //Done Waypoint 5- deposit and elevator up + //Waypoint to get away from basket and drop elevator + this.wayPoints[6] = new WayPoint(); + this.wayPoints[6].x = 562.0; + this.wayPoints[6].y = 478.0; + this.wayPoints[6].y_speed = 0.3; + this.wayPoints[6].x_speed = 0.3; + this.wayPoints[6].facing = Constants.NORTH; + //Done waypoint 6 + this.wayPoints[7] = new WayPoint(); + this.wayPoints[7].x = 270.0; + this.wayPoints[7].y = 250.0; + this.wayPoints[7].y_speed = 0.3; + this.wayPoints[7].x_speed = 0.3; + this.wayPoints[7].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[8] = new WayPoint(); + this.wayPoints[8].x = 270.0; + this.wayPoints[8].y = 530.0; + this.wayPoints[8].y_speed = 0.3; + this.wayPoints[8].x_speed = 0.3; + this.wayPoints[8].facing = Constants.NORTH; + // Waypoint + this.wayPoints[9] = new WayPoint(); + this.wayPoints[9].x = 695.0; + this.wayPoints[9].y = 530.0; + this.wayPoints[9].y_speed = 0.3; + this.wayPoints[9].x_speed = 0.3; + this.wayPoints[9].facing = Constants.NORTH; + // Waypoint + this.wayPoints[10] = new WayPoint(); + this.wayPoints[10].x = 0.0; + this.wayPoints[10].y = 720.0; + this.wayPoints[10].y_speed = 0.3; + this.wayPoints[10].x_speed = 0.3; + this.wayPoints[10].facing = Constants.NORTH; + + + }//init + + @Override + public void loop() { + driveTrain.odometer.update(); + dashboardtelemtry.update(); + // Send information to the display + telemetry.addData("Sequence Step", this.current_step); + + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + if (this.wayPoints[this.current_step] != null) { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step].x, this.wayPoints[this.current_step].y); + // ftc-dashboard telemetry + + } else { + telemetry.addData("At Waypoint", this.at_xy(this.wayPoints[this.current_step - 1])); + telemetry.addData("At Waypoint X", this.at_x(this.wayPoints[this.current_step - 1].x)); + telemetry.addData("At Waypoint Y", this.at_y(this.wayPoints[this.current_step - 1].y)); + telemetry.addData("Current Waypoint", "X: %.3f, Y %.3f", this.wayPoints[this.current_step - 1].x, this.wayPoints[this.current_step - 1].y); + // ftc-dashboard telemetry + + } + telemetry.addData("heading:", "%.2f", driveTrain.heading); + telemetry.addData("Elevator State:", elevator.getState()); + // ftc-dashboard telemetry + dashboardtelemtry.addData("Step:",this.current_step); + + + //starting waypoint navigation + switch (this.current_step) { + // drive 2 inches form submersible + case 1: + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + + //dashboardtelemtry.addData("made to else", ""); + //dashboardtelemtry.update(); + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + break; + case 2: + // strafe to line up with basketr + + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + // this.claw.openClaw(); + this.current_step++; + } + break; + case 3://turn to SW + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + //elevator.setElevatorState(3); + this.current_step++; + } + break; + case 4:// drive to basket at 135 heading + //elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + this.claw.openClaw(); + this.current_step++; + } + break; + + // need to add: back away from basket, lower elevator, drive to submerssable , raise arm to touch bar + case 5: + if (!this.at_xy(this.wayPoints[current_step])) + { + this.goto_xy(this.wayPoints[current_step]); + } + else + { + + elevator.setElevatorState(0); + dashboardtelemtry.addData("drop elevator??", elevator.getState()); + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + case 6: + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[current_step])) + { + this.goto_xy(this.wayPoints[current_step]); + } + else { + elevator.setElevatorState(0); + this.elbow.setElbowState(3); +// if (this.elbow.getElbowState() <= 0.6) +// { +// this.elbow.setElbowState(3); +// } +// else { + //this.claw.closeClaw(); + dashboardtelemtry.addData("Elbow State:", elbow.getElbowState()); + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + + break; + case 7: + this.elbow.setElbowState(0); + elevator.setElevatorState(3); +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } + this.claw.closeClaw(); + this.current_step++; + break; +// case 8: +// driveTrain.setDirection(this.wayPoints[this.current_step].facing); +// this.current_step++; +// break; +// case 9: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; +// case 10: +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); +// this.current_step++; +// } +// break; + default: + //elevator.setElevatorState(3); + //driveTrain.setDirection(Constants.NORTH); + break; + } + // Done Processing Waypoints + // Process loop + driveTrain.loop(); + this.elevator.loop(); + + } + + @Override + public void stop() { + elevator.setElevatorState(0); + super.stop(); + } + + public boolean at_xy(WayPoint targetWaypoint) { + //return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + double errorX = Math.abs(targetWaypoint.x - driveTrain.getXPosition()); + double errorY = Math.abs(targetWaypoint.y - driveTrain.getYPosition()); + //double totalError = Math.sqrt(errorX * errorX + errorY * errorY); + dashboardtelemtry.addData("Error X", errorX); + dashboardtelemtry.addData("Error Y", errorY); + dashboardtelemtry.addData("Y pos:", driveTrain.getYPosition()); + dashboardtelemtry.addData("X pos:", driveTrain.getXPosition()); + + dashboardtelemtry.addData("heading:", driveTrain.heading); + dashboardtelemtry.addData("heading error",driveTrain.headingError); + dashboardtelemtry.addData("heading setpoint",driveTrain.headingSetPoint); + // dashboardtelemtry.addData("Error",totalError); + // return (totalError <= Constants.AT_XY_TOLERANCE); + return (errorX <= Constants.AT_XY_TOLERANCE && errorY <= Constants.AT_XY_TOLERANCE); + } + + public boolean at_x(double targetX) { + return (Math.abs(targetX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double targetY) { + return (Math.abs(targetY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public void goto_xy(WayPoint targetWaypoint) { + + + driveTrain.xControl.setSetPoint(targetWaypoint.x); + driveTrain.yControl.setSetPoint(targetWaypoint.y); + driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition()) * Constants.AUTO_DRIVE_SPEED, driveTrain.yControl.calculate(driveTrain.getYPosition()) * Constants.AUTO_DRIVE_SPEED); + dashboardtelemtry.addData("X Calc", driveTrain.xControl.calculate()); + dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); + dashboardtelemtry.update(); + + // pack.put("error:",totalError); + + +// if ( totalError <= Constants.AT_XY_TOLERANCE) +// { return true;} +// else +// {return false;} + return; + } +} \ No newline at end of file From 3f8dd4a5d6022e2ea400ed4ac35b0ab6f2f9e4b5 Mon Sep 17 00:00:00 2001 From: bruce Date: Sat, 22 Feb 2025 13:09:49 -0700 Subject: [PATCH 109/129] created new two basket auto/ading claw time --- .../ftc/teamcode/Auto/Compbasket_2High.java | 20 ++++++++++++------- .../ftc/teamcode/Mechanisms/Elbow.java | 2 +- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java index 36b9c72..b7793b3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java @@ -15,7 +15,7 @@ import java.util.Locale; -@Autonomous(name = "CompBasket& Park") +@Autonomous(name = "CompBasket2High") public class Compbasket_2High extends BaseAutoComp { private DriveTrain2025 driveTrain; private int current_step; @@ -76,7 +76,7 @@ public void init() { this.wayPoints[1].facing = Constants.NORTH; //Done Waypoint 1 this.wayPoints[2] = new WayPoint(); - this.wayPoints[2].x = 500.0; + this.wayPoints[2].x = 425.0; this.wayPoints[2].x_speed = 0.3; this.wayPoints[2].y = 400.0; this.wayPoints[2].y_speed = 0.3; @@ -190,7 +190,7 @@ public void loop() { } break; case 2: - // strafe to line up with basketr + // strafe to line up with basket if (!this.at_xy(this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step]); @@ -257,11 +257,11 @@ public void loop() { driveTrain.drive(0.0, 0.0); this.current_step++; } - + // resetRuntime(); break; case 7: - this.elbow.setElbowState(0); - elevator.setElevatorState(3); + this.elbow.setElbowState(3); + elevator.setElevatorState(0); // if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) // { // this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); @@ -271,7 +271,13 @@ public void loop() { // driveTrain.drive(0.0,0.0); // this.current_step++; // } - this.claw.closeClaw(); + // resetRuntime(); + dashboardtelemtry.addData("runtime:",getRuntime()); + dashboardtelemtry.update(); + // updateTelemetry(); + if (getRuntime() >= 6.0) { // runtime in seconds?? trying to confirm + this.claw.closeClaw(); + } this.current_step++; break; // case 8: diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java index 4a363bb..df17348 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java @@ -45,7 +45,7 @@ public void setElbowState(int position) { // case 2: //Case for deposit sample on low bucket // armServo.setPosition(Constants.ELBOW_DEPOSIT_SAMPLE_LOW_BUCKET); //break; - case 3: //Case for deposit sample on high bucket + case 3: //Case for deposit sample on high bucket??? armServo.setPosition(Constants.ELBOW_DEPOSIT_SAMPLE_HIGH_BUCKET); break; } From e88976d7fd6c1ae24cfba5c6581928ffb5416c16 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Sat, 22 Feb 2025 15:26:38 -0700 Subject: [PATCH 110/129] Added a timely element to for claw closure --- .../ftc/teamcode/Auto/Compbasket_2High.java | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java index b7793b3..5c12451 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java @@ -22,6 +22,8 @@ public class Compbasket_2High extends BaseAutoComp { private int total_waypoints = 20; WayPoint[] wayPoints; CAITelemetry dashboardtelemtry; + private boolean settime = false; + private double startTime; public void init_loop(){ driveTrain.odometer.update(); @@ -275,8 +277,17 @@ public void loop() { dashboardtelemtry.addData("runtime:",getRuntime()); dashboardtelemtry.update(); // updateTelemetry(); - if (getRuntime() >= 6.0) { // runtime in seconds?? trying to confirm - this.claw.closeClaw(); + if (!settime){ + startTime = getRuntime(); + settime = true; + } + else { + if (getRuntime() - startTime >= 0.2) + { + this.claw.closeClaw(); + this.current_step++; + settime = false; + } } this.current_step++; break; From 1070b922d7a73c0f5dc63247c131cb636e61e2cd Mon Sep 17 00:00:00 2001 From: gnazarey Date: Thu, 6 Mar 2025 23:30:21 -0700 Subject: [PATCH 111/129] Created Logging module to save log files to microsd card on the Control Hub --- .../ftc/teamcode/Mechanisms/Logger.java | 62 +++++++++++++++++++ 1 file changed, 62 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Logger.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Logger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Logger.java new file mode 100644 index 0000000..dbcdbd3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Logger.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.teamcode.Mechanisms; + +import android.os.Environment; +import java.io.FileNotFoundException; +import java.io.PrintStream; +import java.text.SimpleDateFormat; +import java.util.Date; +import java.util.Locale; + +///////////////////////////////////////////////// +// This is the logging module. +// +// Written By: George Nazarey +// Date: 3/5/2025 +// Email: george@nazarey.ca +// +public class Logger { + + //private final String fileName = "/sdcard/FIRST/data/test.log"; + private final String fileName; + private PrintStream logWriter; + private Boolean logging = true; + private final SimpleDateFormat dateFormat; + + public Logger(String filename){ + + this.fileName = String.format("%s/FIRST/data/%s",Environment.getExternalStorageDirectory().getPath(), filename); + this.dateFormat = new SimpleDateFormat("yyyy-MM-dd@HH-mm-ss", Locale.US); + this.init(); + } + + private void init(){ + try{ + this.logWriter = new PrintStream(this.fileName); + } + catch (FileNotFoundException e) { + this.logging = false; + } + } + + public boolean getLogging(){ + return this.logging; + } + + public String getTimeStamp(){ + return this.dateFormat.format(new Date()); + } + + public void writeLog(String logLine) + { + if (this.logging) { + this.logWriter.println(logLine); + } + } + + public void stopLog() { + if (this.logging) { + this.logWriter.flush(); + this.logWriter.close(); + } + } +} From 2253b9f2d6c3a55b2667bce692ebd77f6b5ca9a8 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Tue, 11 Mar 2025 16:36:37 -0600 Subject: [PATCH 112/129] Added the updates to logger to include a timestamp --- .../ftc/teamcode/Mechanisms/Logger.java | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Logger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Logger.java index dbcdbd3..7bc8bc4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Logger.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Logger.java @@ -21,13 +21,17 @@ public class Logger { private PrintStream logWriter; private Boolean logging = true; private final SimpleDateFormat dateFormat; + private final Boolean addTimeStamp; - public Logger(String filename){ - + public Logger(String filename, Boolean addTimeStamp){ + this.addTimeStamp = addTimeStamp; this.fileName = String.format("%s/FIRST/data/%s",Environment.getExternalStorageDirectory().getPath(), filename); this.dateFormat = new SimpleDateFormat("yyyy-MM-dd@HH-mm-ss", Locale.US); this.init(); } + public Logger(String filename){ + this(filename,false); + } private void init(){ try{ @@ -49,7 +53,11 @@ public String getTimeStamp(){ public void writeLog(String logLine) { if (this.logging) { - this.logWriter.println(logLine); + if (addTimeStamp) { + this.logWriter.printf(Locale.ENGLISH, "%s %s%n", this.getTimeStamp(), logLine); + } else { + this.logWriter.println(logLine); + } } } From b37efbafd71ce7c5a080ad009b3f597455024cc8 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Tue, 11 Mar 2025 16:59:28 -0600 Subject: [PATCH 113/129] This class will turn 90 every 0.2 seconds --- .../ftc/teamcode/Testing/AutoTurn.java | 94 +++++++++++++++++++ 1 file changed, 94 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java new file mode 100644 index 0000000..b45ace8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java @@ -0,0 +1,94 @@ +package org.firstinspires.ftc.teamcode.Testing; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; +import org.firstinspires.ftc.teamcode.Mechanisms.Claw; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Elbow; +import org.firstinspires.ftc.teamcode.Mechanisms.Elevator; +import org.firstinspires.ftc.teamcode.Mechanisms.Logger; +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; + +import java.util.Locale; + +@Autonomous(name = "TurnTest") +public class AutoTurn extends BaseAutoComp { + private DriveTrain2025 driveTrain; + private int current_step = 0; + private int total_waypoints = 20; + WayPoint[] wayPoints; + CAITelemetry dashboardtelemtry; + private Logger logger; + private boolean settime = false; + private double startTime; + + public void init_loop() { + driveTrain.odometer.update(); + // driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + } + + public void init() { + + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[0] = new WayPoint(); + this.wayPoints[0].facing = Constants.NORTH; + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].facing = Constants.WEST; + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].facing = Constants.SOUTH; + this.wayPoints[3] = new WayPoint(); + this.wayPoints[3].facing = Constants.EAST; + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + this.logger = new Logger("autoturn.log",true); + this.logger.writeLog("Auto Turning Starting"); + this.current_step = 1; + dashboardtelemtry = new CAITelemetry(telemetry); + // Display Setup + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + } + + public void loop(){ + driveTrain.odometer.update(); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.update(); + if (!settime){ + startTime = getRuntime(); + settime = true; + } + else { + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (getRuntime() - startTime >= 0.2) + { + this.current_step++; + settime = false; + } + } + if (this.current_step > 3){ + this.current_step = 0; + } + this.logger.writeLog(String.format(Locale.US,"X: %.3f, Y: %.3f, H: %.3f",driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES))); + } + + +} \ No newline at end of file From d846746171a0c02277f2cb1f0f751783905f4775 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 14 Mar 2025 19:55:23 -0600 Subject: [PATCH 114/129] Updated telemetry to dashboard --- .../firstinspires/ftc/teamcode/Testing/AutoTurn.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java index b45ace8..daf5d3a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java @@ -58,20 +58,20 @@ public void init() { this.current_step = 1; dashboardtelemtry = new CAITelemetry(telemetry); // Display Setup - telemetry.addData("Sequence Step", this.current_step); + dashboardtelemtry.addData("Sequence Step", this.current_step); Pose2D pos = driveTrain.odometer.getPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); - telemetry.addData("Position", data); + dashboardtelemtry.addData("Position", data); data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); - telemetry.addData("Function Position", data); + dashboardtelemtry.addData("Function Position", data); } public void loop(){ driveTrain.odometer.update(); Pose2D pos = driveTrain.odometer.getPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); - telemetry.addData("Function Position", data); - telemetry.update(); + dashboardtelemtry.addData("Function Position", data); + dashboardtelemtry.update(); if (!settime){ startTime = getRuntime(); settime = true; From 1d24ce6eae1d975b553d3fdf86ace0607b539205 Mon Sep 17 00:00:00 2001 From: bruce Date: Fri, 28 Mar 2025 20:06:17 -0600 Subject: [PATCH 115/129] created new two basket auto/ading claw time --- .../ftc/teamcode/Auto/CompBasket_andPark.java | 2 +- .../ftc/teamcode/Mechanisms/Constants.java | 4 ++-- .../firstinspires/ftc/teamcode/Testing/AutoTurn.java | 10 ++++++---- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_andPark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_andPark.java index b66bee4..9d1862e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_andPark.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/CompBasket_andPark.java @@ -76,7 +76,7 @@ public void init() { this.wayPoints[1].facing = Constants.NORTH; //Done Waypoint 1 this.wayPoints[2] = new WayPoint(); - this.wayPoints[2].x = 500.0; + this.wayPoints[2].x = 425.0; this.wayPoints[2].x_speed = 0.3; this.wayPoints[2].y = 400.0; this.wayPoints[2].y_speed = 0.3; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index cbceff3..b83037e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -86,8 +86,8 @@ //odometer constants public static int ODOMETER_PRACTICE_MODE = 1; public static int ODOMETER_COMPETITION_MODE = 8; - public static double ODOMETER_X_OFFSET = 0;// uppdated after contact with gobilda support - public static double ODOMETER_Y_OFFSET = 21.0; + public static double ODOMETER_X_OFFSET = 0;// left offset is + uppdated after contact with gobilda support + public static double ODOMETER_Y_OFFSET = 24.0; // auto constants public static double XPID_Kp = 0.018;// was 0.14 public static double YPID_Kp = 0.018; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java index daf5d3a..e66aabc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java @@ -63,11 +63,12 @@ public void init() { String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); dashboardtelemtry.addData("Position", data); data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); - dashboardtelemtry.addData("Function Position", data); + // dashboardtelemtry.addData("Function Position", data); } public void loop(){ driveTrain.odometer.update(); + driveTrain.loop(); Pose2D pos = driveTrain.odometer.getPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); dashboardtelemtry.addData("Function Position", data); @@ -77,8 +78,9 @@ public void loop(){ settime = true; } else { - driveTrain.setFacing(this.wayPoints[this.current_step].facing); - if (getRuntime() - startTime >= 0.2) + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + dashboardtelemtry.addData("got here",this.current_step); + if (getRuntime() - startTime >= 1.0) { this.current_step++; settime = false; @@ -87,7 +89,7 @@ public void loop(){ if (this.current_step > 3){ this.current_step = 0; } - this.logger.writeLog(String.format(Locale.US,"X: %.3f, Y: %.3f, H: %.3f",driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES))); + this.logger.writeLog(String.format(Locale.US,"X: %.3f, Y: %.3f, H: %.3f",pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES))); } From 5a22d60c5e6b2226d56ff8b2499b2a0cab88faff Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 28 Mar 2025 20:41:36 -0600 Subject: [PATCH 116/129] Driving Test Auto --- .../ftc/teamcode/Testing/WayPoints.java | 157 ++++++++++++++++++ 1 file changed, 157 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/WayPoints.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/WayPoints.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/WayPoints.java new file mode 100644 index 0000000..03004e3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/WayPoints.java @@ -0,0 +1,157 @@ +package org.firstinspires.ftc.teamcode.Testing; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.teamcode.Mechanisms.CAITelemetry; +import org.firstinspires.ftc.teamcode.Mechanisms.Constants; +import org.firstinspires.ftc.teamcode.Mechanisms.DriveTrain2025; +import org.firstinspires.ftc.teamcode.Mechanisms.Logger; +import org.firstinspires.ftc.teamcode.OLD_AutoPrograms.BaseAutoComp; + +import java.util.Locale; + +@Config +@Autonomous(name = "DriveTest") +public class WayPoints extends BaseAutoComp { + private DriveTrain2025 driveTrain; + private int current_step = 0; + private int total_waypoints = 20; + WayPoint[] wayPoints; + CAITelemetry dashboardtelemtry; + private Logger logger; + private boolean settime = false; + private double startTime; + public static double pause_time = 0.2; + private int circle_count = 0; + + public void init_loop() { + driveTrain.odometer.update(); + // driveTrain.odometer.resetPosAndIMU(); + telemetry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + telemetry.addData("Function Position", data); + telemetry.update(); + } + + public void init() { + + this.wayPoints = new WayPoint[this.total_waypoints + 1]; + this.wayPoints[1] = new WayPoint(); + this.wayPoints[1].facing = Constants.NORTH; + this.wayPoints[1].x = 254.0; + this.wayPoints[1].y = 254.0; + this.wayPoints[2] = new WayPoint(); + this.wayPoints[2].facing = Constants.NORTH; + this.wayPoints[2].x = 254.0; + this.wayPoints[2].y = 254.0; + driveTrain = new DriveTrain2025(hardwareMap); + driveTrain.init(); + driveTrain.odometer.resetPosAndIMU(); + driveTrain.odometer.update(); + this.logger = new Logger("autoturn.log",true); + this.logger.writeLog("Auto Turning Starting"); + this.current_step = 1; + dashboardtelemtry = new CAITelemetry(telemetry); + // Display Setup + dashboardtelemtry.addData("Sequence Step", this.current_step); + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES)); + dashboardtelemtry.addData("Position", data); + data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + dashboardtelemtry.addData("Function Position", data); + driveTrain.odometer.resetPosAndIMU(); + } + + public void loop(){ + Pose2D pos = driveTrain.odometer.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); + dashboardtelemtry.addData("Function Position", data); + dashboardtelemtry.addData("Circle Count",this.circle_count); + dashboardtelemtry.update(); + switch (this.current_step) { + case 1: + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + + //dashboardtelemtry.addData("made to else", ""); + //dashboardtelemtry.update(); + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + break; + case 2: + // strafe to line up with basket + + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + // this.claw.openClaw(); + this.current_step++; + } + break; + // drive 2 inches form submersible + + default: + //elevator.setElevatorState(3); + //driveTrain.setDirection(Constants.NORTH); + break; + } + this.logger.writeLog(String.format(Locale.US,"X: %.3f, Y: %.3f, H: %.3f",driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES))); + driveTrain.loop(); + } + + public boolean at_xy(WayPoint targetWaypoint) { + //return this.at_x(previousWaypoint.x,currentWaypoint.x) && this.at_y(previousWaypoint.y,currentWaypoint.y); + double errorX = Math.abs(targetWaypoint.x - driveTrain.getXPosition()); + double errorY = Math.abs(targetWaypoint.y - driveTrain.getYPosition()); + //double totalError = Math.sqrt(errorX * errorX + errorY * errorY); + dashboardtelemtry.addData("Error X", errorX); + dashboardtelemtry.addData("Error Y", errorY); + dashboardtelemtry.addData("Y pos:", driveTrain.getYPosition()); + dashboardtelemtry.addData("X pos:", driveTrain.getXPosition()); + + dashboardtelemtry.addData("heading:", driveTrain.heading); + dashboardtelemtry.addData("heading error",driveTrain.headingError); + dashboardtelemtry.addData("heading setpoint",driveTrain.headingSetPoint); + // dashboardtelemtry.addData("Error",totalError); + // return (totalError <= Constants.AT_XY_TOLERANCE); + return (errorX <= Constants.AT_XY_TOLERANCE && errorY <= Constants.AT_XY_TOLERANCE); + } + + public boolean at_x(double targetX) { + return (Math.abs(targetX) - Math.abs(driveTrain.getXPosition())) <= Constants.AUTO_X_DISTANCE_ERROR; + } + + public boolean at_y(double targetY) { + return (Math.abs(targetY) - Math.abs(driveTrain.getYPosition())) <= Constants.AUTO_Y_DISTANCE_ERROR; + } + + public void goto_xy(WayPoint targetWaypoint) { + + + driveTrain.xControl.setSetPoint(targetWaypoint.x); + driveTrain.yControl.setSetPoint(targetWaypoint.y); + driveTrain.drive(driveTrain.xControl.calculate(driveTrain.getXPosition()) * Constants.AUTO_DRIVE_SPEED, driveTrain.yControl.calculate(driveTrain.getYPosition()) * Constants.AUTO_DRIVE_SPEED); + dashboardtelemtry.addData("X Calc", driveTrain.xControl.calculate()); + dashboardtelemtry.addData("Y Calc", driveTrain.yControl.calculate()); + dashboardtelemtry.update(); + + // pack.put("error:",totalError); + + +// if ( totalError <= Constants.AT_XY_TOLERANCE) +// { return true;} +// else +// {return false;} + return; + } +} \ No newline at end of file From 875b6a1d82b186cea6ddc99c8ade006bdc84bbd7 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 28 Mar 2025 20:42:34 -0600 Subject: [PATCH 117/129] Added Public Static variables for dashboard --- .../ftc/teamcode/Mechanisms/DriveTrain2025.java | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java index c09c1fc..d86be2d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/DriveTrain2025.java @@ -7,6 +7,7 @@ import com.arcrobotics.ftclib.hardware.SimpleServo; import com.arcrobotics.ftclib.hardware.motors.Motor; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.PIDCoefficients; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @@ -29,7 +30,10 @@ public class DriveTrain2025 { MecanumDrive driveBase; public GoBildaPinpointDriver odometer; - private PIDController headingControl = null; + public PIDController headingControl = null; + public static PIDCoefficients headingpid = new PIDCoefficients(Constants.HEADING_Kp, 0.001, 0.000); + public static PIDCoefficients xpid = new PIDCoefficients(Constants.XPID_Kp, 0.0, 0.00); + public static PIDCoefficients ypid = new PIDCoefficients(Constants.YPID_Kp, 0.0, 0.00); public PIDController xControl = null; public PIDController yControl = null; @@ -74,10 +78,10 @@ public DriveTrain2025(HardwareMap hwMap) { } public void init() { - headingControl = new PIDController(Constants.HEADING_Kp, 0.001, 0.000); + headingControl = new PIDController(headingpid.p, headingpid.i, headingpid.d); headingControl.setTolerance(Constants.HEADING_ERROR_Tolerance);// was 3 increased to see if affects spinnning ..cbw - xControl = new PIDController(Constants.XPID_Kp, 0.0, 0.00);//FOR AUTO - yControl = new PIDController(Constants.YPID_Kp, 0.0, 0.00);//For AUTO + xControl = new PIDController(xpid.p, xpid.i, xpid.d);//FOR AUTO + yControl = new PIDController(ypid.p, ypid.i, ypid.d);//For AUTO // redundant as default is brake mode leftBackDrive.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); From 4dc9c6506dcce969c57855ea8c038aff519ccec3 Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 28 Mar 2025 20:42:49 -0600 Subject: [PATCH 118/129] Turning auto --- .../ftc/teamcode/Testing/AutoTurn.java | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java index daf5d3a..46fd70d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/AutoTurn.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.Testing; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @@ -16,6 +17,7 @@ import java.util.Locale; +@Config @Autonomous(name = "TurnTest") public class AutoTurn extends BaseAutoComp { private DriveTrain2025 driveTrain; @@ -26,6 +28,8 @@ public class AutoTurn extends BaseAutoComp { private Logger logger; private boolean settime = false; private double startTime; + public static double pause_time = 0.2; + private int circle_count = 0; public void init_loop() { driveTrain.odometer.update(); @@ -36,6 +40,7 @@ public void init_loop() { telemetry.addData("Position", data); data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); telemetry.addData("Function Position", data); + telemetry.update(); } public void init() { @@ -64,6 +69,7 @@ public void init() { dashboardtelemtry.addData("Position", data); data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); dashboardtelemtry.addData("Function Position", data); + driveTrain.odometer.resetPosAndIMU(); } public void loop(){ @@ -71,14 +77,15 @@ public void loop(){ Pose2D pos = driveTrain.odometer.getPosition(); String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); dashboardtelemtry.addData("Function Position", data); + dashboardtelemtry.addData("Circle Count",this.circle_count); dashboardtelemtry.update(); if (!settime){ startTime = getRuntime(); settime = true; } else { - driveTrain.setFacing(this.wayPoints[this.current_step].facing); - if (getRuntime() - startTime >= 0.2) + driveTrain.setDirection(this.wayPoints[this.current_step].facing); + if (getRuntime() - startTime >= pause_time) { this.current_step++; settime = false; @@ -86,8 +93,12 @@ public void loop(){ } if (this.current_step > 3){ this.current_step = 0; + this.circle_count++; + driveTrain.stop(); + driveTrain.stop(); } this.logger.writeLog(String.format(Locale.US,"X: %.3f, Y: %.3f, H: %.3f",driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES))); + driveTrain.loop(); } From ed686f1f8b51cbd3ae7281c09ab87cf0e39a8d7e Mon Sep 17 00:00:00 2001 From: gnazarey Date: Fri, 28 Mar 2025 23:08:35 -0600 Subject: [PATCH 119/129] Added a new test. --- .../ftc/teamcode/Testing/WayPoints.java | 18 +++--------------- 1 file changed, 3 insertions(+), 15 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/WayPoints.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/WayPoints.java index 03004e3..707727f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/WayPoints.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Testing/WayPoints.java @@ -49,8 +49,8 @@ public void init() { this.wayPoints[1].y = 254.0; this.wayPoints[2] = new WayPoint(); this.wayPoints[2].facing = Constants.NORTH; - this.wayPoints[2].x = 254.0; - this.wayPoints[2].y = 254.0; + this.wayPoints[2].x = 127.0; + this.wayPoints[2].y = 512.0; driveTrain = new DriveTrain2025(hardwareMap); driveTrain.init(); driveTrain.odometer.resetPosAndIMU(); @@ -71,38 +71,26 @@ public void init() { public void loop(){ Pose2D pos = driveTrain.odometer.getPosition(); - String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES)); - dashboardtelemtry.addData("Function Position", data); - dashboardtelemtry.addData("Circle Count",this.circle_count); dashboardtelemtry.update(); switch (this.current_step) { case 1: if (!this.at_xy(this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step]); } else { - - //dashboardtelemtry.addData("made to else", ""); - //dashboardtelemtry.update(); driveTrain.drive(0.0, 0.0); this.current_step++; } break; case 2: - // strafe to line up with basket - +// driveTrain.setFacing(this.wayPoints[this.current_step].facing); if (!this.at_xy(this.wayPoints[this.current_step])) { this.goto_xy(this.wayPoints[this.current_step]); } else { driveTrain.drive(0.0, 0.0); - // this.claw.openClaw(); this.current_step++; } break; - // drive 2 inches form submersible - default: - //elevator.setElevatorState(3); - //driveTrain.setDirection(Constants.NORTH); break; } this.logger.writeLog(String.format(Locale.US,"X: %.3f, Y: %.3f, H: %.3f",driveTrain.getXPosition(), driveTrain.getYPosition(), pos.getHeading(AngleUnit.DEGREES))); From 2c54dd93fa7c6db57c1b42a9dbebb3a1e673189e Mon Sep 17 00:00:00 2001 From: ftc16596 Date: Fri, 4 Apr 2025 19:25:40 -0600 Subject: [PATCH 120/129] just testing team laptop --- .../java/org/firstinspires/ftc/teamcode/PracticeBotTesting.java | 1 + 1 file changed, 1 insertion(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBotTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBotTesting.java index 8f43e35..4b488d9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBotTesting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PracticeBotTesting.java @@ -28,6 +28,7 @@ public class PracticeBotTesting extends OpMode { GoBildaPinpointDriver odometer; @Override + //just testing team laptop public void init() { telemetry = new CAITelemetry(telemetry); ((CAITelemetry)telemetry).setDashboardEnabled(true); From d29402d287dffcdb85a9277e84519013d4e74eb3 Mon Sep 17 00:00:00 2001 From: ftc16596 Date: Fri, 4 Apr 2025 19:53:53 -0600 Subject: [PATCH 121/129] minor changes in elevator testing --- .../java/org/firstinspires/ftc/teamcode/ElevatorTesting.java | 4 ++-- .../org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java index f67e536..f98a532 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java @@ -18,7 +18,7 @@ import org.firstinspires.ftc.teamcode.Mechanisms.PracticeDriveTrain2025; //uncomment below to add back to opmode list -//@TeleOp(name = "testElevator") +@TeleOp(name = "testElevator") // this is just used to test elevator and get encoder values for pre-sets public class ElevatorTesting extends OpMode { @@ -46,7 +46,7 @@ public void init() { @Override public void start() { - operator = new GamepadEx(gamepad2); + // operator = new GamepadEx(gamepad2); //driveTrain.start(); //reset encoders operator = new GamepadEx(gamepad2); // This controls the movement of items on the robot elbow.setElbowState(0);// start position diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java index 56a0254..a3e95ff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java @@ -92,7 +92,7 @@ public int setElevatorState(int position) { public void move(double speed) {// this method is for manual elevator moves double elevatorSpeed = 0.0; - elevatorMotor.setRunMode(Motor.RunMode.RawPower); + //elevatorMotor.setRunMode(Motor.RunMode.RawPower); elevatorSpeed = speed * Constants.ELEVATOR_SPEED_FACTOR; elevatorMotor.set(elevatorSpeed); From 984962df1a764b6b78089dca41ac453a6a80ed08 Mon Sep 17 00:00:00 2001 From: bruce Date: Fri, 4 Apr 2025 20:21:01 -0600 Subject: [PATCH 122/129] fixed hwmap issues --- .../firstinspires/ftc/teamcode/ElevatorTesting.java | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java index f98a532..89e41a1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java @@ -24,11 +24,11 @@ public class ElevatorTesting extends OpMode { static Motor elevatorMotor = null; public static double height; - private HardwareMap hwMap; + //private HardwareMap hwMap; private Elevator elevator; public double elevatorHeight; public GamepadEx operator = null; - private PracticeDriveTrain2025 driveTrain; + private DriveTrain2025 driveTrain; private Elbow elbow; private double elevatorSpeed; @@ -36,12 +36,11 @@ public class ElevatorTesting extends OpMode { public void init() { telemetry = new CAITelemetry(telemetry); ((CAITelemetry) telemetry).setDashboardEnabled(true); - driveTrain = new PracticeDriveTrain2025(hardwareMap); + driveTrain = new DriveTrain2025(hardwareMap); elevator = new Elevator(hardwareMap); driveTrain.init(); elbow = new Elbow(hardwareMap); // this.setHeight(Constants.ELEVATOR_START); - this.hwMap = hwMap; } @Override @@ -51,8 +50,8 @@ public void start() { operator = new GamepadEx(gamepad2); // This controls the movement of items on the robot elbow.setElbowState(0);// start position //driveTrain.resetIMU(); - //this.hwMap = hwMap; - elevatorMotor = new Motor(hwMap, "elevator");// expansion hub port 0 + // this.hwMap = hwMap; + elevatorMotor = new Motor(hardwareMap, "elevator");// expansion hub port 0 elevatorMotor.setRunMode(Motor.RunMode.RawPower); elevatorMotor.resetEncoder(); } From 410ed1eff0c0a691a9c112d76fed052890faab27 Mon Sep 17 00:00:00 2001 From: ftc16596 Date: Fri, 4 Apr 2025 20:41:00 -0600 Subject: [PATCH 123/129] elevator.printTelemetry(telemetry); --- .../java/org/firstinspires/ftc/teamcode/ElevatorTesting.java | 1 + 1 file changed, 1 insertion(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java index 89e41a1..45b2105 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java @@ -61,6 +61,7 @@ public void loop(){ operator.readButtons(); driveTrain.loop(); elevator.move(operator.getLeftY()); + elevator.printTelemetry(telemetry); } public double getHeight(){ From d1b781816364accef0ad80c5faed92994c0fe5fd Mon Sep 17 00:00:00 2001 From: ftc16596 Date: Fri, 4 Apr 2025 20:45:58 -0600 Subject: [PATCH 124/129] elevator.init(); --- .../java/org/firstinspires/ftc/teamcode/ElevatorTesting.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java index 45b2105..9710c1a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ElevatorTesting.java @@ -39,8 +39,10 @@ public void init() { driveTrain = new DriveTrain2025(hardwareMap); elevator = new Elevator(hardwareMap); driveTrain.init(); + elevator.init(); elbow = new Elbow(hardwareMap); // this.setHeight(Constants.ELEVATOR_START); + } @Override @@ -62,6 +64,7 @@ public void loop(){ driveTrain.loop(); elevator.move(operator.getLeftY()); elevator.printTelemetry(telemetry); + } public double getHeight(){ From 20fe80972ac5bd9ac5bb922af871351a3b2f6d48 Mon Sep 17 00:00:00 2001 From: Annika Date: Fri, 11 Apr 2025 20:45:47 -0600 Subject: [PATCH 125/129] Coded auto to pickup a sample from the tape, dropped in the basket --- .../ftc/teamcode/Auto/Compbasket_2High.java | 126 +++++++++++++++--- .../ftc/teamcode/Mechanisms/Constants.java | 6 +- 2 files changed, 107 insertions(+), 25 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java index 5c12451..01f93a0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java @@ -107,7 +107,7 @@ public void init() { //Done Waypoint 5- deposit and elevator up //Waypoint to get away from basket and drop elevator this.wayPoints[6] = new WayPoint(); - this.wayPoints[6].x = 562.0; + this.wayPoints[6].x = 542.0;//was 562 this.wayPoints[6].y = 478.0; this.wayPoints[6].y_speed = 0.3; this.wayPoints[6].x_speed = 0.3; @@ -128,19 +128,35 @@ public void init() { this.wayPoints[8].facing = Constants.NORTH; // Waypoint this.wayPoints[9] = new WayPoint(); - this.wayPoints[9].x = 695.0; - this.wayPoints[9].y = 530.0; + this.wayPoints[9].x = 110.0; + this.wayPoints[9].y = 90.0; this.wayPoints[9].y_speed = 0.3; this.wayPoints[9].x_speed = 0.3; - this.wayPoints[9].facing = Constants.NORTH; + this.wayPoints[9].facing = Constants.SOUTH_WEST; // Waypoint - this.wayPoints[10] = new WayPoint(); - this.wayPoints[10].x = 0.0; - this.wayPoints[10].y = 720.0; + this.wayPoints[10] = new WayPoint();//open claw at basket + this.wayPoints[10].x = 25.0; + this.wayPoints[10].y = 650.0; this.wayPoints[10].y_speed = 0.3; this.wayPoints[10].x_speed = 0.3; - this.wayPoints[10].facing = Constants.NORTH; - + this.wayPoints[10].facing = Constants.SOUTH_WEST; + //Done Waypoint 10 + //Waypoint 11 + this.wayPoints[11] = new WayPoint();// small backup and drop elevator + this.wayPoints[11].x = 75.0; + this.wayPoints[11].y = 650.0; + this.wayPoints[11].y_speed = 0.3; + this.wayPoints[11].x_speed = 0.3; + this.wayPoints[11].facing = Constants.NORTH; + //Done waypoint 11 + //Waypoint to get away from basket and drop elevator + this.wayPoints[12] = new WayPoint(); + this.wayPoints[12].x = 542.0;//was 562 + this.wayPoints[12].y = 500.0; + this.wayPoints[12].y_speed = 0.3; + this.wayPoints[12].x_speed = 0.3; + this.wayPoints[12].facing = Constants.NORTH; + //Done waypoint 12 }//init @@ -271,31 +287,97 @@ public void loop() { // else // { // driveTrain.drive(0.0,0.0); -// this.current_step++; + this.current_step++; // } - // resetRuntime(); - dashboardtelemtry.addData("runtime:",getRuntime()); - dashboardtelemtry.update(); - // updateTelemetry(); - if (!settime){ + break; + case 8: + dashboardtelemtry.addData("Claw State:", claw.getClawState()); + if (this.claw.getClawState()==Constants.CLAW_OPEN) { + this.claw.closeClaw(); startTime = getRuntime(); settime = true; } else { - if (getRuntime() - startTime >= 0.2) + if (getRuntime() - startTime >= 0.6) { - this.claw.closeClaw(); this.current_step++; settime = false; } } - this.current_step++; break; -// case 8: -// driveTrain.setDirection(this.wayPoints[this.current_step].facing); + case 9: + this.elbow.setElbowState(0); + //turn to SW + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + //elevator.setElevatorState(3); + this.current_step++; + } + break; + case 10: + // drive to basket at 135 heading + //elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + this.claw.openClaw(); + this.current_step++; + } + break; + case 11: + if (!this.at_xy(this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step]); + } + else + { + + elevator.setElevatorState(0); + dashboardtelemtry.addData("in else 11", elevator.getState()); + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + + case 12: + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[current_step])) + { + this.goto_xy(this.wayPoints[current_step]); + } + else { + elevator.setElevatorState(0); + this.elbow.setElbowState(3); +// if (this.elbow.getElbowState() <= 0.6) +// { +// this.elbow.setElbowState(3); +// } +// else { + //this.claw.closeClaw(); + dashboardtelemtry.addData("Elbow State:", elbow.getElbowState()); + driveTrain.drive(0.0, 0.0); + this.current_step++; + } + // resetRuntime(); + break; +// if (!this.at_xy(this.wayPoints[current_step])) +// { +// this.goto_xy(this.wayPoints[current_step]); +// } +// else { +// +// elevator.setElevatorState(0); +// dashboardtelemtry.addData("drop elevator??", elevator.getState()); +// driveTrain.drive(0.0, 0.0); // this.current_step++; -// break; -// case 9: +// } +// break; // if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) // { // this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index b83037e..91f1b2a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -53,9 +53,9 @@ // public static final double ELEVATOR_POSITION_TOLERANCE = 15.0; // public static final double ELEVATOR_POSITION_COEFFICIENT = 0.05; // public static final int ELEVATOR_STUCK_COUNT = 5; - //Claw constants -// public static boolean CLAW_OPEN = true; -// public static boolean CLAW_CLOSED = false; + // Claw constants + public static boolean CLAW_OPEN = true; + public static boolean CLAW_CLOSED = false; public static final double SERVO_CLAW_OPEN = 0.2 ; public static final double SERVO_CLAW_CLOSED = 0.8 ; From 389cfeddf3c0f7ed7508482e51d9149060125d4c Mon Sep 17 00:00:00 2001 From: Aila-skibidi Date: Fri, 2 May 2025 15:53:54 -0600 Subject: [PATCH 126/129] coding tutorials and elevator tests --- TeamCode/src/main/java/Testing_Code.java | 30 +++++++++++++++++++ .../ftc/teamcode/Mechanisms/Elevator.java | 3 +- 2 files changed, 32 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/Testing_Code.java diff --git a/TeamCode/src/main/java/Testing_Code.java b/TeamCode/src/main/java/Testing_Code.java new file mode 100644 index 0000000..5dd56ff --- /dev/null +++ b/TeamCode/src/main/java/Testing_Code.java @@ -0,0 +1,30 @@ +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +@TeleOp +public class Testing_Code extends OpMode { + + private DcMotor leftMotor; + private DcMotor rightMotor; + + @Override + public void init() { + leftMotor = hardwareMap.get(DcMotor.class,"left"); + rightMotor = hardwareMap.get(DcMotor.class,"right"); + leftMotor.setDirection(DcMotorSimple.Direction.REVERSE); + + + } + + @Override + public void loop() { + double leftPower = -gamepad1.left_stick_y; + double rightPower = -gamepad1.left_stick_y; + + leftMotor.setPower(leftPower); + rightMotor.setPower(rightPower); + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java index a3e95ff..b2d4fbc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java @@ -4,11 +4,12 @@ import com.arcrobotics.ftclib.controller.PIDController; import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.arcrobotics.ftclib.hardware.motors.Motor; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.PIDCoefficients; import org.firstinspires.ftc.robotcore.external.Telemetry; -@Config +@TeleOp public class Elevator { static Motor elevatorMotor = null; int target_height; From a329e2636029bd2e452df490744133ce557d5a8a Mon Sep 17 00:00:00 2001 From: Aila-skibidi Date: Fri, 2 May 2025 21:02:55 -0600 Subject: [PATCH 127/129] 3 samples --- .../ftc/teamcode/Auto/Compbasket_2High.java | 109 +++++++++++++++++- .../ftc/teamcode/Mechanisms/Claw.java | 1 + .../ftc/teamcode/Mechanisms/Elevator.java | 2 +- 3 files changed, 108 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java index 01f93a0..c57ac55 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java @@ -151,13 +151,46 @@ public void init() { //Done waypoint 11 //Waypoint to get away from basket and drop elevator this.wayPoints[12] = new WayPoint(); - this.wayPoints[12].x = 542.0;//was 562 - this.wayPoints[12].y = 500.0; + this.wayPoints[12].x = 479.0;//was 562 + this.wayPoints[12].y = 752.0; this.wayPoints[12].y_speed = 0.3; this.wayPoints[12].x_speed = 0.3; this.wayPoints[12].facing = Constants.NORTH; //Done waypoint 12 - + this.wayPoints[13] = new WayPoint(); + this.wayPoints[13].x = 270.0; + this.wayPoints[13].y = 250.0; + this.wayPoints[13].y_speed = 0.3; + this.wayPoints[13].x_speed = 0.3; + this.wayPoints[13].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[14] = new WayPoint(); + this.wayPoints[14].x = 270.0; + this.wayPoints[14].y = 530.0; + this.wayPoints[14].y_speed = 0.3; + this.wayPoints[14].x_speed = 0.3; + this.wayPoints[14].facing = Constants.NORTH; + // Waypoint + this.wayPoints[15] = new WayPoint(); + this.wayPoints[15].x = 110.0; + this.wayPoints[15].y = 90.0; + this.wayPoints[15].y_speed = 0.3; + this.wayPoints[15].x_speed = 0.3; + this.wayPoints[15].facing = Constants.SOUTH_WEST; + // Waypoint + this.wayPoints[16] = new WayPoint();//open claw at basket + this.wayPoints[16].x = 25.0; + this.wayPoints[16].y = 650.0; + this.wayPoints[16].y_speed = 0.3; + this.wayPoints[16].x_speed = 0.3; + this.wayPoints[16].facing = Constants.SOUTH_WEST; + //were not ready yet + this.wayPoints[17] = new WayPoint();// small backup and drop elevator + this.wayPoints[17].x = 786.0; + this.wayPoints[17].y = 650.0; + this.wayPoints[17].y_speed = 0.3; + this.wayPoints[17].x_speed = 0.3; + this.wayPoints[17].facing = Constants.NORTH; }//init @Override @@ -366,6 +399,76 @@ public void loop() { } // resetRuntime(); break; + + case 13: + this.elbow.setElbowState(3); + elevator.setElevatorState(0); +// if (!this.at_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step])) +// { +// this.goto_xy(this.wayPoints[this.current_step-1],this.wayPoints[this.current_step]); +// } +// else +// { +// driveTrain.drive(0.0,0.0); + this.current_step++; +// } + break; + case 14: + dashboardtelemtry.addData("Claw State:", claw.getClawState()); + if (this.claw.getClawState()==Constants.CLAW_OPEN) { + this.claw.closeClaw(); + startTime = getRuntime(); + settime = true; + } + else { + if (getRuntime() - startTime >= 0.6) + { + this.current_step++; + settime = false; + } + } + break; + case 15: + this.elbow.setElbowState(0); + //turn to SW + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + //elevator.setElevatorState(3); + this.current_step++; + } + break; + case 16: + // drive to basket at 135 heading + //elevator.setElevatorState(3); + if (!this.at_xy(this.wayPoints[this.current_step])) { + this.goto_xy(this.wayPoints[this.current_step]); + } else { + driveTrain.drive(0.0, 0.0); + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + this.claw.openClaw(); + this.current_step++; + } + break; + //non functional beyond this point + case 17: + if (!this.at_xy(this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step]); + } + else + { + + elevator.setElevatorState(0); + dashboardtelemtry.addData("in else 17", elevator.getState()); + driveTrain.drive(0.0,0.0); + this.current_step++; + } + break; + // if (!this.at_xy(this.wayPoints[current_step])) // { // this.goto_xy(this.wayPoints[current_step]); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Claw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Claw.java index 4e3a846..1dbde0a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Claw.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Claw.java @@ -5,6 +5,7 @@ //import com.qualcomm.robotcore.hardware.Servo; public class + Claw { private SimpleServo servo; // private HardwareMap hwMap; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java index b2d4fbc..9a12482 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elevator.java @@ -9,7 +9,7 @@ import com.qualcomm.robotcore.hardware.PIDCoefficients; import org.firstinspires.ftc.robotcore.external.Telemetry; -@TeleOp +@Config public class Elevator { static Motor elevatorMotor = null; int target_height; From 8aeb205f2549742b7568ec8df9e6421e1f0f99af Mon Sep 17 00:00:00 2001 From: Aila-skibidi Date: Fri, 9 May 2025 20:49:36 -0600 Subject: [PATCH 128/129] got farther in auto and figured out problems with practice chassie(reversed) --- .../ftc/teamcode/Auto/Compbasket_2High.java | 27 ++++++++++++++++--- .../ftc/teamcode/Mechanisms/Constants.java | 11 ++++---- .../ftc/teamcode/Mechanisms/Elbow.java | 2 +- 3 files changed, 31 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java index c57ac55..5658875 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java @@ -184,13 +184,20 @@ public void init() { this.wayPoints[16].y_speed = 0.3; this.wayPoints[16].x_speed = 0.3; this.wayPoints[16].facing = Constants.SOUTH_WEST; - //were not ready yet + this.wayPoints[17] = new WayPoint();// small backup and drop elevator - this.wayPoints[17].x = 786.0; + this.wayPoints[17].x = 75.0; this.wayPoints[17].y = 650.0; this.wayPoints[17].y_speed = 0.3; this.wayPoints[17].x_speed = 0.3; this.wayPoints[17].facing = Constants.NORTH; + + this.wayPoints[18] = new WayPoint();// line up with submersible + this.wayPoints[18].x = 1368.0; + this.wayPoints[18].y = 649.6; + this.wayPoints[18].y_speed = 0.3; + this.wayPoints[18].x_speed = 0.3; + this.wayPoints[18].facing = Constants.NORTH; }//init @Override @@ -453,7 +460,7 @@ public void loop() { this.current_step++; } break; - //non functional beyond this point + case 17: if (!this.at_xy(this.wayPoints[this.current_step])) { @@ -465,10 +472,24 @@ public void loop() { elevator.setElevatorState(0); dashboardtelemtry.addData("in else 17", elevator.getState()); driveTrain.drive(0.0,0.0); + claw.closeClaw(); this.current_step++; } break; + case 18: + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0, 0.0); + //elevator.setElevatorState(3); + this.current_step++; + } + break; // if (!this.at_xy(this.wayPoints[current_step])) // { // this.goto_xy(this.wayPoints[current_step]); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java index 91f1b2a..62b382b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Constants.java @@ -41,26 +41,27 @@ public static final double RF_OPEN = 0.2 ; // open position*/ // for elevator moves public static final int ELEVATOR_START = 0; - public static final int ELEVATOR_PICKUP_SPECIMEN = 100; - public static final int ELEVATOR_HIGH_BAR = 700; // nov 22 + public static final int ELEVATOR_PICKUP_SPECIMEN = 40; + public static final int ELEVATOR_HIGH_BAR = 1833; // nov 22 // public static final int ELEVATOR_LOW_BAR= 350; // nov 22 - public static final int ELEVATOR_HIGH_BASKET = 1285; - public static final int ELEVATOR_MAX = 1300; + public static final int ELEVATOR_HIGH_BASKET = 3578; + public static final int ELEVATOR_MAX = 4337; public static final int ELEVATOR_MIN = 0; public static final double ELEVATOR_SPEED_FACTOR = 0.7; // 1.0 Old elevator and 0.3 for new elevator // public static final double ELEVATOR_POSITION_TOLERANCE = 15.0; // public static final double ELEVATOR_POSITION_COEFFICIENT = 0.05; // public static final int ELEVATOR_STUCK_COUNT = 5; // Claw constants + //robot 2 need to be fixed, values were revered public static boolean CLAW_OPEN = true; public static boolean CLAW_CLOSED = false; public static final double SERVO_CLAW_OPEN = 0.2 ; public static final double SERVO_CLAW_CLOSED = 0.8 ; // =========== for elbow movements ====================== - public static final int ELBOW_START = 0; +//robot 2 need to be fixed, values were revered // public static final double ELBOW_BOTTOM = 0.3; public static final double ELBOW_START_POSITION = 0.0; // public static final double ELBOW_PICKUP_SAMPLE = 1.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java index df17348..cfd956e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Mechanisms/Elbow.java @@ -10,7 +10,7 @@ public class Elbow { private HardwareMap hwMap; public void init() { - this.setElbowState(Constants.ELBOW_START); + this.armServo.setPosition(Constants.ELBOW_START_POSITION); } public Elbow(HardwareMap hwMap) { From 62f85d77e3ed39748eb5ac18a6acec1476aaf3b2 Mon Sep 17 00:00:00 2001 From: Aila-skibidi Date: Fri, 16 May 2025 19:30:18 -0600 Subject: [PATCH 129/129] last waypoint --- .../ftc/teamcode/Auto/Compbasket_2High.java | 49 ++++++++++++++++++- 1 file changed, 48 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java index 5658875..5aea354 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Compbasket_2High.java @@ -19,7 +19,7 @@ public class Compbasket_2High extends BaseAutoComp { private DriveTrain2025 driveTrain; private int current_step; - private int total_waypoints = 20; + private int total_waypoints = 25; WayPoint[] wayPoints; CAITelemetry dashboardtelemtry; private boolean settime = false; @@ -198,6 +198,21 @@ public void init() { this.wayPoints[18].y_speed = 0.3; this.wayPoints[18].x_speed = 0.3; this.wayPoints[18].facing = Constants.NORTH; + + this.wayPoints[19] = new WayPoint();// line up with submersible + this.wayPoints[19].x = 1368.0; + this.wayPoints[19].y = -37; + this.wayPoints[19].y_speed = 0.3; + this.wayPoints[19].x_speed = 0.3; + this.wayPoints[19].facing = Constants.EAST; + + this.wayPoints[20] = new WayPoint();// line up with submersible + this.wayPoints[20].x = 1368.0; + this.wayPoints[20].y = -87; + this.wayPoints[20].y_speed = 0.3; + this.wayPoints[20].x_speed = 0.3; + this.wayPoints[20].facing = Constants.EAST; + }//init @Override @@ -490,6 +505,38 @@ public void loop() { this.current_step++; } break; + case 19: + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step]); + } + else + { + driveTrain.drive(0.0, 0.0); + //elevator.setElevatorState(3); + this.current_step++; + } + break; + + case 20: + driveTrain.setFacing(this.wayPoints[this.current_step].facing); + if (!this.at_xy(this.wayPoints[this.current_step])) + { + this.goto_xy(this.wayPoints[this.current_step]); + elevator.setElevatorState(3); + } + else + { + driveTrain.drive(0.0, 0.0); + elevator.setElevatorState(3); + this.current_step++; + } + break; + +// + + // if (!this.at_xy(this.wayPoints[current_step])) // { // this.goto_xy(this.wayPoints[current_step]);