Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into photon-and-pathplanner-Overhaul-Swerve

Browse Source
This commit is contained in:
Michael Mikovsky
2025-01-14 17:42:24 -07:00
parent cfe3ece8e5 b4fc99f670
commit 187e9fe31a
5 changed files with 162 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+18 -15
View File
@@ -51,6 +51,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
@@ -98,50 +99,52 @@ 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
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.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.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;
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);
src/main/java/frc4388/robot/Robot.java
+9
View File
@@ -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() {
src/main/java/frc4388/robot/RobotContainer.java
+15 -3
View File
@@ -14,11 +14,12 @@ 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;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -99,7 +100,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();
@@ -150,7 +151,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()));
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+2 -2
View File
@@ -79,7 +79,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)
);
@@ -181,7 +181,7 @@ public class SwerveDrive extends Subsystem {
}
public void stopModules() {
swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
// swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
}
@Override
src/main/java/frc4388/utility/Trim.java
+118
View File
@@ -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<Trim> trims = new ArrayList<Trim>();
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();
}
}
}