From e807b5cbca5ef6e8372fd7069828d97aa3d6b5f5 Mon Sep 17 00:00:00 2001 From: Nicholas Garnsworthy Date: Tue, 11 Nov 2025 19:48:30 -0600 Subject: [PATCH 1/2] Created odometry for Drivetrain.java --- .../firstinspires/ftc/teamcode/Constants.java | 20 ++++++++++ .../ftc/teamcode/OpModes/PrimaryOpMode.java | 9 ++++- .../ftc/teamcode/subsystems/Drivetrain.java | 39 ++++++++++++++++++- 3 files changed, 66 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java index bb7a2a8f22f8..33919f04bb18 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java @@ -1,11 +1,31 @@ package org.firstinspires.ftc.teamcode; +import com.arcrobotics.ftclib.geometry.Translation2d; +import com.arcrobotics.ftclib.kinematics.wpilibkinematics.MecanumDriveKinematics; + public final class Constants { public static class Drivetrain { public static final String flMotorName = "frontLeftDrive"; public static final String frMotorName = "frontRightDrive"; public static final String blMotorName = "backLeftDrive"; public static final String brMotorName = "backRightDrive"; + + private static final double length = 0.2873375; + private static final double width = 0.3444875; + public static final Translation2d frontLeftLocation = + new Translation2d(length/2, 0.381); + public static final Translation2d frontRightLocation = + new Translation2d(length/2, -0.381); + public static final Translation2d backLeftLocation = + new Translation2d(-length/2, 0.381); + public static final Translation2d backRightLocation = + new Translation2d(-length/2, -0.381); + + public static final MecanumDriveKinematics kinematics = new MecanumDriveKinematics + ( + frontLeftLocation, frontRightLocation, + backLeftLocation, backRightLocation + ); } public static class Shooter { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/PrimaryOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/PrimaryOpMode.java index 77b5c3e44524..ea26b93abd9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/PrimaryOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/PrimaryOpMode.java @@ -3,7 +3,10 @@ import com.arcrobotics.ftclib.command.CommandOpMode; import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.arcrobotics.ftclib.gamepad.GamepadKeys; +import com.arcrobotics.ftclib.hardware.GyroEx; +import com.arcrobotics.ftclib.hardware.RevIMU; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.IntegratingGyroscope; import org.firstinspires.ftc.teamcode.Constants; import org.firstinspires.ftc.teamcode.commands.RunIntake; @@ -21,18 +24,22 @@ public class PrimaryOpMode extends CommandOpMode { private GamepadEx driverOp; private GamepadEx coOp; + private GyroEx gyroscope; @Override public void initialize() { driverOp = new GamepadEx(gamepad1); coOp = new GamepadEx(gamepad2); + gyroscope = new RevIMU(hardwareMap); + drivetrain = new Drivetrain( hardwareMap, Constants.Drivetrain.flMotorName, Constants.Drivetrain.frMotorName, Constants.Drivetrain.blMotorName, - Constants.Drivetrain.brMotorName + Constants.Drivetrain.brMotorName, + gyroscope ); drivetrain.setDefaultCommand(new UserDrive(drivetrain, driverOp)); register(drivetrain); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java index c717e67e8c50..da885948864f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java @@ -2,26 +2,63 @@ import com.arcrobotics.ftclib.command.SubsystemBase; import com.arcrobotics.ftclib.drivebase.MecanumDrive; +import com.arcrobotics.ftclib.geometry.Pose2d; +import com.arcrobotics.ftclib.geometry.Rotation2d; +import com.arcrobotics.ftclib.hardware.GyroEx; import com.arcrobotics.ftclib.hardware.motors.Motor; +import com.arcrobotics.ftclib.kinematics.HolonomicOdometry; +import com.arcrobotics.ftclib.kinematics.wpilibkinematics.MecanumDriveOdometry; +import com.arcrobotics.ftclib.kinematics.wpilibkinematics.MecanumDriveWheelSpeeds; +import com.qualcomm.robotcore.hardware.Gyroscope; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IntegratingGyroscope; + +import org.firstinspires.ftc.teamcode.Constants; public class Drivetrain extends SubsystemBase { private final Motor fL, fR, bL, bR; + private final Motor.Encoder fLE, fRE, bLE, bRE; + private final MecanumDrive drive; + private final MecanumDriveOdometry odometry; + private final GyroEx gyroscope; + private Pose2d pose; - public Drivetrain(HardwareMap hardwareMap, String flName, String frName, String blName, String brName) { + public Drivetrain(HardwareMap hardwareMap, String flName, String frName, String blName, String brName, GyroEx gyroscope) { fL = new Motor(hardwareMap, flName); + fLE = fL.encoder; fR = new Motor(hardwareMap, frName); + fRE = fR.encoder; bL = new Motor(hardwareMap, blName); + bLE = bL.encoder; bR = new Motor(hardwareMap, brName); + bRE = bR.encoder; + this.gyroscope = gyroscope; drive = new MecanumDrive(fL, fR, bL, bR); + + odometry = new MecanumDriveOdometry( + Constants.Drivetrain.kinematics, gyroscope.getRotation2d(), + new Pose2d(0.0, 0.0, new Rotation2d() + )); + } + + @Override + public void periodic(){ + MecanumDriveWheelSpeeds wheelSpeeds = new MecanumDriveWheelSpeeds( + fLE.getRate(), fRE.getRate(), + bLE.getRate(), bRE.getRate() + ); + + pose = odometry.updateWithTime(System.currentTimeMillis()/1000.0, gyroscope.getRotation2d(), wheelSpeeds); } public void move(double strafe, double forward, double rotate) { drive.driveRobotCentric(strafe, forward, rotate); } + public Pose2d getPose() {return pose;} + public void stop() { drive.stop(); } From ed42b2e2096dd9e1df7b5662054ecfd59112c653 Mon Sep 17 00:00:00 2001 From: Nick Date: Tue, 18 Nov 2025 18:44:50 -0600 Subject: [PATCH 2/2] Fix stupid errors from sleep depervation --- .../java/org/firstinspires/ftc/teamcode/Constants.java | 8 ++++---- .../firstinspires/ftc/teamcode/OpModes/BackupOpMode.java | 7 ++++++- .../firstinspires/ftc/teamcode/subsystems/Drivetrain.java | 3 --- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java index 33919f04bb18..466a3c40016c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java @@ -13,13 +13,13 @@ public static class Drivetrain { private static final double length = 0.2873375; private static final double width = 0.3444875; public static final Translation2d frontLeftLocation = - new Translation2d(length/2, 0.381); + new Translation2d(length/2, width/2); public static final Translation2d frontRightLocation = - new Translation2d(length/2, -0.381); + new Translation2d(length/2, -width/2); public static final Translation2d backLeftLocation = - new Translation2d(-length/2, 0.381); + new Translation2d(-length/2, width/2); public static final Translation2d backRightLocation = - new Translation2d(-length/2, -0.381); + new Translation2d(-length/2, -width/2); public static final MecanumDriveKinematics kinematics = new MecanumDriveKinematics ( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/BackupOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/BackupOpMode.java index 952819913fca..c2509bb52026 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/BackupOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/BackupOpMode.java @@ -3,6 +3,8 @@ import com.arcrobotics.ftclib.command.CommandOpMode; import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.arcrobotics.ftclib.gamepad.GamepadKeys; +import com.arcrobotics.ftclib.hardware.GyroEx; +import com.arcrobotics.ftclib.hardware.RevIMU; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.Constants; @@ -24,12 +26,15 @@ public void initialize() { driverOp = new GamepadEx(gamepad1); coOp = new GamepadEx(gamepad2); + GyroEx gyroscope = new RevIMU(hardwareMap); + drivetrain = new Drivetrain( hardwareMap, Constants.Drivetrain.flMotorName, Constants.Drivetrain.frMotorName, Constants.Drivetrain.blMotorName, - Constants.Drivetrain.brMotorName + Constants.Drivetrain.brMotorName, + gyroscope ); drivetrain.setDefaultCommand(new UserDrive(drivetrain, driverOp)); register(drivetrain); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java index da885948864f..6ac0a610af85 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java @@ -6,12 +6,9 @@ import com.arcrobotics.ftclib.geometry.Rotation2d; import com.arcrobotics.ftclib.hardware.GyroEx; import com.arcrobotics.ftclib.hardware.motors.Motor; -import com.arcrobotics.ftclib.kinematics.HolonomicOdometry; import com.arcrobotics.ftclib.kinematics.wpilibkinematics.MecanumDriveOdometry; import com.arcrobotics.ftclib.kinematics.wpilibkinematics.MecanumDriveWheelSpeeds; -import com.qualcomm.robotcore.hardware.Gyroscope; import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.IntegratingGyroscope; import org.firstinspires.ftc.teamcode.Constants;