More work bringing 2026 into 2027, ignoring errors for pos/vel conversion factor for now
This commit is contained in:
Bradley committed
1 parent
d036299528
commit
e9a164a276
24 files changed
+1635
-12
No files matched your search
@@ -0,0 +1,390 @@
|
||||
package first.robot.utilities;
|
||||
|
||||
import org.wpilib.networktables.NetworkTableInstance;
|
||||
import org.wpilib.networktables.PubSubOption;
|
||||
import org.wpilib.networktables.StringPublisher;
|
||||
import org.wpilib.networktables.StringTopic;
|
||||
|
||||
import io.avaje.json.JsonException;
|
||||
import io.avaje.jsonb.Json;
|
||||
import io.avaje.jsonb.JsonType;
|
||||
import io.avaje.jsonb.Jsonb;
|
||||
|
||||
public final class Elastic {
|
||||
private static final StringTopic notificationTopic =
|
||||
NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications");
|
||||
private static final StringPublisher notificationPublisher =
|
||||
notificationTopic.publish(PubSubOption.SEND_ALL, PubSubOption.KEEP_DUPLICATES);
|
||||
private static final StringTopic selectedTabTopic =
|
||||
NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab");
|
||||
private static final StringPublisher selectedTabPublisher =
|
||||
selectedTabTopic.publish(PubSubOption.KEEP_DUPLICATES);
|
||||
private static final Jsonb jsonb = Jsonb.builder().build();
|
||||
private static final JsonType<Notification> notificationJsonType =
|
||||
jsonb.type(Notification.class);
|
||||
|
||||
/**
|
||||
* Represents the possible levels of notifications for the Elastic dashboard. These levels are
|
||||
* used to indicate the severity or type of notification.
|
||||
*/
|
||||
public enum NotificationLevel {
|
||||
/** Informational Message */
|
||||
INFO,
|
||||
/** Warning message */
|
||||
WARNING,
|
||||
/** Error message */
|
||||
ERROR
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends an notification to the Elastic dashboard. The notification is serialized as a JSON string
|
||||
* before being published.
|
||||
*
|
||||
* @param notification the {@link Notification} object containing notification details
|
||||
*/
|
||||
public static void sendNotification(Notification notification) {
|
||||
try {
|
||||
notificationPublisher.set(notificationJsonType.toJson(notification));
|
||||
} catch (JsonException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Selects the tab of the dashboard with the given name. If no tab matches the name, this will
|
||||
* have no effect on the widgets or tabs in view.
|
||||
*
|
||||
* <p>If the given name is a number, Elastic will select the tab whose index equals the number
|
||||
* provided.
|
||||
*
|
||||
* @param tabName the name of the tab to select
|
||||
*/
|
||||
public static void selectTab(String tabName) {
|
||||
selectedTabPublisher.set(tabName);
|
||||
}
|
||||
|
||||
/**
|
||||
* Selects the tab of the dashboard at the given index. If this index is greater than or equal to
|
||||
* the number of tabs, this will have no effect.
|
||||
*
|
||||
* @param tabIndex the index of the tab to select.
|
||||
*/
|
||||
public static void selectTab(int tabIndex) {
|
||||
selectTab(Integer.toString(tabIndex));
|
||||
}
|
||||
|
||||
/**
|
||||
* Represents an notification object to be sent to the Elastic dashboard. This object holds
|
||||
* properties such as level, title, description, display time, and dimensions to control how the
|
||||
* notification is displayed on the dashboard.
|
||||
*/
|
||||
@Json
|
||||
public static class Notification {
|
||||
@Json.Property("level")
|
||||
private NotificationLevel level;
|
||||
|
||||
@Json.Property("title")
|
||||
private String title;
|
||||
|
||||
@Json.Property("description")
|
||||
private String description;
|
||||
|
||||
@Json.Property("displayTime")
|
||||
private int displayTimeMillis;
|
||||
|
||||
@Json.Property("width")
|
||||
private double width;
|
||||
|
||||
@Json.Property("height")
|
||||
private double height;
|
||||
|
||||
/**
|
||||
* Creates a new Notification with all default parameters. This constructor is intended to be
|
||||
* used with the chainable decorator methods
|
||||
*
|
||||
* <p>Title and description fields are empty.
|
||||
*/
|
||||
public Notification() {
|
||||
this(NotificationLevel.INFO, "", "");
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new Notification with all properties specified.
|
||||
*
|
||||
* @param level the level of the notification (e.g., INFO, WARNING, ERROR)
|
||||
* @param title the title text of the notification
|
||||
* @param description the descriptive text of the notification
|
||||
* @param displayTimeMillis the time in milliseconds for which the notification is displayed
|
||||
* @param width the width of the notification display area
|
||||
* @param height the height of the notification display area, inferred if below zero
|
||||
*/
|
||||
public Notification(
|
||||
NotificationLevel level,
|
||||
String title,
|
||||
String description,
|
||||
int displayTimeMillis,
|
||||
double width,
|
||||
double height) {
|
||||
this.level = level;
|
||||
this.title = title;
|
||||
this.displayTimeMillis = displayTimeMillis;
|
||||
this.description = description;
|
||||
this.height = height;
|
||||
this.width = width;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new Notification with default display time and dimensions.
|
||||
*
|
||||
* @param level the level of the notification
|
||||
* @param title the title text of the notification
|
||||
* @param description the descriptive text of the notification
|
||||
*/
|
||||
public Notification(NotificationLevel level, String title, String description) {
|
||||
this(level, title, description, 3000, 350, -1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new Notification with a specified display time and default dimensions.
|
||||
*
|
||||
* @param level the level of the notification
|
||||
* @param title the title text of the notification
|
||||
* @param description the descriptive text of the notification
|
||||
* @param displayTimeMillis the display time in milliseconds
|
||||
*/
|
||||
public Notification(
|
||||
NotificationLevel level, String title, String description, int displayTimeMillis) {
|
||||
this(level, title, description, displayTimeMillis, 350, -1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new Notification with specified dimensions and default display time. If the height
|
||||
* is below zero, it is automatically inferred based on screen size.
|
||||
*
|
||||
* @param level the level of the notification
|
||||
* @param title the title text of the notification
|
||||
* @param description the descriptive text of the notification
|
||||
* @param width the width of the notification display area
|
||||
* @param height the height of the notification display area, inferred if below zero
|
||||
*/
|
||||
public Notification(
|
||||
NotificationLevel level, String title, String description, double width, double height) {
|
||||
this(level, title, description, 3000, width, height);
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the level of this notification
|
||||
*
|
||||
* @param level the level to set the notification to
|
||||
*/
|
||||
public void setLevel(NotificationLevel level) {
|
||||
this.level = level;
|
||||
}
|
||||
|
||||
/**
|
||||
* @return the level of this notification
|
||||
*/
|
||||
public NotificationLevel getLevel() {
|
||||
return level;
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the title of this notification
|
||||
*
|
||||
* @param title the title to set the notification to
|
||||
*/
|
||||
public void setTitle(String title) {
|
||||
this.title = title;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the title of this notification
|
||||
*
|
||||
* @return the title of this notification
|
||||
*/
|
||||
public String getTitle() {
|
||||
return title;
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the description of this notification
|
||||
*
|
||||
* @param description the description to set the notification to
|
||||
*/
|
||||
public void setDescription(String description) {
|
||||
this.description = description;
|
||||
}
|
||||
|
||||
public String getDescription() {
|
||||
return description;
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the display time of the notification
|
||||
*
|
||||
* @param seconds the number of seconds to display the notification for
|
||||
*/
|
||||
public void setDisplayTimeSeconds(double seconds) {
|
||||
setDisplayTimeMillis((int) Math.round(seconds * 1000));
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the display time of the notification in milliseconds
|
||||
*
|
||||
* @param displayTimeMillis the number of milliseconds to display the notification for
|
||||
*/
|
||||
public void setDisplayTimeMillis(int displayTimeMillis) {
|
||||
this.displayTimeMillis = displayTimeMillis;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the display time of the notification in milliseconds
|
||||
*
|
||||
* @return the number of milliseconds the notification is displayed for
|
||||
*/
|
||||
public int getDisplayTimeMillis() {
|
||||
return displayTimeMillis;
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the width of the notification
|
||||
*
|
||||
* @param width the width to set the notification to
|
||||
*/
|
||||
public void setWidth(double width) {
|
||||
this.width = width;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the width of the notification
|
||||
*
|
||||
* @return the width of the notification
|
||||
*/
|
||||
public double getWidth() {
|
||||
return width;
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the height of the notification
|
||||
*
|
||||
* <p>If the height is set to -1, the height will be determined automatically by the dashboard
|
||||
*
|
||||
* @param height the height to set the notification to
|
||||
*/
|
||||
public void setHeight(double height) {
|
||||
this.height = height;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the height of the notification
|
||||
*
|
||||
* @return the height of the notification
|
||||
*/
|
||||
public double getHeight() {
|
||||
return height;
|
||||
}
|
||||
|
||||
/**
|
||||
* Modifies the notification's level and returns itself to allow for method chaining
|
||||
*
|
||||
* @param level the level to set the notification to
|
||||
* @return the current notification
|
||||
*/
|
||||
public Notification withLevel(NotificationLevel level) {
|
||||
this.level = level;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Modifies the notification's title and returns itself to allow for method chaining
|
||||
*
|
||||
* @param title the title to set the notification to
|
||||
* @return the current notification
|
||||
*/
|
||||
public Notification withTitle(String title) {
|
||||
setTitle(title);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Modifies the notification's description and returns itself to allow for method chaining
|
||||
*
|
||||
* @param description the description to set the notification to
|
||||
* @return the current notification
|
||||
*/
|
||||
public Notification withDescription(String description) {
|
||||
setDescription(description);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Modifies the notification's display time and returns itself to allow for method chaining
|
||||
*
|
||||
* @param seconds the number of seconds to display the notification for
|
||||
* @return the current notification
|
||||
*/
|
||||
public Notification withDisplaySeconds(double seconds) {
|
||||
return withDisplayMilliseconds((int) Math.round(seconds * 1000));
|
||||
}
|
||||
|
||||
/**
|
||||
* Modifies the notification's display time and returns itself to allow for method chaining
|
||||
*
|
||||
* @param displayTimeMillis the number of milliseconds to display the notification for
|
||||
* @return the current notification
|
||||
*/
|
||||
public Notification withDisplayMilliseconds(int displayTimeMillis) {
|
||||
setDisplayTimeMillis(displayTimeMillis);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Modifies the notification's width and returns itself to allow for method chaining
|
||||
*
|
||||
* @param width the width to set the notification to
|
||||
* @return the current notification
|
||||
*/
|
||||
public Notification withWidth(double width) {
|
||||
setWidth(width);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Modifies the notification's height and returns itself to allow for method chaining
|
||||
*
|
||||
* @param height the height to set the notification to
|
||||
* @return the current notification
|
||||
*/
|
||||
public Notification withHeight(double height) {
|
||||
setHeight(height);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Modifies the notification's height and returns itself to allow for method chaining
|
||||
*
|
||||
* <p>This will set the height to -1 to have it automatically determined by the dashboard
|
||||
*
|
||||
* @return the current notification
|
||||
*/
|
||||
public Notification withAutomaticHeight() {
|
||||
setHeight(-1);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Modifies the notification to disable the auto dismiss behavior
|
||||
*
|
||||
* <p>This sets the display time to 0 milliseconds
|
||||
*
|
||||
* <p>The auto dismiss behavior can be re-enabled by setting the display time to a number
|
||||
* greater than 0
|
||||
*
|
||||
* @return the current notification
|
||||
*/
|
||||
public Notification withNoAutoDismiss() {
|
||||
setDisplayTimeMillis(0);
|
||||
return this;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,10 @@
|
||||
package first.robot.utilities;
|
||||
|
||||
import org.wpilib.math.geometry.Transform3d;
|
||||
|
||||
public record PhotonVisionConfig (
|
||||
String cameraName,
|
||||
Transform3d robotToCamera,
|
||||
double cameraHeightMeters,
|
||||
double cameraPitchRadians
|
||||
) {}
|
||||
@@ -0,0 +1,38 @@
|
||||
package first.robot.utilities;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import org.wpilib.command2.Command;
|
||||
import org.wpilib.command2.SubsystemBase;
|
||||
import org.wpilib.hardware.bus.CANPort;
|
||||
|
||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
|
||||
/**
|
||||
* A simple subsystem that can be used to test a single SparkMax and associated NEO motor
|
||||
*/
|
||||
public class SparkMAXTester extends SubsystemBase {
|
||||
private SparkMax spark;
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
*
|
||||
* @param deviceID The CAN ID of the SparkMAX that needs testing
|
||||
*/
|
||||
public SparkMAXTester(CANPort canPort, int deviceID) {
|
||||
spark = new SparkMax(canPort, deviceID, MotorType.kBrushless);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the speed of the motor
|
||||
*
|
||||
* @param speed A method or lambda which returns a double between -1 and 1
|
||||
* @return A Command object that runs indefinitely to control motor speed
|
||||
*/
|
||||
public Command setSpeed(DoubleSupplier speed) {
|
||||
return run(() -> {
|
||||
spark.setThrottle(speed.getAsDouble());
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,226 @@
|
||||
package first.robot.utilities;
|
||||
|
||||
import org.wpilib.hardware.bus.CANPort;
|
||||
import org.wpilib.hardware.rotation.AnalogEncoder;
|
||||
import org.wpilib.math.geometry.Rotation2d;
|
||||
import org.wpilib.math.kinematics.SwerveModulePosition;
|
||||
import org.wpilib.math.kinematics.SwerveModuleVelocity;
|
||||
|
||||
import com.ctre.phoenix6.CANBus;
|
||||
import com.ctre.phoenix6.controls.VelocityVoltage;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.revrobotics.PersistMode;
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
import com.revrobotics.ResetMode;
|
||||
import com.revrobotics.spark.SparkClosedLoopController;
|
||||
import com.revrobotics.spark.SparkLowLevel.ControlType;
|
||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
|
||||
import first.robot.constants.ModuleConstants;
|
||||
import first.robot.constants.ModuleConstants.ModuleName;
|
||||
|
||||
/*
|
||||
* This thread
|
||||
*
|
||||
* https://www.chiefdelphi.com/t/best-easiest-way-to-connect-wire-and-program-thrifty-absolute-magnetic-encoder-to-rev-spark-max-motor-controller/439040/30
|
||||
*
|
||||
* implies that the best use of the thrifty absolute encoder is to use it as a reference for the Spark relative encoder and then
|
||||
* used the closed loop control on the controller for turning
|
||||
*
|
||||
* IDK if that's really necessary, the read rate of the analog ports is 100HZ, I suppose the only benefit is the higher rate of
|
||||
* the controller closed loop controller.
|
||||
*/
|
||||
public class SwerveModule {
|
||||
private ModuleName moduleName;
|
||||
|
||||
private TalonFX drive;
|
||||
private SparkMax turning;
|
||||
|
||||
private RelativeEncoder turningRelativeEncoder;
|
||||
|
||||
private AnalogEncoder turningAbsoluteEncoder;
|
||||
|
||||
private SparkClosedLoopController turningClosedLoopController;
|
||||
|
||||
private VelocityVoltage driveVelocityRequest;
|
||||
|
||||
private SwerveModuleVelocity lastTargetState;
|
||||
private SwerveModuleVelocity lastTargetStateOptimized;
|
||||
|
||||
private boolean isAbsoluteEncoderDisabled;
|
||||
private boolean turningEncoderAutoRezeroEnabled;
|
||||
|
||||
/**
|
||||
* Builds the swerve module but with the Absolute Encoder disabled.
|
||||
*
|
||||
* This constructor assumes you zeroed the swerve modules (faced all the bevel gears to the left)
|
||||
* before booting up the robot.
|
||||
*
|
||||
* @param canPort The CANPort enumeration that represents the CAN bus that the motors for this module live on
|
||||
* @param moduleName The module name, Front Left, Front Right, etc.
|
||||
* @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
|
||||
* @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
|
||||
*/
|
||||
public SwerveModule(CANPort canPort, ModuleName moduleName, int drivingCANID, int turningCANID) {
|
||||
this(canPort, moduleName, drivingCANID, turningCANID, -1, -1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Builds the swerve module with the normal features, disables automatic rezeroing of the turning encoder
|
||||
* from the absolute encoder.
|
||||
*
|
||||
* @param canPort The CANPort enumeration that represents the CAN bus that the motors for this module live on
|
||||
* @param moduleName The module name, Front Left, Front Right, etc.
|
||||
* @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
|
||||
* @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
|
||||
* @param analogEncoderID The Analog In port ID for the Thrify Absolute Encoder
|
||||
* @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module
|
||||
*/
|
||||
public SwerveModule(CANPort canPort, ModuleName moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) {
|
||||
this(canPort, moduleName, drivingCANID, turningCANID, analogEncoderID, analogEncoderOffset, false);
|
||||
}
|
||||
|
||||
/**
|
||||
* Builds the swerve module with the normal features, and gives the option to enable automatic turning encoder rezeroing
|
||||
* when the turning motor is not moving
|
||||
*
|
||||
* @param canPort The CANPort enumeration that represents the CAN bus that the motors for this module live on
|
||||
* @param moduleName The module name, Front Left, Front Right, etc.
|
||||
* @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
|
||||
* @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
|
||||
* @param analogEncoderID The Analog In port ID for the Thrify Absolute Encoder
|
||||
* @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module
|
||||
* @param turningEncoderAutoRezeroEnabled Should the turning encoder in the NEO automatically rezero from the absolute encoder
|
||||
*/
|
||||
public SwerveModule(CANPort canPort, ModuleName moduleName, int drivingCANID, int turningCANID,
|
||||
int analogEncoderID, double analogEncoderOffset, boolean turningEncoderAutoRezeroEnabled) {
|
||||
isAbsoluteEncoderDisabled = (analogEncoderID == -1) || (analogEncoderOffset < 0);
|
||||
|
||||
drive = new TalonFX(drivingCANID, new CANBus(canPort));
|
||||
turning = new SparkMax(canPort, turningCANID, MotorType.kBrushless);
|
||||
|
||||
turningRelativeEncoder = turning.getEncoder();
|
||||
|
||||
if(!isAbsoluteEncoderDisabled) {
|
||||
turningAbsoluteEncoder = new AnalogEncoder(analogEncoderID, 2 * Math.PI, analogEncoderOffset);
|
||||
}
|
||||
|
||||
turningClosedLoopController = turning.getClosedLoopController();
|
||||
|
||||
drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
|
||||
drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
|
||||
drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
|
||||
drive.getConfigurator().apply(ModuleConstants.kAudioConfig);
|
||||
drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
|
||||
drive.getConfigurator().apply(ModuleConstants.kDriveClosedLoopRampConfig);
|
||||
|
||||
turning.configure(
|
||||
ModuleConstants.turningConfig,
|
||||
ResetMode.kResetSafeParameters,
|
||||
PersistMode.kPersistParameters
|
||||
);
|
||||
|
||||
driveVelocityRequest = new VelocityVoltage(0);
|
||||
|
||||
if(isAbsoluteEncoderDisabled){
|
||||
turningRelativeEncoder.setPosition(0);
|
||||
} else {
|
||||
turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get());
|
||||
}
|
||||
|
||||
drive.setPosition(0);
|
||||
|
||||
this.lastTargetState = getState();
|
||||
this.lastTargetStateOptimized = getState();
|
||||
|
||||
this.turningEncoderAutoRezeroEnabled = turningEncoderAutoRezeroEnabled;
|
||||
|
||||
this.moduleName = moduleName;
|
||||
}
|
||||
|
||||
public void periodic() {
|
||||
|
||||
if(!isAbsoluteEncoderDisabled) {
|
||||
Logger.recordOutput(moduleName.getLoggableName() + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
|
||||
}
|
||||
|
||||
Logger.recordOutput(moduleName.getLoggableName() + "/ModuleTargetState", lastTargetState);
|
||||
Logger.recordOutput(moduleName.getLoggableName() + "/ModuleTargetStateOptimized", lastTargetStateOptimized);
|
||||
Logger.recordOutput(moduleName.getLoggableName() + "/SwerveModuleState", getState());
|
||||
Logger.recordOutput(moduleName.getLoggableName() + "/SwerveModulePosition", getPosition());
|
||||
Logger.recordOutput(moduleName.getLoggableName() + "/RelativeEncoderPosition", getTurningEncoderPosition());
|
||||
|
||||
// TODO Re-enable this? Was turned off when there was drivetrain issues
|
||||
// Now that there aren't, do we try this again?
|
||||
/*
|
||||
if(!isAbsoluteEncoderDisabled && turningEncoderAutoRezeroEnabled) {
|
||||
if(Math.abs(getState().angle.getRadians() - lastTargetState.angle.getRadians()) <= ModuleConstants.kAutoResetPositionDeadband) {
|
||||
resetEncoders();
|
||||
}
|
||||
}*/
|
||||
}
|
||||
|
||||
public ModuleName getModuleName() {
|
||||
return moduleName;
|
||||
}
|
||||
|
||||
public SwerveModuleVelocity getState() {
|
||||
return new SwerveModuleVelocity(
|
||||
drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters,
|
||||
new Rotation2d(getTurningEncoderPosition())
|
||||
);
|
||||
}
|
||||
|
||||
public SwerveModulePosition getPosition() {
|
||||
return new SwerveModulePosition(
|
||||
drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters,
|
||||
new Rotation2d(getTurningEncoderPosition())
|
||||
);
|
||||
}
|
||||
|
||||
public void disableOutput() {
|
||||
drive.disable();
|
||||
turning.disable();
|
||||
}
|
||||
|
||||
public void setDesiredState(SwerveModuleVelocity desiredState) {
|
||||
lastTargetState = new SwerveModuleVelocity(desiredState.velocity, desiredState.angle);
|
||||
|
||||
desiredState.optimize(new Rotation2d(getTurningEncoderPosition()));
|
||||
|
||||
lastTargetStateOptimized = desiredState;
|
||||
|
||||
drive.setControl(
|
||||
driveVelocityRequest.withVelocity(
|
||||
desiredState.velocity / ModuleConstants.kWheelCircumferenceMeters
|
||||
).withFeedForward(
|
||||
desiredState.velocity / ModuleConstants.kWheelCircumferenceMeters
|
||||
)
|
||||
);
|
||||
|
||||
turningClosedLoopController.setSetpoint(
|
||||
desiredState.angle.getRadians(),
|
||||
ControlType.kPosition
|
||||
);
|
||||
}
|
||||
|
||||
public double getTurningEncoderPosition() {
|
||||
return turningRelativeEncoder.getPosition().get() * (ModuleConstants.kIsEncoderInverted ? -1 : 1);
|
||||
}
|
||||
|
||||
public void resetEncoders() {
|
||||
drive.setPosition(0);
|
||||
|
||||
zeroTurningEncoder();
|
||||
}
|
||||
|
||||
public void zeroTurningEncoder() {
|
||||
if(isAbsoluteEncoderDisabled) {
|
||||
turningRelativeEncoder.setPosition(0);
|
||||
} else {
|
||||
turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,119 @@
|
||||
package first.robot.utilities;
|
||||
|
||||
import java.util.Optional;
|
||||
|
||||
import org.wpilib.driverstation.Alliance;
|
||||
import org.wpilib.driverstation.MatchState;
|
||||
import org.wpilib.math.geometry.Pose2d;
|
||||
|
||||
import first.robot.constants.CompetitionConstants;
|
||||
|
||||
public class Utilities {
|
||||
public static final double kG = -9.81;
|
||||
|
||||
/**
|
||||
* Returns the Alliance enumeration that indicates who will have the first
|
||||
* shift. Returns null if the data is not available.
|
||||
*
|
||||
* @return The Alliance that will have the first shift, or null if game specific data
|
||||
* is not present
|
||||
*/
|
||||
public static Alliance whoHasFirstShift() {
|
||||
Optional<String> gameData = MatchState.getGameData();
|
||||
|
||||
if(gameData.isPresent()) {
|
||||
String gameDataString = gameData.get();
|
||||
|
||||
if(gameDataString.length() > 0) {
|
||||
switch (gameDataString.charAt(0)) {
|
||||
case 'B':
|
||||
return Alliance.RED;
|
||||
case 'R':
|
||||
return Alliance.BLUE;
|
||||
default:
|
||||
return null;
|
||||
}
|
||||
}
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the pose of the hub for the given alliance assigned to our robot.
|
||||
* If no alliance is assigned (which is unlikely) this method returns
|
||||
* the Blue hub pose, which is the closet to the field origin
|
||||
*
|
||||
* @return The Pose2d object which represents the appropriate pose of the Hub
|
||||
*/
|
||||
public static Pose2d getHubPose() {
|
||||
Optional<Alliance> alliance = MatchState.getAlliance();
|
||||
|
||||
if(alliance.isEmpty() || alliance.get() == Alliance.BLUE) {
|
||||
return CompetitionConstants.kBlueHubLocation;
|
||||
} else {
|
||||
return CompetitionConstants.kRedHubLocation;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the AprilTag ID of the tag that is in the center of the hub
|
||||
* for the robot's assigned alliance. If no alliance is assigned (which is unlikely)
|
||||
* the Blue hub's center tag is returned.
|
||||
*
|
||||
* @return The tag ID that is in the center of the assigned alliance's hub
|
||||
*/
|
||||
public static int getHubCenterAprilTagID() {
|
||||
Optional<Alliance> alliance = MatchState.getAlliance();
|
||||
|
||||
if(alliance.isEmpty() || alliance.get() == Alliance.BLUE) {
|
||||
return 26;
|
||||
} else {
|
||||
return 10;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* A ChatGPT possible hallucination related to calcuating whether a shot is possible
|
||||
* for a given speed and change in X and Y position
|
||||
*
|
||||
* Note that X in this scenario is the physical distance from the shooter exit to
|
||||
* target. Y is the change in height from the shooter exit to the target height
|
||||
*
|
||||
* @param targetVMPS The target velocity of the shooter in Meters per Second
|
||||
* @param deltaXM The "as the crow flies" distance between the shooter exit and the target
|
||||
* @param deltaYM The height difference between the shooter exit and the target
|
||||
* @return A true or false value indicating whether the shot is possible
|
||||
*/
|
||||
public static boolean shotPossible(double targetVMPS, double deltaXM, double deltaYM) {
|
||||
return Math.pow(targetVMPS, 4) >=
|
||||
-9.81 * (-9.81 * Math.pow(deltaXM, 2) + 2 * deltaYM * Math.pow(targetVMPS, 2));
|
||||
}
|
||||
|
||||
/**
|
||||
* A ChatGPT possible hallucination that calculates the angle required to make a shot for
|
||||
* a given speed and change in X and Y position
|
||||
*
|
||||
* Note that X in this scenario is the physical disatance from the shooter exit to
|
||||
* target. Y is the change in height from the shooter exit to the target height
|
||||
*
|
||||
* Setting softerShot to true changes the angle of attack to a soft, long range shot. False
|
||||
* makes the shot more of a lob
|
||||
*
|
||||
* @param targetVMPS The target velocity of the shooter in Meters per Second
|
||||
* @param deltaXM The "as the crow flies" distance between the shooter exit and the target
|
||||
* @param deltaYM The height difference between the shooter exit and the target
|
||||
* @param softerShot True for a long range shot, false for a short range lob
|
||||
* @return The angle required of the shooter to make the shot described by the input parameters
|
||||
*/
|
||||
public static double shotAngle(double targetVMPS, double deltaXM, double deltaYM, boolean softerShot) {
|
||||
double vPow2 = Math.pow(targetVMPS, 2);
|
||||
double vPow4 = Math.pow(targetVMPS, 4);
|
||||
double sqrtPiece = Math.sqrt(vPow4 - kG * (kG * Math.pow(deltaXM, 2) + 2 * deltaYM * vPow2));
|
||||
|
||||
if(softerShot) {
|
||||
return Math.atan((vPow2 - sqrtPiece) / (kG * deltaXM));
|
||||
} else {
|
||||
return Math.atan((vPow2 + sqrtPiece) / (kG * deltaXM));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,9 @@
|
||||
package first.robot.utilities;
|
||||
|
||||
import org.wpilib.math.geometry.Pose2d;
|
||||
|
||||
/**
|
||||
* A record class which represents the source of a visual pose, the pose itself
|
||||
* and the timestamp the pose was generated.
|
||||
*/
|
||||
public record VisualPose(String cameraName, Pose2d visualPose, double timestamp) {}
|
||||
Reference in new issue
Block a user