From b6850a11a7277252c107770180178a4a10e12bb5 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Sat, 11 Jan 2025 11:06:52 -0700 Subject: [PATCH 1/4] Calibrate 2025 swerve drive --- src/main/java/frc4388/robot/Constants.java | 30 +++++++++---------- .../java/frc4388/robot/RobotContainer.java | 2 +- .../frc4388/robot/subsystems/SwerveDrive.java | 4 +-- 3 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3bfe1d4..f31f9cb 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -80,50 +80,50 @@ public final class Constants { public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084; public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; - public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270 + public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270 private static final class ModuleSpecificConstants { //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125); + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; - private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH); private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625); + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); + private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.35205078125+.25); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); + private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; - private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); + private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH); private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); } public static final class IDs { - public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 2); - public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 3); - public static final CanDevice RIGHT_FRONT_ENCODER = new CanDevice("RIGHT_FRONT_ENCODER", 10); + public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4); + public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 5); + public static final CanDevice RIGHT_FRONT_ENCODER = new CanDevice("RIGHT_FRONT_ENCODER", 11); - public static final CanDevice LEFT_FRONT_WHEEL = new CanDevice("LEFT_FRONT_WHEEL", 4); - public static final CanDevice LEFT_FRONT_STEER = new CanDevice("LEFT_FRONT_STEER", 5); - public static final CanDevice LEFT_FRONT_ENCODER = new CanDevice("LEFT_FRONT_ENCODER", 11); + public static final CanDevice LEFT_FRONT_WHEEL = new CanDevice("LEFT_FRONT_WHEEL", 2); + public static final CanDevice LEFT_FRONT_STEER = new CanDevice("LEFT_FRONT_STEER", 3); + public static final CanDevice LEFT_FRONT_ENCODER = new CanDevice("LEFT_FRONT_ENCODER", 10); public static final CanDevice LEFT_BACK_WHEEL = new CanDevice("LEFT_BACK_WHEEL", 6); public static final CanDevice LEFT_BACK_STEER = new CanDevice("LEFT_BACK_STEER", 7); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 63f9eec..882fba7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -93,7 +93,7 @@ public class RobotContainer { m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(), - true); + false); }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); m_robotSwerveDrive.setToSlow(); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index c685f23..db8aba0 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -78,7 +78,7 @@ public class SwerveDrive extends Subsystem { if (fieldRelative) { swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() - .withVelocityX(leftStick.getX()*-speedAdjust) + .withVelocityX(leftStick.getX()*speedAdjust) .withVelocityY(leftStick.getY()*speedAdjust) .withRotationalRate(rightStick.getX()*rotSpeedAdjust) ); @@ -163,7 +163,7 @@ public class SwerveDrive extends Subsystem { } public void stopModules() { - swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); + // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); } @Override From 339b91b5c735d06e197b3211ff3140baa93747fd Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Mon, 13 Jan 2025 16:42:57 -0700 Subject: [PATCH 2/4] recalibrate 2025 back left --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f31f9cb..a336a4f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -100,7 +100,7 @@ public final class Constants { private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.35205078125+.25); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 882fba7..0a28e41 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -14,6 +14,7 @@ import java.util.ArrayList; import java.util.List; import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.GenericHID; import frc4388.utility.controller.XboxController; import frc4388.utility.controller.DeadbandedXboxController; From 739890a52ad626e10d1009e4c399d920b74610aa Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Mon, 13 Jan 2025 19:43:19 -0700 Subject: [PATCH 3/4] Create Trim.java --- src/main/java/frc4388/utility/Trim.java | 118 ++++++++++++++++++++++++ 1 file changed, 118 insertions(+) create mode 100644 src/main/java/frc4388/utility/Trim.java diff --git a/src/main/java/frc4388/utility/Trim.java b/src/main/java/frc4388/utility/Trim.java new file mode 100644 index 0000000..3d985ea --- /dev/null +++ b/src/main/java/frc4388/utility/Trim.java @@ -0,0 +1,118 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.utility; + +import java.io.FileInputStream; +import java.io.FileOutputStream; +import java.util.ArrayList; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; + +/** + * Reboot persistant Trims. + * @author Zachary Wilke + */ +public class Trim { + private static ArrayList trims = new ArrayList(); + private static ShuffleboardTab trimTab = Shuffleboard.getTab("Trims"); + + private String trimName; + private double upperBound; + private double lowerBound; + private double step; + + private boolean modified = false; + private double currentValue; + + private GenericEntry trimElement = null; + + /** + * Creates a Trim with a given name, upper and lower bounds, step size and intial value + * @param trimName please keep the trim name without special symbols + * @param upperBound the upper limit inclusive + * @param lowerBound the lower limit inclusive + * @param step the step size + * @param inital the inital value, will get overridden if the persistant trim exists on disk. + */ + public Trim(String trimName, double upperBound, double lowerBound, double step, double inital) { + this.trimName = trimName; + this.upperBound = upperBound; + this.lowerBound = lowerBound; + this.step = step; + currentValue = inital; + load(); + trimElement = trimTab.add(trimName, currentValue).getEntry(); + + trims.add(this); + } + + private void clampModify() { + currentValue = Math.min(upperBound, Math.max(currentValue, lowerBound)); + if (trimElement != null) + trimElement.setValue(currentValue); + modified = true; + } + + public void stepUp() { + this.currentValue += step; + clampModify(); + } + + public void stepDown() { + this.currentValue -= step; + clampModify(); + } + + public void set(double value) { + this.currentValue = value; + clampModify(); + } + + public double get() { + return this.currentValue; + } + + public boolean isModified() { + return modified; + } + + public boolean load() { + try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) { + double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8)); + currentValue = fileValue; + clampModify(); + modified = false; + if (fileValue != currentValue) { + System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping..."); + modified = true; + } + return true; + } catch (Exception e) { + // e.printStackTrace(); + System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value..."); + return false; + } + } + + public void dump() { + try (FileOutputStream stream = new FileOutputStream("/home/lvuser/trims/" + trimName)) { + stream.write(DataUtils.doubleToByteArray(currentValue)); + modified = false; + } catch (Exception e) { + // e.printStackTrace(); + System.out.println("TRIMS: Unable to write to trim file `" + trimName + "`!?!"); + } + } + + public static void dumpAll() { + for (int i = 0; i > trims.size(); i++) { + Trim trim = trims.get(i); + if (trim.isModified()) + trim.dump(); + } + } +} From 600a53b85b90ef9c80ef503504f2e224094f02ca Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Tue, 14 Jan 2025 17:16:09 -0700 Subject: [PATCH 4/4] fix trim dump iteration --- src/main/java/frc4388/robot/Constants.java | 3 +++ src/main/java/frc4388/robot/Robot.java | 9 +++++++++ src/main/java/frc4388/robot/RobotContainer.java | 14 +++++++++++++- src/main/java/frc4388/utility/Trim.java | 4 ++-- 4 files changed, 27 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a336a4f..1aaef2f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -33,6 +33,7 @@ import edu.wpi.first.units.measure.Distance; import frc4388.utility.CanDevice; import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; +import frc4388.utility.Trim; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -82,6 +83,8 @@ public final class Constants { public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270 + public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10); + private static final class ModuleSpecificConstants { //Front Left private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25); diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e64ca36..18e54f5 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -25,6 +25,7 @@ import frc4388.utility.DeferredBlock; import frc4388.utility.RobotTime; import frc4388.utility.Status; import frc4388.utility.Subsystem; +import frc4388.utility.Trim; import frc4388.utility.Status.Report; import frc4388.utility.Status.ReportLevel; //import frc4388.robot.subsystems.LED; @@ -160,6 +161,14 @@ public class Robot extends TimedRobot { // m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft()); } + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopExit() { // the only OTHER mode that teleop can enter into is disabled. + Trim.dumpAll(); + } + @Override public void testInit() { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0a28e41..6f8887c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.GenericHID; import frc4388.utility.controller.XboxController; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.robot.Constants.OIConstants; +import frc4388.robot.Constants.SwerveDriveConstants; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -145,7 +146,18 @@ public class RobotContainer { DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - + + // @ /* Trim Test Buttons */ + + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepUp())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepDown())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.load())); + // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); diff --git a/src/main/java/frc4388/utility/Trim.java b/src/main/java/frc4388/utility/Trim.java index 3d985ea..7a9d017 100644 --- a/src/main/java/frc4388/utility/Trim.java +++ b/src/main/java/frc4388/utility/Trim.java @@ -56,7 +56,7 @@ public class Trim { trimElement.setValue(currentValue); modified = true; } - + public void stepUp() { this.currentValue += step; clampModify(); @@ -109,7 +109,7 @@ public class Trim { } public static void dumpAll() { - for (int i = 0; i > trims.size(); i++) { + for (int i = 0; i < trims.size(); i++) { Trim trim = trims.get(i); if (trim.isModified()) trim.dump();