Skip to content

Created odometry for Drivetrain.java #33

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
ngarnsworthy merged 2 commits into from
Nov 19, 2025
Merged

Created odometry for Drivetrain.java #33

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e807b5c
Created odometry for Drivetrain.java
ngarnsworthy Nov 12, 2025
ed42b2e
Fix stupid errors from sleep depervation
ngarnsworthy Nov 19, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 20 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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;
Copy link

Copilot AI Nov 19, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Extra space before double - should be a single space for consistency.

Suggested change
private static final double width = 0.3444875;
private static final double width = 0.3444875;

Copilot uses AI. Check for mistakes.
Copy link

Copilot AI Nov 19, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Extra space between final and double should be removed for consistency with the line above.

Copilot uses AI. Check for mistakes.
public static final Translation2d frontLeftLocation =
new Translation2d(length/2, width/2);
public static final Translation2d frontRightLocation =
new Translation2d(length/2, -width/2);
public static final Translation2d backLeftLocation =
new Translation2d(-length/2, width/2);
public static final Translation2d backRightLocation =
new Translation2d(-length/2, -width/2);

public static final MecanumDriveKinematics kinematics = new MecanumDriveKinematics
(
frontLeftLocation, frontRightLocation,
backLeftLocation, backRightLocation
);
}

public static class Shooter {
Expand Down
7 changes: 6 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/BackupOpMode.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);
Expand Down
9 changes: 8 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/PrimaryOpMode.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Copy link

Copilot AI Nov 19, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This import is unused and should be removed.

Suggested change
import com.qualcomm.robotcore.hardware.IntegratingGyroscope;

Copilot uses AI. Check for mistakes.
Copy link

Copilot AI Nov 19, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The import com.qualcomm.robotcore.hardware.IntegratingGyroscope is unused and should be removed.

Suggested change
import com.qualcomm.robotcore.hardware.IntegratingGyroscope;

Copilot uses AI. Check for mistakes.

import org.firstinspires.ftc.teamcode.Constants;
import org.firstinspires.ftc.teamcode.commands.RunIntake;
Expand All @@ -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);
Expand Down
36 changes: 35 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,26 +2,60 @@

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.wpilibkinematics.MecanumDriveOdometry;
import com.arcrobotics.ftclib.kinematics.wpilibkinematics.MecanumDriveWheelSpeeds;
import com.qualcomm.robotcore.hardware.HardwareMap;

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;
Copy link

Copilot AI Nov 19, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The pose field is not initialized and will be null until periodic() is called. This will cause a NullPointerException if getPose() is called before the first periodic() execution. Consider initializing it in the constructor: pose = new Pose2d(0.0, 0.0, new Rotation2d());

Copilot uses AI. Check for mistakes.
Copy link

Copilot AI Nov 19, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The pose field is not initialized in the constructor, which means it will be null until the first periodic() call. This could cause a NullPointerException if getPose() is called before periodic() runs. Initialize pose in the constructor with the initial pose value: this.pose = new Pose2d(0.0, 0.0, new Rotation2d());

Copilot uses AI. Check for mistakes.

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(){
Copy link

Copilot AI Nov 19, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Missing space after method name - should be periodic() { for consistency with Java style conventions.

Suggested change
public void periodic(){
public void periodic() {

Copilot uses AI. Check for mistakes.
MecanumDriveWheelSpeeds wheelSpeeds = new MecanumDriveWheelSpeeds(
fLE.getRate(), fRE.getRate(),
bLE.getRate(), bRE.getRate()
);

pose = odometry.updateWithTime(System.currentTimeMillis()/1000.0, gyroscope.getRotation2d(), wheelSpeeds);
Copy link

Copilot AI Nov 19, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The time calculation System.currentTimeMillis()/1000.0 performs integer division before converting to double, potentially losing precision. Use System.currentTimeMillis()/1000.0 which should work, but consider using System.nanoTime() for better precision: System.nanoTime() / 1e9 or explicitly cast to double: (double)System.currentTimeMillis() / 1000.0

Copilot uses AI. Check for mistakes.
}

public void move(double strafe, double forward, double rotate) {
drive.driveRobotCentric(strafe, forward, rotate);
}

public Pose2d getPose() {return pose;}
Copy link

Copilot AI Nov 19, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Missing space after getPose() and before {. Should be formatted as public Pose2d getPose() { return pose; } or split across multiple lines for consistency.

Suggested change
public Pose2d getPose() {return pose;}
public Pose2d getPose() { return pose; }

Copilot uses AI. Check for mistakes.

public void stop() {
drive.stop();
}
Expand Down
Loading