From 01453c24d4cb2dbe0497b5dd8bd043496460a899 Mon Sep 17 00:00:00 2001 From: Ryan Brott Date: Tue, 5 Dec 2023 23:18:58 -0800 Subject: [PATCH] Add TODO comments --- .../firstinspires/ftc/teamcode/MecanumDrive.java | 13 +++++++++++++ .../org/firstinspires/ftc/teamcode/TankDrive.java | 15 ++++++++++++++- .../ftc/teamcode/ThreeDeadWheelLocalizer.java | 3 +++ .../ftc/teamcode/TwoDeadWheelLocalizer.java | 3 +++ .../ftc/teamcode/tuning/TuningOpModes.java | 1 + 5 files changed, 34 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 34dc335..db06bbf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -34,6 +34,7 @@ import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.VoltageSensor; @@ -53,6 +54,8 @@ import java.util.List; public final class MecanumDrive { public static class Params { // IMU orientation + // TODO: fill in these values based on + // see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = @@ -136,6 +139,9 @@ public final class MecanumDrive { lastRightFrontPos = rightFront.getPositionAndVelocity().position; lastHeading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + + // TODO: reverse encoders if needed + // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); } @Override @@ -193,6 +199,8 @@ public final class MecanumDrive { module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); } + // TODO: make sure your config has motors with these names (or change them) + // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); leftBack = hardwareMap.get(DcMotorEx.class, "leftBack"); rightBack = hardwareMap.get(DcMotorEx.class, "rightBack"); @@ -203,6 +211,11 @@ public final class MecanumDrive { rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + // TODO: reverse motor directions if needed + // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + + // TODO: make sure your config has an IMU with this name (can be BNO or BHI) + // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html imu = hardwareMap.get(IMU.class, "imu"); IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( PARAMS.logoFacingDirection, PARAMS.usbFacingDirection)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java index a0da555..ae2b28e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java @@ -40,12 +40,12 @@ import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage; -import org.firstinspires.ftc.teamcode.messages.MecanumEncodersMessage; import org.firstinspires.ftc.teamcode.messages.PoseMessage; import org.firstinspires.ftc.teamcode.messages.TankCommandMessage; import org.firstinspires.ftc.teamcode.messages.TankEncodersMessage; @@ -60,6 +60,8 @@ import java.util.List; public final class TankDrive { public static class Params { // IMU orientation + // TODO: fill in these values based on + // see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = @@ -150,6 +152,9 @@ public final class TankDrive { lastRightPos /= rightEncs.size(); this.rightEncs = Collections.unmodifiableList(rightEncs); } + + // TODO: reverse encoder directions if needed + // leftEncs.get(0).setDirection(DcMotorSimple.Direction.REVERSE); } @Override @@ -205,6 +210,9 @@ public final class TankDrive { module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); } + // TODO: make sure your config has motors with these names (or change them) + // add additional motors on each side if you have them + // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html leftMotors = Arrays.asList(hardwareMap.get(DcMotorEx.class, "left")); rightMotors = Arrays.asList(hardwareMap.get(DcMotorEx.class, "right")); @@ -215,6 +223,11 @@ public final class TankDrive { m.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); } + // TODO: reverse motor directions if needed + // leftMotors.get(0).setDirection(DcMotorSimple.Direction.REVERSE); + + // TODO: make sure your config has an IMU with this name (can be BNO or BHI) + // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html imu = hardwareMap.get(IMU.class, "imu"); IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( PARAMS.logoFacingDirection, PARAMS.usbFacingDirection)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java index d17abce..59f6063 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java @@ -30,6 +30,9 @@ public final class ThreeDeadWheelLocalizer implements Localizer { private int lastPar0Pos, lastPar1Pos, lastPerpPos; public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) { + // TODO: make sure your config has **motors** with these names (or change them) + // the encoders should be plugged into the slot matching the named motor + // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par0"))); par1 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par1"))); perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp"))); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java index d1e3208..1ad010d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java @@ -37,6 +37,9 @@ public final class TwoDeadWheelLocalizer implements Localizer { private double lastRawHeadingVel, headingVelOffset; public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick) { + // TODO: make sure your config has **motors** with these names (or change them) + // the encoders should be plugged into the slot matching the named motor + // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html par = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par"))); perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp"))); this.imu = imu; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java index 658f790..78ce476 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java @@ -31,6 +31,7 @@ import java.util.Arrays; import java.util.List; public final class TuningOpModes { + // TODO: change this to TankDrive.class if you're using tank public static final Class DRIVE_CLASS = MecanumDrive.class; public static final String GROUP = "quickstart";