diff --git a/build.gradle b/build.gradle index cc290c3..20f2ef4 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2" + id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-3" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.25.0" id "pmd" @@ -151,49 +151,49 @@ tasks.withType(JavaCompile) { } // Create version file -// project.compileJava.dependsOn(createVersionFile) -// gversion { -// srcDir = "src/main/java/" -// classPackage = "frc.robot" -// className = "BuildConstants" -// dateFormat = "yyyy-MM-dd HH:mm:ss z" -// timeZone = "America/New_York" -// indent = " " -// } +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "frc.robot" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/New_York" + indent = " " +} // TODO: set this shit up // Create commit with working changes on event branches -// task(eventDeploy) { -// doLast { -// if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) { -// def branchPrefix = "event" -// def branch = 'git branch --show-current'.execute().text.trim() -// def commitMessage = "Update at '${new Date().toString()}'" - -// if (branch.startsWith(branchPrefix)) { -// exec { -// workingDir(projectDir) -// executable 'git' -// args 'add', '-A' -// } -// exec { -// workingDir(projectDir) -// executable 'git' -// args 'commit', '-m', commitMessage -// ignoreExitValue = true -// } - -// println "Committed to branch: '$branch'" -// println "Commit message: '$commitMessage'" -// } else { -// println "Not on an event branch, skipping commit" -// } -// } else { -// println "Not running deploy task, skipping commit" -// } -// } -// } -// createVersionFile.dependsOn(eventDeploy) +task(eventDeploy) { + doLast { + if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) { + def branchPrefix = "event" + def branch = 'git branch --show-current'.execute().text.trim() + def commitMessage = "Update at '${new Date().toString()}'" + + if (branch.startsWith(branchPrefix)) { + exec { + workingDir(projectDir) + executable 'git' + args 'add', '-A' + } + exec { + workingDir(projectDir) + executable 'git' + args 'commit', '-m', commitMessage + ignoreExitValue = true + } + + println "Committed to branch: '$branch'" + println "Commit message: '$commitMessage'" + } else { + println "Not on an event branch, skipping commit" + } + } else { + println "Not running deploy task, skipping commit" + } + } +} +createVersionFile.dependsOn(eventDeploy) // Spotless formatting project.compileJava.dependsOn(spotlessApply) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 00ed33c..f2cac57 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,22 +23,22 @@ public class Robot extends LoggedRobot { public Robot() { // Record metadata - Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); - Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); - Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); - Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); - Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); - switch (BuildConstants.DIRTY) { - case 0: - Logger.recordMetadata("GitDirty", "All changes committed"); - break; - case 1: - Logger.recordMetadata("GitDirty", "Uncomitted changes"); - break; - default: - Logger.recordMetadata("GitDirty", "Unknown"); - break; - } + // Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + // Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + // Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + // Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + // Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + // switch (BuildConstants.DIRTY) { + // case 0: + // Logger.recordMetadata("GitDirty", "All changes committed"); + // break; + // case 1: + // Logger.recordMetadata("GitDirty", "Uncomitted changes"); + // break; + // default: + // Logger.recordMetadata("GitDirty", "Unknown"); + // break; + // } // Set up data receivers & replay source switch (Constants.CURRENT_MODE) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 01be9a4..f874204 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -81,7 +81,7 @@ public RobotContainer() { /* create a swerve drive simulation */ this.swerveDriveSimulation = new SwerveDriveSimulation( - SimulationConstants.ROBOT_MASS_KG, + 60, DriveConstants.TRACK_WIDTH, DriveConstants.WHEEL_BASE, DriveConstants.TRACK_WIDTH + .2, diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java index 5434889..76f1e64 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java @@ -359,14 +359,20 @@ public void update() { Voltage voltage = Volts.of(poseVoltController.calculate(getPosition().in(Radians), output)); Voltage feedforwardVoltage = - feedforward.calculate(getVelocity(), velocityForVolts(voltage)); + Volts.of( + feedforward.calculate( + getVelocity().in(RadiansPerSecond), + velocityForVolts(voltage).in(RadiansPerSecond))); driveAtVoltage(feedforwardVoltage.plus(voltage)); } case VELOCITY -> { Voltage voltage = Volts.of(veloVoltController.calculate(getVelocity().in(RadiansPerSecond), output)); Voltage feedforwardVoltage = - feedforward.calculate(getVelocity(), RadiansPerSecond.of(output)); + Volts.of( + feedforward.calculate( + getVelocity().in(RadiansPerSecond), + RadiansPerSecond.of(output).in(RadiansPerSecond))); driveAtVoltage(voltage.plus(feedforwardVoltage)); } } @@ -381,14 +387,20 @@ public void update() { Amps.of(poseCurrentController.calculate(getPosition().in(Radians), output)); Voltage voltage = voltsForAmps(current, getVelocity()); Voltage feedforwardVoltage = - feedforward.calculate(getVelocity(), velocityForVolts(voltage)); + Volts.of( + feedforward.calculate( + getVelocity().in(RadiansPerSecond), + velocityForVolts(voltage).in(RadiansPerSecond))); driveAtVoltage(feedforwardVoltage.plus(voltage)); } case VELOCITY -> { Current current = Amps.of(veloCurrentController.calculate(getPosition().in(Radians), output)); Voltage feedforwardVoltage = - feedforward.calculate(getVelocity(), RadiansPerSecond.of(output)); + Volts.of( + feedforward.calculate( + getVelocity().in(RadiansPerSecond), + RadiansPerSecond.of(output).in(RadiansPerSecond))); Voltage voltage = voltsForAmps(current, getVelocity()).plus(feedforwardVoltage); driveAtVoltage(voltage); } diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java index 87037b7..5c2c25e 100644 --- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java +++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java @@ -295,8 +295,7 @@ public double getDriveEncoderUnGearedPositionRad() { * @return the final position of the drive encoder (wheel rotations), in radians */ public double getDriveEncoderFinalPositionRad() { - return getDriveEncoderUnGearedPositionRad(); - // / DRIVE_GEAR_RATIO; + return getDriveEncoderUnGearedPositionRad() / DRIVE_GEAR_RATIO; } /** @@ -323,8 +322,7 @@ public double getDriveEncoderUnGearedSpeedRadPerSec() { * @return the final speed of the drive wheel, in radians per second */ public double getDriveWheelFinalSpeedRadPerSec() { - return getDriveEncoderUnGearedSpeedRadPerSec(); - // / DRIVE_GEAR_RATIO; + return getDriveEncoderUnGearedSpeedRadPerSec() / DRIVE_GEAR_RATIO; } /** diff --git a/src/main/java/frc/robot/extras/util/ProceduralStructGenerator.java b/src/main/java/frc/robot/extras/util/ProceduralStructGenerator.java new file mode 100644 index 0000000..7a47a72 --- /dev/null +++ b/src/main/java/frc/robot/extras/util/ProceduralStructGenerator.java @@ -0,0 +1,857 @@ +package frc.robot.extras.util; + +import edu.wpi.first.units.Measure; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import java.lang.annotation.Documented; +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; +import java.lang.reflect.AnnotatedElement; +import java.lang.reflect.Field; +import java.lang.reflect.InvocationTargetException; +import java.lang.reflect.Modifier; +import java.lang.reflect.RecordComponent; +import java.nio.ByteBuffer; +import java.util.ArrayList; +import java.util.Collection; +import java.util.HashMap; +import java.util.HashSet; +import java.util.Iterator; +import java.util.List; +import java.util.Optional; +import java.util.OptionalInt; +import java.util.Set; +import java.util.function.Supplier; +import java.util.stream.BaseStream; + +/** A utility class for procedurally generating {@link Struct}s from records and enums. */ +public final class ProceduralStructGenerator { + private ProceduralStructGenerator() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * A functional interface representing a method that retrives a value from a {@link ByteBuffer}. + */ + @FunctionalInterface + private interface Unpacker { + T unpack(ByteBuffer buffer); + } + + /** A functional interface representing a method that packs a value into a {@link ByteBuffer}. */ + @FunctionalInterface + private interface Packer { + ByteBuffer pack(ByteBuffer buffer, T value); + + static Packer fromStruct(Struct struct) { + return (buffer, value) -> { + struct.pack(buffer, value); + return buffer; + }; + } + } + + private record PrimType(String name, int size, Unpacker unpacker, Packer packer) {} + + /** A map of primitive types to their schema types. */ + private static final HashMap, PrimType> primitiveTypeMap = new HashMap<>(); + + private static void addPrimType( + Class boxedClass, + Class primitiveClass, + String name, + int size, + Unpacker unpacker, + Packer packer) { + PrimType primType = new PrimType<>(name, size, unpacker, packer); + primitiveTypeMap.put(boxedClass, primType); + primitiveTypeMap.put(primitiveClass, primType); + } + + // Add primitive types to the map + static { + addPrimType( + Integer.class, int.class, "int32", Integer.BYTES, ByteBuffer::getInt, ByteBuffer::putInt); + addPrimType( + Double.class, + double.class, + "float64", + Double.BYTES, + ByteBuffer::getDouble, + ByteBuffer::putDouble); + addPrimType( + Float.class, + float.class, + "float32", + Float.BYTES, + ByteBuffer::getFloat, + ByteBuffer::putFloat); + addPrimType( + Boolean.class, + boolean.class, + "bool", + Byte.BYTES, + buffer -> buffer.get() != 0, + (buffer, value) -> buffer.put((byte) (value ? 1 : 0))); + addPrimType( + Character.class, + char.class, + "char", + Character.BYTES, + ByteBuffer::getChar, + ByteBuffer::putChar); + addPrimType(Byte.class, byte.class, "uint8", Byte.BYTES, ByteBuffer::get, ByteBuffer::put); + addPrimType( + Short.class, short.class, "int16", Short.BYTES, ByteBuffer::getShort, ByteBuffer::putShort); + addPrimType( + Long.class, long.class, "int64", Long.BYTES, ByteBuffer::getLong, ByteBuffer::putLong); + } + + /** + * A map of types to their custom struct schemas. + * + *

This allows adding custom struct implementations for types that are not supported by + * default. Think of vendor-specific. + */ + private static final HashMap, Struct> customStructTypeMap = new HashMap<>(); + + /** + * Add a custom struct to the structifier. + * + * @param The type the struct is for. + * @param clazz The class of the type. + * @param struct The struct to add. + * @param override Whether to override an existing struct. An existing struct could mean the type + * already has a {@code struct} field and implemnts {@link StructSerializable} or that the + * type is already in the custom struct map. + */ + public static void addCustomStruct(Class clazz, Struct struct, boolean override) { + if (override) { + customStructTypeMap.put(clazz, struct); + } else if (!StructSerializable.class.isAssignableFrom(clazz)) { + customStructTypeMap.putIfAbsent(clazz, struct); + } + } + + /** + * Returns a {@link Struct} for the given {@link StructSerializable} marked class. Due to the + * non-contractual nature of the marker this can fail. If the {@code struct} field could not be + * accessed for any reason, an empty {@link Optional} is returned. + * + * @param The type of the class. + * @param clazz The class object to extract the struct from. + * @return An optional containing the struct if it could be extracted. + */ + @SuppressWarnings("unchecked") + public static Optional> extractClassStruct( + Class clazz) { + try { + var possibleField = Optional.ofNullable(clazz.getDeclaredField("struct")); + return possibleField.flatMap( + field -> { + field.setAccessible(true); + if (Struct.class.isAssignableFrom(field.getType())) { + try { + return Optional.ofNullable((Struct) field.get(null)); + } catch (IllegalAccessException e) { + return Optional.empty(); + } + } else { + return Optional.empty(); + } + }); + } catch (NoSuchFieldException e) { + return Optional.empty(); + } + } + + /** + * Returns a {@link Struct} for the given class. This does not do compile time checking that the + * class is a {@link StructSerializable}. Whenever possible it is reccomended to use {@link + * #extractClassStruct(Class)}. + * + * @param clazz The class object to extract the struct from. + * @return An optional containing the struct if it could be extracted. + */ + @SuppressWarnings("unchecked") + public static Optional> extractClassStructDynamic(Class clazz) { + if (StructSerializable.class.isAssignableFrom(clazz)) { + return extractClassStruct((Class) clazz).map(struct -> struct); + } else { + return Optional.empty(); + } + } + + @Retention(RetentionPolicy.RUNTIME) + @Target({ElementType.FIELD, ElementType.RECORD_COMPONENT}) + @Documented + public @interface IgnoreStructField {} + + @Retention(RetentionPolicy.RUNTIME) + @Target({ElementType.FIELD, ElementType.RECORD_COMPONENT}) + @Documented + public @interface FixedSizeArray { + int size(); + } + + private static OptionalInt arraySize(AnnotatedElement field) { + return Optional.ofNullable(field.getAnnotation(FixedSizeArray.class)) + .map(FixedSizeArray::size) + .map(OptionalInt::of) + .orElse(OptionalInt.empty()); + } + + private static boolean shouldIgnore(AnnotatedElement field) { + return field.isAnnotationPresent(IgnoreStructField.class); + } + + private record StructField( + String name, + String type, + int size, + boolean immutable, + Set> structsToLoad, + Unpacker unpacker, + Packer packer) { + + public static StructField fromField(Field field) { + return StructField.fromNameAndClass( + field.getName(), + field.getType(), + arraySize(field), + Modifier.isFinal(field.getModifiers())); + } + + public static StructField fromRecordComponent(RecordComponent component) { + return StructField.fromNameAndClass( + component.getName(), component.getType(), arraySize(component), true); + } + + @SuppressWarnings("unchecked") + public static StructField fromNameAndClass( + String name, Class clazz, OptionalInt arraySize, boolean isFinal) { + if (!isFixedSize(clazz, arraySize)) { + return null; + } + if (clazz.isArray() && arraySize.isPresent()) { + final Class componentType = clazz.getComponentType(); + final int size = arraySize.getAsInt(); + final StructField componentField = + fromNameAndClass( + componentType.getSimpleName(), componentType, OptionalInt.empty(), false); + return new StructField( + name + "[" + size + "]", + componentField.type, + componentField.size * size, + isFinal, + componentField.structsToLoad, + buffer -> { + Object[] array = new Object[size]; + for (int i = 0; i < size; i++) { + array[i] = componentField.unpacker.unpack(buffer); + } + return array; + }, + (buffer, value) -> { + for (Object obj : (Object[]) value) { + ((Packer) componentField.packer).pack(buffer, obj); + } + return buffer; + }); + } else if (Measure.class.isAssignableFrom(clazz)) { + return new StructField( + name, + "float64", + Double.BYTES, + isFinal, + Set.of(), + buffer -> { + throw new UnsupportedOperationException("Cannot unpack Measure"); + }, + (buffer, value) -> buffer.putDouble(((Measure) value).baseUnitMagnitude())); + } else if (primitiveTypeMap.containsKey(clazz)) { + PrimType primType = primitiveTypeMap.get(clazz); + return new StructField( + name, + primType.name, + primType.size, + isFinal, + Set.of(), + primType.unpacker, + primType.packer); + } else { + Struct struct = null; + if (customStructTypeMap.containsKey(clazz)) { + struct = customStructTypeMap.get(clazz); + } else if (StructSerializable.class.isAssignableFrom(clazz)) { + struct = extractClassStructDynamic(clazz).orElse(null); + } + if (struct == null) { + return null; + } + Set> structsToLoad = new HashSet<>(Set.of(struct.getNested())); + structsToLoad.add(struct); + return new StructField( + name, + struct.getTypeName(), + struct.getSize(), + struct.isImmutable() && isFinal, + structsToLoad, + struct::unpack, + Packer.fromStruct(struct)); + } + } + } + + /** + * Introspects a class to determine if it's a fixed size. + * + *

Fixed size means no collections, no strings, no arrays, etc. + * + * @param clazz The class to introspect. + * @return Whether the class is fixed size. + */ + public static boolean isFixedSize(Class clazz, OptionalInt arraySize) { + if (clazz.isArray()) { + if (arraySize.isEmpty()) { + return false; + } else { + Class componentType = clazz.getComponentType(); + return isFixedSize(componentType, OptionalInt.empty()); + } + } else if (clazz.isRecord()) { + for (RecordComponent component : clazz.getRecordComponents()) { + if (!isFixedSize(component.getType(), arraySize(component))) { + return false; + } + } + } else { + for (Field field : clazz.getDeclaredFields()) { + Class fieldClass = field.getType(); + if (field.isEnumConstant() || Modifier.isStatic(field.getModifiers())) { + continue; + } + if (Collection.class.isAssignableFrom(fieldClass) + || Iterator.class.isAssignableFrom(fieldClass) + || Iterable.class.isAssignableFrom(fieldClass) + || BaseStream.class.isAssignableFrom(fieldClass) + || fieldClass.isArray() + || fieldClass == String.class + || fieldClass == Optional.class) { + return false; + } + if (!primitiveTypeMap.containsKey(fieldClass) + && !isFixedSize(fieldClass, arraySize(field))) { + return false; + } + } + } + return true; + } + + /** + * Introspects a class to determine if it's interiorly mutable. + * + *

Interior mutability means that the class has fields that are mutable. + * + * @param clazz The class to introspect. + * @return Whether the class is interiorly mutable. + */ + public static boolean isInteriorlyMutable(Class clazz) { + if (clazz.isArray()) { + return true; + } else if (clazz.isRecord()) { + for (RecordComponent component : clazz.getRecordComponents()) { + if (isInteriorlyMutable(component.getType())) { + return true; + } + } + } else { + for (Field field : clazz.getDeclaredFields()) { + if (field.isEnumConstant() || Modifier.isStatic(field.getModifiers())) { + continue; + } + if (!Modifier.isFinal(field.getModifiers())) { + return true; + } + if (!primitiveTypeMap.containsKey(field.getType()) + && isInteriorlyMutable(field.getType())) { + return true; + } + } + } + return false; + } + + /** A utility for building schema syntax in a procedural manner. */ + @SuppressWarnings("PMD.AvoidStringBufferField") + public static class SchemaBuilder { + /** A utility for building enum fields in a procedural manner. */ + public static final class EnumFieldBuilder { + private final StringBuilder m_builder = new StringBuilder(); + private final String m_fieldName; + private boolean m_firstVariant = true; + + /** + * Creates a new enum field builder. + * + * @param fieldName The name of the field. + */ + public EnumFieldBuilder(String fieldName) { + this.m_fieldName = fieldName; + m_builder.append("enum {"); + } + + /** + * Adds a variant to the enum field. + * + * @param name The name of the variant. + * @param value The value of the variant. + * @return The builder for chaining. + */ + public EnumFieldBuilder addVariant(String name, int value) { + if (!m_firstVariant) { + m_builder.append(','); + } + m_firstVariant = false; + m_builder.append(name).append('=').append(value); + return this; + } + + /** + * Builds the enum field. If this object is being used with {@link SchemaBuilder#addEnumField} + * then {@link #build()} does not have to be called by the user. + * + * @return The built enum field. + */ + public String build() { + m_builder.append("} int8 ").append(m_fieldName).append(';'); + return m_builder.toString(); + } + } + + /** Creates a new schema builder. */ + public SchemaBuilder() {} + + private final StringBuilder m_builder = new StringBuilder(); + + /** + * Adds a field to the schema. + * + * @param name The name of the field. + * @param type The type of the field. + * @return The builder for chaining. + */ + public SchemaBuilder addField(StructField field) { + m_builder.append(field.type).append(' ').append(field.name).append(';'); + return this; + } + + /** + * Adds an inline enum field to the schema. + * + * @param enumFieldBuilder The builder for the enum field. + * @return The builder for chaining. + */ + public SchemaBuilder addEnumField(EnumFieldBuilder enumFieldBuilder) { + m_builder.append(enumFieldBuilder.build()); + return this; + } + + /** + * Builds the schema. + * + * @return The built schema. + */ + public String build() { + return m_builder.toString(); + } + } + + public static Struct noopStruct(Class cls) { + return new Struct<>() { + @Override + public Class getTypeClass() { + return cls; + } + + @Override + public String getTypeName() { + return cls.getSimpleName(); + } + + @Override + public String getSchema() { + return ""; + } + + @Override + public int getSize() { + return 0; + } + + @Override + public void pack(ByteBuffer buffer, T value) {} + + @Override + public T unpack(ByteBuffer buffer) { + return null; + } + }; + } + + private abstract static class ProcStruct implements Struct { + protected final Class typeClass; + protected final List fields; + private final String schema; + + // stored values so we never recompute them + private final int size; + private final boolean isImmutable; + private final Struct[] nested; + + public ProcStruct(Class typeClass, List fields, String schema) { + this.typeClass = typeClass; + this.fields = fields; + this.schema = schema; + + this.size = fields.stream().mapToInt(StructField::size).sum(); + this.isImmutable = fields.stream().allMatch(StructField::immutable); + this.nested = + fields.stream() + .map(StructField::structsToLoad) + .flatMap(Collection::stream) + .toArray(Struct[]::new); + + ProceduralStructGenerator.customStructTypeMap.put(typeClass, this); + } + + @Override + public Class getTypeClass() { + return typeClass; + } + + @Override + public String getTypeName() { + return typeClass.getSimpleName(); + } + + @Override + public String getSchema() { + return schema; + } + + @Override + public int getSize() { + return size; + } + + @Override + public boolean isCloneable() { + return Cloneable.class.isAssignableFrom(typeClass); + } + + @Override + @SuppressWarnings("unchecked") + public T clone(T obj) throws CloneNotSupportedException { + if (isCloneable()) { + try { + return (T) typeClass.getMethod("clone").invoke(obj); + } catch (IllegalAccessException | InvocationTargetException | NoSuchMethodException e) { + throw new CloneNotSupportedException(); + } + } else { + throw new CloneNotSupportedException(); + } + } + + @Override + public boolean isImmutable() { + return isImmutable; + } + + @Override + public Struct[] getNested() { + return nested; + } + + @Override + public String toString() { + return this.getTypeName() + "<" + this.getSize() + ">" + " {" + this.schema + "}"; + } + } + + /** + * Generates a {@link Struct} for the given {@link Record} class. If a {@link Struct} cannot be + * generated from the {@link Record}, the errors encountered will be printed and a no-op {@link + * Struct} will be returned. + * + * @param The type of the record. + * @param recordClass The class of the record. + * @return The generated struct. + */ + @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"}) + public static Struct genRecord(final Class recordClass) { + final RecordComponent[] components = recordClass.getRecordComponents(); + final SchemaBuilder schemaBuilder = new SchemaBuilder(); + final ArrayList fields = new ArrayList<>(); + + for (final RecordComponent component : components) { + if (shouldIgnore(component)) { + continue; + } + component.getAccessor().setAccessible(true); + fields.add(StructField.fromRecordComponent(component)); + } + + if (fields.stream().anyMatch(f -> f == null)) { + return noopStruct(recordClass); + } + fields.forEach(schemaBuilder::addField); + + return new ProcStruct<>(recordClass, fields, schemaBuilder.build()) { + @Override + public void pack(ByteBuffer buffer, R value) { + boolean failed = false; + int startingPosition = buffer.position(); + for (int i = 0; i < components.length; i++) { + Packer packer = (Packer) fields.get(i).packer(); + try { + Object componentValue = components[i].getAccessor().invoke(value); + if (componentValue == null) { + throw new IllegalArgumentException("Component is null"); + } + packer.pack(buffer, componentValue); + } catch (IllegalAccessException + | IllegalArgumentException + | InvocationTargetException e) { + failed = true; + break; + } + } + if (failed) { + buffer.put(startingPosition, new byte[this.getSize()]); + } + } + + @Override + public R unpack(ByteBuffer buffer) { + try { + Object[] args = new Object[components.length]; + Class[] argTypes = new Class[components.length]; + for (int i = 0; i < components.length; i++) { + args[i] = fields.get(i).unpacker().unpack(buffer); + argTypes[i] = components[i].getType(); + } + return recordClass.getConstructor(argTypes).newInstance(args); + } catch (InstantiationException + | IllegalAccessException + | InvocationTargetException + | NoSuchMethodException + | SecurityException e) { + System.err.println( + "Could not unpack record: " + + recordClass.getSimpleName() + + "\n " + + e.getMessage()); + return null; + } + } + }; + } + + /** + * Generates a {@link Struct} for the given {@link Enum} class. If a {@link Struct} cannot be + * generated from the {@link Enum}, the errors encountered will be printed and a no-op {@link + * Struct} will be returned. + * + * @param The type of the enum. + * @param enumClass The class of the enum. + * @return The generated struct. + */ + @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"}) + public static > Struct genEnum(Class enumClass) { + final E[] enumVariants = enumClass.getEnumConstants(); + final Field[] allEnumFields = enumClass.getDeclaredFields(); + final SchemaBuilder schemaBuilder = new SchemaBuilder(); + final SchemaBuilder.EnumFieldBuilder enumFieldBuilder = + new SchemaBuilder.EnumFieldBuilder("variant"); + final HashMap enumMap = new HashMap<>(); + final ArrayList fields = new ArrayList<>(); + + if (enumVariants == null || enumVariants.length == 0) { + return noopStruct(enumClass); + } + + for (final E constant : enumVariants) { + final String name = constant.name(); + final int ordinal = constant.ordinal(); + + enumFieldBuilder.addVariant(name, ordinal); + enumMap.put(ordinal, constant); + } + schemaBuilder.addEnumField(enumFieldBuilder); + fields.add( + new StructField( + "variant", + "int8", + 1, + true, + Set.of(), + ByteBuffer::get, + (buffer, value) -> buffer.put((byte) ((Enum) value).ordinal()))); + + final List enumFields = + List.of(allEnumFields).stream() + .filter( + f -> + !f.isEnumConstant() && !Modifier.isStatic(f.getModifiers()) && !shouldIgnore(f)) + .toList(); + + for (final Field field : enumFields) { + field.setAccessible(true); + fields.add(StructField.fromField(field)); + } + if (fields.stream().anyMatch(f -> f == null)) { + return noopStruct(enumClass); + } + for (int i = 1; i < fields.size(); i++) { + // do this to skip the variant field + schemaBuilder.addField(fields.get(i)); + } + + return new ProcStruct<>(enumClass, fields, schemaBuilder.build()) { + @Override + public void pack(ByteBuffer buffer, E value) { + boolean failed = false; + int startingPosition = buffer.position(); + buffer.put((byte) value.ordinal()); + for (int i = 0; i < enumFields.size(); i++) { + Packer packer = (Packer) fields.get(i + 1).packer(); + Field field = enumFields.get(i); + try { + Object fieldValue = field.get(value); + if (fieldValue == null) { + throw new IllegalArgumentException("Field is null"); + } + packer.pack(buffer, fieldValue); + } catch (IllegalArgumentException | IllegalAccessException e) { + System.err.println( + "Could not pack enum field: " + + enumClass.getSimpleName() + + "#" + + field.getName() + + "\n " + + e.getMessage()); + failed = true; + break; + } + } + if (failed) { + buffer.put(startingPosition, new byte[this.getSize()]); + } + } + + final byte[] m_spongeBuffer = new byte[this.getSize() - 1]; + + @Override + public E unpack(ByteBuffer buffer) { + int ordinal = buffer.get(); + buffer.get(m_spongeBuffer); + return enumMap.getOrDefault(ordinal, null); + } + + public boolean isCloneable() { + return true; + } + ; + + public E clone(E obj) throws CloneNotSupportedException { + return obj; + } + ; + }; + } + + /** + * Generates a {@link Struct} for the given {@link Object} class. If a {@link Struct} cannot be + * generated from the {@link Object}, the errors encountered will be printed and a no-op {@link + * Struct} will be returned. + * + * @param The type of the object. + * @param objectClass The class of the object. + * @param objectSupplier A supplier for the object. + * @return The generated struct. + */ + @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"}) + public static Struct genObject(Class objectClass, Supplier objectSupplier) { + final SchemaBuilder schemaBuilder = new SchemaBuilder(); + final Field[] allFields = + List.of(objectClass.getDeclaredFields()).stream() + .filter(f -> !shouldIgnore(f) && !Modifier.isStatic(f.getModifiers())) + .toArray(Field[]::new); + final ArrayList fields = new ArrayList<>(allFields.length); + + for (final Field field : allFields) { + field.setAccessible(true); + fields.add(StructField.fromField(field)); + } + + if (fields.stream().anyMatch(f -> f == null)) { + return noopStruct(objectClass); + } + fields.forEach(schemaBuilder::addField); + + return new ProcStruct<>(objectClass, fields, schemaBuilder.build()) { + @Override + public void pack(ByteBuffer buffer, O value) { + boolean failed = false; + int startingPosition = buffer.position(); + for (int i = 0; i < allFields.length; i++) { + Packer packer = (Packer) fields.get(i).packer(); + try { + Object fieldValue = allFields[i].get(value); + if (fieldValue == null) { + throw new IllegalArgumentException("Field is null"); + } + packer.pack(buffer, fieldValue); + } catch (IllegalArgumentException | IllegalAccessException e) { + System.err.println( + "Could not pack object field: " + + objectClass.getSimpleName() + + "#" + + allFields[i].getName() + + "\n " + + e.getMessage()); + failed = true; + break; + } + } + if (failed) { + buffer.put(startingPosition, new byte[this.getSize()]); + } + } + + @Override + public O unpack(ByteBuffer buffer) { + try { + O obj = objectSupplier.get(); + for (int i = 0; i < allFields.length; i++) { + Object fieldValue = fields.get(i).unpacker().unpack(buffer); + allFields[i].set(obj, fieldValue); + } + return obj; + } catch (IllegalArgumentException | IllegalAccessException e) { + System.err.println( + "Could not unpack object: " + + objectClass.getSimpleName() + + "\n " + + e.getMessage()); + return null; + } + } + }; + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 88982ec..5efcdc9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -8,20 +8,24 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP; import frc.robot.extras.util.DeviceCANBus; import frc.robot.extras.util.TimeUtil; import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; +import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; import frc.robot.subsystems.swerve.gyro.GyroInputsAutoLogged; import frc.robot.subsystems.swerve.gyro.GyroInterface; import frc.robot.subsystems.swerve.module.ModuleInterface; import frc.robot.subsystems.swerve.odometryThread.OdometryThread; import frc.robot.subsystems.swerve.odometryThread.OdometryThreadInputsAutoLogged; +import frc.robot.subsystems.swerve.setpointGen.SwerveSetpoint; +import frc.robot.subsystems.swerve.setpointGen.SwerveSetpointGenerator; import frc.robot.subsystems.vision.VisionConstants; import java.util.Optional; import org.littletonrobotics.junction.AutoLogOutput; @@ -37,6 +41,19 @@ public class SwerveDrive extends SubsystemBase { private final SwerveModulePosition[] lastModulePositions; private final SwerveDrivePoseEstimator odometry; + private final SwerveSetpointGenerator setpointGenerator = + new SwerveSetpointGenerator( + DriveConstants.MODULE_TRANSLATIONS, + DCMotor.getKrakenX60(1).withReduction(ModuleConstants.DRIVE_GEAR_RATIO), + DCMotor.getFalcon500(1).withReduction(1), + 60, + 58, + 7, + ModuleConstants.WHEEL_DIAMETER_METERS, + WHEEL_GRIP.TIRE_WHEEL.cof, + 0.0); + private SwerveSetpoint setpoint = SwerveSetpoint.zeroed(); + private final OdometryThread odometryThread; private Optional alliance; @@ -89,7 +106,7 @@ public SwerveDrive( gyroDisconnectedAlert.set(false); } - // // /** Updates the pose estimator with the pose calculated from the swerve modules. */ + /** Updates the pose estimator with the pose calculated from the swerve modules. */ // public void addPoseEstimatorSwerveMeasurement() { // for (int timestampIndex = 0; // timestampIndex < odometryThreadInputs.measurementTimeStamps.length; @@ -180,24 +197,16 @@ private void modulesPeriodic() { * @param fieldRelative Whether the provided x and y speeds are relative to the field. */ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative) { - ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); - if (fieldRelative) { - speeds.toRobotRelativeSpeeds(getOdometryAllianceRelativeRotation2d()); - } - // ChassisSpeeds discreteSpeeds = - // // ChassisSpeeds.discretize( - // fieldRelative - // ? ChassisSpeeds.fromFieldRelativeSpeeds( - // xSpeed, ySpeed, rotationSpeed, getOdometryAllianceRelativeRotation2d()) - // : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); - // // , 0.02); - SwerveModuleState[] swerveModuleStates = - DriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(speeds); - SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, DriveConstants.MAX_SPEED_METERS_PER_SECOND); - - setModuleStates(swerveModuleStates); - Logger.recordOutput("SwerveStates/DesiredStates", swerveModuleStates); + ChassisSpeeds desiredSpeeds = + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rotationSpeed, getOdometryAllianceRelativeRotation2d()) + : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); + + setpoint = setpointGenerator.generateSetpoint(setpoint, desiredSpeeds, 0.02); + + setModuleStates(setpoint.moduleStates()); + Logger.recordOutput("SwerveStates/DesiredStates", setpoint.moduleStates()); } /** @@ -272,7 +281,7 @@ public Rotation2d getOdometryAllianceRelativeRotation2d() { */ public void setModuleStates(SwerveModuleState[] desiredStates) { for (int i = 0; i < 4; i++) { - swerveModules[i].runSetPoint(desiredStates[i]); + swerveModules[i].runSetPoint((desiredStates[i])); } } @@ -281,6 +290,23 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { * * @param timestampIndex index of the timestamp to sample the pose at */ + // private void addPoseEstimatorSwerveMeasurement(int timestampIndex) { + // final SwerveModulePosition[] modulePositions = getModulePositions(), + // moduleDeltas = getModulesDelta(modulePositions); + + // if (gyroInputs.isConnected) { + // rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex]; + // } else { + // Twist2d twist = DriveConstants.DRIVE_KINEMATICS.toTwist2d(moduleDeltas); + // rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + // } + + // poseEstimator.updateWithTime( + // odometryThreadInputs.measurementTimeStamps[timestampIndex], + // rawGyroRotation, + // modulePositions); + // } + public void addPoseEstimatorSwerveMeasurement() { // int timestampIndex final SwerveModulePosition[] modulePositions = getModulePositions(), moduleDeltas = getModulesDelta(modulePositions); @@ -298,6 +324,22 @@ public void addPoseEstimatorSwerveMeasurement() { // int timestampIndex Logger.getTimestamp(), rawGyroRotation, modulePositions); } + /** + * Gets the modules positions, sampled at the indexed timestamp. + * + * @param timestampIndex the timestamp index to sample. + * @return a list of SwerveModulePosition, containing relative drive position and absolute turn + * rotation at the sampled timestamp. + */ + // private SwerveModulePosition[] getModulesPosition(int timestampIndex) { + // SwerveModulePosition[] swerveModulePositions = new + // SwerveModulePosition[swerveModules.length]; + // for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) + // swerveModulePositions[moduleIndex] = + // swerveModules[moduleIndex].getOdometryPositions()[timestampIndex]; + // return swerveModulePositions; + // } + private SwerveModulePosition[] getModulesDelta(SwerveModulePosition[] freshModulesPosition) { SwerveModulePosition[] deltas = new SwerveModulePosition[swerveModules.length]; for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index c4a2317..033af94 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -7,8 +7,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; -// import edu.wpi.first.w; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; @@ -21,8 +19,6 @@ public class SwerveModule extends SubsystemBase { private final String name; private final ModuleInputsAutoLogged inputs = new ModuleInputsAutoLogged(); - private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; - private final Alert hardwareFaultAlert; public SwerveModule(ModuleInterface io, String name) { @@ -45,17 +41,9 @@ public void updateOdometryInputs() { @Override public void periodic() { // updateOdometryPositions(); + Logger.recordOutput("Drive/current turn angle", getTurnRotation().getRotations()); } - // private void updateOdometryPositions() { - // odometryPositions = new SwerveModulePosition[inputs.odometryDriveWheelRevolutions.length]; - // for (int i = 0; i < odometryPositions.length; i++) { - // double positionMeters = getDrivePositionMeters(); - // Rotation2d angle = getTurnRotation(); - // odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); - // } - // } - public void setVoltage(Voltage volts) { io.setDriveVoltage(volts); io.setTurnVoltage(Volts.zero()); @@ -72,13 +60,11 @@ public double getCharacterizationVelocity() { /** Runs the module with the specified setpoint state. Returns optimized setpoint */ public void runSetPoint(SwerveModuleState state) { state.optimize(getTurnRotation()); - state.cosineScale(getTurnRotation()); - - if (Math.abs(state.speedMetersPerSecond) < 0.01) { + if (state.speedMetersPerSecond < 0.01) { io.stopModule(); } - io.setDesiredState(state); + Logger.recordOutput("Drive/desired turn angle", state.angle.getRotations()); } /** Returns the current turn angle of the module. */ @@ -92,12 +78,12 @@ public double getTurnVelocity() { /** Returns the current drive position of the module in meters. */ public double getDrivePositionMeters() { - return ModuleConstants.DRIVE_TO_METERS * inputs.drivePosition; // Wheel circumference + return ModuleConstants.WHEEL_CIRCUMFERENCE_METERS * inputs.drivePosition; } /** Returns the current drive velocity of the module in meters per second. */ public double getDriveVelocityMetersPerSec() { - return ModuleConstants.DRIVE_TO_METERS_PER_SECOND * inputs.driveVelocity; + return ModuleConstants.WHEEL_CIRCUMFERENCE_METERS * inputs.driveVelocity; } /** Returns the module state (turn angle and drive velocity). */ @@ -105,11 +91,6 @@ public SwerveModuleState getMeasuredState() { return new SwerveModuleState(getDriveVelocityMetersPerSec(), getTurnRotation()); } - /** Returns the module positions received this cycle. */ - public SwerveModulePosition[] getOdometryPositions() { - return odometryPositions; - } - /** * Gets the module position consisting of the distance it has traveled and the angle it is * rotated. diff --git a/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java index 249138a..44094b5 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java @@ -7,9 +7,7 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.extras.simulation.OdometryTimestampsSim; import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation; import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants; @@ -18,16 +16,16 @@ public class SimulatedModule implements ModuleInterface { private final SwerveModuleSimulation moduleSimulation; - private final PIDController drivePID = new PIDController(.05, 0, 0); - private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.1, 0.13); + private final PIDController drivePID = new PIDController(.27, 0, 0); + private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(1, 1.5); private final Constraints turnConstraints = new Constraints( - RadiansPerSecond.of(2 * Math.PI).in(RotationsPerSecond), - RadiansPerSecondPerSecond.of(4 * Math.PI).in(RotationsPerSecondPerSecond)); + ModuleConstants.MAX_ANGULAR_SPEED_ROTATIONS_PER_SECOND, + ModuleConstants.MAX_ANGULAR_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED); private final ProfiledPIDController turnPID = - new ProfiledPIDController(Radians.of(30).in(Rotations), 0, 0, turnConstraints); - private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(0.77, 0.75, 0); + new ProfiledPIDController(18, 0, 0, turnConstraints); + private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(0, 0, 0); public SimulatedModule(SwerveModuleSimulation moduleSimulation) { this.moduleSimulation = moduleSimulation; @@ -37,19 +35,19 @@ public SimulatedModule(SwerveModuleSimulation moduleSimulation) { @Override public void updateInputs(ModuleInputs inputs) { inputs.drivePosition = - Units.radiansToRotations(moduleSimulation.getDriveEncoderFinalPositionRad()); + Radians.of(moduleSimulation.getDriveEncoderFinalPositionRad()).in(Rotations); inputs.driveVelocity = - Units.radiansToRotations(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()); + RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()) + .in(RotationsPerSecond); inputs.driveAppliedVolts = moduleSimulation.getDriveMotorAppliedVolts(); inputs.driveCurrentAmps = moduleSimulation.getDriveMotorSupplyCurrentAmps(); inputs.turnAbsolutePosition = moduleSimulation.getTurnAbsolutePosition(); - inputs.turnVelocity = moduleSimulation.getTurnAbsoluteEncoderSpeedRadPerSec(); + inputs.turnVelocity = + Radians.of(moduleSimulation.getTurnAbsoluteEncoderSpeedRadPerSec()).in(Rotations); inputs.turnAppliedVolts = moduleSimulation.getTurnMotorAppliedVolts(); inputs.turnCurrentAmps = moduleSimulation.getTurnMotorSupplyCurrentAmps(); - inputs.odometryDriveWheelRevolutions = moduleSimulation.getCachedDriveWheelFinalPositionsRad(); - inputs.odometrySteerPositions = moduleSimulation.getCachedTurnAbsolutePositions(); inputs.odometryTimestamps = OdometryTimestampsSim.getTimestamps(); @@ -74,12 +72,6 @@ public void setDesiredState(SwerveModuleState desiredState) { desiredState.speedMetersPerSecond * ModuleConstants.DRIVE_GEAR_RATIO / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS; - SmartDashboard.putNumber("desired drive RPS", desiredDriveRPS); - SmartDashboard.putNumber( - "current drive RPS", - RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()) - .in(RotationsPerSecond)); - SmartDashboard.putNumber("desired angle", desiredState.angle.getDegrees()); moduleSimulation.requestDriveVoltageOut( Volts.of( @@ -88,13 +80,13 @@ public void setDesiredState(SwerveModuleState desiredState) { .in(RotationsPerSecond) * ModuleConstants.WHEEL_CIRCUMFERENCE_METERS, desiredDriveRPS)) - .plus(driveFF.calculate(RotationsPerSecond.of(desiredDriveRPS)))); + .plus(Volts.of(driveFF.calculate(desiredDriveRPS)))); moduleSimulation.requestTurnVoltageOut( Volts.of( turnPID.calculate( moduleSimulation.getTurnAbsolutePosition().getRotations(), desiredState.angle.getRotations())) - .plus(turnFF.calculate(RotationsPerSecond.of(turnPID.getSetpoint().velocity)))); + .plus(Volts.of(turnFF.calculate(turnPID.getSetpoint().velocity)))); } @Override diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/AdvancedSwerveModuleState.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/AdvancedSwerveModuleState.java new file mode 100644 index 0000000..064f2dd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/setpointGen/AdvancedSwerveModuleState.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems.swerve.setpointGen; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.util.struct.Struct; + +public final class AdvancedSwerveModuleState extends SwerveModuleState { + public double steerVelocityFF; + public double driveAccelerationFF; + + public AdvancedSwerveModuleState( + double speedMetersPerSecond, + Rotation2d angle, + double steerVelocityFF, + double driveAccelerationFF) { + super(speedMetersPerSecond, angle); + this.steerVelocityFF = steerVelocityFF; + this.driveAccelerationFF = driveAccelerationFF; + } + + // todo: implement custom struct + public static final Struct struct = SwerveModuleState.struct; + + public static AdvancedSwerveModuleState fromBase(SwerveModuleState base) { + return new AdvancedSwerveModuleState(base.speedMetersPerSecond, base.angle, 0.0, 0.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SPGCalcs.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SPGCalcs.java new file mode 100644 index 0000000..23cf036 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SPGCalcs.java @@ -0,0 +1,285 @@ +package frc.robot.subsystems.swerve.setpointGen; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import frc.robot.extras.util.ProceduralStructGenerator; +import frc.robot.extras.util.ProceduralStructGenerator.FixedSizeArray; +import frc.robot.extras.util.ProceduralStructGenerator.IgnoreStructField; +import java.nio.ByteBuffer; +import java.util.Arrays; + +class SPGCalcs { + private static final double kEpsilon = 1E-8; + static final int NUM_MODULES = 4; + + static double unwrapAngle(double ref, double angle) { + double diff = angle - ref; + if (diff > Math.PI) { + return angle - 2.0 * Math.PI; + } else if (diff < -Math.PI) { + return angle + 2.0 * Math.PI; + } else { + return angle; + } + } + + static double findSteeringMaxS( + double prevVx, + double prevVy, + double prevHeading, + double desiredVx, + double desiredVy, + double desiredHeading, + double maxDeviation) { + desiredHeading = unwrapAngle(prevHeading, desiredHeading); + double diff = desiredHeading - prevHeading; + if (Math.abs(diff) <= maxDeviation) { + // Can go all the way to s=1. + return 1.0; + } + + double target = prevHeading + Math.copySign(maxDeviation, diff); + + // Rotate the velocity vectors such that the target angle becomes the +X + // axis. We only need find the Y components, h_0 and h_1, since they are + // proportional to the distances from the two points to the solution + // point (x_0 + (x_1 - x_0)s, y_0 + (y_1 - y_0)s). + double sin = Math.sin(-target); + double cos = Math.cos(-target); + double h_0 = sin * prevVx + cos * prevVy; + double h_1 = sin * desiredVx + cos * desiredVy; + + // Undo linear interpolation from h_0 to h_1: + // 0 = h_0 + (h_1 - h_0) * s + // -h_0 = (h_1 - h_0) * s + // -h_0 / (h_1 - h_0) = s + // h_0 / (h_0 - h_1) = s + // Guaranteed to not divide by zero, since if h_0 was equal to h_1, theta_0 + // would be equal to theta_1, which is caught by the difference check. + return h_0 / (h_0 - h_1); + } + + private static boolean isValidS(double s) { + return Double.isFinite(s) && s >= 0 && s <= 1; + } + + static double findDriveMaxS(double x_0, double y_0, double x_1, double y_1, double max_vel_step) { + // Derivation: + // Want to find point P(s) between (x_0, y_0) and (x_1, y_1) where the + // length of P(s) is the target T. P(s) is linearly interpolated between the + // points, so P(s) = (x_0 + (x_1 - x_0) * s, y_0 + (y_1 - y_0) * s). + // Then, + // T = sqrt(P(s).x^2 + P(s).y^2) + // T^2 = (x_0 + (x_1 - x_0) * s)^2 + (y_0 + (y_1 - y_0) * s)^2 + // T^2 = x_0^2 + 2x_0(x_1-x_0)s + (x_1-x_0)^2*s^2 + // + y_0^2 + 2y_0(y_1-y_0)s + (y_1-y_0)^2*s^2 + // T^2 = x_0^2 + 2x_0x_1s - 2x_0^2*s + x_1^2*s^2 - 2x_0x_1s^2 + x_0^2*s^2 + // + y_0^2 + 2y_0y_1s - 2y_0^2*s + y_1^2*s^2 - 2y_0y_1s^2 + y_0^2*s^2 + // 0 = (x_0^2 + y_0^2 + x_1^2 + y_1^2 - 2x_0x_1 - 2y_0y_1)s^2 + // + (2x_0x_1 + 2y_0y_1 - 2x_0^2 - 2y_0^2)s + // + (x_0^2 + y_0^2 - T^2). + // + // To simplify, we can factor out some common parts: + // Let l_0 = x_0^2 + y_0^2, l_1 = x_1^2 + y_1^2, and + // p = x_0 * x_1 + y_0 * y_1. + // Then we have + // 0 = (l_0 + l_1 - 2p)s^2 + 2(p - l_0)s + (l_0 - T^2), + // with which we can solve for s using the quadratic formula. + + double l_0 = x_0 * x_0 + y_0 * y_0; + double l_1 = x_1 * x_1 + y_1 * y_1; + double sqrt_l_0 = Math.sqrt(l_0); + double diff = Math.sqrt(l_1) - sqrt_l_0; + if (Math.abs(diff) <= max_vel_step) { + // Can go all the way to s=1. + return 1.0; + } + + double target = sqrt_l_0 + Math.copySign(max_vel_step, diff); + double p = x_0 * x_1 + y_0 * y_1; + + // Quadratic of s + double a = l_0 + l_1 - 2 * p; + double b = 2 * (p - l_0); + double c = l_0 - target * target; + double root = Math.sqrt(b * b - 4 * a * c); + + // Check if either of the solutions are valid + // Won't divide by zero because it is only possible for a to be zero if the + // target velocity is exactly the same or the reverse of the current + // velocity, which would be caught by the difference check. + double s_1 = (-b + root) / (2 * a); + if (isValidS(s_1)) { + return s_1; + } + double s_2 = (-b - root) / (2 * a); + if (isValidS(s_2)) { + return s_2; + } + + // Since we passed the initial max_vel_step check, a solution should exist, + // but if no solution was found anyway, just don't limit movement + return 1.0; + } + + static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + static boolean epsilonEquals(double a, double b) { + return epsilonEquals(a, b, kEpsilon); + } + + static boolean epsilonEquals(ChassisSpeeds s1, ChassisSpeeds s2) { + return epsilonEquals(s1.vxMetersPerSecond, s2.vxMetersPerSecond) + && epsilonEquals(s1.vyMetersPerSecond, s2.vyMetersPerSecond) + && epsilonEquals(s1.omegaRadiansPerSecond, s2.omegaRadiansPerSecond); + } + + /** + * Check if it would be faster to go to the opposite of the goal heading (and reverse drive + * direction). + * + * @param prevToGoal The rotation from the previous state to the goal state (i.e. + * prev.inverse().rotateBy(goal)). + * @return True if the shortest path to achieve this rotation involves flipping the drive + * direction. + */ + static boolean flipHeading(double prevToGoal) { + return Math.abs(prevToGoal) > Math.PI / 2.0; + } + + private static final Struct structVectors; + private static final Struct structVars; + + static { + final var structVectorsProc = + ProceduralStructGenerator.genObject(LocalVectors.class, LocalVectors::new); + structVectors = + new Struct() { + @Override + public String getSchema() { + return "float64 vx;float64 vy;float64 cos;float64 sin;"; + } + + @Override + public int getSize() { + return 32; + } + + @Override + public Class getTypeClass() { + return LocalVectors.class; + } + + @Override + public String getTypeName() { + return "LocalVectors"; + } + + @Override + public void pack(ByteBuffer bb, LocalVectors value) { + bb.putDouble(value.vx); + bb.putDouble(value.vy); + bb.putDouble(value.cos); + bb.putDouble(value.sin); + } + + @Override + public LocalVectors unpack(ByteBuffer bb) { + return structVectorsProc.unpack(bb); + } + + @Override + public String toString() { + return this.getTypeName() + "<" + this.getSize() + ">" + " {" + this.getSchema() + "}"; + } + }; + structVars = ProceduralStructGenerator.genObject(LocalVars.class, LocalVars::new); + System.out.println(structVectors); + System.out.println(structVars); + } + + static final class LocalVectors implements StructSerializable { + public double vx, vy, cos, sin; + + public LocalVectors() {} + + public void reset() { + vx = vy = cos = sin = 0.0; + } + + public void applyModuleState(SwerveModuleState state) { + cos = state.angle.getCos(); + sin = state.angle.getSin(); + vx = cos * state.speedMetersPerSecond; + vy = sin * state.speedMetersPerSecond; + if (state.speedMetersPerSecond < 0.0) { + applyRotation(Rotation2d.k180deg.getCos(), Rotation2d.k180deg.getSin()); + } + } + + public LocalVectors applyRotation(double otherCos, double otherSin) { + double newCos = cos * otherCos - sin * otherSin; + double newSin = cos * otherSin + sin * otherCos; + cos = newCos; + sin = newSin; + + return this; + } + + public double radians() { + return Math.atan2(sin, cos); + } + + public static final Struct struct = structVectors; + } + + static final class LocalVars implements StructSerializable { + @FixedSizeArray(size = NUM_MODULES) + public LocalVectors[] prev; + + @FixedSizeArray(size = NUM_MODULES) + public LocalVectors[] desired; + + public boolean needToSteer = true, allModulesShouldFlip = true; + public double minS, dt; + public double dx, dy, dtheta; + public ChassisSpeeds prevSpeeds, desiredSpeeds; + + @FixedSizeArray(size = NUM_MODULES) + public SwerveModuleState[] prevModuleStates; + + @FixedSizeArray(size = NUM_MODULES) + public SwerveModuleState[] desiredModuleStates; + + @IgnoreStructField public Rotation2d[] steeringOverride; + + public LocalVars() { + desiredSpeeds = prevSpeeds = new ChassisSpeeds(); + prev = new LocalVectors[NUM_MODULES]; + desired = new LocalVectors[NUM_MODULES]; + steeringOverride = new Rotation2d[NUM_MODULES]; + for (int i = 0; i < NUM_MODULES; i++) { + prev[i] = new LocalVectors(); + desired[i] = new LocalVectors(); + } + } + + public LocalVars reset() { + needToSteer = allModulesShouldFlip = true; + Arrays.fill(steeringOverride, null); + for (int i = 0; i < NUM_MODULES; i++) { + prev[i].reset(); + desired[i].reset(); + } + + return this; + } + + public static final Struct struct = structVars; + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpoint.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpoint.java new file mode 100644 index 0000000..9ec1266 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpoint.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.swerve.setpointGen; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import frc.robot.extras.util.ProceduralStructGenerator; +import frc.robot.extras.util.ProceduralStructGenerator.FixedSizeArray; + +public record SwerveSetpoint( + ChassisSpeeds chassisSpeeds, + @FixedSizeArray(size = 4) AdvancedSwerveModuleState[] moduleStates) // + implements StructSerializable { + public static SwerveSetpoint zeroed() { + return new SwerveSetpoint( + new ChassisSpeeds(), + new AdvancedSwerveModuleState[] { + new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0), + new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0), + new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0), + new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0) + }); + } + + public static final Struct struct; + + static { + struct = ProceduralStructGenerator.genRecord(SwerveSetpoint.class); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java new file mode 100644 index 0000000..392f9e1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java @@ -0,0 +1,367 @@ +package frc.robot.subsystems.swerve.setpointGen; + +import static frc.robot.subsystems.swerve.setpointGen.SPGCalcs.*; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.RobotController; + +/** + * Swerve setpoint generatoR that has been passed around so many times its hard to keep track, just + * know i didn't write most the logic in this code that credit goes to 254 and mjansen + * + *

Takes a prior setpoint, a desired setpoint, and outputs a new setpoint that respects all the + * kinematic constraints on module rotation and wheel velocity/torque, as well as preventing any + * forces acting on a module's wheel from exceeding the force of friction. + */ +public class SwerveSetpointGenerator { + private static final ChassisSpeeds ZERO_SPEEDS = new ChassisSpeeds(); + + private static final int NUM_MODULES = SPGCalcs.NUM_MODULES; + + private final SwerveDriveKinematics kinematics; + private final Translation2d[] moduleLocations; + private final DCMotor driveMotor; + private final double driveCurrentLimitAmps, + maxDriveVelocityMPS, + maxSteerVelocityRadsPerSec, + massKg, + moiKgMetersSquared, + wheelRadiusMeters, + wheelFrictionForce, + // maxTorqueFriction, + torqueLoss; + + public SwerveSetpointGenerator( + final Translation2d[] moduleLocations, + final DCMotor driveMotor, + final DCMotor angleMotor, + final double driveCurrentLimitAmps, + final double massKg, + final double moiKgMetersSquared, + final double wheelDiameterMeters, + final double wheelCoF, + final double torqueLoss) { + + if (moduleLocations.length != NUM_MODULES) { + throw new IllegalArgumentException("Module locations must have 4 elements"); + } + + this.driveMotor = driveMotor; + this.driveCurrentLimitAmps = driveCurrentLimitAmps; + this.maxSteerVelocityRadsPerSec = angleMotor.freeSpeedRadPerSec; + kinematics = new SwerveDriveKinematics(moduleLocations); + this.moduleLocations = moduleLocations; + this.massKg = massKg; + this.moiKgMetersSquared = moiKgMetersSquared; + this.wheelRadiusMeters = wheelDiameterMeters / 2; + this.maxDriveVelocityMPS = driveMotor.freeSpeedRadPerSec * wheelRadiusMeters; + + wheelFrictionForce = wheelCoF * ((massKg / 4) * 9.81); + // maxTorqueFriction = this.wheelFrictionForce * wheelRadiusMeters; + this.torqueLoss = torqueLoss; + } + + // alot of work was done to reduce allocations in this hot loop, + // migrating everything over to a vars object that gets reused + // was the best way to do this. + private static final LocalVars VARS_TEMPLATE = new LocalVars(); + + /** + * Generate a new setpoint. Note: Do not discretize ChassisSpeeds passed into or returned from + * this method. This method will discretize the speeds for you. + * + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredRobotRelativeSpeeds The desired state of motion, such as from the driver sticks + * or a path following algorithm. + * @param dt The loop time. + * @return A Setpoint object that satisfies all the kinematic/friction limits while converging to + * desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredRobotRelativeSpeeds, double dt) { + // https://github.com/wpilibsuite/allwpilib/issues/7332 + SwerveModuleState[] desiredModuleStates = + kinematics.toSwerveModuleStates(desiredRobotRelativeSpeeds); + // Make sure desiredState respects velocity limits. + SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleStates, maxDriveVelocityMPS); + desiredRobotRelativeSpeeds = kinematics.toChassisSpeeds(desiredModuleStates); + + final LocalVars vars = VARS_TEMPLATE.reset(); + vars.dt = dt; + vars.prevSpeeds = prevSetpoint.chassisSpeeds(); + vars.desiredSpeeds = desiredRobotRelativeSpeeds; + vars.desiredModuleStates = desiredModuleStates; + vars.prevModuleStates = prevSetpoint.moduleStates(); + vars.dx = + desiredRobotRelativeSpeeds.vxMetersPerSecond + - prevSetpoint.chassisSpeeds().vxMetersPerSecond; + vars.dy = + desiredRobotRelativeSpeeds.vyMetersPerSecond + - prevSetpoint.chassisSpeeds().vyMetersPerSecond; + vars.dtheta = + desiredRobotRelativeSpeeds.omegaRadiansPerSecond + - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond; + vars.minS = 1.0; + + checkNeedToSteer(vars); + makeVectors(vars); + + // if (vars.allModulesShouldFlip + // && !epsilonEquals(prevSetpoint.chassisSpeeds(), ZERO_SPEEDS) + // && !epsilonEquals(desiredRobotRelativeSpeeds, ZERO_SPEEDS)) { + // // It will (likely) be faster to stop the robot, rotate the modules in place to the + // complement + // // of the desired angle, and accelerate again. + // return generateSetpoint(prevSetpoint, ZERO_SPEEDS, dt); + // } + + solveSteering(vars); + + solveDriving(vars); + + ChassisSpeeds retSpeeds = + new ChassisSpeeds( + vars.prevSpeeds.vxMetersPerSecond + vars.minS * vars.dx, + vars.prevSpeeds.vyMetersPerSecond + vars.minS * vars.dy, + vars.prevSpeeds.omegaRadiansPerSecond + vars.minS * vars.dtheta); + retSpeeds.discretize( + retSpeeds.vxMetersPerSecond, + retSpeeds.vyMetersPerSecond, + retSpeeds.omegaRadiansPerSecond, + dt); + + double chassisAccelX = (retSpeeds.vxMetersPerSecond - vars.prevSpeeds.vxMetersPerSecond) / dt; + double chassisAccelY = (retSpeeds.vyMetersPerSecond - vars.prevSpeeds.vyMetersPerSecond) / dt; + double angularAccel = + (retSpeeds.omegaRadiansPerSecond - vars.prevSpeeds.omegaRadiansPerSecond) / dt; + + SwerveModuleState[] retStates = kinematics.toSwerveModuleStates(retSpeeds); + SwerveModuleState[] accelStates = + kinematics.toSwerveModuleStates( + new ChassisSpeeds(chassisAccelX, chassisAccelY, angularAccel)); + + AdvancedSwerveModuleState[] outputStates = new AdvancedSwerveModuleState[NUM_MODULES]; + for (int m = 0; m < NUM_MODULES; m++) { + retStates[m].optimize(vars.prevModuleStates[m].angle); + double steerVelocity = + (vars.prevModuleStates[m].angle.getRadians() - retStates[m].angle.getRadians()) / dt; + outputStates[m] = + new AdvancedSwerveModuleState( + retStates[m].speedMetersPerSecond, + retStates[m].angle, + steerVelocity, + accelStates[m].speedMetersPerSecond); + } + + // log("output", + return new SwerveSetpoint(retSpeeds, outputStates); + } + + public SwerveSetpoint generateSimpleSetpoint( + final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredRobotRelativeSpeeds, double dt) { + AdvancedSwerveModuleState[] outputStates = new AdvancedSwerveModuleState[NUM_MODULES]; + SwerveModuleState[] desiredModuleStates = + kinematics.toSwerveModuleStates(desiredRobotRelativeSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleStates, maxDriveVelocityMPS); + for (int m = 0; m < NUM_MODULES; m++) { + desiredModuleStates[m].optimize(prevSetpoint.moduleStates()[m].angle); + outputStates[m] = AdvancedSwerveModuleState.fromBase(desiredModuleStates[m]); + } + + return new SwerveSetpoint(kinematics.toChassisSpeeds(desiredModuleStates), outputStates); + } + + private static void checkNeedToSteer(LocalVars vars) { + if (epsilonEquals(vars.desiredSpeeds, ZERO_SPEEDS)) { + vars.needToSteer = false; + for (int m = 0; m < NUM_MODULES; m++) { + vars.desiredModuleStates[m].angle = vars.prevModuleStates[m].angle; + vars.desiredModuleStates[m].speedMetersPerSecond = 0.0; + } + } + } + + private static double rotateBy(double rad, double otherCos, double otherSin) { + return Math.atan2( + Math.sin(rad) * otherCos + Math.cos(rad) * otherSin, + Math.cos(rad) * otherCos - Math.sin(rad) * otherSin); + } + + private static double requiredRotation(double prevRadians, double desiredRads) { + // this looks messy without using Rotation2d methods. + // this is roughly equivalent to: + // + // double r = vars.prev[m].rot2d().unaryMinus() + // .rotateBy(vars.desired[m].rot2d()).getRadians(); + double unaryMinusPrevRads = -prevRadians; + return rotateBy(unaryMinusPrevRads, Math.cos(desiredRads), Math.sin(desiredRads)); + } + + private static void makeVectors(LocalVars vars) { + for (int m = 0; m < NUM_MODULES; m++) { + vars.prev[m].applyModuleState(vars.prevModuleStates[m]); + vars.desired[m].applyModuleState(vars.desiredModuleStates[m]); + if (vars.allModulesShouldFlip) { + double requiredRots = requiredRotation(vars.prev[m].radians(), vars.desired[m].radians()); + if (flipHeading(requiredRots)) { + vars.allModulesShouldFlip = false; + } + } + } + } + + private void solveSteering(LocalVars vars) { + // In cases where an individual module is stopped, we want to remember the right steering angle + // to command (since inverse kinematics doesn't care about angle, we can be opportunistically + // lazy). + // Enforce steering velocity limits. We do this by taking the derivative of steering angle at + // the current angle, and then backing out the maximum interpolant between start and goal + // states. We remember the minimum across all modules, since that is the active constraint. + for (int m = 0; m < NUM_MODULES; m++) { + if (!vars.needToSteer) { + vars.steeringOverride[m] = vars.prevModuleStates[m].angle; + continue; + } + + double maxThetaStep = vars.dt * maxSteerVelocityRadsPerSec; + + if (epsilonEquals(vars.prevModuleStates[m].speedMetersPerSecond, 0.0)) { + // If module is stopped, we know that we will need to move straight to the final steering + // angle, so limit based purely on rotation in place. + if (epsilonEquals(vars.desiredModuleStates[m].speedMetersPerSecond, 0.0)) { + // Goal angle doesn't matter. Just leave module at its current angle. + vars.steeringOverride[m] = vars.prevModuleStates[m].angle; + continue; + } + + double requiredRots = requiredRotation(vars.prev[m].radians(), vars.desired[m].radians()); + if (flipHeading(requiredRots)) { + requiredRots = + rotateBy(requiredRots, Rotation2d.k180deg.getCos(), Rotation2d.k180deg.getSin()); + } + + // radians bounds to +/- Pi. + final double numStepsNeeded = Math.abs(requiredRots) / maxThetaStep; + + if (numStepsNeeded <= 1.0) { + // Steer directly to goal angle. + vars.steeringOverride[m] = vars.desiredModuleStates[m].angle; + } else { + // Adjust steering by max_theta_step. + // there really is no way to avoid this allocation. + Rotation2d adjusted = + vars.prevModuleStates[m].angle.rotateBy( + Rotation2d.fromRadians(Math.signum(requiredRots) * maxThetaStep)); + vars.steeringOverride[m] = adjusted; + vars.minS = 0.0; + } + continue; + } + if (vars.minS == 0.0) { + // s can't get any lower. Save some CPU. + continue; + } + + double maxS = + findSteeringMaxS( + vars.prev[m].vx, + vars.prev[m].vy, + vars.prev[m].radians(), + vars.desired[m].vx, + vars.desired[m].vy, + vars.desired[m].radians(), + maxThetaStep); + vars.minS = Math.min(vars.minS, maxS); + } + } + + private void solveDriving(LocalVars vars) { + // Enforce drive wheel torque limits + Translation2d chassisForceVec = Translation2d.kZero; + double chassisTorque = 0.0; + for (int m = 0; m < NUM_MODULES; m++) { + double lastVelRadPerSec = vars.prevModuleStates[m].speedMetersPerSecond / wheelRadiusMeters; + // Use the current battery voltage since we won't be able to supply 12v if the + // battery is sagging down to 11v, which will affect the max torque output + double currentDraw = + driveMotor.getCurrent(Math.abs(lastVelRadPerSec), RobotController.getInputVoltage()); + currentDraw = Math.min(currentDraw, driveCurrentLimitAmps); + double moduleTorque = driveMotor.getTorque(currentDraw); + + double prevSpeed = vars.prevModuleStates[m].speedMetersPerSecond; + vars.desiredModuleStates[m].optimize(vars.prevModuleStates[m].angle); + double desiredSpeed = vars.desiredModuleStates[m].speedMetersPerSecond; + + int forceSign; + Rotation2d forceAngle = vars.prevModuleStates[m].angle; + if (epsilonEquals(prevSpeed, 0.0) + || (prevSpeed > 0 && desiredSpeed >= prevSpeed) + || (prevSpeed < 0 && desiredSpeed <= prevSpeed)) { + // Torque loss will be fighting motor + moduleTorque -= torqueLoss; + forceSign = 1; // Force will be applied in direction of module + if (prevSpeed < 0) { + forceAngle = forceAngle.plus(Rotation2d.k180deg); + } + } else { + // Torque loss will be helping the motor + moduleTorque += torqueLoss; + forceSign = -1; // Force will be applied in opposite direction of module + if (prevSpeed > 0) { + forceAngle = forceAngle.plus(Rotation2d.k180deg); + } + } + + // Limit torque to prevent wheel slip + moduleTorque = Math.min(moduleTorque, wheelFrictionForce * wheelRadiusMeters); + + double forceAtCarpet = moduleTorque / wheelRadiusMeters; + Translation2d moduleForceVec = new Translation2d(forceAtCarpet * forceSign, forceAngle); + + // Add the module force vector to the chassis force vector + chassisForceVec = chassisForceVec.plus(moduleForceVec); + + // Calculate the torque this module will apply to the chassis + Rotation2d angleToModule = moduleLocations[m].getAngle(); + Rotation2d theta = moduleForceVec.getAngle().minus(angleToModule); + chassisTorque += forceAtCarpet * moduleLocations[m].getNorm() * theta.getSin(); + } + + Translation2d chassisAccelVec = chassisForceVec.div(massKg); + double chassisAngularAccel = chassisTorque / moiKgMetersSquared; + + // Use kinematics to convert chassis accelerations to module accelerations + ChassisSpeeds chassisAccel = + new ChassisSpeeds(chassisAccelVec.getX(), chassisAccelVec.getY(), chassisAngularAccel); + var accelStates = kinematics.toSwerveModuleStates(chassisAccel); + + for (int m = 0; m < NUM_MODULES; m++) { + if (vars.minS == 0.0) { + // No need to carry on. + break; + } + + double maxVelStep = Math.abs(accelStates[m].speedMetersPerSecond * vars.dt); + + double vxMinS; + double vyMinS; + if (vars.minS == 1.0) { + vxMinS = vars.desired[m].vx; + vyMinS = vars.desired[m].vy; + } else { + vxMinS = (vars.desired[m].vx - vars.prev[m].vx) * vars.minS + vars.prev[m].vx; + vyMinS = (vars.desired[m].vy - vars.prev[m].vy) * vars.minS + vars.prev[m].vy; + } + // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we + // already know we can't go faster than that. + double s = findDriveMaxS(vars.prev[m].vx, vars.prev[m].vy, vxMinS, vyMinS, maxVelStep); + vars.minS = Math.min(vars.minS, s); + } + } +}