From 7f4093b14a6986151c9bae5202d7a6a25cd36dc3 Mon Sep 17 00:00:00 2001 From: miapirobotics <34904974+miapirobotics@users.noreply.github.com> Date: Fri, 3 Feb 2023 16:45:49 -0600 Subject: [PATCH 01/31] Calculate trajectories off the RIO --- .gitignore | 17 +- build.gradle | 418 ++++++++--------- .../com/team1816/example/ExampleAutoPath.java | 144 +++--- .../com/team1816/lib/auto/paths/AutoPath.java | 422 +++++++++--------- .../com/team1816/lib/auto/paths/PathUtil.java | 325 +++++++------- .../com/team1816/lib/motion/Trajectories.java | 51 +++ .../season/auto/paths/DriveStraightPath.java | 120 ++--- src/main/resources/trajectories/README.md | 5 + 8 files changed, 794 insertions(+), 708 deletions(-) create mode 100644 src/main/java/com/team1816/lib/motion/Trajectories.java create mode 100644 src/main/resources/trajectories/README.md diff --git a/.gitignore b/.gitignore index ee3b0587..c848a24a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,8 +1,9 @@ -venv -.idea -build -.gradle -simgui-window.json -bin/ -*.bag -Compiler +venv +.idea +build +.gradle +simgui-window.json +bin/ +*.bag +Compiler +src/main/resources/trajectories/*.json \ No newline at end of file diff --git a/build.gradle b/build.gradle index 7555b052..db7a86dd 100644 --- a/build.gradle +++ b/build.gradle @@ -1,204 +1,214 @@ -import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO -import org.gradle.nativeplatform.platform.internal.DefaultNativePlatform - -plugins { - id "java" - id "edu.wpi.first.GradleRIO" version "2023.2.1" - id 'org.jsonschema2dataclass' version '4.0.1' -} - -sourceCompatibility = JavaVersion.VERSION_17 -targetCompatibility = JavaVersion.VERSION_17 - -def ROBOT_MAIN_CLASS = "com.team1816.season.Main" - -// Define my targets (RoboRIO) and artifacts (deployable files) -// This is added by GradleRIO's backing project DeployUtils. -deploy { - targets { - roborio(getTargetTypeClass('RoboRIO')) { - // Team number is loaded either from the .wpilib/wpilib_preferences.json - // or from command line. If not found an exception will be thrown. - // You can use getTeamOrDefault(team) instead of getTeamNumber if you - // want to store a team number in this file. - team = 1816 - debug = project.frc.getDebugOrDefault(false) - checkImage = false - artifacts { - // First part is artifact name, 2nd is artifact type - // getTargetTypeClass is a shortcut to get the class type using a string - - frcJava(getArtifactTypeClass('FRCJavaArtifact')) { - } - - // Static files artifact - frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { - files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - } - } - } - } -} - -def deployArtifact = deploy.targets.roborio.artifacts.frcJava - -// Set to true to use debug for JNI. -wpi.java.debugJni = false - -// Set this to true to enable desktop support. -def includeDesktopSupport = true - -repositories { - mavenLocal() - mavenCentral() - maven { url "https://www.jitpack.io" } -} - -// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 4. -dependencies { - implementation wpi.java.deps.wpilib() - implementation wpi.java.vendor.java() - - roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) - roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) - - roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) - roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) - - nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) - nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) - simulationDebug wpi.sim.enableDebug() - - nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) - nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) - simulationRelease wpi.sim.enableRelease() - - implementation "com.github.dominikwin:badlog:v0.1.1" - implementation 'org.yaml:snakeyaml:1.30' - implementation 'gov.nist.math:jama:1.0.3' - implementation 'com.google.inject:guice:5.1.0' - implementation 'com.google.inject.extensions:guice-assistedinject:5.1.0' - implementation 'org.apache.commons:commons-math3:3.6.1' - - testImplementation 'junit:junit:4.13.2' - testImplementation 'org.mockito:mockito-inline:4.7.0' - testImplementation 'org.hamcrest:hamcrest:2.2' -} - -// Simulation configuration (e.g. environment variables). -wpi.sim.addGui().defaultEnabled.set(true) -wpi.sim.addDriverstation() - -// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') -// in order to make them all available at runtime. Also adding the manifest so WPILib -// knows where to look for our Robot Class. -jar { - from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) - duplicatesStrategy = DuplicatesStrategy.INCLUDE - -} - -jsonSchema2Pojo { - targetPackage = 'com.team1816.lib' - useTitleAsClassname = true - includeGetters = false - includeSetters = false - removeOldOutput = true - includeAdditionalProperties = false - includeToString = true - includeHashcodeAndEquals = false - source.setFrom files( - "${project.rootDir}/src/main/resources/schemas" - ) -} - -task runAlpha(type: JavaExec) { - String jniDir = "${project.buildDir}/jni/release" - environment "PATH", jniDir - String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath - if (!DefaultNativePlatform.currentOperatingSystem.isWindows()) { - environment "LD_LIBRARY_PATH", jniDir - environment "DYLD_FALLBACK_LIBRARY_PATH", jniDir - environment "DYLD_LIBRARY_PATH", jniDir - jvmArgs "-XstartOnFirstThread" - } - environment "HALSIM_EXTENSIONS", hal - environment "ROBOT_NAME", "alpha" - dependsOn(jar, simulateExternalJavaRelease) - classpath = files(jar) - jvmArgs "-Djava.library.path=${jniDir}" -} - -task runZero(type: JavaExec) { - String jniDir = "${project.buildDir}/jni/release" - environment "PATH", jniDir - String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath - if (!DefaultNativePlatform.currentOperatingSystem.isWindows()) { - environment "LD_LIBRARY_PATH", jniDir - environment "DYLD_FALLBACK_LIBRARY_PATH", jniDir - environment "DYLD_LIBRARY_PATH", jniDir - jvmArgs "-XstartOnFirstThread" - } - environment "HALSIM_EXTENSIONS", hal - environment "ROBOT_NAME", "zero" - dependsOn(jar, simulateExternalJavaRelease) - classpath = files(jar) - jvmArgs "-Djava.library.path=${jniDir}" -} - -task runZoffseason(type: JavaExec) { - String jniDir = "${project.buildDir}/jni/release" - environment "PATH", jniDir - String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath - if (!DefaultNativePlatform.currentOperatingSystem.isWindows()) { - environment "LD_LIBRARY_PATH", jniDir - environment "DYLD_FALLBACK_LIBRARY_PATH", jniDir - environment "DYLD_LIBRARY_PATH", jniDir - jvmArgs "-XstartOnFirstThread" - } - environment "HALSIM_EXTENSIONS", hal - environment "ROBOT_NAME", "zoffseason" - dependsOn(jar, simulateExternalJavaRelease) - classpath = files(jar) - jvmArgs "-Djava.library.path=${jniDir}" -} - - -task runZodiac(type: JavaExec) { - String jniDir = "${project.buildDir}/jni/release" - environment "PATH", jniDir - String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath - if (!DefaultNativePlatform.currentOperatingSystem.isWindows()) { - environment "LD_LIBRARY_PATH", jniDir - environment "DYLD_FALLBACK_LIBRARY_PATH", jniDir - environment "DYLD_LIBRARY_PATH", jniDir - jvmArgs "-XstartOnFirstThread" - } - environment "HALSIM_EXTENSIONS", hal - environment "ROBOT_NAME", "zodiac_pro" - dependsOn(jar, simulateExternalJavaRelease) - classpath = files(jar) - jvmArgs "-Djava.library.path=${jniDir}" -} - -processResources { - include '**/*.config.yml' - eachFile { fileCopyDetails -> - } -} - -wrapper { - gradleVersion = '7.2' -} - -// Configure jar and deploy tasks -deployArtifact.jarTask = jar -wpi.java.configureExecutableTasks(jar) -wpi.java.configureTestTasks(test) - -compileJava { - //dependsOn tasks.named("spotlessApply") -} +import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO +import org.gradle.nativeplatform.platform.internal.DefaultNativePlatform + +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2023.2.1" + id 'org.jsonschema2dataclass' version '4.0.1' +} + +sourceCompatibility = JavaVersion.VERSION_17 +targetCompatibility = JavaVersion.VERSION_17 + +def ROBOT_MAIN_CLASS = "com.team1816.season.Main" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = 1816 + debug = project.frc.getDebugOrDefault(false) + checkImage = false + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcJava + +// Set to true to use debug for JNI. +wpi.java.debugJni = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = true + +repositories { + mavenLocal() + mavenCentral() + maven { url "https://www.jitpack.io" } +} + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 4. +dependencies { + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() + + implementation "com.github.dominikwin:badlog:v0.1.1" + implementation 'org.yaml:snakeyaml:1.30' + implementation 'gov.nist.math:jama:1.0.3' + implementation 'com.google.inject:guice:5.1.0' + implementation 'com.google.inject.extensions:guice-assistedinject:5.1.0' + implementation 'org.apache.commons:commons-math3:3.6.1' + + testImplementation 'junit:junit:4.13.2' + testImplementation 'org.mockito:mockito-inline:4.7.0' + testImplementation 'org.hamcrest:hamcrest:2.2' +} + +// Simulation configuration (e.g. environment variables). +wpi.sim.addGui().defaultEnabled.set(true) +wpi.sim.addDriverstation() + +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// knows where to look for our Robot Class. +jar { + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + duplicatesStrategy = DuplicatesStrategy.INCLUDE + +} + +jsonSchema2Pojo { + targetPackage = 'com.team1816.lib' + useTitleAsClassname = true + includeGetters = false + includeSetters = false + removeOldOutput = true + includeAdditionalProperties = false + includeToString = true + includeHashcodeAndEquals = false + source.setFrom files( + "${project.rootDir}/src/main/resources/schemas" + ) +} + +task runAlpha(type: JavaExec) { + String jniDir = "${project.buildDir}/jni/release" + environment "PATH", jniDir + String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath + if (!DefaultNativePlatform.currentOperatingSystem.isWindows()) { + environment "LD_LIBRARY_PATH", jniDir + environment "DYLD_FALLBACK_LIBRARY_PATH", jniDir + environment "DYLD_LIBRARY_PATH", jniDir + jvmArgs "-XstartOnFirstThread" + } + environment "HALSIM_EXTENSIONS", hal + environment "ROBOT_NAME", "alpha" + dependsOn(jar, simulateExternalJavaRelease) + classpath = files(jar) + jvmArgs "-Djava.library.path=${jniDir}" +} + +task runZero(type: JavaExec) { + String jniDir = "${project.buildDir}/jni/release" + environment "PATH", jniDir + String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath + if (!DefaultNativePlatform.currentOperatingSystem.isWindows()) { + environment "LD_LIBRARY_PATH", jniDir + environment "DYLD_FALLBACK_LIBRARY_PATH", jniDir + environment "DYLD_LIBRARY_PATH", jniDir + jvmArgs "-XstartOnFirstThread" + } + environment "HALSIM_EXTENSIONS", hal + environment "ROBOT_NAME", "zero" + dependsOn(jar, simulateExternalJavaRelease) + classpath = files(jar) + jvmArgs "-Djava.library.path=${jniDir}" +} + +task runZoffseason(type: JavaExec) { + String jniDir = "${project.buildDir}/jni/release" + environment "PATH", jniDir + String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath + if (!DefaultNativePlatform.currentOperatingSystem.isWindows()) { + environment "LD_LIBRARY_PATH", jniDir + environment "DYLD_FALLBACK_LIBRARY_PATH", jniDir + environment "DYLD_LIBRARY_PATH", jniDir + jvmArgs "-XstartOnFirstThread" + } + environment "HALSIM_EXTENSIONS", hal + environment "ROBOT_NAME", "zoffseason" + dependsOn(jar, simulateExternalJavaRelease) + classpath = files(jar) + jvmArgs "-Djava.library.path=${jniDir}" +} + + +task runZodiac(type: JavaExec) { + String jniDir = "${project.buildDir}/jni/release" + environment "PATH", jniDir + String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath + if (!DefaultNativePlatform.currentOperatingSystem.isWindows()) { + environment "LD_LIBRARY_PATH", jniDir + environment "DYLD_FALLBACK_LIBRARY_PATH", jniDir + environment "DYLD_LIBRARY_PATH", jniDir + jvmArgs "-XstartOnFirstThread" + } + environment "HALSIM_EXTENSIONS", hal + environment "ROBOT_NAME", "zodiac_pro" + dependsOn(jar, simulateExternalJavaRelease) + classpath = files(jar) + jvmArgs "-Djava.library.path=${jniDir}" +} +apply plugin: 'java'; +task calculateTrajectories() { + dependsOn compileJava + doLast { + URL[] urls = sourceSets.main.runtimeClasspath.files.collect { it.toURI().toURL() } as URL[] + def classloader = new java.net.URLClassLoader(urls) + Class type = classloader.loadClass("com.team1816.lib.motion.Trajectories"); + type.calculate() + } +} + +processResources { + include '**/*.config.yml' + eachFile { fileCopyDetails -> + } +} + +wrapper { + gradleVersion = '7.2' +} + +// Configure jar and deploy tasks +deployArtifact.jarTask = jar +wpi.java.configureExecutableTasks(jar) +wpi.java.configureTestTasks(test) + +compileJava { + //dependsOn tasks.named("spotlessApply") +} diff --git a/src/main/java/com/team1816/example/ExampleAutoPath.java b/src/main/java/com/team1816/example/ExampleAutoPath.java index 946ad2ab..99203415 100644 --- a/src/main/java/com/team1816/example/ExampleAutoPath.java +++ b/src/main/java/com/team1816/example/ExampleAutoPath.java @@ -1,72 +1,72 @@ -package com.team1816.example; - -import com.team1816.lib.auto.paths.AutoPath; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; - -import java.util.List; - -public class ExampleAutoPath extends AutoPath { - - /** - * To build your path, clone our cheesy-path repository (git clone https://github.com/TheGreenMachine/cheesy-path.git) - * and follow instructions on the ReadME. - *

- * Here's a more in-depth guide if you want/need it-> - * https://docs.google.com/document/d/1jCRidlvIZ5hZg-OFc1bZWmaZgIgztRfeLw-Fo9ySd6s/edit?usp=sharing - *

- * - * @return A list of points on the field that the robot must pass through - these points are linked together - * into a trajectory for use in AutoModes - */ - @Override - protected List getWaypoints() { - // path looks like a deformed and mirrored C shape - return List.of( - new Pose2d(1.72, 0.49, Rotation2d.fromDegrees(0)), - new Pose2d(6.76, 1.15, Rotation2d.fromDegrees(0)), - new Pose2d(9.09, 1.46, Rotation2d.fromDegrees(45)), - new Pose2d(9.04, 4.4, Rotation2d.fromDegrees(135)), - new Pose2d(1.77, 4.93, Rotation2d.fromDegrees(180)) - ); - } - - /** - * Note: this is only used for SwerveDrive robots b/c TankDrive heading is the same as its Pose2d's Rotation2d component. - * Each waypoint should have a corresponding waypointHeading - more or less than the # of waypoints will cause faulty behavior - * - * @return a list of headings to go through when generating the full trajectory - as of now, we don't have anything - * in chezy path that includes this, you just have to eyeball where the robot should face using the simulator. - */ - @Override - protected List getWaypointHeadings() { - // this will make a SwerveDrive robot face the opponent alliance wall throughout the entire path - return List.of( - Rotation2d.fromDegrees(0), - Rotation2d.fromDegrees(0), - Rotation2d.fromDegrees(0), - Rotation2d.fromDegrees(0), - Rotation2d.fromDegrees(0) - ); - } - - @Override - protected List getReflectedWaypoints() { - return null; - } - - @Override - protected List getReflectedWaypointHeadings() { - return null; - } - - /** - * Honestly we shouldn't even have this here anymore - * - * @return just make this return true - we're always using the app - */ - @Override - protected boolean usingApp() { - return true; - } // TODO remove me :) -} +package com.team1816.example; + +import com.team1816.lib.auto.paths.AutoPath; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +import java.util.List; + +public class ExampleAutoPath extends AutoPath { + + /** + * To build your path, clone our cheesy-path repository (git clone https://github.com/TheGreenMachine/cheesy-path.git) + * and follow instructions on the ReadME. + *

+ * Here's a more in-depth guide if you want/need it-> + * https://docs.google.com/document/d/1jCRidlvIZ5hZg-OFc1bZWmaZgIgztRfeLw-Fo9ySd6s/edit?usp=sharing + *

+ * + * @return A list of points on the field that the robot must pass through - these points are linked together + * into a trajectory for use in AutoModes + */ + @Override + public List getWaypoints() { + // path looks like a deformed and mirrored C shape + return List.of( + new Pose2d(1.72, 0.49, Rotation2d.fromDegrees(0)), + new Pose2d(6.76, 1.15, Rotation2d.fromDegrees(0)), + new Pose2d(9.09, 1.46, Rotation2d.fromDegrees(45)), + new Pose2d(9.04, 4.4, Rotation2d.fromDegrees(135)), + new Pose2d(1.77, 4.93, Rotation2d.fromDegrees(180)) + ); + } + + /** + * Note: this is only used for SwerveDrive robots b/c TankDrive heading is the same as its Pose2d's Rotation2d component. + * Each waypoint should have a corresponding waypointHeading - more or less than the # of waypoints will cause faulty behavior + * + * @return a list of headings to go through when generating the full trajectory - as of now, we don't have anything + * in chezy path that includes this, you just have to eyeball where the robot should face using the simulator. + */ + @Override + protected List getWaypointHeadings() { + // this will make a SwerveDrive robot face the opponent alliance wall throughout the entire path + return List.of( + Rotation2d.fromDegrees(0), + Rotation2d.fromDegrees(0), + Rotation2d.fromDegrees(0), + Rotation2d.fromDegrees(0), + Rotation2d.fromDegrees(0) + ); + } + + @Override + public List getReflectedWaypoints() { + return null; + } + + @Override + protected List getReflectedWaypointHeadings() { + return null; + } + + /** + * Honestly we shouldn't even have this here anymore + * + * @return just make this return true - we're always using the app + */ + @Override + protected boolean usingApp() { + return true; + } // TODO remove me :) +} diff --git a/src/main/java/com/team1816/lib/auto/paths/AutoPath.java b/src/main/java/com/team1816/lib/auto/paths/AutoPath.java index 2301e6b7..e2ae6928 100644 --- a/src/main/java/com/team1816/lib/auto/paths/AutoPath.java +++ b/src/main/java/com/team1816/lib/auto/paths/AutoPath.java @@ -1,209 +1,213 @@ -package com.team1816.lib.auto.paths; - -import com.team1816.lib.auto.Color; -import com.team1816.lib.auto.Symmetry; -import com.team1816.season.configuration.Constants; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.Trajectory; - -import java.util.ArrayList; -import java.util.List; - -/** - * Abstract class containing all information necessary for running a trajectory as a TrajectoryAction - * - * @see com.team1816.lib.auto.actions.TrajectoryAction - * @see Trajectory - */ -public abstract class AutoPath { - - /** - * State: contains information about the trajectory that defines the path - */ - Trajectory trajectory; - - /** - * State: headings corresponding to the path that can be transposed onto the trajectory for omni-directional drivetrains like swerve - */ - List headings; - - /** - * State: contains information about whether the trajectory should be reflected and how - */ - boolean reflected; - - /** - * State: contains information about whether the trajectory should be rotated and how - */ - boolean rotated; - - public AutoPath() { - } - - public AutoPath(Color color) { - if (Constants.fieldSymmetry == Symmetry.AXIS && color == Color.RED) { - setReflected(true); - setRotated(false); - } else if (Constants.fieldSymmetry == Symmetry.ORIGIN && color == Color.RED) { - setReflected(false); - setRotated(true); - } else { - setReflected(false); - setRotated(false); - } - } - - /** - * Sets the reflected state to passed argument - * - * @param reflected - reflected - */ - protected void setReflected(boolean reflected) { - this.reflected = reflected; - } - - /** - * Sets the rotated state to passed argument - * - * @param rotated - rotated - */ - protected void setRotated(boolean rotated) { - this.rotated = rotated; - } - - /** - * Returns a list of Pose2d's that define the trajectory /path to be followed - * - * @return waypoints - */ - protected abstract List getWaypoints(); - - /** - * Returns a list of Pose2d's that define the reflected trajectory/path to be followed - * - * @return waypoints - */ - protected List getReflectedWaypoints() { - List waypoints = getWaypoints(); - List reflectedWaypoints = new ArrayList<>(); - for (int i = 0; i < waypoints.size(); i++) { - Pose2d waypoint = new Pose2d(2 * Constants.fieldCenterX - waypoints.get(i).getX(), waypoints.get(i).getY(), Rotation2d.fromDegrees(180 - waypoints.get(i).getRotation().getDegrees())); - reflectedWaypoints.add(i, waypoint); - } - return reflectedWaypoints; - } - - /** - * Returns a list of Pose2d's that define the rotated trajectory/path to be followed - * - * @return waypoints - */ - protected List getRotatedWaypoints() { - List waypoints = getWaypoints(); - List rotatedWaypoints = new ArrayList<>(); - for (int i = 0; i < waypoints.size(); i++) { - Pose2d waypoint = new Pose2d(2 * Constants.fieldCenterX - waypoints.get(i).getX(), 2 * Constants.fieldCenterY - waypoints.get(i).getY(), Rotation2d.fromDegrees(180 + waypoints.get(i).getRotation().getDegrees())); - rotatedWaypoints.add(i, waypoint); - } - return rotatedWaypoints; - } - - /** - * Returns a list of Rotation2d's corresponding to the rotation respect to a trajectory - * - * @return waypointHeadings - */ - protected abstract List getWaypointHeadings(); - - /** - * Returns a list of Rotation2d's corresponding to the rotation respect to a reflected trajectory - * - * @return waypointHeadings - */ - protected List getReflectedWaypointHeadings() { - List waypointHeadings = getWaypointHeadings(); - List reflectedWaypointHeadings = new ArrayList<>(); - for (int i = 0; i < waypointHeadings.size(); i++) { - var waypointRotation = Rotation2d.fromDegrees(180 - waypointHeadings.get(i).getDegrees()); - reflectedWaypointHeadings.add(i, waypointRotation); - } - return reflectedWaypointHeadings; - } - - /** - * Returns a list of Rotation2d's corresponding to the rotation respect to a rotated trajectory - * - * @return waypointHeadings - */ - protected List getRotatedWaypointHeadings() { - List waypointHeadings = getWaypointHeadings(); - List rotatedWaypointHeadings = new ArrayList<>(); - for (int i = 0; i < waypointHeadings.size(); i++) { - var waypointRotation = Rotation2d.fromDegrees(180 + waypointHeadings.get(i).getDegrees()); - rotatedWaypointHeadings.add(i, waypointRotation); - } - return rotatedWaypointHeadings; - } - - /** - * Checks if the path was made using the CheesyPath web application. - * If false, the starting pose of the trajectory will be set to the default starting pose. - * - * @return boolean usingApp - */ - protected abstract boolean usingApp(); - - /** - * Returns the generated trajectory associated with the AutoPath - * - * @return trajectory - * @see Trajectory - */ - public Trajectory getAsTrajectory() { - if (trajectory == null) { - if (!reflected && !rotated) { - trajectory = PathUtil.generateTrajectory(usingApp(), getWaypoints()); - } else if (reflected) { - trajectory = PathUtil.generateTrajectory(usingApp(), getReflectedWaypoints()); - } else { - trajectory = PathUtil.generateTrajectory(usingApp(), getRotatedWaypoints()); - } - } - return trajectory; - } - - /** - * Returns the list of trajectory headings that are transposed onto the path, ignored for tank based drivetrains - * - * @return trajectoryHeadings - * @see com.team1816.lib.auto.actions.TrajectoryAction - */ - public List getAsTrajectoryHeadings() { - if (headings == null) { - if (!reflected && !rotated) { - headings = - PathUtil.generateHeadings( - usingApp(), - getWaypoints(), - getWaypointHeadings() - ); - } else if (reflected) { - headings = - PathUtil.generateHeadings( - usingApp(), - getReflectedWaypoints(), - getReflectedWaypointHeadings() - ); - } else { - headings = - PathUtil.generateHeadings( - usingApp(), - getRotatedWaypoints(), - getRotatedWaypointHeadings() - ); - } - } - return headings; - } -} +package com.team1816.lib.auto.paths; + +import com.team1816.lib.auto.Color; +import com.team1816.lib.auto.Symmetry; +import com.team1816.season.configuration.Constants; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.Trajectory; + +import java.util.ArrayList; +import java.util.List; + +/** + * Abstract class containing all information necessary for running a trajectory as a TrajectoryAction + * + * @see com.team1816.lib.auto.actions.TrajectoryAction + * @see Trajectory + */ +public abstract class AutoPath { + + /** + * State: contains information about the trajectory that defines the path + */ + Trajectory trajectory; + + /** + * State: headings corresponding to the path that can be transposed onto the trajectory for omni-directional drivetrains like swerve + */ + List headings; + + /** + * State: contains information about whether the trajectory should be reflected and how + */ + boolean reflected; + + /** + * State: contains information about whether the trajectory should be rotated and how + */ + boolean rotated; + + public AutoPath() { + } + + public AutoPath(Color color) { + if (Constants.fieldSymmetry == Symmetry.AXIS && color == Color.RED) { + setReflected(true); + setRotated(false); + } else if (Constants.fieldSymmetry == Symmetry.ORIGIN && color == Color.RED) { + setReflected(false); + setRotated(true); + } else { + setReflected(false); + setRotated(false); + } + } + + /** + * Sets the reflected state to passed argument + * + * @param reflected - reflected + */ + protected void setReflected(boolean reflected) { + this.reflected = reflected; + } + + /** + * Sets the rotated state to passed argument + * + * @param rotated - rotated + */ + protected void setRotated(boolean rotated) { + this.rotated = rotated; + } + + /** + * Returns a list of Pose2d's that define the trajectory /path to be followed + * + * @return waypoints + */ + public abstract List getWaypoints(); + + /** + * Returns a list of Pose2d's that define the reflected trajectory/path to be followed + * + * @return waypoints + */ + public List getReflectedWaypoints() { + List waypoints = getWaypoints(); + List reflectedWaypoints = new ArrayList<>(); + for (int i = 0; i < waypoints.size(); i++) { + Pose2d waypoint = new Pose2d(2 * Constants.fieldCenterX - waypoints.get(i).getX(), waypoints.get(i).getY(), Rotation2d.fromDegrees(180 - waypoints.get(i).getRotation().getDegrees())); + reflectedWaypoints.add(i, waypoint); + } + return reflectedWaypoints; + } + + /** + * Returns a list of Pose2d's that define the rotated trajectory/path to be followed + * + * @return waypoints + */ + public List getRotatedWaypoints() { + List waypoints = getWaypoints(); + List rotatedWaypoints = new ArrayList<>(); + for (int i = 0; i < waypoints.size(); i++) { + Pose2d waypoint = new Pose2d(2 * Constants.fieldCenterX - waypoints.get(i).getX(), 2 * Constants.fieldCenterY - waypoints.get(i).getY(), Rotation2d.fromDegrees(180 + waypoints.get(i).getRotation().getDegrees())); + rotatedWaypoints.add(i, waypoint); + } + return rotatedWaypoints; + } + + /** + * Returns a list of Rotation2d's corresponding to the rotation respect to a trajectory + * + * @return waypointHeadings + */ + protected abstract List getWaypointHeadings(); + + /** + * Returns a list of Rotation2d's corresponding to the rotation respect to a reflected trajectory + * + * @return waypointHeadings + */ + protected List getReflectedWaypointHeadings() { + List waypointHeadings = getWaypointHeadings(); + List reflectedWaypointHeadings = new ArrayList<>(); + for (int i = 0; i < waypointHeadings.size(); i++) { + var waypointRotation = Rotation2d.fromDegrees(180 - waypointHeadings.get(i).getDegrees()); + reflectedWaypointHeadings.add(i, waypointRotation); + } + return reflectedWaypointHeadings; + } + + /** + * Returns a list of Rotation2d's corresponding to the rotation respect to a rotated trajectory + * + * @return waypointHeadings + */ + protected List getRotatedWaypointHeadings() { + List waypointHeadings = getWaypointHeadings(); + List rotatedWaypointHeadings = new ArrayList<>(); + for (int i = 0; i < waypointHeadings.size(); i++) { + var waypointRotation = Rotation2d.fromDegrees(180 + waypointHeadings.get(i).getDegrees()); + rotatedWaypointHeadings.add(i, waypointRotation); + } + return rotatedWaypointHeadings; + } + + /** + * Checks if the path was made using the CheesyPath web application. + * If false, the starting pose of the trajectory will be set to the default starting pose. + * + * @return boolean usingApp + */ + protected abstract boolean usingApp(); + + /** + * Returns the generated trajectory associated with the AutoPath + * + * @return trajectory + * @see Trajectory + */ + public Trajectory getAsTrajectory() { + if (trajectory == null) { + if (!reflected && !rotated) { + trajectory = PathUtil.generateTrajectory(getClass().getName(), usingApp(), getWaypoints()); + } else if (reflected) { + trajectory = PathUtil.generateTrajectory(getClass().getName() + "-reversed", usingApp(), getReflectedWaypoints()); + } else { + trajectory = PathUtil.generateTrajectory(getClass().getName() + "-rotated", usingApp(), getRotatedWaypoints()); + } + } + return trajectory; + } + + /** + * Returns the list of trajectory headings that are transposed onto the path, ignored for tank based drivetrains + * + * @return trajectoryHeadings + * @see com.team1816.lib.auto.actions.TrajectoryAction + */ + public List getAsTrajectoryHeadings() { + if (headings == null) { + String name = getClass().getName(); + if (!reflected && !rotated) { + headings = + PathUtil.generateHeadings( + name, + usingApp(), + getWaypoints(), + getWaypointHeadings() + ); + } else if (reflected) { + headings = + PathUtil.generateHeadings( + name + "-reversed", + usingApp(), + getReflectedWaypoints(), + getReflectedWaypointHeadings() + ); + } else { + headings = + PathUtil.generateHeadings( + name + "-rotated", + usingApp(), + getRotatedWaypoints(), + getRotatedWaypointHeadings() + ); + } + } + return headings; + } +} diff --git a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java index c67c9b7e..aab68b2e 100644 --- a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java +++ b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java @@ -1,155 +1,170 @@ -package com.team1816.lib.auto.paths; - -import com.team1816.season.configuration.Constants; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; - -import java.util.ArrayList; -import java.util.List; - -import static com.team1816.lib.subsystems.drive.Drive.kPathFollowingMaxAccelMeters; -import static com.team1816.lib.subsystems.drive.Drive.kPathFollowingMaxVelMeters; - -public class PathUtil { - - /** - * Constraints - */ - private static final double kMaxVelocity = kPathFollowingMaxVelMeters; - private static final double kMaxAccel = kPathFollowingMaxAccelMeters; - - /** - * Generates a trajectory based on a list of waypoints based on WPIlib's TrajectoryGenerator - * - * @param usingApp - * @param waypoints - * @return trajectory - * @see com.team1816.lib.auto.modes.AutoMode - * @see AutoPath - * @see edu.wpi.first.math.trajectory.TrajectoryGenerator - */ - public static Trajectory generateTrajectory( - boolean usingApp, - List waypoints - ) { - /* Inch to meter conversions for waypoints for trajectory calculations */ - List waypointsMeters = new ArrayList<>(); - for (Pose2d pose2d : waypoints) { - waypointsMeters.add( - new Pose2d( - pose2d.getX(), - pose2d.getY(), - pose2d.getRotation() - ) - ); - } - /* Configures trajectory constraints */ - TrajectoryConfig config = new TrajectoryConfig(kMaxVelocity, kMaxAccel); - var baseTrajectory = edu.wpi.first.math.trajectory.TrajectoryGenerator.generateTrajectory( - waypointsMeters, - config - ); - /* If web application is not used, then the starting pose is transformed to the default starting pose, this has no impact on how the trajectory is run */ - if (!usingApp) { - baseTrajectory = - baseTrajectory.transformBy( - new Transform2d( - Constants.kDefaultZeroingPose.getTranslation(), - Constants.kDefaultZeroingPose.getRotation() - ) - ); - } - return baseTrajectory; - } - - /** - * Generates headings that can be transposed onto a trajectory with time calibration via a differential model - * - * @param usingApp - * @param waypoints - * @param swerveHeadings - * @return headings - */ - public static List generateHeadings( - boolean usingApp, - List waypoints, - List swerveHeadings - ) { - if (waypoints == null || swerveHeadings == null) { - return null; - } - - double startX = .5; - double startY = Constants.fieldCenterY; - - /* Trajectory is generated */ - Trajectory trajectory = generateTrajectory(usingApp, waypoints); - List waypointsMeters = new ArrayList<>(); - if (usingApp) { - startX = 0; - startY = 0; - } - /* Inch to meter conversions */ - for (Pose2d pose2d : waypoints) { - waypointsMeters.add( - new Pose2d( - pose2d.getX(), - pose2d.getY(), - pose2d.getRotation() - ) - ); - } - - /* Time constraint calibration */ - List waypointTimes = new ArrayList<>(); - List waypointIndexes = new ArrayList<>(); - int iWaypointCheckpoint = 0; - for (Pose2d waypointPose2d : waypointsMeters) { - for (int i = iWaypointCheckpoint; i < trajectory.getStates().size(); i++) { - var trajectoryPose2d = trajectory.getStates().get(i).poseMeters; - if (trajectoryPose2d.equals(waypointPose2d)) { - waypointTimes.add(trajectory.getStates().get(i).timeSeconds); - waypointIndexes.add(i); - break; - } - iWaypointCheckpoint++; - } - } - - /* List of headings generated with equivalent length to the trajectory poses */ - List generatedHeadings = new ArrayList<>(); - for ( - int nextCheckpoint = 1; - nextCheckpoint < waypointsMeters.size(); - nextCheckpoint++ - ) { - int iStart = waypointIndexes.get(nextCheckpoint - 1); - int iEnd = waypointIndexes.get(nextCheckpoint); - double totalDHeading = ( - swerveHeadings.get(nextCheckpoint).getDegrees() - - swerveHeadings.get(nextCheckpoint - 1).getDegrees() - ); - double timeBetweenWaypoints = - waypointTimes.get(nextCheckpoint) - waypointTimes.get(nextCheckpoint - 1); - double dHeading = totalDHeading / timeBetweenWaypoints; - - for (int i = iStart; i < iEnd; i++) { - generatedHeadings.add( - Rotation2d.fromDegrees( - swerveHeadings.get(nextCheckpoint - 1).getDegrees() + dHeading * ( - trajectory.getStates().get(i).timeSeconds - waypointTimes.get(nextCheckpoint - 1) - ) - ) - ); - } - } - - /* Adds final heading to be the same as the previous so the path is aligned with the heading */ - generatedHeadings.add(swerveHeadings.get(swerveHeadings.size() - 1)); - - return generatedHeadings; - } -} +package com.team1816.lib.auto.paths; + +import com.team1816.season.configuration.Constants; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import com.team1816.lib.motion.Trajectories; + +import java.util.ArrayList; +import java.util.List; + +import static com.team1816.lib.subsystems.drive.Drive.kPathFollowingMaxAccelMeters; +import static com.team1816.lib.subsystems.drive.Drive.kPathFollowingMaxVelMeters; + +public class PathUtil { + + /** + * Constraints + */ + private static final double kMaxVelocity = kPathFollowingMaxVelMeters; + private static final double kMaxAccel = kPathFollowingMaxAccelMeters; + + public static Trajectory generateTrajectory( + String pathName, + boolean usingApp, + List waypoints + ) { + return generateTrajectory(pathName, usingApp, waypoints, false); + } + + /** + * Generates a trajectory based on a list of waypoints based on WPIlib's TrajectoryGenerator + * + * @param usingApp + * @param waypoints + * @return trajectory + * @see com.team1816.lib.auto.modes.AutoMode + * @see AutoPath + * @see edu.wpi.first.math.trajectory.TrajectoryGenerator + */ + public static Trajectory generateTrajectory( + String pathName, + boolean usingApp, + List waypoints, + boolean noLoad + ) { + if (!noLoad) { + return Trajectories.loadTrajectory(pathName); + } + /* Inch to meter conversions for waypoints for trajectory calculations */ + List waypointsMeters = new ArrayList<>(); + for (Pose2d pose2d : waypoints) { + waypointsMeters.add( + new Pose2d( + pose2d.getX(), + pose2d.getY(), + pose2d.getRotation() + ) + ); + } + /* Configures trajectory constraints */ + TrajectoryConfig config = new TrajectoryConfig(kMaxVelocity, kMaxAccel); + var baseTrajectory = edu.wpi.first.math.trajectory.TrajectoryGenerator.generateTrajectory( + waypointsMeters, + config + ); + /* If web application is not used, then the starting pose is transformed to the default starting pose, this has no impact on how the trajectory is run */ + if (!usingApp) { + baseTrajectory = + baseTrajectory.transformBy( + new Transform2d( + Constants.kDefaultZeroingPose.getTranslation(), + Constants.kDefaultZeroingPose.getRotation() + ) + ); + } + return baseTrajectory; + } + + /** + * Generates headings that can be transposed onto a trajectory with time calibration via a differential model + * + * @param usingApp + * @param waypoints + * @param swerveHeadings + * @return headings + */ + public static List generateHeadings( + String name, + boolean usingApp, + List waypoints, + List swerveHeadings + ) { + if (waypoints == null || swerveHeadings == null) { + return null; + } + + double startX = .5; + double startY = Constants.fieldCenterY; + + /* Trajectory is generated */ + Trajectory trajectory = generateTrajectory(name, usingApp, waypoints); + List waypointsMeters = new ArrayList<>(); + if (usingApp) { + startX = 0; + startY = 0; + } + /* Inch to meter conversions */ + for (Pose2d pose2d : waypoints) { + waypointsMeters.add( + new Pose2d( + pose2d.getX(), + pose2d.getY(), + pose2d.getRotation() + ) + ); + } + + /* Time constraint calibration */ + List waypointTimes = new ArrayList<>(); + List waypointIndexes = new ArrayList<>(); + int iWaypointCheckpoint = 0; + for (Pose2d waypointPose2d : waypointsMeters) { + for (int i = iWaypointCheckpoint; i < trajectory.getStates().size(); i++) { + var trajectoryPose2d = trajectory.getStates().get(i).poseMeters; + if (trajectoryPose2d.equals(waypointPose2d)) { + waypointTimes.add(trajectory.getStates().get(i).timeSeconds); + waypointIndexes.add(i); + break; + } + iWaypointCheckpoint++; + } + } + + /* List of headings generated with equivalent length to the trajectory poses */ + List generatedHeadings = new ArrayList<>(); + for ( + int nextCheckpoint = 1; + nextCheckpoint < waypointsMeters.size(); + nextCheckpoint++ + ) { + int iStart = waypointIndexes.get(nextCheckpoint - 1); + int iEnd = waypointIndexes.get(nextCheckpoint); + double totalDHeading = ( + swerveHeadings.get(nextCheckpoint).getDegrees() - + swerveHeadings.get(nextCheckpoint - 1).getDegrees() + ); + double timeBetweenWaypoints = + waypointTimes.get(nextCheckpoint) - waypointTimes.get(nextCheckpoint - 1); + double dHeading = totalDHeading / timeBetweenWaypoints; + + for (int i = iStart; i < iEnd; i++) { + generatedHeadings.add( + Rotation2d.fromDegrees( + swerveHeadings.get(nextCheckpoint - 1).getDegrees() + dHeading * ( + trajectory.getStates().get(i).timeSeconds - waypointTimes.get(nextCheckpoint - 1) + ) + ) + ); + } + } + + /* Adds final heading to be the same as the previous so the path is aligned with the heading */ + generatedHeadings.add(swerveHeadings.get(swerveHeadings.size() - 1)); + + return generatedHeadings; + } +} diff --git a/src/main/java/com/team1816/lib/motion/Trajectories.java b/src/main/java/com/team1816/lib/motion/Trajectories.java new file mode 100644 index 00000000..b367d09c --- /dev/null +++ b/src/main/java/com/team1816/lib/motion/Trajectories.java @@ -0,0 +1,51 @@ +package com.team1816.lib.motion; +import com.team1816.season.auto.paths.*; +import com.team1816.lib.auto.paths.AutoPath; +import com.team1816.lib.auto.paths.*; +import com.fasterxml.jackson.core.type.TypeReference; +import com.fasterxml.jackson.databind.*; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.trajectory.Trajectory; +import java.io.*; +import java.util.List; + +public class Trajectories { + public static void calculate() { + calcAllTrajectories(new LivingRoomPath()); + calcAllTrajectories(new DriveStraightPath()); + }; + public static void calcAllTrajectories(AutoPath path) { + String name = path.getClass().getName(); + calcTrajectory(name, path.getWaypoints()); + calcTrajectory(name + "-reversed", path.getReflectedWaypoints()); + calcTrajectory(name + "-rotated", path.getRotatedWaypoints()); + }; + public static void calcTrajectory(String name, List waypoints) { + if (waypoints == null) { + return; + } + var trajectory = PathUtil.generateTrajectory(name, true, waypoints, true); + ObjectMapper mapper = new ObjectMapper(); + try { + mapper.writeValue( + new File(System.getProperty("user.dir") + "/src/resources/trajectories/" + name + ".json"), + trajectory.getStates() + ); + } catch (Exception e) { + System.out.println("Error while writing JSON: " + e.getMessage()); + } + } + public static Trajectory loadTrajectory(String name) { + ObjectMapper mapper = new ObjectMapper(); + try { + List list = mapper.readValue( + Trajectories.class.getResource("trajectories/" + name + ".json"), + new TypeReference>() { } + ); + return new Trajectory(list); + } catch (Exception e) { + System.out.println("Error parsing path JSON: " + name + "\n" + e.getMessage()); + return new Trajectory(); + } + } +} diff --git a/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java b/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java index 060e7734..f222be95 100644 --- a/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java +++ b/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java @@ -1,60 +1,60 @@ -package com.team1816.season.auto.paths; - -import com.team1816.lib.auto.paths.AutoPath; -import com.team1816.season.configuration.Constants; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; - -import java.util.List; - -import static com.team1816.lib.subsystems.drive.Drive.kPathFollowingMaxVelMeters; - -public class DriveStraightPath extends AutoPath { - - private final int driveDistance; - - public DriveStraightPath(int driveDistance, double maxVel) { - this.driveDistance = driveDistance; - } - - public DriveStraightPath(int driveDistance) { - this(driveDistance, kPathFollowingMaxVelMeters); - } - - public DriveStraightPath() { - this(12); - } - - @Override - public List getWaypoints() { - var waypoints = List.of( - new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0)), - new Pose2d((driveDistance), 0.0, Rotation2d.fromDegrees(0)) - ); - return waypoints; - } - - @Override - public List getWaypointHeadings() { - return null; - } - - @Override - protected List getReflectedWaypoints() { - var waypoints = List.of( - new Pose2d(Constants.fieldCenterX * 2 - 0.0, 0.0, Rotation2d.fromDegrees(180 - 0)), - new Pose2d(Constants.fieldCenterX * 2 - (driveDistance), 0.0, Rotation2d.fromDegrees(180 - 0)) - ); - return waypoints; - } - - @Override - protected List getReflectedWaypointHeadings() { - return null; - } - - @Override - public boolean usingApp() { - return false; - } -} +package com.team1816.season.auto.paths; + +import com.team1816.lib.auto.paths.AutoPath; +import com.team1816.season.configuration.Constants; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +import java.util.List; + +import static com.team1816.lib.subsystems.drive.Drive.kPathFollowingMaxVelMeters; + +public class DriveStraightPath extends AutoPath { + + private final int driveDistance; + + public DriveStraightPath(int driveDistance, double maxVel) { + this.driveDistance = driveDistance; + } + + public DriveStraightPath(int driveDistance) { + this(driveDistance, kPathFollowingMaxVelMeters); + } + + public DriveStraightPath() { + this(12); + } + + @Override + public List getWaypoints() { + var waypoints = List.of( + new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0)), + new Pose2d((driveDistance), 0.0, Rotation2d.fromDegrees(0)) + ); + return waypoints; + } + + @Override + public List getWaypointHeadings() { + return null; + } + + @Override + public List getReflectedWaypoints() { + var waypoints = List.of( + new Pose2d(Constants.fieldCenterX * 2 - 0.0, 0.0, Rotation2d.fromDegrees(180 - 0)), + new Pose2d(Constants.fieldCenterX * 2 - (driveDistance), 0.0, Rotation2d.fromDegrees(180 - 0)) + ); + return waypoints; + } + + @Override + protected List getReflectedWaypointHeadings() { + return null; + } + + @Override + public boolean usingApp() { + return false; + } +} diff --git a/src/main/resources/trajectories/README.md b/src/main/resources/trajectories/README.md new file mode 100644 index 00000000..b78dedda --- /dev/null +++ b/src/main/resources/trajectories/README.md @@ -0,0 +1,5 @@ +This is a directory anchor for the src/main/resources/trajectories directory. + +Said directory holds the JSON stringified states of auto trajectories that are loaded on startup. + +Populate this directory with ``gradlew calcTrajectories``. From 9b502650ecf2912616c4251cb7c9decdc4a0d637 Mon Sep 17 00:00:00 2001 From: miapirobotics <34904974+miapirobotics@users.noreply.github.com> Date: Sat, 4 Feb 2023 12:24:58 -0600 Subject: [PATCH 02/31] Squash all fixes into one --- build.gradle | 27 +++++++++++++------ .../com/team1816/lib/auto/paths/AutoPath.java | 1 + .../com/team1816/lib/auto/paths/PathUtil.java | 1 + .../com/team1816/lib/motion/Trajectories.java | 19 ++++++++----- 4 files changed, 33 insertions(+), 15 deletions(-) diff --git a/build.gradle b/build.gradle index db7a86dd..106986da 100644 --- a/build.gradle +++ b/build.gradle @@ -98,7 +98,6 @@ jar { from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE - } jsonSchema2Pojo { @@ -184,14 +183,21 @@ task runZodiac(type: JavaExec) { jvmArgs "-Djava.library.path=${jniDir}" } apply plugin: 'java'; -task calculateTrajectories() { - dependsOn compileJava - doLast { - URL[] urls = sourceSets.main.runtimeClasspath.files.collect { it.toURI().toURL() } as URL[] - def classloader = new java.net.URLClassLoader(urls) - Class type = classloader.loadClass("com.team1816.lib.motion.Trajectories"); - type.calculate() +task calculateTrajectories(type: JavaExec) { + String jniDir = "${project.buildDir}/jni/release" + environment "PATH", jniDir + String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath + if (!DefaultNativePlatform.currentOperatingSystem.isWindows()) { + environment "LD_LIBRARY_PATH", jniDir + environment "DYLD_FALLBACK_LIBRARY_PATH", jniDir + environment "DYLD_LIBRARY_PATH", jniDir + jvmArgs "-XstartOnFirstThread" } + environment "HALSIM_EXTENSIONS", hal + environment "ROBOT_NAME", "alpha" + classpath = files(jar) + jvmArgs "-Djava.library.path=${jniDir}" + main = "com.team1816.lib.motion.Trajectories" } processResources { @@ -212,3 +218,8 @@ wpi.java.configureTestTasks(test) compileJava { //dependsOn tasks.named("spotlessApply") } + +task fullDeploy() { + dependsOn tasks.named("calculateTrajectories") + dependsOn 'deploy' +} \ No newline at end of file diff --git a/src/main/java/com/team1816/lib/auto/paths/AutoPath.java b/src/main/java/com/team1816/lib/auto/paths/AutoPath.java index e2ae6928..2f440609 100644 --- a/src/main/java/com/team1816/lib/auto/paths/AutoPath.java +++ b/src/main/java/com/team1816/lib/auto/paths/AutoPath.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.Trajectory; +import com.team1816.lib.motion.Trajectories; import java.util.ArrayList; import java.util.List; diff --git a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java index aab68b2e..499606ba 100644 --- a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java +++ b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java @@ -46,6 +46,7 @@ public static Trajectory generateTrajectory( List waypoints, boolean noLoad ) { + pathName = Trajectories.formatClassName(pathName); if (!noLoad) { return Trajectories.loadTrajectory(pathName); } diff --git a/src/main/java/com/team1816/lib/motion/Trajectories.java b/src/main/java/com/team1816/lib/motion/Trajectories.java index b367d09c..ba6f0baa 100644 --- a/src/main/java/com/team1816/lib/motion/Trajectories.java +++ b/src/main/java/com/team1816/lib/motion/Trajectories.java @@ -10,16 +10,20 @@ import java.util.List; public class Trajectories { - public static void calculate() { + public static void main(String[] args) { calcAllTrajectories(new LivingRoomPath()); calcAllTrajectories(new DriveStraightPath()); }; public static void calcAllTrajectories(AutoPath path) { - String name = path.getClass().getName(); + String name = formatClassName(path.getClass().getName()); calcTrajectory(name, path.getWaypoints()); calcTrajectory(name + "-reversed", path.getReflectedWaypoints()); calcTrajectory(name + "-rotated", path.getRotatedWaypoints()); }; + public static String formatClassName(String name) { + String[] parts = name.split("\\."); + return parts[parts.length - 1]; + } public static void calcTrajectory(String name, List waypoints) { if (waypoints == null) { return; @@ -27,10 +31,11 @@ public static void calcTrajectory(String name, List waypoints) { var trajectory = PathUtil.generateTrajectory(name, true, waypoints, true); ObjectMapper mapper = new ObjectMapper(); try { - mapper.writeValue( - new File(System.getProperty("user.dir") + "/src/resources/trajectories/" + name + ".json"), - trajectory.getStates() - ); + File file = new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"); + if (!file.exists()) { + file.createNewFile(); + } + mapper.writeValue(file, trajectory.getStates()); } catch (Exception e) { System.out.println("Error while writing JSON: " + e.getMessage()); } @@ -39,7 +44,7 @@ public static Trajectory loadTrajectory(String name) { ObjectMapper mapper = new ObjectMapper(); try { List list = mapper.readValue( - Trajectories.class.getResource("trajectories/" + name + ".json"), + new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"), new TypeReference>() { } ); return new Trajectory(list); From 8b460651a5eb10fc2e555bb64f8a494dca0d17f3 Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Sat, 4 Feb 2023 17:40:38 -0600 Subject: [PATCH 03/31] documenting TrajectoryCalculator --- build.gradle | 37 +++-- .../com/team1816/lib/auto/paths/AutoPath.java | 1 - .../com/team1816/lib/auto/paths/PathUtil.java | 6 +- .../com/team1816/lib/motion/Trajectories.java | 56 -------- .../lib/motion/TrajectoryCalculator.java | 127 ++++++++++++++++++ src/main/resources/trajectories/README.md | 5 - 6 files changed, 154 insertions(+), 78 deletions(-) delete mode 100644 src/main/java/com/team1816/lib/motion/Trajectories.java create mode 100644 src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java delete mode 100644 src/main/resources/trajectories/README.md diff --git a/build.gradle b/build.gradle index 106986da..24ca5e3e 100644 --- a/build.gradle +++ b/build.gradle @@ -81,6 +81,7 @@ dependencies { implementation 'com.google.inject:guice:5.1.0' implementation 'com.google.inject.extensions:guice-assistedinject:5.1.0' implementation 'org.apache.commons:commons-math3:3.6.1' + implementation group: 'commons-io', name: 'commons-io', version: '2.7' testImplementation 'junit:junit:4.13.2' testImplementation 'org.mockito:mockito-inline:4.7.0' @@ -100,6 +101,7 @@ jar { duplicatesStrategy = DuplicatesStrategy.INCLUDE } +// Converts JSON Schema to Classes jsonSchema2Pojo { targetPackage = 'com.team1816.lib' useTitleAsClassname = true @@ -114,7 +116,9 @@ jsonSchema2Pojo { ) } -task runAlpha(type: JavaExec) { +// Calculates Trajectories +apply plugin: 'java'; +task calculateTrajectories(type: JavaExec) { String jniDir = "${project.buildDir}/jni/release" environment "PATH", jniDir String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath @@ -126,12 +130,14 @@ task runAlpha(type: JavaExec) { } environment "HALSIM_EXTENSIONS", hal environment "ROBOT_NAME", "alpha" - dependsOn(jar, simulateExternalJavaRelease) classpath = files(jar) jvmArgs "-Djava.library.path=${jniDir}" + main = "com.team1816.lib.motion.TrajectoryCalculator" } -task runZero(type: JavaExec) { +// Simulates robot with characterization alpha.config.yml +task runAlpha(type: JavaExec) { + calculateTrajectories String jniDir = "${project.buildDir}/jni/release" environment "PATH", jniDir String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath @@ -142,13 +148,15 @@ task runZero(type: JavaExec) { jvmArgs "-XstartOnFirstThread" } environment "HALSIM_EXTENSIONS", hal - environment "ROBOT_NAME", "zero" + environment "ROBOT_NAME", "alpha" dependsOn(jar, simulateExternalJavaRelease) classpath = files(jar) jvmArgs "-Djava.library.path=${jniDir}" } -task runZoffseason(type: JavaExec) { +// Simulates robot with characterization zero.config.yml +task runZero(type: JavaExec) { + calculateTrajectories String jniDir = "${project.buildDir}/jni/release" environment "PATH", jniDir String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath @@ -159,14 +167,15 @@ task runZoffseason(type: JavaExec) { jvmArgs "-XstartOnFirstThread" } environment "HALSIM_EXTENSIONS", hal - environment "ROBOT_NAME", "zoffseason" + environment "ROBOT_NAME", "zero" dependsOn(jar, simulateExternalJavaRelease) classpath = files(jar) jvmArgs "-Djava.library.path=${jniDir}" } - -task runZodiac(type: JavaExec) { +// Simulates robot with characterization zoffseason.config.yml +task runZoffseason(type: JavaExec) { + calculateTrajectories String jniDir = "${project.buildDir}/jni/release" environment "PATH", jniDir String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath @@ -177,13 +186,15 @@ task runZodiac(type: JavaExec) { jvmArgs "-XstartOnFirstThread" } environment "HALSIM_EXTENSIONS", hal - environment "ROBOT_NAME", "zodiac_pro" + environment "ROBOT_NAME", "zoffseason" dependsOn(jar, simulateExternalJavaRelease) classpath = files(jar) jvmArgs "-Djava.library.path=${jniDir}" } -apply plugin: 'java'; -task calculateTrajectories(type: JavaExec) { + +// Simulates robot with characterization zodiac_pro.config.yml +task runZodiac(type: JavaExec) { + calculateTrajectories String jniDir = "${project.buildDir}/jni/release" environment "PATH", jniDir String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath @@ -194,10 +205,10 @@ task calculateTrajectories(type: JavaExec) { jvmArgs "-XstartOnFirstThread" } environment "HALSIM_EXTENSIONS", hal - environment "ROBOT_NAME", "alpha" + environment "ROBOT_NAME", "zodiac_pro" + dependsOn(jar, simulateExternalJavaRelease) classpath = files(jar) jvmArgs "-Djava.library.path=${jniDir}" - main = "com.team1816.lib.motion.Trajectories" } processResources { diff --git a/src/main/java/com/team1816/lib/auto/paths/AutoPath.java b/src/main/java/com/team1816/lib/auto/paths/AutoPath.java index 2f440609..e2ae6928 100644 --- a/src/main/java/com/team1816/lib/auto/paths/AutoPath.java +++ b/src/main/java/com/team1816/lib/auto/paths/AutoPath.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.Trajectory; -import com.team1816.lib.motion.Trajectories; import java.util.ArrayList; import java.util.List; diff --git a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java index 499606ba..08871fa3 100644 --- a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java +++ b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java @@ -6,7 +6,7 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; -import com.team1816.lib.motion.Trajectories; +import com.team1816.lib.motion.TrajectoryCalculator; import java.util.ArrayList; import java.util.List; @@ -46,9 +46,9 @@ public static Trajectory generateTrajectory( List waypoints, boolean noLoad ) { - pathName = Trajectories.formatClassName(pathName); + pathName = TrajectoryCalculator.formatClassName(pathName); if (!noLoad) { - return Trajectories.loadTrajectory(pathName); + return TrajectoryCalculator.loadTrajectory(pathName); } /* Inch to meter conversions for waypoints for trajectory calculations */ List waypointsMeters = new ArrayList<>(); diff --git a/src/main/java/com/team1816/lib/motion/Trajectories.java b/src/main/java/com/team1816/lib/motion/Trajectories.java deleted file mode 100644 index ba6f0baa..00000000 --- a/src/main/java/com/team1816/lib/motion/Trajectories.java +++ /dev/null @@ -1,56 +0,0 @@ -package com.team1816.lib.motion; -import com.team1816.season.auto.paths.*; -import com.team1816.lib.auto.paths.AutoPath; -import com.team1816.lib.auto.paths.*; -import com.fasterxml.jackson.core.type.TypeReference; -import com.fasterxml.jackson.databind.*; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.trajectory.Trajectory; -import java.io.*; -import java.util.List; - -public class Trajectories { - public static void main(String[] args) { - calcAllTrajectories(new LivingRoomPath()); - calcAllTrajectories(new DriveStraightPath()); - }; - public static void calcAllTrajectories(AutoPath path) { - String name = formatClassName(path.getClass().getName()); - calcTrajectory(name, path.getWaypoints()); - calcTrajectory(name + "-reversed", path.getReflectedWaypoints()); - calcTrajectory(name + "-rotated", path.getRotatedWaypoints()); - }; - public static String formatClassName(String name) { - String[] parts = name.split("\\."); - return parts[parts.length - 1]; - } - public static void calcTrajectory(String name, List waypoints) { - if (waypoints == null) { - return; - } - var trajectory = PathUtil.generateTrajectory(name, true, waypoints, true); - ObjectMapper mapper = new ObjectMapper(); - try { - File file = new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"); - if (!file.exists()) { - file.createNewFile(); - } - mapper.writeValue(file, trajectory.getStates()); - } catch (Exception e) { - System.out.println("Error while writing JSON: " + e.getMessage()); - } - } - public static Trajectory loadTrajectory(String name) { - ObjectMapper mapper = new ObjectMapper(); - try { - List list = mapper.readValue( - new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"), - new TypeReference>() { } - ); - return new Trajectory(list); - } catch (Exception e) { - System.out.println("Error parsing path JSON: " + name + "\n" + e.getMessage()); - return new Trajectory(); - } - } -} diff --git a/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java b/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java new file mode 100644 index 00000000..884da3fa --- /dev/null +++ b/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java @@ -0,0 +1,127 @@ +package com.team1816.lib.motion; +import com.team1816.season.auto.paths.*; +import com.team1816.lib.auto.paths.AutoPath; +import com.team1816.lib.auto.paths.*; +import com.fasterxml.jackson.core.type.TypeReference; +import com.fasterxml.jackson.databind.*; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.trajectory.Trajectory; +import org.apache.commons.io.FileUtils; + +import java.io.*; +import java.util.List; + +/** + * Class that efficiently pre-calculates trajectories to limit resource utilization. + * Generated Trajectories are located in src/resources/trajectories + * + * @see Trajectory + */ +public class TrajectoryCalculator { + + /** + * Root directory that trajectories are placed in + */ + private static final File directory = new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/"); + + /** + * List of all AutoPaths that are to be calculated + * @see AutoPath + */ + private static final List paths = List.of( + new DriveStraightPath(), + new LivingRoomPath() + ); + + /** + * Entry point of gradle task that calculates trajectories + * @param args + */ + public static void main(String[] args) { + deleteTrajectories(); + System.out.println("Calculating " + paths.size() + " Trajectories!"); + for (AutoPath path: paths) { + calcAllTrajectoryFormats(path); + } + System.out.println("Calculated " + paths.size() + " Trajectories!"); + } + + /** + * Calculates all formats of the trajectories associated with an AutoPath {standard, reflected, rotated} + * @param path AutoPath + */ + public static void calcAllTrajectoryFormats(AutoPath path) { + if (!directory.exists()) { + directory.mkdir(); + } + String name = formatClassName(path.getClass().getName()); + calcTrajectory(name, path.getWaypoints()); + calcTrajectory(name + "_Reflected", path.getReflectedWaypoints()); + calcTrajectory(name + "_Rotated", path.getRotatedWaypoints()); + } + + /** + * Formats the class name in regex by matching characters + * @param name class name + * @return name + */ + public static String formatClassName(String name) { + String[] parts = name.split("\\."); + return parts[parts.length - 1]; + } + + /** + * Calculates the Trajectory using the waypoints provided in the AutoPath and PathUtil + * @param name path name + * @param waypoints path waypoints + * @see AutoPath + * @see PathUtil + */ + public static void calcTrajectory(String name, List waypoints) { + if (waypoints == null) { + return; + } + var trajectory = PathUtil.generateTrajectory(name, true, waypoints, true); + ObjectMapper mapper = new ObjectMapper(); + try { + File file = new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"); + if (!file.exists()) { + file.createNewFile(); + } + mapper.writeValue(file, trajectory.getStates()); + } catch (Exception e) { + System.out.println("Error while writing JSON: " + e.getMessage()); + } + } + + /** + * Loads the Trajectory states into the associated JSON storing it + * @param name path name + * @return Trajectory + * @see Trajectory + */ + public static Trajectory loadTrajectory(String name) { + ObjectMapper mapper = new ObjectMapper(); + try { + List list = mapper.readValue( + new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"), + new TypeReference>() { } + ); + return new Trajectory(list); + } catch (Exception e) { + System.out.println("Error parsing path JSON: " + name + "\n" + e.getMessage()); + return new Trajectory(); + } + } + + /** + * Clears the Trajectories located in the resources directory + */ + public static void deleteTrajectories() { + try { + FileUtils.cleanDirectory(new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/")); + } catch (Exception e) { + System.out.println("Error deleting contents of trajectories directory" + e.getStackTrace()); + } + } +} diff --git a/src/main/resources/trajectories/README.md b/src/main/resources/trajectories/README.md deleted file mode 100644 index b78dedda..00000000 --- a/src/main/resources/trajectories/README.md +++ /dev/null @@ -1,5 +0,0 @@ -This is a directory anchor for the src/main/resources/trajectories directory. - -Said directory holds the JSON stringified states of auto trajectories that are loaded on startup. - -Populate this directory with ``gradlew calcTrajectories``. From 3bcc85b4795be3513235add3a5b3aa023634b0ae Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Sat, 4 Feb 2023 17:44:40 -0600 Subject: [PATCH 04/31] retying calculation to simulation tasks --- build.gradle | 12 ++++++++---- .../team1816/lib/motion/TrajectoryCalculator.java | 4 ++-- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/build.gradle b/build.gradle index 24ca5e3e..be1a25a8 100644 --- a/build.gradle +++ b/build.gradle @@ -137,7 +137,8 @@ task calculateTrajectories(type: JavaExec) { // Simulates robot with characterization alpha.config.yml task runAlpha(type: JavaExec) { - calculateTrajectories + dependsOn tasks.named("calculateTrajectories") + String jniDir = "${project.buildDir}/jni/release" environment "PATH", jniDir String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath @@ -156,7 +157,8 @@ task runAlpha(type: JavaExec) { // Simulates robot with characterization zero.config.yml task runZero(type: JavaExec) { - calculateTrajectories + dependsOn tasks.named("calculateTrajectories") + String jniDir = "${project.buildDir}/jni/release" environment "PATH", jniDir String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath @@ -175,7 +177,8 @@ task runZero(type: JavaExec) { // Simulates robot with characterization zoffseason.config.yml task runZoffseason(type: JavaExec) { - calculateTrajectories + dependsOn tasks.named("calculateTrajectories") + String jniDir = "${project.buildDir}/jni/release" environment "PATH", jniDir String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath @@ -194,7 +197,8 @@ task runZoffseason(type: JavaExec) { // Simulates robot with characterization zodiac_pro.config.yml task runZodiac(type: JavaExec) { - calculateTrajectories + dependsOn tasks.named("calculateTrajectories") + String jniDir = "${project.buildDir}/jni/release" environment "PATH", jniDir String hal = fileTree(dir: "${project.buildDir}/jni/release", include: '*halsim_*.*').asPath diff --git a/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java b/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java index 884da3fa..cdb4e632 100644 --- a/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java +++ b/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java @@ -39,11 +39,11 @@ public class TrajectoryCalculator { */ public static void main(String[] args) { deleteTrajectories(); - System.out.println("Calculating " + paths.size() + " Trajectories!"); + System.out.println("Calculating " + paths.size() + " Trajectories:"); for (AutoPath path: paths) { calcAllTrajectoryFormats(path); + System.out.println("\tCalculated " + path.getClass().getName()); } - System.out.println("Calculated " + paths.size() + " Trajectories!"); } /** From 6b2bd9a4655ad7146f87a10940b517797d36d02e Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Sat, 4 Feb 2023 18:25:19 -0600 Subject: [PATCH 05/31] added headings --- .../com/team1816/example/ExampleAutoPath.java | 4 +-- .../com/team1816/lib/auto/paths/AutoPath.java | 6 ++-- .../com/team1816/lib/auto/paths/PathUtil.java | 5 +-- .../lib/motion/TrajectoryCalculator.java | 32 +++++++++++++++++++ .../season/auto/paths/DriveStraightPath.java | 14 -------- 5 files changed, 40 insertions(+), 21 deletions(-) diff --git a/src/main/java/com/team1816/example/ExampleAutoPath.java b/src/main/java/com/team1816/example/ExampleAutoPath.java index 99203415..0452bd68 100644 --- a/src/main/java/com/team1816/example/ExampleAutoPath.java +++ b/src/main/java/com/team1816/example/ExampleAutoPath.java @@ -39,7 +39,7 @@ public List getWaypoints() { * in chezy path that includes this, you just have to eyeball where the robot should face using the simulator. */ @Override - protected List getWaypointHeadings() { + public List getWaypointHeadings() { // this will make a SwerveDrive robot face the opponent alliance wall throughout the entire path return List.of( Rotation2d.fromDegrees(0), @@ -56,7 +56,7 @@ public List getReflectedWaypoints() { } @Override - protected List getReflectedWaypointHeadings() { + public List getReflectedWaypointHeadings() { return null; } diff --git a/src/main/java/com/team1816/lib/auto/paths/AutoPath.java b/src/main/java/com/team1816/lib/auto/paths/AutoPath.java index e2ae6928..c4e2cb30 100644 --- a/src/main/java/com/team1816/lib/auto/paths/AutoPath.java +++ b/src/main/java/com/team1816/lib/auto/paths/AutoPath.java @@ -114,14 +114,14 @@ public List getRotatedWaypoints() { * * @return waypointHeadings */ - protected abstract List getWaypointHeadings(); + public abstract List getWaypointHeadings(); /** * Returns a list of Rotation2d's corresponding to the rotation respect to a reflected trajectory * * @return waypointHeadings */ - protected List getReflectedWaypointHeadings() { + public List getReflectedWaypointHeadings() { List waypointHeadings = getWaypointHeadings(); List reflectedWaypointHeadings = new ArrayList<>(); for (int i = 0; i < waypointHeadings.size(); i++) { @@ -136,7 +136,7 @@ protected List getReflectedWaypointHeadings() { * * @return waypointHeadings */ - protected List getRotatedWaypointHeadings() { + public List getRotatedWaypointHeadings() { List waypointHeadings = getWaypointHeadings(); List rotatedWaypointHeadings = new ArrayList<>(); for (int i = 0; i < waypointHeadings.size(); i++) { diff --git a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java index 08871fa3..61f369ed 100644 --- a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java +++ b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java @@ -35,6 +35,7 @@ public static Trajectory generateTrajectory( * * @param usingApp * @param waypoints + * @param loadTrajectories * @return trajectory * @see com.team1816.lib.auto.modes.AutoMode * @see AutoPath @@ -44,10 +45,10 @@ public static Trajectory generateTrajectory( String pathName, boolean usingApp, List waypoints, - boolean noLoad + boolean loadTrajectories ) { pathName = TrajectoryCalculator.formatClassName(pathName); - if (!noLoad) { + if (loadTrajectories) { return TrajectoryCalculator.loadTrajectory(pathName); } /* Inch to meter conversions for waypoints for trajectory calculations */ diff --git a/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java b/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java index cdb4e632..d779f516 100644 --- a/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java +++ b/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java @@ -5,6 +5,7 @@ import com.fasterxml.jackson.core.type.TypeReference; import com.fasterxml.jackson.databind.*; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.Trajectory; import org.apache.commons.io.FileUtils; @@ -55,9 +56,16 @@ public static void calcAllTrajectoryFormats(AutoPath path) { directory.mkdir(); } String name = formatClassName(path.getClass().getName()); + calcTrajectory(name, path.getWaypoints()); calcTrajectory(name + "_Reflected", path.getReflectedWaypoints()); calcTrajectory(name + "_Rotated", path.getRotatedWaypoints()); + + if (path.getWaypointHeadings() != null) { + calcHeadings(name + "Headings", path.getWaypoints(), path.getWaypointHeadings()); + calcHeadings(name + "Headings_Reflected", path.getReflectedWaypoints(), path.getReflectedWaypointHeadings()); + calcHeadings(name + "Headings_Rotated", path.getRotatedWaypoints(), path.getRotatedWaypointHeadings()); + } } /** @@ -94,6 +102,30 @@ public static void calcTrajectory(String name, List waypoints) { } } + /** + * Calculates the Headings associated with the AutoPath + * @param name path name + * @param waypoints path waypoints + * @param headings path headings + */ + public static void calcHeadings(String name, List waypoints, List headings) { + if (headings == null) { + return; + } + + var trajectoryHeadings = PathUtil.generateHeadings(name, true, waypoints, headings); + ObjectMapper mapper = new ObjectMapper(); + try { + File file = new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"); + if (!file.exists()) { + file.createNewFile(); + } + mapper.writeValue(file, trajectoryHeadings); + } catch (Exception e) { + System.out.println("Error while writing JSON: " + e.getMessage()); + } + } + /** * Loads the Trajectory states into the associated JSON storing it * @param name path name diff --git a/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java b/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java index f222be95..fe217ce9 100644 --- a/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java +++ b/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java @@ -39,20 +39,6 @@ public List getWaypointHeadings() { return null; } - @Override - public List getReflectedWaypoints() { - var waypoints = List.of( - new Pose2d(Constants.fieldCenterX * 2 - 0.0, 0.0, Rotation2d.fromDegrees(180 - 0)), - new Pose2d(Constants.fieldCenterX * 2 - (driveDistance), 0.0, Rotation2d.fromDegrees(180 - 0)) - ); - return waypoints; - } - - @Override - protected List getReflectedWaypointHeadings() { - return null; - } - @Override public boolean usingApp() { return false; From 1166a75de801d32cadb25c75c6b98e539b9366de Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Sat, 4 Feb 2023 18:26:18 -0600 Subject: [PATCH 06/31] added heading functionality to calculator --- src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java b/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java index d779f516..a902df9f 100644 --- a/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java +++ b/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java @@ -103,7 +103,7 @@ public static void calcTrajectory(String name, List waypoints) { } /** - * Calculates the Headings associated with the AutoPath + * Calculates the headings associated with the AutoPath * @param name path name * @param waypoints path waypoints * @param headings path headings From 9752854418b444f7fe7637571dec7021211f791c Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Sat, 4 Feb 2023 18:27:07 -0600 Subject: [PATCH 07/31] update documentation --- src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java b/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java index a902df9f..860f0ea2 100644 --- a/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java +++ b/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java @@ -147,7 +147,7 @@ public static Trajectory loadTrajectory(String name) { } /** - * Clears the Trajectories located in the resources directory + * Clears the json files storing trajectories and headings located in the resources directory */ public static void deleteTrajectories() { try { From 88899bfe3f946745d0629cae2885dac39e29c520 Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Sat, 4 Feb 2023 18:52:24 -0600 Subject: [PATCH 08/31] introduce parser and moved TrajectoryCalculator to trajectoryUtils --- build.gradle | 2 +- .../com/team1816/lib/auto/paths/PathUtil.java | 2 +- .../trajectoryUtil}/TrajectoryCalculator.java | 4 +- .../util/trajectoryUtil/TrajectoryParser.java | 109 ++++++++++++++++++ 4 files changed, 113 insertions(+), 4 deletions(-) rename src/main/java/com/team1816/lib/{motion => util/trajectoryUtil}/TrajectoryCalculator.java (97%) create mode 100644 src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryParser.java diff --git a/build.gradle b/build.gradle index be1a25a8..a3923fa1 100644 --- a/build.gradle +++ b/build.gradle @@ -132,7 +132,7 @@ task calculateTrajectories(type: JavaExec) { environment "ROBOT_NAME", "alpha" classpath = files(jar) jvmArgs "-Djava.library.path=${jniDir}" - main = "com.team1816.lib.motion.TrajectoryCalculator" + main = "com.team1816.lib.util.trajectoryUtil.TrajectoryCalculator" } // Simulates robot with characterization alpha.config.yml diff --git a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java index 61f369ed..4150ecbf 100644 --- a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java +++ b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java @@ -6,7 +6,7 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; -import com.team1816.lib.motion.TrajectoryCalculator; +import com.team1816.lib.util.trajectoryUtil.TrajectoryCalculator; import java.util.ArrayList; import java.util.List; diff --git a/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java similarity index 97% rename from src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java rename to src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java index 860f0ea2..6036c811 100644 --- a/src/main/java/com/team1816/lib/motion/TrajectoryCalculator.java +++ b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java @@ -1,4 +1,4 @@ -package com.team1816.lib.motion; +package com.team1816.lib.util.trajectoryUtil; import com.team1816.season.auto.paths.*; import com.team1816.lib.auto.paths.AutoPath; import com.team1816.lib.auto.paths.*; @@ -13,7 +13,7 @@ import java.util.List; /** - * Class that efficiently pre-calculates trajectories to limit resource utilization. + * Utility that efficiently pre-calculates trajectories to limit resource utilization. * Generated Trajectories are located in src/resources/trajectories * * @see Trajectory diff --git a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryParser.java b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryParser.java new file mode 100644 index 00000000..d4d98990 --- /dev/null +++ b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryParser.java @@ -0,0 +1,109 @@ +package com.team1816.lib.util.trajectoryUtil; + +import com.fasterxml.jackson.core.type.TypeReference; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.Trajectory; + +import java.io.File; +import java.util.ArrayList; +import java.util.List; + +/** + * Utility that parses trajectories calculated with the TrajectoryCalculator + * @see TrajectoryCalculator + */ +public class TrajectoryParser { + + /** + * Root directory that trajectories are placed in + */ + private static final File directory = new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/"); + + /** + * Static object mapper for parsing + */ + private static final ObjectMapper mapper = new ObjectMapper(); + + /** + * Parses trajectory + * + * @param name auto path name + * @return trajectory + */ + public static Trajectory parseTrajectory(String name) { + File file = new File(directory.getPath() + name + ".json"); + try { + List trajectoryStates = mapper.readValue( + file, new TypeReference>() {} + ); + var trajectory = new Trajectory(trajectoryStates); + return trajectory; + } catch (Exception e) { + System.out.println("Error parsing trajectory: " + e.getStackTrace()); + } + return new Trajectory(); + } + + /** + * Parses reflected trajectory + * + * @param name auto path name + * @return trajectory + */ + public static Trajectory parseReflectedTrajectory(String name) { + return parseTrajectory(name+"_Reflected"); + } + + /** + * Parses rotated trajectory + * + * @param name auto path name + * @return trajectory + */ + public static Trajectory parseRotatedTrajectory(String name) { + return parseTrajectory(name+"_Rotated"); + } + + /** + * Parses trajectory headings + * NOTE: need to append "Headings" to {name} + * + * @param name auto path name + * @return trajectory headings + */ + public static List parseTrajectoryHeadings(String name) { + File file = new File(directory.getPath() + name + ".json"); + try { + List trajectoryHeadings = mapper.readValue( + file, new TypeReference>() {} + ); + return trajectoryHeadings; + } catch (Exception e) { + System.out.println("Error parsing trajectory: " + e.getStackTrace()); + } + return new ArrayList<>(); + } + + /** + * Parses reflected trajectory headings + * NOTE: need to append "Headings" to {name} + * + * @param name auto path name + * @return trajectory headings + */ + public static List parseReflectedTrajectoryHeadings(String name) { + return parseTrajectoryHeadings(name+"_Reflected"); + } + + /** + * Parses rotated trajectory headings + * NOTE: need to append "Headings" to {name} + * + * @param name auto path name + * @return trajectory headings + */ + public static List parseRotatedTrajectoryHeadings(String name) { + return parseTrajectoryHeadings(name+"_Rotated"); + } +} From fb21843bcd73fb752ceba5b8da8e8f6936b25078 Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Sat, 4 Feb 2023 19:02:50 -0600 Subject: [PATCH 09/31] parser doesn't work --- .../team1816/lib/util/trajectoryUtil/TrajectoryParser.java | 4 +--- .../com/team1816/season/auto/modes/DriveStraightMode.java | 4 ++-- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryParser.java b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryParser.java index d4d98990..3d95d369 100644 --- a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryParser.java +++ b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryParser.java @@ -34,9 +34,7 @@ public class TrajectoryParser { public static Trajectory parseTrajectory(String name) { File file = new File(directory.getPath() + name + ".json"); try { - List trajectoryStates = mapper.readValue( - file, new TypeReference>() {} - ); + List trajectoryStates = mapper.readValue(file, new TypeReference>(){}); var trajectory = new Trajectory(trajectoryStates); return trajectory; } catch (Exception e) { diff --git a/src/main/java/com/team1816/season/auto/modes/DriveStraightMode.java b/src/main/java/com/team1816/season/auto/modes/DriveStraightMode.java index e36776c2..8d010b1b 100644 --- a/src/main/java/com/team1816/season/auto/modes/DriveStraightMode.java +++ b/src/main/java/com/team1816/season/auto/modes/DriveStraightMode.java @@ -3,12 +3,12 @@ import com.team1816.lib.auto.AutoModeEndedException; import com.team1816.lib.auto.actions.WaitAction; import com.team1816.lib.auto.modes.AutoMode; +import com.team1816.lib.util.trajectoryUtil.TrajectoryParser; import com.team1816.season.auto.actions.DriveOpenLoopAction; public class DriveStraightMode extends AutoMode { - public DriveStraightMode() { - } + public DriveStraightMode() {} @Override protected void routine() throws AutoModeEndedException { From 85282d64d95e7a463fb9cf5628d903b44fc515a3 Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Sat, 4 Feb 2023 19:02:56 -0600 Subject: [PATCH 10/31] parser doesn't work --- .../java/com/team1816/season/auto/modes/DriveStraightMode.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/com/team1816/season/auto/modes/DriveStraightMode.java b/src/main/java/com/team1816/season/auto/modes/DriveStraightMode.java index 8d010b1b..64bf6ade 100644 --- a/src/main/java/com/team1816/season/auto/modes/DriveStraightMode.java +++ b/src/main/java/com/team1816/season/auto/modes/DriveStraightMode.java @@ -3,7 +3,6 @@ import com.team1816.lib.auto.AutoModeEndedException; import com.team1816.lib.auto.actions.WaitAction; import com.team1816.lib.auto.modes.AutoMode; -import com.team1816.lib.util.trajectoryUtil.TrajectoryParser; import com.team1816.season.auto.actions.DriveOpenLoopAction; public class DriveStraightMode extends AutoMode { From a320b6e90472b4437c539244c040110db8cf3363 Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Sun, 5 Feb 2023 10:49:22 -0600 Subject: [PATCH 11/31] trajectories calculated but velocity capped at 1.0??? --- .../com/team1816/example/ExampleAutoPath.java | 4 +- .../com/team1816/lib/auto/paths/AutoPath.java | 52 ++++++++------ .../com/team1816/lib/auto/paths/PathUtil.java | 71 ++++++------------- .../trajectoryUtil/TrajectoryCalculator.java | 24 ++++++- .../auto/actions/TrajectoryToTargetPath.java | 12 +--- .../season/auto/paths/DriveStraightPath.java | 14 ++-- .../season/auto/paths/LivingRoomPath.java | 2 +- .../season/configuration/Constants.java | 2 +- 8 files changed, 91 insertions(+), 90 deletions(-) diff --git a/src/main/java/com/team1816/example/ExampleAutoPath.java b/src/main/java/com/team1816/example/ExampleAutoPath.java index 0452bd68..e2fd88d5 100644 --- a/src/main/java/com/team1816/example/ExampleAutoPath.java +++ b/src/main/java/com/team1816/example/ExampleAutoPath.java @@ -66,7 +66,7 @@ public List getReflectedWaypointHeadings() { * @return just make this return true - we're always using the app */ @Override - protected boolean usingApp() { + protected boolean isPrecalculated() { return true; - } // TODO remove me :) + } } diff --git a/src/main/java/com/team1816/lib/auto/paths/AutoPath.java b/src/main/java/com/team1816/lib/auto/paths/AutoPath.java index c4e2cb30..e1a60b28 100644 --- a/src/main/java/com/team1816/lib/auto/paths/AutoPath.java +++ b/src/main/java/com/team1816/lib/auto/paths/AutoPath.java @@ -38,10 +38,16 @@ public abstract class AutoPath { */ boolean rotated; + /** + * State: contains information about whether the trajectory is generated with TrajectoryCalculator + */ + boolean precalculated; + public AutoPath() { } public AutoPath(Color color) { + setPrecalculated(false); if (Constants.fieldSymmetry == Symmetry.AXIS && color == Color.RED) { setReflected(true); setRotated(false); @@ -72,6 +78,20 @@ protected void setRotated(boolean rotated) { this.rotated = rotated; } + /** + * Sets whether the trajectory is pre-calculated or generated + */ + protected void setPrecalculated(boolean precalculated) { + this.precalculated = precalculated; + } + + /** + * Returns whether the trajectory is pre-calculated + */ + protected boolean isPrecalculated() { + return precalculated; + } + /** * Returns a list of Pose2d's that define the trajectory /path to be followed * @@ -146,14 +166,6 @@ public List getRotatedWaypointHeadings() { return rotatedWaypointHeadings; } - /** - * Checks if the path was made using the CheesyPath web application. - * If false, the starting pose of the trajectory will be set to the default starting pose. - * - * @return boolean usingApp - */ - protected abstract boolean usingApp(); - /** * Returns the generated trajectory associated with the AutoPath * @@ -163,11 +175,11 @@ public List getRotatedWaypointHeadings() { public Trajectory getAsTrajectory() { if (trajectory == null) { if (!reflected && !rotated) { - trajectory = PathUtil.generateTrajectory(getClass().getName(), usingApp(), getWaypoints()); + trajectory = PathUtil.generateTrajectory(getClass().getName(), getWaypoints(), isPrecalculated()); } else if (reflected) { - trajectory = PathUtil.generateTrajectory(getClass().getName() + "-reversed", usingApp(), getReflectedWaypoints()); + trajectory = PathUtil.generateTrajectory(getClass().getName() + "_Reversed", getReflectedWaypoints(), isPrecalculated()); } else { - trajectory = PathUtil.generateTrajectory(getClass().getName() + "-rotated", usingApp(), getRotatedWaypoints()); + trajectory = PathUtil.generateTrajectory(getClass().getName() + "_Rotated", getRotatedWaypoints(), isPrecalculated()); } } return trajectory; @@ -181,30 +193,30 @@ public Trajectory getAsTrajectory() { */ public List getAsTrajectoryHeadings() { if (headings == null) { - String name = getClass().getName(); + String name = getClass().getName()+"Headings"; if (!reflected && !rotated) { headings = PathUtil.generateHeadings( name, - usingApp(), getWaypoints(), - getWaypointHeadings() + getWaypointHeadings(), + isPrecalculated() ); } else if (reflected) { headings = PathUtil.generateHeadings( - name + "-reversed", - usingApp(), + name + "_Reversed", getReflectedWaypoints(), - getReflectedWaypointHeadings() + getReflectedWaypointHeadings(), + isPrecalculated() ); } else { headings = PathUtil.generateHeadings( - name + "-rotated", - usingApp(), + name + "_Rotated", getRotatedWaypoints(), - getRotatedWaypointHeadings() + getRotatedWaypointHeadings(), + isPrecalculated() ); } } diff --git a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java index 1407f82d..3a137f09 100644 --- a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java +++ b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java @@ -1,9 +1,11 @@ package com.team1816.lib.auto.paths; +import com.team1816.lib.util.trajectoryUtil.TrajectoryCalculator; import com.team1816.season.configuration.Constants; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; @@ -21,18 +23,23 @@ public class PathUtil { private static final double kMaxVelocity = kPathFollowingMaxVelMeters; private static final double kMaxAccel = kPathFollowingMaxAccelMeters; + /** + * Generates a trajectory when TrajectoryCalculator is not used + * + * @param pathName + * @param waypoints + * @return + */ public static Trajectory generateTrajectory( String pathName, - boolean usingApp, List waypoints ) { - return generateTrajectory(pathName, usingApp, waypoints, false); + return generateTrajectory(pathName, waypoints, false); } /** * Generates a trajectory based on a list of waypoints based on WPIlib's TrajectoryGenerator * - * @param usingApp * @param waypoints * @param loadTrajectories * @return trajectory @@ -42,7 +49,6 @@ public static Trajectory generateTrajectory( */ public static Trajectory generateTrajectory( String pathName, - boolean usingApp, List waypoints, boolean loadTrajectories ) { @@ -67,23 +73,12 @@ public static Trajectory generateTrajectory( waypointsMeters, config ); - /* If web application is not used, then the starting pose is transformed to the default starting pose, this has no impact on how the trajectory is run */ - if (!usingApp) { - baseTrajectory = - baseTrajectory.transformBy( - new Transform2d( - Constants.kDefaultZeroingPose.getTranslation(), - Constants.kDefaultZeroingPose.getRotation() - ) - ); - } return baseTrajectory; } /** * Generates a trajectory based on a list of waypoints based on WPIlib's TrajectoryGenerator * - * @param usingApp * @param initial * @param waypoints * @return trajectory @@ -92,7 +87,6 @@ public static Trajectory generateTrajectory( * @see edu.wpi.first.math.trajectory.TrajectoryGenerator */ public static Trajectory generateTrajectory( - boolean usingApp, ChassisSpeeds initial, List waypoints ) { @@ -115,47 +109,34 @@ public static Trajectory generateTrajectory( waypointsMeters, config ); - /* If web application is not used, then the starting pose is transformed to the default starting pose, this has no impact on how the trajectory is run */ - if (!usingApp) { - baseTrajectory = - baseTrajectory.transformBy( - new Transform2d( - Constants.kDefaultZeroingPose.getTranslation(), - Constants.kDefaultZeroingPose.getRotation() - ) - ); - } return baseTrajectory; } /** * Generates headings that can be transposed onto a trajectory with time calibration via a differential model * - * @param usingApp * @param waypoints * @param swerveHeadings * @return headings */ public static List generateHeadings( String name, - boolean usingApp, List waypoints, - List swerveHeadings + List swerveHeadings, + boolean loadTrajectories ) { + name = TrajectoryCalculator.formatClassName(name); + if (loadTrajectories) { + return TrajectoryCalculator.loadTrajectoryHeadings(name); + } if (waypoints == null || swerveHeadings == null) { return null; } - double startX = .5; - double startY = Constants.fieldCenterY; - /* Trajectory is generated */ - Trajectory trajectory = generateTrajectory(name, usingApp, waypoints); + Trajectory trajectory = generateTrajectory(name, waypoints); List waypointsMeters = new ArrayList<>(); - if (usingApp) { - startX = 0; - startY = 0; - } + /* Inch to meter conversions */ for (Pose2d pose2d : waypoints) { waypointsMeters.add( @@ -220,14 +201,12 @@ public static List generateHeadings( /** * Generates headings that can be transposed onto a trajectory with time calibration via a differential model * - * @param usingApp * @param waypoints * @param swerveHeadings * @param initial * @return headings */ public static List generateHeadings( - boolean usingApp, List waypoints, List swerveHeadings, ChassisSpeeds initial @@ -236,22 +215,16 @@ public static List generateHeadings( return null; } - double startX = .5; - double startY = Constants.fieldCenterY; - /* Trajectory is generated */ - Trajectory trajectory = generateTrajectory(usingApp, initial, waypoints); + Trajectory trajectory = generateTrajectory(initial, waypoints); List waypointsMeters = new ArrayList<>(); - if (usingApp) { - startX = 0; - startY = 0; - } + /* Inch to meter conversions */ for (Pose2d pose2d : waypoints) { waypointsMeters.add( new Pose2d( - pose2d.getX() + startX, - pose2d.getY() + startY, + pose2d.getX(), + pose2d.getY(), pose2d.getRotation() ) ); diff --git a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java index 6036c811..b4c828e4 100644 --- a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java +++ b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java @@ -10,6 +10,7 @@ import org.apache.commons.io.FileUtils; import java.io.*; +import java.util.ArrayList; import java.util.List; /** @@ -89,7 +90,7 @@ public static void calcTrajectory(String name, List waypoints) { if (waypoints == null) { return; } - var trajectory = PathUtil.generateTrajectory(name, true, waypoints, true); + var trajectory = PathUtil.generateTrajectory(name, waypoints, false); ObjectMapper mapper = new ObjectMapper(); try { File file = new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"); @@ -113,7 +114,7 @@ public static void calcHeadings(String name, List waypoints, List waypoints, List loadTrajectoryHeadings(String name) { + ObjectMapper mapper = new ObjectMapper(); + try { + List list = mapper.readValue( + new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"), + new TypeReference>() { } + ); + return list; + } catch (Exception e) { + System.out.println("Error parsing path JSON: " + name + "\n" + e.getMessage()); + return new ArrayList<>(); + } + } + /** * Clears the json files storing trajectories and headings located in the resources directory */ diff --git a/src/main/java/com/team1816/season/auto/actions/TrajectoryToTargetPath.java b/src/main/java/com/team1816/season/auto/actions/TrajectoryToTargetPath.java index 1ce9d22d..4f3badba 100644 --- a/src/main/java/com/team1816/season/auto/actions/TrajectoryToTargetPath.java +++ b/src/main/java/com/team1816/season/auto/actions/TrajectoryToTargetPath.java @@ -32,7 +32,7 @@ public TrajectoryToTargetPath() { } @Override - protected List getWaypoints() { + public List getWaypoints() { List waypoints = new ArrayList<>(); waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); waypoints.add(target); @@ -40,7 +40,7 @@ protected List getWaypoints() { } @Override - protected List getWaypointHeadings() { + public List getWaypointHeadings() { List headings = new ArrayList<>(); headings.add(robotState.fieldToVehicle.getRotation()); headings.add(target.getRotation()); @@ -58,7 +58,7 @@ public Trajectory getAsTrajectory() { translatedVelocity.getY(), robotState.deltaVehicle.omegaRadiansPerSecond ); - return PathUtil.generateTrajectory(usingApp(), translatedChassisSpeeds, getWaypoints()); + return PathUtil.generateTrajectory(translatedChassisSpeeds, getWaypoints()); } @Override @@ -73,15 +73,9 @@ public List getAsTrajectoryHeadings() { robotState.deltaVehicle.omegaRadiansPerSecond ); return PathUtil.generateHeadings( - usingApp(), getWaypoints(), getWaypointHeadings(), translatedChassisSpeeds ); } - - @Override - protected boolean usingApp() { - return true; - } } diff --git a/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java b/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java index fe217ce9..917152d6 100644 --- a/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java +++ b/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java @@ -28,19 +28,23 @@ public DriveStraightPath() { @Override public List getWaypoints() { var waypoints = List.of( - new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0)), - new Pose2d((driveDistance), 0.0, Rotation2d.fromDegrees(0)) + new Pose2d(0.0, 4.0, Rotation2d.fromDegrees(0)), + new Pose2d((driveDistance), 4.0, Rotation2d.fromDegrees(0)) ); return waypoints; } @Override public List getWaypointHeadings() { - return null; + var headings = List.of( + Constants.EmptyRotation2d, + Constants.EmptyRotation2d + ); + return headings; } @Override - public boolean usingApp() { - return false; + public boolean isPrecalculated() { + return true; } } diff --git a/src/main/java/com/team1816/season/auto/paths/LivingRoomPath.java b/src/main/java/com/team1816/season/auto/paths/LivingRoomPath.java index dd7f8bb2..7b6bfe50 100644 --- a/src/main/java/com/team1816/season/auto/paths/LivingRoomPath.java +++ b/src/main/java/com/team1816/season/auto/paths/LivingRoomPath.java @@ -36,7 +36,7 @@ public List getWaypointHeadings() { } @Override - public boolean usingApp() { + public boolean isPrecalculated() { return true; } } diff --git a/src/main/java/com/team1816/season/configuration/Constants.java b/src/main/java/com/team1816/season/configuration/Constants.java index c06d0e9b..d6d8a478 100644 --- a/src/main/java/com/team1816/season/configuration/Constants.java +++ b/src/main/java/com/team1816/season/configuration/Constants.java @@ -69,7 +69,7 @@ public class Constants { public static final double gravitationalAccelerationConstant = 9.8d; public static double kMaxAccelDiffThreshold = 2d; // m/s^2 public static double kMaxBalancingVelocity = 0.2; // m/s - public static double kMinTrajectoryDistance = 0.05; // m + public static double kMinTrajectoryDistance = 0.1; // m public static double kMaxProximityThresholdCentimeters = 25; // cm /** From 5c0c4780b021f794d0cde02e39cb19b1eff818ab Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Sun, 5 Feb 2023 10:58:34 -0600 Subject: [PATCH 12/31] swerve module changes --- src/main/java/com/team1816/lib/subsystems/drive/Drive.java | 2 ++ .../java/com/team1816/lib/subsystems/drive/SwerveModule.java | 5 ++--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/team1816/lib/subsystems/drive/Drive.java b/src/main/java/com/team1816/lib/subsystems/drive/Drive.java index 69a3c4a0..da89dcdf 100644 --- a/src/main/java/com/team1816/lib/subsystems/drive/Drive.java +++ b/src/main/java/com/team1816/lib/subsystems/drive/Drive.java @@ -82,6 +82,7 @@ public interface Factory { protected Pose2d startingPose = Constants.kDefaultZeroingPose; protected Trajectory trajectory; protected static double timestamp; + protected static double prevTimestamp; /** * Simulator @@ -215,6 +216,7 @@ public void onStart(double timestamp) { @Override public void onLoop(double timestamp) { synchronized (Drive.this) { + Drive.prevTimestamp = Drive.timestamp; Drive.timestamp = timestamp; switch (controlState) { case OPEN_LOOP: diff --git a/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java b/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java index 49fd68b2..0bf27f85 100644 --- a/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java +++ b/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java @@ -15,8 +15,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.Timer; -import static com.team1816.lib.subsystems.drive.Drive.NAME; -import static com.team1816.lib.subsystems.drive.Drive.factory; +import static com.team1816.lib.subsystems.drive.Drive.*; public class SwerveModule implements ISwerveModule { @@ -160,7 +159,7 @@ public void update() { moduleState.speedMetersPerSecond = driveActual; moduleState.angle = Rotation2d.fromDegrees(azimuthActual); - drivePosition += driveActual * Constants.kLooperDt; + drivePosition += driveActual * (timestamp-prevTimestamp); modulePosition.distanceMeters = drivePosition; modulePosition.angle = Rotation2d.fromDegrees(azimuthActual); From fd2ed1aa5b45e1e84738fa2fa7cc7de166a1c0c5 Mon Sep 17 00:00:00 2001 From: miapirobotics <34904974+miapirobotics@users.noreply.github.com> Date: Sun, 5 Feb 2023 19:05:29 -0600 Subject: [PATCH 13/31] Compile trajectories for all robot configs --- .../com/team1816/lib/auto/paths/PathUtil.java | 8 +- .../lib/hardware/factory/RobotFactory.java | 1094 +++++++++-------- .../trajectoryUtil/TrajectoryCalculator.java | 366 +++--- 3 files changed, 744 insertions(+), 724 deletions(-) diff --git a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java index 3a137f09..994dcc62 100644 --- a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java +++ b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java @@ -20,9 +20,13 @@ public class PathUtil { /** * Constraints */ - private static final double kMaxVelocity = kPathFollowingMaxVelMeters; - private static final double kMaxAccel = kPathFollowingMaxAccelMeters; + private static double kMaxVelocity = kPathFollowingMaxVelMeters; + private static double kMaxAccel = kPathFollowingMaxAccelMeters; + public static void setCalculateParams(double kMaxVel, double kMaxAc) { + kMaxVelocity = kMaxVel; + kMaxAccel = kMaxAc; + } /** * Generates a trajectory when TrajectoryCalculator is not used * diff --git a/src/main/java/com/team1816/lib/hardware/factory/RobotFactory.java b/src/main/java/com/team1816/lib/hardware/factory/RobotFactory.java index ef5860db..f53e71db 100644 --- a/src/main/java/com/team1816/lib/hardware/factory/RobotFactory.java +++ b/src/main/java/com/team1816/lib/hardware/factory/RobotFactory.java @@ -1,545 +1,549 @@ -package com.team1816.lib.hardware.factory; - -import com.ctre.phoenix.led.CANdle; -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; -import com.google.inject.Singleton; -import com.team1816.lib.hardware.*; -import com.team1816.lib.hardware.components.gyro.GhostPigeonIMU; -import com.team1816.lib.hardware.components.gyro.IPigeonIMU; -import com.team1816.lib.hardware.components.gyro.Pigeon2Impl; -import com.team1816.lib.hardware.components.gyro.PigeonIMUImpl; -import com.team1816.lib.hardware.components.ledManager.CANdleImpl; -import com.team1816.lib.hardware.components.ledManager.CanifierImpl; -import com.team1816.lib.hardware.components.ledManager.GhostLEDManager; -import com.team1816.lib.hardware.components.ledManager.ILEDManager; -import com.team1816.lib.hardware.components.motor.IGreenMotor; -import com.team1816.lib.hardware.components.motor.LazySparkMax; -import com.team1816.lib.hardware.components.pcm.*; -import com.team1816.lib.hardware.components.sensor.GhostProximitySensor; -import com.team1816.lib.hardware.components.sensor.IProximitySensor; -import com.team1816.lib.hardware.components.sensor.ProximitySensor; -import com.team1816.lib.subsystems.drive.SwerveModule; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.RobotBase; - -import javax.annotation.Nonnull; -import java.util.Map; -import java.util.Objects; - -/** - * This class employs the MotorFactory and SensorFactory with yaml integrations and is the initial entry point to - * create and initialize any and all components on a robot. - * - * @see MotorFactory - * @see SensorFactory - * @see YamlConfig - */ -@Singleton -public class RobotFactory { - - private RobotConfiguration config; - - public RobotFactory() { - var robotName = System.getenv("ROBOT_NAME"); - if (robotName == null) { - robotName = "default"; - DriverStation.reportWarning( - "ROBOT_NAME environment variable not defined, falling back to default.config.yml!", - false - ); - } - System.out.println("Loading Config for " + robotName); - try { - config = - YamlConfig.loadFrom( - this.getClass() - .getClassLoader() - .getResourceAsStream("yaml/" + robotName + ".config.yml") - ); - } catch (Exception e) { - DriverStation.reportError("Yaml Config error!", e.getStackTrace()); - } - } - - public IGreenMotor getMotor( - String subsystemName, - String name, - Map pidConfigs, - int remoteSensorId - ) { - IGreenMotor motor = null; - var subsystem = getSubsystem(subsystemName); - - // Identifying motor type - if (subsystem.implemented) { - if (isHardwareValid(subsystem.talons, name)) { - motor = - MotorFactory.createDefaultTalon( - subsystem.talons.get(name), - name, - false, - subsystem, - pidConfigs, - remoteSensorId, - config.infrastructure.canivoreBusName - ); - } else if (isHardwareValid(subsystem.falcons, name)) { - motor = - MotorFactory.createDefaultTalon( - subsystem.falcons.get(name), - name, - true, - subsystem, - pidConfigs, - remoteSensorId, - config.infrastructure.canivoreBusName - ); - } else if (isHardwareValid(subsystem.sparkmaxes, name)) { - motor = - MotorFactory.createSpark( - subsystem.sparkmaxes.get(name), - name, - subsystem, - pidConfigs - ); - } - // Never make the victor a main - } - - // report creation of motor - if (motor == null) { - reportGhostWarning("Motor", subsystemName, name); - motor = - MotorFactory.createGhostMotor( - (int) (getConstant(subsystemName, "maxVelTicks100ms", 1, false)), - 0, - name, - subsystem - ); - } else { - System.out.println( - "Created " + - motor.getClass().getSimpleName() + - " id:" + - motor.getDeviceID() - ); - } - - return motor; - } - - public IGreenMotor getMotor(String subsystemName, String name) { - return getMotor(subsystemName, name, getSubsystem(subsystemName).pidConfig, -1); - } - - public IGreenMotor getFollowerMotor( - String subsystemName, - String name, - IGreenMotor main - ) { - IGreenMotor followerMotor = null; - var subsystem = getSubsystem(subsystemName); - if (subsystem.implemented && main != null) { - if (isHardwareValid(subsystem.talons, name)) { - // Talons must be following another Talon, cannot follow a Victor. - followerMotor = - MotorFactory.createFollowerTalon( - subsystem.talons.get(name), - name, - false, - main, - subsystem, - subsystem.pidConfig, - config.infrastructure.canivoreBusName - ); - } else if (isHardwareValid(subsystem.falcons, name)) { - followerMotor = - MotorFactory.createFollowerTalon( - subsystem.falcons.get(name), - name, - true, - main, - subsystem, - subsystem.pidConfig, - config.infrastructure.canivoreBusName - ); - } else if (isHardwareValid(subsystem.sparkmaxes, name)) { - followerMotor = - MotorFactory.createSpark( - subsystem.sparkmaxes.get(name), - name, - subsystem - ); - ((LazySparkMax) followerMotor).follow( - main, - subsystem.invertMotor.contains(name) - ); - followerMotor.setInverted(main.getInverted()); - } else if (isHardwareValid(subsystem.victors, name)) { - // Victors can follow Talons or another Victor. - followerMotor = - MotorFactory.createFollowerVictor( - subsystem.victors.get(name), - name, - main - ); - } - } - if (followerMotor == null) { - if (subsystem.implemented) reportGhostWarning("Motor", subsystemName, name); - followerMotor = - MotorFactory.createGhostMotor( - (int) getConstant(subsystemName, "maxVelTicks100ms"), - 0, - name, - subsystem - ); - } - if (main != null) { - followerMotor.setInverted(main.getInverted()); - } - return followerMotor; - } - - public SwerveModule getSwerveModule(String subsystemName, String name) { - var subsystem = getSubsystem(subsystemName); - ModuleConfiguration module = subsystem.swerveModules.modules.get(name); - if (module == null) { - DriverStation.reportError( - "No swerve module with name " + name + " subsystem " + subsystemName, - true - ); - return null; - } - - var moduleConfig = new SwerveModule.ModuleConfig(); - moduleConfig.moduleName = name; - moduleConfig.azimuthMotorName = module.azimuth; // getAzimuth and drive give ID I think - not the module name (ex: leftRear) - moduleConfig.azimuthPid = - getPidSlotConfig(subsystemName, "slot0", PIDConfig.Azimuth); - moduleConfig.driveMotorName = module.drive; - moduleConfig.drivePid = getPidSlotConfig(subsystemName, "slot0", PIDConfig.Drive); - moduleConfig.azimuthEncoderHomeOffset = module.constants.get("encoderOffset"); - - var canCoder = getCanCoder(subsystemName, name); - - return new SwerveModule(subsystemName, moduleConfig, canCoder); - } - - public CANCoder getCanCoder(String subsystemName, String name) { - var subsystem = getSubsystem(subsystemName); - var module = subsystem.swerveModules.modules.get(name); - CANCoder canCoder = null; - if ( - module != null && - module.canCoder != null && - subsystem.canCoders.get(module.canCoder) >= 0 - ) { - canCoder = - MotorFactory.createCanCoder( - subsystem.canCoders.get(module.canCoder), - subsystem.canCoders.get(subsystem.invertCanCoder) != null && - subsystem.invertCanCoder.contains(module.canCoder) - ); - } - - // purposefully return null so that swerve modules default to quad encoders - return canCoder; - } - - @Nonnull - public ISolenoid getSolenoid(String subsystemName, String name) { - var subsystem = getSubsystem(subsystemName); - if (subsystem.implemented) { - if (isHardwareValid(subsystem.solenoids, name) && isPcmEnabled()) { - return new SolenoidImpl( - config.infrastructure.pcmId, - config.infrastructure.pcmIsRev - ? PneumaticsModuleType.REVPH - : PneumaticsModuleType.CTREPCM, - subsystem.solenoids.get(name) - ); - } - reportGhostWarning("Solenoid", subsystemName, name); - } - return new GhostSolenoid(); - } - - @Nonnull - public IDoubleSolenoid getDoubleSolenoid(String subsystemName, String name) { - var subsystem = getSubsystem(subsystemName); - if (getSubsystem(subsystemName).doubleSolenoids != null) { - DoubleSolenoidConfig solenoidConfig = getSubsystem(subsystemName) - .doubleSolenoids.get(name); - if ( - subsystem.implemented && - solenoidConfig != null && - isHardwareValid(solenoidConfig.forward) && - isHardwareValid(solenoidConfig.reverse) && - isPcmEnabled() - ) { - return new DoubleSolenoidImpl( - config.infrastructure.pcmId, - PneumaticsModuleType.REVPH, - solenoidConfig.forward, - solenoidConfig.reverse - ); - } - } - reportGhostWarning("DoubleSolenoid", subsystemName, name); - return new GhostDoubleSolenoid(); - } - - public ILEDManager getLEDManager(String subsystemName) { - var subsystem = getSubsystem(subsystemName); - ILEDManager ledManager = null; - if (subsystem.implemented) { - if (isHardwareValid(subsystem.canifier)) { - ledManager = new CanifierImpl(subsystem.canifier); - } else if (isHardwareValid(subsystem.candle)) { - ledManager = - new CANdleImpl( - subsystem.candle, - config.infrastructure.canivoreBusName - ); - } - if (ledManager != null) { - if (getConstant("resetFactoryDefaults") > 0) { - ledManager.configFactoryDefault(); - ledManager.configStatusLedState(true); - ledManager.configLOSBehavior(true); - ledManager.configLEDType(CANdle.LEDStripType.BRG); - ledManager.configBrightnessScalar(1); - } - return ledManager; - } - reportGhostWarning("LEDManager", subsystemName, ""); - } - - return new GhostLEDManager(); - } - - public ICompressor getCompressor() { - if (isPcmEnabled() && config.infrastructure.compressorEnabled) { - PneumaticsModuleType pcmType = config.infrastructure.pcmIsRev - ? PneumaticsModuleType.REVPH - : PneumaticsModuleType.CTREPCM; - return new CompressorImpl(getPcmId(), pcmType); - } - reportGhostWarning("Compressor", "ROOT", "on PCM ID " + getPcmId()); // root? - return new GhostCompressor(); - } - - private boolean isHardwareValid(Map map, String name) { - if (map != null) { - Integer hardwareId = map.get(name); - return hardwareId != null && hardwareId > -1 && RobotBase.isReal(); - } - return false; - } - - private boolean isHardwareValid(Integer hardwareId) { - return hardwareId != null && hardwareId > -1 && RobotBase.isReal(); - } - - public Double getConstant(String name) { - return getConstant(name, 0); - } - - public Map getConstants() { - return config.constants; - } - - public SubsystemConfig getSubsystem(String subsystemName) { - if (config.subsystems.containsKey(subsystemName)) { - var subsystem = config.subsystems.get(subsystemName); - if (subsystem == null) { - subsystem = new SubsystemConfig(); - subsystem.implemented = false; - System.out.println("Subsystem not defined: " + subsystemName); - } - return subsystem; - } - SubsystemConfig subsystem = new SubsystemConfig(); - subsystem.implemented = false; - return subsystem; - } - - public double getConstant(String name, double defaultVal) { - if (getConstants() == null || !getConstants().containsKey(name)) { - DriverStation.reportWarning("Yaml constants:" + name + " missing", true); - return defaultVal; - } - return getConstants().get(name); - } - - public String getControlBoard() { - return Objects.requireNonNullElse(config.controlboard, "empty"); - } - - public double getConstant(String subsystemName, String name) { - return getConstant(subsystemName, name, 0.0); - } - - public double getConstant(String subsystemName, String name, double defaultVal) { - return getConstant(subsystemName, name, defaultVal, true); - } - - public double getConstant( - String subsystemName, - String name, - double defaultVal, - boolean showWarning - ) { - if (!getSubsystem(subsystemName).implemented) { - return defaultVal; - } - if ( - getSubsystem(subsystemName).constants == null || - !getSubsystem(subsystemName).constants.containsKey(name) - ) { - if (showWarning) { - DriverStation.reportWarning( - "Yaml: subsystem \"" + - subsystemName + - "\" constant \"" + - name + - "\" missing", - defaultVal == 0 - ); - } - return defaultVal; - } - return getSubsystem(subsystemName).constants.get(name); - } - - public PIDSlotConfiguration getPidSlotConfig(String subsystemName) { - return getPidSlotConfig(subsystemName, "slot0", PIDConfig.Generic); - } - - public PIDSlotConfiguration getPidSlotConfig(String subsystemName, String slot) { - return getPidSlotConfig(subsystemName, slot, PIDConfig.Generic); - } - - public PIDSlotConfiguration getPidSlotConfig( - String subsystemName, - String slot, - PIDConfig configType - ) { - var subsystem = getSubsystem(subsystemName); - Map config = null; - if (subsystem.implemented) { - switch (configType) { - case Azimuth: - config = subsystem.swerveModules.azimuthPID; - break; - case Drive: - config = subsystem.swerveModules.drivePID; - break; - case Generic: - config = subsystem.pidConfig; - break; - } - } - if (config != null && config.get(slot) != null) return config.get(slot); - else { - if (subsystem.implemented) { - DriverStation.reportError( - "pidConfig missing for " + subsystemName + " " + slot, - true - ); - return null; - } else { - // return a default config if not implemented - PIDSlotConfiguration pidSlotConfiguration = new PIDSlotConfiguration(); - pidSlotConfiguration.kP = 0.0; - pidSlotConfiguration.kI = 0.0; - pidSlotConfiguration.kD = 0.0; - pidSlotConfiguration.kF = 0.0; - pidSlotConfiguration.iZone = 0; - pidSlotConfiguration.allowableError = 0.0; - return pidSlotConfiguration; - } - } - } - - public PowerDistribution getPd() { - return new PowerDistribution( - config.infrastructure.pdId, - config.infrastructure.pdIsRev - ? PowerDistribution.ModuleType.kRev - : PowerDistribution.ModuleType.kCTRE - ); - } - - public IPigeonIMU getPigeon() { - int id = config.infrastructure.pigeonId; - IPigeonIMU pigeon; - if (!isHardwareValid(id)) { - pigeon = new GhostPigeonIMU(id); - } else if (config.infrastructure.isPigeon2) { - System.out.println("Using Pigeon 2 for id: " + id); - pigeon = new Pigeon2Impl(id, config.infrastructure.canivoreBusName); - } else { - System.out.println("Using old Pigeon for id: " + id); - pigeon = new PigeonIMUImpl(id); - } - if (getConstant("resetFactoryDefaults") > 0) { - pigeon.configFactoryDefault(); - pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 100); - pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.BiasedStatus_6_Accel, 100); - } - return pigeon; - } - - public IProximitySensor getProximitySensor(String name) { - if (config.infrastructure.proximitySensors == null) { - return new GhostProximitySensor(); - } - int id = config.infrastructure.proximitySensors.getOrDefault(name, -1); - if (id < 0) { - System.out.println("Incorrect Name: Proximity sensor not found, using ghost!"); - return new GhostProximitySensor(); - } - System.out.println("Creating Proximity Sensor: " + name + " at port: " + id); - return new ProximitySensor(name, config.infrastructure.proximitySensors.get(name)); - } - - public int getPcmId() { - if (config.infrastructure == null && config.infrastructure.pcmId == null) return -1; - return config.infrastructure.pcmId; - } - - public boolean isPcmEnabled() { - return getPcmId() > -1; - } - - public boolean isCompressorEnabled() { - return config.infrastructure.compressorEnabled; - } - - private void reportGhostWarning( - String type, - String subsystemName, - String componentName - ) { - System.out.println( - " " + - type + - " \"" + - componentName + - "\" invalid in Yaml for subsystem \"" + - subsystemName + - "\", using ghost!" - ); - } - - private enum PIDConfig { - Azimuth, - Drive, - Generic, - } -} +package com.team1816.lib.hardware.factory; + +import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; +import com.google.inject.Singleton; +import com.team1816.lib.hardware.*; +import com.team1816.lib.hardware.components.gyro.GhostPigeonIMU; +import com.team1816.lib.hardware.components.gyro.IPigeonIMU; +import com.team1816.lib.hardware.components.gyro.Pigeon2Impl; +import com.team1816.lib.hardware.components.gyro.PigeonIMUImpl; +import com.team1816.lib.hardware.components.ledManager.CANdleImpl; +import com.team1816.lib.hardware.components.ledManager.CanifierImpl; +import com.team1816.lib.hardware.components.ledManager.GhostLEDManager; +import com.team1816.lib.hardware.components.ledManager.ILEDManager; +import com.team1816.lib.hardware.components.motor.IGreenMotor; +import com.team1816.lib.hardware.components.motor.LazySparkMax; +import com.team1816.lib.hardware.components.pcm.*; +import com.team1816.lib.hardware.components.sensor.GhostProximitySensor; +import com.team1816.lib.hardware.components.sensor.IProximitySensor; +import com.team1816.lib.hardware.components.sensor.ProximitySensor; +import com.team1816.lib.subsystems.drive.SwerveModule; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.RobotBase; + +import javax.annotation.Nonnull; +import java.util.Map; +import java.util.Objects; + +/** + * This class employs the MotorFactory and SensorFactory with yaml integrations and is the initial entry point to + * create and initialize any and all components on a robot. + * + * @see MotorFactory + * @see SensorFactory + * @see YamlConfig + */ +@Singleton +public class RobotFactory { + + private RobotConfiguration config; + + public RobotFactory() { + var robotName = System.getenv("ROBOT_NAME"); + if (robotName == null) { + robotName = "default"; + DriverStation.reportWarning( + "ROBOT_NAME environment variable not defined, falling back to default.config.yml!", + false + ); + } + loadConfig(robotName); + } + + public void loadConfig(String robotName) { + System.out.println("Loading Config for " + robotName); + try { + config = + YamlConfig.loadFrom( + this.getClass() + .getClassLoader() + .getResourceAsStream("yaml/" + robotName + ".config.yml") + ); + } catch (Exception e) { + DriverStation.reportError("Yaml Config error!", e.getStackTrace()); + } + } + + public IGreenMotor getMotor( + String subsystemName, + String name, + Map pidConfigs, + int remoteSensorId + ) { + IGreenMotor motor = null; + var subsystem = getSubsystem(subsystemName); + + // Identifying motor type + if (subsystem.implemented) { + if (isHardwareValid(subsystem.talons, name)) { + motor = + MotorFactory.createDefaultTalon( + subsystem.talons.get(name), + name, + false, + subsystem, + pidConfigs, + remoteSensorId, + config.infrastructure.canivoreBusName + ); + } else if (isHardwareValid(subsystem.falcons, name)) { + motor = + MotorFactory.createDefaultTalon( + subsystem.falcons.get(name), + name, + true, + subsystem, + pidConfigs, + remoteSensorId, + config.infrastructure.canivoreBusName + ); + } else if (isHardwareValid(subsystem.sparkmaxes, name)) { + motor = + MotorFactory.createSpark( + subsystem.sparkmaxes.get(name), + name, + subsystem, + pidConfigs + ); + } + // Never make the victor a main + } + + // report creation of motor + if (motor == null) { + reportGhostWarning("Motor", subsystemName, name); + motor = + MotorFactory.createGhostMotor( + (int) (getConstant(subsystemName, "maxVelTicks100ms", 1, false)), + 0, + name, + subsystem + ); + } else { + System.out.println( + "Created " + + motor.getClass().getSimpleName() + + " id:" + + motor.getDeviceID() + ); + } + + return motor; + } + + public IGreenMotor getMotor(String subsystemName, String name) { + return getMotor(subsystemName, name, getSubsystem(subsystemName).pidConfig, -1); + } + + public IGreenMotor getFollowerMotor( + String subsystemName, + String name, + IGreenMotor main + ) { + IGreenMotor followerMotor = null; + var subsystem = getSubsystem(subsystemName); + if (subsystem.implemented && main != null) { + if (isHardwareValid(subsystem.talons, name)) { + // Talons must be following another Talon, cannot follow a Victor. + followerMotor = + MotorFactory.createFollowerTalon( + subsystem.talons.get(name), + name, + false, + main, + subsystem, + subsystem.pidConfig, + config.infrastructure.canivoreBusName + ); + } else if (isHardwareValid(subsystem.falcons, name)) { + followerMotor = + MotorFactory.createFollowerTalon( + subsystem.falcons.get(name), + name, + true, + main, + subsystem, + subsystem.pidConfig, + config.infrastructure.canivoreBusName + ); + } else if (isHardwareValid(subsystem.sparkmaxes, name)) { + followerMotor = + MotorFactory.createSpark( + subsystem.sparkmaxes.get(name), + name, + subsystem + ); + ((LazySparkMax) followerMotor).follow( + main, + subsystem.invertMotor.contains(name) + ); + followerMotor.setInverted(main.getInverted()); + } else if (isHardwareValid(subsystem.victors, name)) { + // Victors can follow Talons or another Victor. + followerMotor = + MotorFactory.createFollowerVictor( + subsystem.victors.get(name), + name, + main + ); + } + } + if (followerMotor == null) { + if (subsystem.implemented) reportGhostWarning("Motor", subsystemName, name); + followerMotor = + MotorFactory.createGhostMotor( + (int) getConstant(subsystemName, "maxVelTicks100ms"), + 0, + name, + subsystem + ); + } + if (main != null) { + followerMotor.setInverted(main.getInverted()); + } + return followerMotor; + } + + public SwerveModule getSwerveModule(String subsystemName, String name) { + var subsystem = getSubsystem(subsystemName); + ModuleConfiguration module = subsystem.swerveModules.modules.get(name); + if (module == null) { + DriverStation.reportError( + "No swerve module with name " + name + " subsystem " + subsystemName, + true + ); + return null; + } + + var moduleConfig = new SwerveModule.ModuleConfig(); + moduleConfig.moduleName = name; + moduleConfig.azimuthMotorName = module.azimuth; // getAzimuth and drive give ID I think - not the module name (ex: leftRear) + moduleConfig.azimuthPid = + getPidSlotConfig(subsystemName, "slot0", PIDConfig.Azimuth); + moduleConfig.driveMotorName = module.drive; + moduleConfig.drivePid = getPidSlotConfig(subsystemName, "slot0", PIDConfig.Drive); + moduleConfig.azimuthEncoderHomeOffset = module.constants.get("encoderOffset"); + + var canCoder = getCanCoder(subsystemName, name); + + return new SwerveModule(subsystemName, moduleConfig, canCoder); + } + + public CANCoder getCanCoder(String subsystemName, String name) { + var subsystem = getSubsystem(subsystemName); + var module = subsystem.swerveModules.modules.get(name); + CANCoder canCoder = null; + if ( + module != null && + module.canCoder != null && + subsystem.canCoders.get(module.canCoder) >= 0 + ) { + canCoder = + MotorFactory.createCanCoder( + subsystem.canCoders.get(module.canCoder), + subsystem.canCoders.get(subsystem.invertCanCoder) != null && + subsystem.invertCanCoder.contains(module.canCoder) + ); + } + + // purposefully return null so that swerve modules default to quad encoders + return canCoder; + } + + @Nonnull + public ISolenoid getSolenoid(String subsystemName, String name) { + var subsystem = getSubsystem(subsystemName); + if (subsystem.implemented) { + if (isHardwareValid(subsystem.solenoids, name) && isPcmEnabled()) { + return new SolenoidImpl( + config.infrastructure.pcmId, + config.infrastructure.pcmIsRev + ? PneumaticsModuleType.REVPH + : PneumaticsModuleType.CTREPCM, + subsystem.solenoids.get(name) + ); + } + reportGhostWarning("Solenoid", subsystemName, name); + } + return new GhostSolenoid(); + } + + @Nonnull + public IDoubleSolenoid getDoubleSolenoid(String subsystemName, String name) { + var subsystem = getSubsystem(subsystemName); + if (getSubsystem(subsystemName).doubleSolenoids != null) { + DoubleSolenoidConfig solenoidConfig = getSubsystem(subsystemName) + .doubleSolenoids.get(name); + if ( + subsystem.implemented && + solenoidConfig != null && + isHardwareValid(solenoidConfig.forward) && + isHardwareValid(solenoidConfig.reverse) && + isPcmEnabled() + ) { + return new DoubleSolenoidImpl( + config.infrastructure.pcmId, + PneumaticsModuleType.REVPH, + solenoidConfig.forward, + solenoidConfig.reverse + ); + } + } + reportGhostWarning("DoubleSolenoid", subsystemName, name); + return new GhostDoubleSolenoid(); + } + + public ILEDManager getLEDManager(String subsystemName) { + var subsystem = getSubsystem(subsystemName); + ILEDManager ledManager = null; + if (subsystem.implemented) { + if (isHardwareValid(subsystem.canifier)) { + ledManager = new CanifierImpl(subsystem.canifier); + } else if (isHardwareValid(subsystem.candle)) { + ledManager = + new CANdleImpl( + subsystem.candle, + config.infrastructure.canivoreBusName + ); + } + if (ledManager != null) { + if (getConstant("resetFactoryDefaults") > 0) { + ledManager.configFactoryDefault(); + ledManager.configStatusLedState(true); + ledManager.configLOSBehavior(true); + ledManager.configLEDType(CANdle.LEDStripType.BRG); + ledManager.configBrightnessScalar(1); + } + return ledManager; + } + reportGhostWarning("LEDManager", subsystemName, ""); + } + + return new GhostLEDManager(); + } + + public ICompressor getCompressor() { + if (isPcmEnabled() && config.infrastructure.compressorEnabled) { + PneumaticsModuleType pcmType = config.infrastructure.pcmIsRev + ? PneumaticsModuleType.REVPH + : PneumaticsModuleType.CTREPCM; + return new CompressorImpl(getPcmId(), pcmType); + } + reportGhostWarning("Compressor", "ROOT", "on PCM ID " + getPcmId()); // root? + return new GhostCompressor(); + } + + private boolean isHardwareValid(Map map, String name) { + if (map != null) { + Integer hardwareId = map.get(name); + return hardwareId != null && hardwareId > -1 && RobotBase.isReal(); + } + return false; + } + + private boolean isHardwareValid(Integer hardwareId) { + return hardwareId != null && hardwareId > -1 && RobotBase.isReal(); + } + + public Double getConstant(String name) { + return getConstant(name, 0); + } + + public Map getConstants() { + return config.constants; + } + + public SubsystemConfig getSubsystem(String subsystemName) { + if (config.subsystems.containsKey(subsystemName)) { + var subsystem = config.subsystems.get(subsystemName); + if (subsystem == null) { + subsystem = new SubsystemConfig(); + subsystem.implemented = false; + System.out.println("Subsystem not defined: " + subsystemName); + } + return subsystem; + } + SubsystemConfig subsystem = new SubsystemConfig(); + subsystem.implemented = false; + return subsystem; + } + + public double getConstant(String name, double defaultVal) { + if (getConstants() == null || !getConstants().containsKey(name)) { + DriverStation.reportWarning("Yaml constants:" + name + " missing", true); + return defaultVal; + } + return getConstants().get(name); + } + + public String getControlBoard() { + return Objects.requireNonNullElse(config.controlboard, "empty"); + } + + public double getConstant(String subsystemName, String name) { + return getConstant(subsystemName, name, 0.0); + } + + public double getConstant(String subsystemName, String name, double defaultVal) { + return getConstant(subsystemName, name, defaultVal, true); + } + + public double getConstant( + String subsystemName, + String name, + double defaultVal, + boolean showWarning + ) { + if (!getSubsystem(subsystemName).implemented) { + return defaultVal; + } + if ( + getSubsystem(subsystemName).constants == null || + !getSubsystem(subsystemName).constants.containsKey(name) + ) { + if (showWarning) { + DriverStation.reportWarning( + "Yaml: subsystem \"" + + subsystemName + + "\" constant \"" + + name + + "\" missing", + defaultVal == 0 + ); + } + return defaultVal; + } + return getSubsystem(subsystemName).constants.get(name); + } + + public PIDSlotConfiguration getPidSlotConfig(String subsystemName) { + return getPidSlotConfig(subsystemName, "slot0", PIDConfig.Generic); + } + + public PIDSlotConfiguration getPidSlotConfig(String subsystemName, String slot) { + return getPidSlotConfig(subsystemName, slot, PIDConfig.Generic); + } + + public PIDSlotConfiguration getPidSlotConfig( + String subsystemName, + String slot, + PIDConfig configType + ) { + var subsystem = getSubsystem(subsystemName); + Map config = null; + if (subsystem.implemented) { + switch (configType) { + case Azimuth: + config = subsystem.swerveModules.azimuthPID; + break; + case Drive: + config = subsystem.swerveModules.drivePID; + break; + case Generic: + config = subsystem.pidConfig; + break; + } + } + if (config != null && config.get(slot) != null) return config.get(slot); + else { + if (subsystem.implemented) { + DriverStation.reportError( + "pidConfig missing for " + subsystemName + " " + slot, + true + ); + return null; + } else { + // return a default config if not implemented + PIDSlotConfiguration pidSlotConfiguration = new PIDSlotConfiguration(); + pidSlotConfiguration.kP = 0.0; + pidSlotConfiguration.kI = 0.0; + pidSlotConfiguration.kD = 0.0; + pidSlotConfiguration.kF = 0.0; + pidSlotConfiguration.iZone = 0; + pidSlotConfiguration.allowableError = 0.0; + return pidSlotConfiguration; + } + } + } + + public PowerDistribution getPd() { + return new PowerDistribution( + config.infrastructure.pdId, + config.infrastructure.pdIsRev + ? PowerDistribution.ModuleType.kRev + : PowerDistribution.ModuleType.kCTRE + ); + } + + public IPigeonIMU getPigeon() { + int id = config.infrastructure.pigeonId; + IPigeonIMU pigeon; + if (!isHardwareValid(id)) { + pigeon = new GhostPigeonIMU(id); + } else if (config.infrastructure.isPigeon2) { + System.out.println("Using Pigeon 2 for id: " + id); + pigeon = new Pigeon2Impl(id, config.infrastructure.canivoreBusName); + } else { + System.out.println("Using old Pigeon for id: " + id); + pigeon = new PigeonIMUImpl(id); + } + if (getConstant("resetFactoryDefaults") > 0) { + pigeon.configFactoryDefault(); + pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 100); + pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.BiasedStatus_6_Accel, 100); + } + return pigeon; + } + + public IProximitySensor getProximitySensor(String name) { + if (config.infrastructure.proximitySensors == null) { + return new GhostProximitySensor(); + } + int id = config.infrastructure.proximitySensors.getOrDefault(name, -1); + if (id < 0) { + System.out.println("Incorrect Name: Proximity sensor not found, using ghost!"); + return new GhostProximitySensor(); + } + System.out.println("Creating Proximity Sensor: " + name + " at port: " + id); + return new ProximitySensor(name, config.infrastructure.proximitySensors.get(name)); + } + + public int getPcmId() { + if (config.infrastructure == null && config.infrastructure.pcmId == null) return -1; + return config.infrastructure.pcmId; + } + + public boolean isPcmEnabled() { + return getPcmId() > -1; + } + + public boolean isCompressorEnabled() { + return config.infrastructure.compressorEnabled; + } + + private void reportGhostWarning( + String type, + String subsystemName, + String componentName + ) { + System.out.println( + " " + + type + + " \"" + + componentName + + "\" invalid in Yaml for subsystem \"" + + subsystemName + + "\", using ghost!" + ); + } + + private enum PIDConfig { + Azimuth, + Drive, + Generic, + } +} diff --git a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java index b4c828e4..77e00c07 100644 --- a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java +++ b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java @@ -1,177 +1,189 @@ -package com.team1816.lib.util.trajectoryUtil; -import com.team1816.season.auto.paths.*; -import com.team1816.lib.auto.paths.AutoPath; -import com.team1816.lib.auto.paths.*; -import com.fasterxml.jackson.core.type.TypeReference; -import com.fasterxml.jackson.databind.*; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import org.apache.commons.io.FileUtils; - -import java.io.*; -import java.util.ArrayList; -import java.util.List; - -/** - * Utility that efficiently pre-calculates trajectories to limit resource utilization. - * Generated Trajectories are located in src/resources/trajectories - * - * @see Trajectory - */ -public class TrajectoryCalculator { - - /** - * Root directory that trajectories are placed in - */ - private static final File directory = new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/"); - - /** - * List of all AutoPaths that are to be calculated - * @see AutoPath - */ - private static final List paths = List.of( - new DriveStraightPath(), - new LivingRoomPath() - ); - - /** - * Entry point of gradle task that calculates trajectories - * @param args - */ - public static void main(String[] args) { - deleteTrajectories(); - System.out.println("Calculating " + paths.size() + " Trajectories:"); - for (AutoPath path: paths) { - calcAllTrajectoryFormats(path); - System.out.println("\tCalculated " + path.getClass().getName()); - } - } - - /** - * Calculates all formats of the trajectories associated with an AutoPath {standard, reflected, rotated} - * @param path AutoPath - */ - public static void calcAllTrajectoryFormats(AutoPath path) { - if (!directory.exists()) { - directory.mkdir(); - } - String name = formatClassName(path.getClass().getName()); - - calcTrajectory(name, path.getWaypoints()); - calcTrajectory(name + "_Reflected", path.getReflectedWaypoints()); - calcTrajectory(name + "_Rotated", path.getRotatedWaypoints()); - - if (path.getWaypointHeadings() != null) { - calcHeadings(name + "Headings", path.getWaypoints(), path.getWaypointHeadings()); - calcHeadings(name + "Headings_Reflected", path.getReflectedWaypoints(), path.getReflectedWaypointHeadings()); - calcHeadings(name + "Headings_Rotated", path.getRotatedWaypoints(), path.getRotatedWaypointHeadings()); - } - } - - /** - * Formats the class name in regex by matching characters - * @param name class name - * @return name - */ - public static String formatClassName(String name) { - String[] parts = name.split("\\."); - return parts[parts.length - 1]; - } - - /** - * Calculates the Trajectory using the waypoints provided in the AutoPath and PathUtil - * @param name path name - * @param waypoints path waypoints - * @see AutoPath - * @see PathUtil - */ - public static void calcTrajectory(String name, List waypoints) { - if (waypoints == null) { - return; - } - var trajectory = PathUtil.generateTrajectory(name, waypoints, false); - ObjectMapper mapper = new ObjectMapper(); - try { - File file = new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"); - if (!file.exists()) { - file.createNewFile(); - } - mapper.writeValue(file, trajectory.getStates()); - } catch (Exception e) { - System.out.println("Error while writing JSON: " + e.getMessage()); - } - } - - /** - * Calculates the headings associated with the AutoPath - * @param name path name - * @param waypoints path waypoints - * @param headings path headings - */ - public static void calcHeadings(String name, List waypoints, List headings) { - if (headings == null) { - return; - } - - var trajectoryHeadings = PathUtil.generateHeadings(name, waypoints, headings, false); - ObjectMapper mapper = new ObjectMapper(); - try { - File file = new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"); - if (!file.exists()) { - file.createNewFile(); - } - mapper.writeValue(file, trajectoryHeadings); - } catch (Exception e) { - System.out.println("Error while writing JSON: " + e.getMessage()); - } - } - - /** - * Loads the Trajectory states with the associated JSON storing it - * @param name path name - * @return Trajectory - * @see Trajectory - */ - public static Trajectory loadTrajectory(String name) { - ObjectMapper mapper = new ObjectMapper(); - try { - List list = mapper.readValue( - new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"), - new TypeReference>() { } - ); - return new Trajectory(list); - } catch (Exception e) { - System.out.println("Error parsing path JSON: " + name + "\n" + e.getMessage()); - return new Trajectory(); - } - } - - /** - * Loads the Headings with the associated JSON storing it - */ - public static List loadTrajectoryHeadings(String name) { - ObjectMapper mapper = new ObjectMapper(); - try { - List list = mapper.readValue( - new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"), - new TypeReference>() { } - ); - return list; - } catch (Exception e) { - System.out.println("Error parsing path JSON: " + name + "\n" + e.getMessage()); - return new ArrayList<>(); - } - } - - /** - * Clears the json files storing trajectories and headings located in the resources directory - */ - public static void deleteTrajectories() { - try { - FileUtils.cleanDirectory(new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/")); - } catch (Exception e) { - System.out.println("Error deleting contents of trajectories directory" + e.getStackTrace()); - } - } -} +package com.team1816.lib.util.trajectoryUtil; +import com.team1816.season.auto.paths.*; +import com.team1816.lib.auto.paths.AutoPath; +import com.team1816.lib.auto.paths.*; +import com.fasterxml.jackson.core.type.TypeReference; +import com.fasterxml.jackson.databind.*; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.Trajectory; +import org.apache.commons.io.FileUtils; +import com.team1816.lib.hardware.factory.*; + +import java.io.*; +import java.util.ArrayList; +import java.util.List; + +/** + * Utility that efficiently pre-calculates trajectories to limit resource utilization. + * Generated Trajectories are located in src/resources/trajectories + * + * @see Trajectory + */ +public class TrajectoryCalculator { + + /** + * Root directory that trajectories are placed in + */ + private static final File directory = new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/"); + + /** + * List of all AutoPaths that are to be calculated + * @see AutoPath + */ + private static final List paths = List.of( + new DriveStraightPath(), + new LivingRoomPath() + ); + + /** + * Entry point of gradle task that calculates trajectories + * @param args + */ + public static void main(String[] args) { + deleteTrajectories(); + String[] robots = {"alpha", "zero", "zoffseason"}; + RobotFactory factory = new RobotFactory(); + for (String robot : robots) { + factory.loadConfig(robot); + PathUtil.setCalculateParams( + factory.getConstant("drivetrain", "maxVelPathFollowing"), + factory.getConstant("drivetrain", "maxAccel", 4) + ); + System.out.println("Calculating " + paths.size() + " Trajectories:"); + for (AutoPath path: paths) { + calcAllTrajectoryFormats(robot, path); + System.out.println("\tCalculated " + path.getClass().getName()); + } + } + } + + + /** + * Calculates all formats of the trajectories associated with an AutoPath {standard, reflected, rotated} + * @param path AutoPath + */ + public static void calcAllTrajectoryFormats(String robotName, AutoPath path) { + if (!directory.exists()) { + directory.mkdir(); + } + String name = robotName + "-" + formatClassName(path.getClass().getName()); + + calcTrajectory(name, path.getWaypoints()); + calcTrajectory(name + "_Reflected", path.getReflectedWaypoints()); + calcTrajectory(name + "_Rotated", path.getRotatedWaypoints()); + + if (path.getWaypointHeadings() != null) { + calcHeadings(name + "Headings", path.getWaypoints(), path.getWaypointHeadings()); + calcHeadings(name + "Headings_Reflected", path.getReflectedWaypoints(), path.getReflectedWaypointHeadings()); + calcHeadings(name + "Headings_Rotated", path.getRotatedWaypoints(), path.getRotatedWaypointHeadings()); + } + } + + /** + * Formats the class name in regex by matching characters + * @param name class name + * @return name + */ + public static String formatClassName(String name) { + String[] parts = name.split("\\."); + return parts[parts.length - 1]; + } + + /** + * Calculates the Trajectory using the waypoints provided in the AutoPath and PathUtil + * @param name path name + * @param waypoints path waypoints + * @see AutoPath + * @see PathUtil + */ + public static void calcTrajectory(String name, List waypoints) { + if (waypoints == null) { + return; + } + var trajectory = PathUtil.generateTrajectory(name, waypoints, false); + ObjectMapper mapper = new ObjectMapper(); + try { + File file = new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"); + if (!file.exists()) { + file.createNewFile(); + } + mapper.writeValue(file, trajectory.getStates()); + } catch (Exception e) { + System.out.println("Error while writing JSON: " + e.getMessage()); + } + } + + /** + * Calculates the headings associated with the AutoPath + * @param name path name + * @param waypoints path waypoints + * @param headings path headings + */ + public static void calcHeadings(String name, List waypoints, List headings) { + if (headings == null) { + return; + } + + var trajectoryHeadings = PathUtil.generateHeadings(name, waypoints, headings, false); + ObjectMapper mapper = new ObjectMapper(); + try { + File file = new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"); + if (!file.exists()) { + file.createNewFile(); + } + mapper.writeValue(file, trajectoryHeadings); + } catch (Exception e) { + System.out.println("Error while writing JSON: " + e.getMessage()); + } + } + + /** + * Loads the Trajectory states with the associated JSON storing it + * @param name path name + * @return Trajectory + * @see Trajectory + */ + public static Trajectory loadTrajectory(String name) { + name = System.getenv("ROBOT_NAME") + "-" + name; + ObjectMapper mapper = new ObjectMapper(); + try { + List list = mapper.readValue( + new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"), + new TypeReference>() { } + ); + return new Trajectory(list); + } catch (Exception e) { + System.out.println("Error parsing path JSON: " + name + "\n" + e.getMessage()); + return new Trajectory(); + } + } + + /** + * Loads the Headings with the associated JSON storing it + */ + public static List loadTrajectoryHeadings(String name) { + ObjectMapper mapper = new ObjectMapper(); + try { + List list = mapper.readValue( + new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"), + new TypeReference>() { } + ); + return list; + } catch (Exception e) { + System.out.println("Error parsing path JSON: " + name + "\n" + e.getMessage()); + return new ArrayList<>(); + } + } + + /** + * Clears the json files storing trajectories and headings located in the resources directory + */ + public static void deleteTrajectories() { + try { + FileUtils.cleanDirectory(new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/")); + } catch (Exception e) { + System.out.println("Error deleting contents of trajectories directory" + e.getStackTrace()); + } + } +} From bb62f4cbe0d4a353eaec03c3c1a573895e785951 Mon Sep 17 00:00:00 2001 From: miapirobotics <34904974+miapirobotics@users.noreply.github.com> Date: Mon, 6 Feb 2023 11:22:05 -0600 Subject: [PATCH 14/31] Move robot trajectories to subdirs --- .gitignore | 2 +- .../trajectoryUtil/TrajectoryCalculator.java | 38 +++++++++++-------- 2 files changed, 24 insertions(+), 16 deletions(-) diff --git a/.gitignore b/.gitignore index c848a24a..6fd7a8b4 100644 --- a/.gitignore +++ b/.gitignore @@ -6,4 +6,4 @@ simgui-window.json bin/ *.bag Compiler -src/main/resources/trajectories/*.json \ No newline at end of file +src/main/resources/trajectories/**/*.json \ No newline at end of file diff --git a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java index 77e00c07..3d46a0da 100644 --- a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java +++ b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java @@ -25,7 +25,7 @@ public class TrajectoryCalculator { /** * Root directory that trajectories are placed in */ - private static final File directory = new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/"); + private static final File mainDirectory = getFile("/src/main/resources/trajectories/"); /** * List of all AutoPaths that are to be calculated @@ -58,25 +58,33 @@ public static void main(String[] args) { } } + private static File getFile(String path) { + return new File(System.getProperty("user.dir") + path); + } /** * Calculates all formats of the trajectories associated with an AutoPath {standard, reflected, rotated} * @param path AutoPath */ public static void calcAllTrajectoryFormats(String robotName, AutoPath path) { + if (!mainDirectory.exists()) { + mainDirectory.mkdir(); + } + File directory = getFile("/src/main/resources/trajectories/" + robotName + "/"); if (!directory.exists()) { directory.mkdir(); } - String name = robotName + "-" + formatClassName(path.getClass().getName()); - calcTrajectory(name, path.getWaypoints()); - calcTrajectory(name + "_Reflected", path.getReflectedWaypoints()); - calcTrajectory(name + "_Rotated", path.getRotatedWaypoints()); + String name = formatClassName(path.getClass().getName()); + + calcTrajectory(robotName, name, path.getWaypoints()); + calcTrajectory(robotName, name + "_Reflected", path.getReflectedWaypoints()); + calcTrajectory(robotName, name + "_Rotated", path.getRotatedWaypoints()); if (path.getWaypointHeadings() != null) { - calcHeadings(name + "Headings", path.getWaypoints(), path.getWaypointHeadings()); - calcHeadings(name + "Headings_Reflected", path.getReflectedWaypoints(), path.getReflectedWaypointHeadings()); - calcHeadings(name + "Headings_Rotated", path.getRotatedWaypoints(), path.getRotatedWaypointHeadings()); + calcHeadings(robotName, name + "Headings", path.getWaypoints(), path.getWaypointHeadings()); + calcHeadings(robotName, name + "Headings_Reflected", path.getReflectedWaypoints(), path.getReflectedWaypointHeadings()); + calcHeadings(robotName, name + "Headings_Rotated", path.getRotatedWaypoints(), path.getRotatedWaypointHeadings()); } } @@ -97,14 +105,14 @@ public static String formatClassName(String name) { * @see AutoPath * @see PathUtil */ - public static void calcTrajectory(String name, List waypoints) { + public static void calcTrajectory(String robotName, String name, List waypoints) { if (waypoints == null) { return; } var trajectory = PathUtil.generateTrajectory(name, waypoints, false); ObjectMapper mapper = new ObjectMapper(); try { - File file = new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"); + File file = getFile("/src/main/resources/trajectories/" + robotName + "/" + name + ".json"); if (!file.exists()) { file.createNewFile(); } @@ -120,7 +128,7 @@ public static void calcTrajectory(String name, List waypoints) { * @param waypoints path waypoints * @param headings path headings */ - public static void calcHeadings(String name, List waypoints, List headings) { + public static void calcHeadings(String robotName, String name, List waypoints, List headings) { if (headings == null) { return; } @@ -128,7 +136,7 @@ public static void calcHeadings(String name, List waypoints, List waypoints, List list = mapper.readValue( - new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"), + getFile("/src/main/resources/trajectories/" + System.getenv("ROBOT_NAME") + "/" + name + ".json"), new TypeReference>() { } ); return new Trajectory(list); @@ -181,7 +188,8 @@ public static List loadTrajectoryHeadings(String name) { */ public static void deleteTrajectories() { try { - FileUtils.cleanDirectory(new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/")); + FileUtils.cleanDirectory(getFile("/src/main/resources/trajectories/")); + FileUtils.cleanDirectory(getFile("/bin/main/trajectories")); } catch (Exception e) { System.out.println("Error deleting contents of trajectories directory" + e.getStackTrace()); } From 90b4cd110be9aa5a4576304e24933d9c1a6cef66 Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Mon, 6 Feb 2023 12:40:53 -0600 Subject: [PATCH 15/31] added headings --- .../java/com/team1816/lib/subsystems/drive/SwerveDrive.java | 4 ++-- .../java/com/team1816/lib/subsystems/drive/TankDrive.java | 4 ++-- src/main/java/com/team1816/lib/subsystems/vision/Camera.java | 2 -- .../lib/util/trajectoryUtil/TrajectoryCalculator.java | 4 ++-- 4 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/team1816/lib/subsystems/drive/SwerveDrive.java b/src/main/java/com/team1816/lib/subsystems/drive/SwerveDrive.java index 0a99b614..ab0884b7 100644 --- a/src/main/java/com/team1816/lib/subsystems/drive/SwerveDrive.java +++ b/src/main/java/com/team1816/lib/subsystems/drive/SwerveDrive.java @@ -275,9 +275,9 @@ public void updateRobotState() { robotState.calculatedVehicleAccel = new ChassisSpeeds( (cs.vxMetersPerSecond - robotState.deltaVehicle.vxMetersPerSecond) / - Constants.kLooperDt, + (timestamp-prevTimestamp), (cs.vyMetersPerSecond - robotState.deltaVehicle.vyMetersPerSecond) / - Constants.kLooperDt, + (timestamp-prevTimestamp), -9.80 ); robotState.deltaVehicle = cs; diff --git a/src/main/java/com/team1816/lib/subsystems/drive/TankDrive.java b/src/main/java/com/team1816/lib/subsystems/drive/TankDrive.java index 8973e44c..39c81db9 100644 --- a/src/main/java/com/team1816/lib/subsystems/drive/TankDrive.java +++ b/src/main/java/com/team1816/lib/subsystems/drive/TankDrive.java @@ -265,9 +265,9 @@ public void updateRobotState() { robotState.calculatedVehicleAccel = new ChassisSpeeds( (cs.vxMetersPerSecond - robotState.deltaVehicle.vxMetersPerSecond) / - Constants.kLooperDt, + (timestamp-prevTimestamp), (cs.vyMetersPerSecond - robotState.deltaVehicle.vyMetersPerSecond) / - Constants.kLooperDt, + (timestamp-prevTimestamp), cs.omegaRadiansPerSecond - robotState.deltaVehicle.omegaRadiansPerSecond ); robotState.deltaVehicle = cs; diff --git a/src/main/java/com/team1816/lib/subsystems/vision/Camera.java b/src/main/java/com/team1816/lib/subsystems/vision/Camera.java index f2c17526..8ab7eb98 100644 --- a/src/main/java/com/team1816/lib/subsystems/vision/Camera.java +++ b/src/main/java/com/team1816/lib/subsystems/vision/Camera.java @@ -157,8 +157,6 @@ public ArrayList getPoints() { p.id = bestTarget.getFiducialId(); p.cameraToTarget = bestTarget.getBestCameraToTarget(); // missing method in PhotonTrackedTarget targets.add(p); - } else { - System.out.println("camera not returning points b/c camera not implemented"); } return targets; } diff --git a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java index 3d46a0da..7b067c96 100644 --- a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java +++ b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java @@ -42,7 +42,7 @@ public class TrajectoryCalculator { */ public static void main(String[] args) { deleteTrajectories(); - String[] robots = {"alpha", "zero", "zoffseason"}; + String[] robots = {"alpha", "zero", "zoffseason", "zodiac_pro"}; RobotFactory factory = new RobotFactory(); for (String robot : robots) { factory.loadConfig(robot); @@ -173,7 +173,7 @@ public static List loadTrajectoryHeadings(String name) { ObjectMapper mapper = new ObjectMapper(); try { List list = mapper.readValue( - new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + name + ".json"), + new File(System.getProperty("user.dir") + "/src/main/resources/trajectories/" + System.getenv("ROBOT_NAME") + "/" + name + ".json"), new TypeReference>() { } ); return list; From b831427f8e02bf3a8ae8fedba112d3e49cdd74e9 Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Mon, 6 Feb 2023 12:44:42 -0600 Subject: [PATCH 16/31] added redundancy --- .../java/com/team1816/lib/auto/paths/PathUtil.java | 10 ++++++++-- .../lib/util/trajectoryUtil/TrajectoryCalculator.java | 2 +- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java index 994dcc62..784e84da 100644 --- a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java +++ b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java @@ -58,7 +58,10 @@ public static Trajectory generateTrajectory( ) { pathName = TrajectoryCalculator.formatClassName(pathName); if (loadTrajectories) { - return TrajectoryCalculator.loadTrajectory(pathName); + var trajectory = TrajectoryCalculator.loadTrajectory(pathName); + if (!trajectory.equals(new Trajectory())) { + return trajectory; + } } /* Inch to meter conversions for waypoints for trajectory calculations */ List waypointsMeters = new ArrayList<>(); @@ -131,7 +134,10 @@ public static List generateHeadings( ) { name = TrajectoryCalculator.formatClassName(name); if (loadTrajectories) { - return TrajectoryCalculator.loadTrajectoryHeadings(name); + var trajectoryHeadings = TrajectoryCalculator.loadTrajectoryHeadings(name); + if (trajectoryHeadings.size() > 0) { + return trajectoryHeadings; + } } if (waypoints == null || swerveHeadings == null) { return null; diff --git a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java index 7b067c96..6602d769 100644 --- a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java +++ b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java @@ -42,7 +42,7 @@ public class TrajectoryCalculator { */ public static void main(String[] args) { deleteTrajectories(); - String[] robots = {"alpha", "zero", "zoffseason", "zodiac_pro"}; + String[] robots = {"alpha", "zero", "zoffseason"}; RobotFactory factory = new RobotFactory(); for (String robot : robots) { factory.loadConfig(robot); From 0430e1f9515240f23c453ebb9d2cee00b641a3e6 Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Mon, 6 Feb 2023 12:47:32 -0600 Subject: [PATCH 17/31] renaming --- .../java/com/team1816/lib/auto/paths/PathUtil.java | 12 +++++------- .../util/trajectoryUtil/TrajectoryCalculator.java | 2 +- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java index 784e84da..d67e1f3f 100644 --- a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java +++ b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java @@ -1,10 +1,8 @@ package com.team1816.lib.auto.paths; import com.team1816.lib.util.trajectoryUtil.TrajectoryCalculator; -import com.team1816.season.configuration.Constants; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; @@ -21,11 +19,11 @@ public class PathUtil { * Constraints */ private static double kMaxVelocity = kPathFollowingMaxVelMeters; - private static double kMaxAccel = kPathFollowingMaxAccelMeters; + private static double kMaxAcceleration = kPathFollowingMaxAccelMeters; - public static void setCalculateParams(double kMaxVel, double kMaxAc) { + public static void setCalculationParameters(double kMaxVel, double kMaxAccel) { kMaxVelocity = kMaxVel; - kMaxAccel = kMaxAc; + kMaxAcceleration = kMaxAccel; } /** * Generates a trajectory when TrajectoryCalculator is not used @@ -75,7 +73,7 @@ public static Trajectory generateTrajectory( ); } /* Configures trajectory constraints */ - TrajectoryConfig config = new TrajectoryConfig(kMaxVelocity, kMaxAccel); + TrajectoryConfig config = new TrajectoryConfig(kMaxVelocity, kMaxAcceleration); var baseTrajectory = edu.wpi.first.math.trajectory.TrajectoryGenerator.generateTrajectory( waypointsMeters, config @@ -109,7 +107,7 @@ public static Trajectory generateTrajectory( ); } /* Configures trajectory constraints */ - TrajectoryConfig config = new TrajectoryConfig(kMaxVelocity, kMaxAccel); + TrajectoryConfig config = new TrajectoryConfig(kMaxVelocity, kMaxAcceleration); config.setStartVelocity(initial.vxMetersPerSecond); config.setEndVelocity(0); var baseTrajectory = edu.wpi.first.math.trajectory.TrajectoryGenerator.generateTrajectory( diff --git a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java index 6602d769..0af58292 100644 --- a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java +++ b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java @@ -46,7 +46,7 @@ public static void main(String[] args) { RobotFactory factory = new RobotFactory(); for (String robot : robots) { factory.loadConfig(robot); - PathUtil.setCalculateParams( + PathUtil.setCalculationParameters( factory.getConstant("drivetrain", "maxVelPathFollowing"), factory.getConstant("drivetrain", "maxAccel", 4) ); From 08d14dbbb658894eeea814743faa04e0a26b87a4 Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Mon, 6 Feb 2023 18:26:47 -0600 Subject: [PATCH 18/31] fix looper --- src/main/java/com/team1816/lib/loops/Looper.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/team1816/lib/loops/Looper.java b/src/main/java/com/team1816/lib/loops/Looper.java index aa16616a..25d53e46 100644 --- a/src/main/java/com/team1816/lib/loops/Looper.java +++ b/src/main/java/com/team1816/lib/loops/Looper.java @@ -2,6 +2,7 @@ import com.team1816.lib.subsystems.SubsystemLooper; import com.team1816.season.Robot; +import com.team1816.season.configuration.Constants; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; @@ -51,7 +52,7 @@ public void run() { } }; - robot.addPeriodic(runnable_, 0.035, 0.005); // offsets periodic loop + robot.addPeriodic(runnable_, Constants.kLooperDt, 0.005); // offsets periodic loop mRunning = false; mLoops = new ArrayList<>(); } From a359db78de02fa374c8ffe091de452874ee077eb Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Mon, 6 Feb 2023 20:34:12 -0600 Subject: [PATCH 19/31] temp --- .../team1816/season/auto/paths/DriveStraightPath.java | 2 +- src/main/resources/yaml/zoffseason.config.yml | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java b/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java index 917152d6..0a5bc17b 100644 --- a/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java +++ b/src/main/java/com/team1816/season/auto/paths/DriveStraightPath.java @@ -22,7 +22,7 @@ public DriveStraightPath(int driveDistance) { } public DriveStraightPath() { - this(12); + this(7); } @Override diff --git a/src/main/resources/yaml/zoffseason.config.yml b/src/main/resources/yaml/zoffseason.config.yml index 38721c92..45c07f92 100644 --- a/src/main/resources/yaml/zoffseason.config.yml +++ b/src/main/resources/yaml/zoffseason.config.yml @@ -39,7 +39,7 @@ subsystems: drive: RLDr azimuth: RLAz constants: - encoderOffset: 2396 + encoderOffset: 539 backRight: drive: RRDr azimuth: RRAz @@ -63,16 +63,16 @@ subsystems: maxVelTicks100ms: 12275.7 # ticks per 100ms openLoopRampRate: 0.8 isSwerve: 1 - encPPR: 13260.36 #Use for distance calibration + encPPR: 12855.51 #Use for distance calibration azimuthEncPPR: 4096 maxRotVel: 14 trackWidth: 21 #inches wheelbaseLength: 21.5 #inches wheelDiameter: 4 kTrackScrubFactor: 1.0 - maxVelOpenLoop: 3 # meters/s - maxVelPathFollowing: 2.5 # meters/s (2.2) - maxAccel: 1.27 # meters/s^ 2 + maxVelOpenLoop: 1 # meters/s + maxVelPathFollowing: .5 # meters/s (2.2) + maxAccel: .5 # meters/s^ 2 isDemoMode: 0 autoBalanceDivider: 30 pitchRollMaxFlat: 2.5 From 8e87ec69eacd3d03f2d83f6742276f4104f00b3c Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Mon, 6 Feb 2023 20:42:44 -0600 Subject: [PATCH 20/31] temp, zoffseason looking good?? --- src/main/resources/yaml/zoffseason.config.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/resources/yaml/zoffseason.config.yml b/src/main/resources/yaml/zoffseason.config.yml index 45c07f92..604216ed 100644 --- a/src/main/resources/yaml/zoffseason.config.yml +++ b/src/main/resources/yaml/zoffseason.config.yml @@ -71,7 +71,7 @@ subsystems: wheelDiameter: 4 kTrackScrubFactor: 1.0 maxVelOpenLoop: 1 # meters/s - maxVelPathFollowing: .5 # meters/s (2.2) + maxVelPathFollowing: 1 # meters/s (2.2) maxAccel: .5 # meters/s^ 2 isDemoMode: 0 autoBalanceDivider: 30 From f5e7d8b40244853c6aa999895105c3055836ceaa Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Tue, 7 Feb 2023 20:37:17 -0600 Subject: [PATCH 21/31] fix conversions and swerve module demand and actual --- .../lib/subsystems/drive/ISwerveModule.java | 2 -- .../lib/subsystems/drive/SwerveModule.java | 29 +++++-------------- .../lib/subsystems/drive/TankDrive.java | 4 +-- .../lib/util/driveUtil/DriveConversions.java | 8 ++--- 4 files changed, 14 insertions(+), 29 deletions(-) diff --git a/src/main/java/com/team1816/lib/subsystems/drive/ISwerveModule.java b/src/main/java/com/team1816/lib/subsystems/drive/ISwerveModule.java index 24030d2f..64b50369 100644 --- a/src/main/java/com/team1816/lib/subsystems/drive/ISwerveModule.java +++ b/src/main/java/com/team1816/lib/subsystems/drive/ISwerveModule.java @@ -18,8 +18,6 @@ public interface ISwerveModule { double getDesiredDrive(); - double getDrivePosition(); - double getDriveError(); // Temperature C diff --git a/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java b/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java index 0bf27f85..205fb79b 100644 --- a/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java +++ b/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java @@ -31,11 +31,11 @@ public class SwerveModule implements ISwerveModule { */ public SwerveModuleState moduleState; public SwerveModulePosition modulePosition; - public double driveDemand; - public double driveActual; - public double drivePosition; - public double azimuthDemand; - public double azimuthActual; + public double driveDemand; // ticks + public double driveActual; // m/s + public double driveDistance; // m + public double azimuthDemand; // ticks + public double azimuthActual; // degrees public double motorTemp; // Drive Motor Temperature /** @@ -105,7 +105,6 @@ public SwerveModule( ); allowableError = 5; // TODO this is a dummy value for checkSystem - drivePosition = 0; moduleState = new SwerveModuleState(); modulePosition = new SwerveModulePosition(); @@ -148,19 +147,17 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) * @see this#getActualPosition() */ public void update() { - driveActual = - DriveConversions.ticksToMeters(driveMotor.getSelectedSensorVelocity(0)) * 10; + driveActual = driveMotor.getSelectedSensorVelocity(0); azimuthActual = DriveConversions.convertTicksToDegrees( azimuthMotor.getSelectedSensorPosition(0) - mModuleConfig.azimuthEncoderHomeOffset ); - moduleState.speedMetersPerSecond = driveActual; + moduleState.speedMetersPerSecond = DriveConversions.ticksPer100msToMetersPerSecond(driveActual); moduleState.angle = Rotation2d.fromDegrees(azimuthActual); - drivePosition += driveActual * (timestamp-prevTimestamp); - modulePosition.distanceMeters = drivePosition; + modulePosition.distanceMeters += moduleState.speedMetersPerSecond * (timestamp-prevTimestamp); modulePosition.angle = Rotation2d.fromDegrees(azimuthActual); motorTemp = driveMotor.getTemperature(); // Celsius @@ -256,16 +253,6 @@ public double getActualDrive() { return driveActual; } - /** - * Returns the "position" of the Drive motor - * - * @return drivePosition - */ - @Override - public double getDrivePosition() { - return drivePosition; - } - /** * Returns the closed loop error of the Drive motor (in-built) * diff --git a/src/main/java/com/team1816/lib/subsystems/drive/TankDrive.java b/src/main/java/com/team1816/lib/subsystems/drive/TankDrive.java index 39c81db9..e411d869 100644 --- a/src/main/java/com/team1816/lib/subsystems/drive/TankDrive.java +++ b/src/main/java/com/team1816/lib/subsystems/drive/TankDrive.java @@ -399,7 +399,7 @@ public synchronized void setBraking(boolean braking) { * @see com.team1816.lib.util.driveUtil.DriveConversions#ticksPer100MSToMPS(double) */ public double getLeftMPSActual() { - return ticksPer100MSToMPS(leftActualVelocity); + return ticksPer100msToMetersPerSecond(leftActualVelocity); } /** @@ -409,7 +409,7 @@ public double getLeftMPSActual() { * @see com.team1816.lib.util.driveUtil.DriveConversions#ticksPer100MSToMPS(double) */ public double getRightMPSActual() { - return ticksPer100MSToMPS(rightActualVelocity); + return ticksPer100msToMetersPerSecond(rightActualVelocity); } /** diff --git a/src/main/java/com/team1816/lib/util/driveUtil/DriveConversions.java b/src/main/java/com/team1816/lib/util/driveUtil/DriveConversions.java index 29ac40ec..c91aabeb 100644 --- a/src/main/java/com/team1816/lib/util/driveUtil/DriveConversions.java +++ b/src/main/java/com/team1816/lib/util/driveUtil/DriveConversions.java @@ -64,16 +64,16 @@ public static double metersPerSecondToTicksPer100ms(double meters_per_second) { return inchesPerSecondToTicksPer100ms(Units.metersToInches(meters_per_second)); } - public static double ticksPerSecondToMetersPer100ms(double ticks_per_second) { - return ((Units.inchesToMeters(ticksPerSecondToInchesPer100ms(ticks_per_second)))); + public static double ticksPer100msToMetersPerSecond(double ticks_per_second) { + return ((Units.inchesToMeters(ticksPer100msToInchesPerSecond(ticks_per_second)))); } public static double inchesPerSecondToTicksPer100ms(double inches_per_second) { return inchesToRotations(inches_per_second) * drivePPR / 10.0; } - public static double ticksPerSecondToInchesPer100ms(double ticks_per_second) { - return rotationsToInches(ticks_per_second / drivePPR) / 10.0; + public static double ticksPer100msToInchesPerSecond(double ticks_per_100ms) { + return rotationsToInches(ticks_per_100ms / drivePPR) * 10.0; } public static double ticksPer100MSToMPS(double ticksPer100MS) { // ticks/100ms to meters / second From b41985e0e7d9c555bdfa744626cc54a0ff5a03b6 Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Tue, 7 Feb 2023 20:49:55 -0600 Subject: [PATCH 22/31] no methods with inches --- .../lib/util/driveUtil/DriveConversions.java | 30 ++++--------------- .../lib/util/team254/SwerveDriveSignal.java | 14 ++------- 2 files changed, 7 insertions(+), 37 deletions(-) diff --git a/src/main/java/com/team1816/lib/util/driveUtil/DriveConversions.java b/src/main/java/com/team1816/lib/util/driveUtil/DriveConversions.java index c91aabeb..0344fa64 100644 --- a/src/main/java/com/team1816/lib/util/driveUtil/DriveConversions.java +++ b/src/main/java/com/team1816/lib/util/driveUtil/DriveConversions.java @@ -20,12 +20,8 @@ public static double ticksToMeters(double ticks) { return ticks / drivePPR * kWheelCircumferenceMeters; } - public static double inchesToTicks(double inches) { - return inches / Math.PI * azimuthPPR / kWheelCircumferenceInches; - } - public static double metersToTicks(double meters) { - return inchesToTicks(Units.metersToInches(meters)); + return meters / Math.PI * azimuthPPR / kWheelCircumferenceMeters; } public static double convertTicksToRadians(double ticks) { @@ -44,36 +40,20 @@ public static double convertDegreesToTicks(double degrees) { return convertRadiansToTicks(Units.degreesToRadians(degrees)); } - public static double rotationsToInches(double rotations) { - return rotations * (kWheelCircumferenceInches); - } - public static double rotationsToMeters(double rotations) { return rotations * (kWheelCircumferenceMeters); } - public static double rpmToInchesPerSecond(double rpm) { - return rotationsToInches(rpm) / 60; - } - - public static double inchesToRotations(double inches) { - return inches / (kDriveWheelDiameterInches * Math.PI); + public static double metersToRotations(double meters) { + return meters / kWheelCircumferenceMeters; } public static double metersPerSecondToTicksPer100ms(double meters_per_second) { - return inchesPerSecondToTicksPer100ms(Units.metersToInches(meters_per_second)); + return metersToRotations(meters_per_second) * drivePPR / 10.0; } public static double ticksPer100msToMetersPerSecond(double ticks_per_second) { - return ((Units.inchesToMeters(ticksPer100msToInchesPerSecond(ticks_per_second)))); - } - - public static double inchesPerSecondToTicksPer100ms(double inches_per_second) { - return inchesToRotations(inches_per_second) * drivePPR / 10.0; - } - - public static double ticksPer100msToInchesPerSecond(double ticks_per_100ms) { - return rotationsToInches(ticks_per_100ms / drivePPR) * 10.0; + return rotationsToMeters(ticks_per_second / drivePPR) * 10.0; } public static double ticksPer100MSToMPS(double ticksPer100MS) { // ticks/100ms to meters / second diff --git a/src/main/java/com/team1816/lib/util/team254/SwerveDriveSignal.java b/src/main/java/com/team1816/lib/util/team254/SwerveDriveSignal.java index e6d22bf8..c9495fc8 100644 --- a/src/main/java/com/team1816/lib/util/team254/SwerveDriveSignal.java +++ b/src/main/java/com/team1816/lib/util/team254/SwerveDriveSignal.java @@ -1,6 +1,7 @@ package com.team1816.lib.util.team254; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import java.text.DecimalFormat; import java.util.Arrays; @@ -8,7 +9,7 @@ import static com.team1816.lib.subsystems.drive.Drive.kOpenLoopMaxVelMeters; import static com.team1816.lib.subsystems.drive.SwerveDrive.kFrontLeft; import static com.team1816.lib.subsystems.drive.SwerveDrive.kFrontRight; -import static com.team1816.lib.util.driveUtil.DriveConversions.inchesPerSecondToTicksPer100ms; +import static com.team1816.lib.util.driveUtil.DriveConversions.metersPerSecondToTicksPer100ms; /** * A drivetrain signal containing the speed and azimuth for each wheel @@ -98,17 +99,6 @@ public double[] getWheelSpeeds() { return mWheelSpeeds; } - public SwerveDriveSignal toVelocity() { - return new SwerveDriveSignal( - Arrays - .stream(this.mWheelSpeeds) - .map(x -> x * inchesPerSecondToTicksPer100ms(kOpenLoopMaxVelMeters)) - .toArray(), - this.mWheelAzimuths, - this.mBrakeMode - ); - } - /** * Returns the wheel azimuth rotations * From 6ffe58c6a8291f30af42179e166092f23ec7ebe4 Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Tue, 7 Feb 2023 23:28:55 -0600 Subject: [PATCH 23/31] near complete erradication of the inch, cleaned up swerve modules --- .../team1816/lib/auto/actions/TrajectoryAction.java | 2 +- .../lib/auto/actions/WaitUntilInsideRegion.java | 4 ++-- .../com/team1816/lib/subsystems/drive/Drive.java | 13 ++----------- .../lib/util/driveUtil/SwerveKinematics.java | 4 ++-- .../team1816/season/configuration/Constants.java | 2 +- .../java/com/team1816/season/states/RobotState.java | 2 +- 6 files changed, 9 insertions(+), 18 deletions(-) diff --git a/src/main/java/com/team1816/lib/auto/actions/TrajectoryAction.java b/src/main/java/com/team1816/lib/auto/actions/TrajectoryAction.java index ad29300c..dd2a4e84 100644 --- a/src/main/java/com/team1816/lib/auto/actions/TrajectoryAction.java +++ b/src/main/java/com/team1816/lib/auto/actions/TrajectoryAction.java @@ -88,7 +88,7 @@ public TrajectoryAction(Trajectory trajectory, List headings) { drive::getPose, new RamseteController(), //defaults of new DifferentialDriveKinematics( - Units.inchesToMeters(kDriveWheelTrackWidthInches) + kDriveWheelTrackWidthMeters ), ((TankDrive) drive)::updateTrajectoryVelocities ); diff --git a/src/main/java/com/team1816/lib/auto/actions/WaitUntilInsideRegion.java b/src/main/java/com/team1816/lib/auto/actions/WaitUntilInsideRegion.java index 753ea380..8bfdea8d 100644 --- a/src/main/java/com/team1816/lib/auto/actions/WaitUntilInsideRegion.java +++ b/src/main/java/com/team1816/lib/auto/actions/WaitUntilInsideRegion.java @@ -80,8 +80,8 @@ public void update() { @Override public boolean isFinished() { Pose2d position = mRobotState.fieldToVehicle; - var x = Units.metersToInches(position.getX()); - var y = Units.metersToInches(position.getY()); + var x = position.getX(); + var y = position.getY(); return ( x > mBottomLeft.getX() && x < mTopRight.getX() && diff --git a/src/main/java/com/team1816/lib/subsystems/drive/Drive.java b/src/main/java/com/team1816/lib/subsystems/drive/Drive.java index da89dcdf..24195712 100644 --- a/src/main/java/com/team1816/lib/subsystems/drive/Drive.java +++ b/src/main/java/com/team1816/lib/subsystems/drive/Drive.java @@ -114,9 +114,6 @@ public interface Factory { NAME, "wheelDiameter" ); - public static final double kWheelCircumferenceInches = - kDriveWheelDiameterInches * Math.PI; - public static final double kDriveWheelRadiusInches = kDriveWheelDiameterInches / 2.0; public static final double kDriveWheelTrackWidthMeters = Units.inchesToMeters( kDriveWheelTrackWidthInches @@ -124,15 +121,9 @@ public interface Factory { public static final double kDriveWheelbaseLengthMeters = Units.inchesToMeters( kDriveWheelbaseLengthInches ); - public static final double kDriveWheelDiameterMeters = Units.inchesToMeters( - kDriveWheelDiameterInches - ); public static final double kWheelCircumferenceMeters = Units.inchesToMeters( - kWheelCircumferenceInches - ); - public static final double kDriveWheelRadiusMeters = Units.inchesToMeters( - kDriveWheelRadiusInches - ); + kDriveWheelDiameterInches + ) * Math.PI; public static double kTrackScrubFactor = factory.getConstant( NAME, "kTrackScrubFactor" diff --git a/src/main/java/com/team1816/lib/util/driveUtil/SwerveKinematics.java b/src/main/java/com/team1816/lib/util/driveUtil/SwerveKinematics.java index 5753e5d5..8739c87c 100644 --- a/src/main/java/com/team1816/lib/util/driveUtil/SwerveKinematics.java +++ b/src/main/java/com/team1816/lib/util/driveUtil/SwerveKinematics.java @@ -37,8 +37,8 @@ private static List updateRotationDirections() { return directions; } - private static final double L = kDriveWheelTrackWidthInches; - private static final double W = kDriveWheelbaseLengthInches; // Intentional + private static final double L = Drive.kDriveWheelTrackWidthMeters; + private static final double W = kDriveWheelbaseLengthInches; private static final double R = Math.hypot(L, W); private static Rotation2d[] prev_wheel_azimuths = SwerveDriveSignal.ZERO_AZIMUTH; diff --git a/src/main/java/com/team1816/season/configuration/Constants.java b/src/main/java/com/team1816/season/configuration/Constants.java index d6d8a478..c50afe58 100644 --- a/src/main/java/com/team1816/season/configuration/Constants.java +++ b/src/main/java/com/team1816/season/configuration/Constants.java @@ -76,7 +76,7 @@ public class Constants { * Camera characterization */ public static final double kCameraMountingAngleY = 20; // degrees - public static final double kTurretZedRadius = Units.inchesToMeters(7); // meters + public static final double kTurretZedRadius = 0.2; // meters /** * Badlog characterization diff --git a/src/main/java/com/team1816/season/states/RobotState.java b/src/main/java/com/team1816/season/states/RobotState.java index 151e4107..3082c844 100644 --- a/src/main/java/com/team1816/season/states/RobotState.java +++ b/src/main/java/com/team1816/season/states/RobotState.java @@ -98,7 +98,7 @@ public synchronized void resetAllStates() { visibleTargets.clear(); drivetrainTemp = 0; vehicleToFloorProximityCentimeters = 0; - target = new Pose2d(5.0, Constants.fieldCenterY, new Rotation2d()); + target = Constants.fieldCenterPose; } /** From 44d3ceb2c80135b318dff2baa0b285769d5d17df Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Wed, 8 Feb 2023 00:30:22 -0600 Subject: [PATCH 24/31] badlog methods --- .../subsystems/drive/DrivetrainLogger.java | 16 +++++++++++- .../lib/subsystems/drive/ISwerveModule.java | 4 +++ .../lib/subsystems/drive/SwerveModule.java | 26 ++++++++++++++----- 3 files changed, 39 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/team1816/lib/subsystems/drive/DrivetrainLogger.java b/src/main/java/com/team1816/lib/subsystems/drive/DrivetrainLogger.java index e5f39f75..4aeee08f 100644 --- a/src/main/java/com/team1816/lib/subsystems/drive/DrivetrainLogger.java +++ b/src/main/java/com/team1816/lib/subsystems/drive/DrivetrainLogger.java @@ -13,7 +13,7 @@ public static void init(TrackableDrivetrain drivetrain) { if (isSwerve) { for ( int i = 0; - i < 1; //((SwerveDrivetrain) drivetrain).getSwerveModules().length; + i < ((SwerveDrivetrain) drivetrain).getSwerveModules().length; // 1; i++ ) { var module = ((SwerveDrivetrain) drivetrain).getSwerveModules()[i]; @@ -41,6 +41,20 @@ public static void init(TrackableDrivetrain drivetrain) { "hide", "join:Drivetrain/AzimuthError" ); + subsystem.createBadLogTopic( + prefix + "NormalizedAzimuthDemand", + "ticks", + module::getDesiredNormalizedAzimuth, + "hide", + "join:Drivetrain/NormalizedAzimuthDemand" + ); + subsystem.createBadLogTopic( + prefix + "NormalizedAzimuthPosition", + "ticks", + module::getActualNormalizedAzimuth, + "hide", + "join:Drivetrain/NormalizedAzimuthPosition" + ); // Drive subsystem.createBadLogTopic( diff --git a/src/main/java/com/team1816/lib/subsystems/drive/ISwerveModule.java b/src/main/java/com/team1816/lib/subsystems/drive/ISwerveModule.java index 64b50369..5e1b78e1 100644 --- a/src/main/java/com/team1816/lib/subsystems/drive/ISwerveModule.java +++ b/src/main/java/com/team1816/lib/subsystems/drive/ISwerveModule.java @@ -13,6 +13,10 @@ public interface ISwerveModule { double getDesiredAzimuth(); + double getDesiredNormalizedAzimuth(); + + double getActualNormalizedAzimuth(); + // velocity ticks/100ms double getActualDrive(); diff --git a/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java b/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java index 205fb79b..988ef2a0 100644 --- a/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java +++ b/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java @@ -148,14 +148,10 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) */ public void update() { driveActual = driveMotor.getSelectedSensorVelocity(0); - azimuthActual = - DriveConversions.convertTicksToDegrees( - azimuthMotor.getSelectedSensorPosition(0) - - mModuleConfig.azimuthEncoderHomeOffset - ); + azimuthActual = azimuthMotor.getSelectedSensorPosition(0); moduleState.speedMetersPerSecond = DriveConversions.ticksPer100msToMetersPerSecond(driveActual); - moduleState.angle = Rotation2d.fromDegrees(azimuthActual); + moduleState.angle = Rotation2d.fromDegrees(DriveConversions.convertTicksToDegrees(azimuthActual - mModuleConfig.azimuthEncoderHomeOffset)); modulePosition.distanceMeters += moduleState.speedMetersPerSecond * (timestamp-prevTimestamp); modulePosition.angle = Rotation2d.fromDegrees(azimuthActual); @@ -263,6 +259,24 @@ public double getDriveError() { return driveMotor.getClosedLoopError(0); } + /** + * Returns the normalized azimuth demand in ticks with respect to the zero offset + * Used for logging only + */ + @Override + public double getDesiredNormalizedAzimuth() { + return azimuthDemand - mModuleConfig.azimuthEncoderHomeOffset; + } + + /** + * Returns the normalized azimuth position in ticks + * Used for logging only + */ + @Override + public double getActualNormalizedAzimuth() { + return azimuthActual - mModuleConfig.azimuthEncoderHomeOffset; + } + /** * If there is no attached absolute CANCoder then this will "zero" / configure the Azimuth sensor on the motor to * its initial position From fe62fb8e68b072a3a23a3d2c1b057a53571868ec Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Wed, 8 Feb 2023 00:33:06 -0600 Subject: [PATCH 25/31] fixed minor errors --- .../java/com/team1816/lib/subsystems/drive/SwerveModule.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java b/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java index 988ef2a0..e5a153fa 100644 --- a/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java +++ b/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java @@ -154,7 +154,7 @@ public void update() { moduleState.angle = Rotation2d.fromDegrees(DriveConversions.convertTicksToDegrees(azimuthActual - mModuleConfig.azimuthEncoderHomeOffset)); modulePosition.distanceMeters += moduleState.speedMetersPerSecond * (timestamp-prevTimestamp); - modulePosition.angle = Rotation2d.fromDegrees(azimuthActual); + modulePosition.angle = Rotation2d.fromDegrees(DriveConversions.convertTicksToDegrees(azimuthActual - mModuleConfig.azimuthEncoderHomeOffset)); motorTemp = driveMotor.getTemperature(); // Celsius } From 2bf0a2b091aa500d057ef516b64960974f3bcc7c Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Wed, 8 Feb 2023 20:02:16 -0600 Subject: [PATCH 26/31] update wpilib --- build.gradle | 2 +- vendordeps/WPILibNewCommands.json | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/build.gradle b/build.gradle index 9fe725a9..11301da8 100644 --- a/build.gradle +++ b/build.gradle @@ -3,7 +3,7 @@ import org.gradle.nativeplatform.platform.internal.DefaultNativePlatform plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.3.1" + id "edu.wpi.first.GradleRIO" version "2023.3.2" id 'org.jsonschema2dataclass' version '4.0.1' } diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index f431e550..551fc882 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -1,7 +1,7 @@ { "fileName": "WPILibNewCommands.json", "name": "WPILib-New-Commands", - "version": "2023.3.1", + "version": "2023.3.2", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", "mavenUrls": [], "jsonUrl": "", From 03890ff94cbc2146bb6f88e96d42cbcc1d1fd9a6 Mon Sep 17 00:00:00 2001 From: miapirobotics <34904974+miapirobotics@users.noreply.github.com> Date: Thu, 9 Feb 2023 15:49:32 -0600 Subject: [PATCH 27/31] Gradle: Deploy a file to the RIO with relevant build/git info --- .gitignore | 2 +- build.gradle | 15 ++ src/main/resources/yaml/zoffseason.config.yml | 210 +++++++++--------- 3 files changed, 121 insertions(+), 106 deletions(-) diff --git a/.gitignore b/.gitignore index 6fd7a8b4..58e347bb 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,6 @@ build .gradle simgui-window.json bin/ -*.bag +*.bag* Compiler src/main/resources/trajectories/**/*.json \ No newline at end of file diff --git a/build.gradle b/build.gradle index 9fe725a9..f8c355c3 100644 --- a/build.gradle +++ b/build.gradle @@ -222,6 +222,20 @@ processResources { } } +task deployInfo(type: Exec) { + Runtime r = Runtime.getRuntime(); + PrintWriter writer = new PrintWriter("deploy.tmp", "UTF-8"); + writer.println(new Scanner(r.exec("git symbolic-ref --short HEAD").getInputStream()).nextLine()); + writer.println(new Scanner(r.exec("git rev-parse HEAD").getInputStream()).nextLine()); + writer.println(new Date().toString()); + writer.close(); + commandLine "scp", "./deploy.tmp", "lvuser@10.18.16.2:/home/lvuser/version.info"; + + doLast { + commandLine "rm", "deploy.tmp"; + } +} + wrapper { gradleVersion = '7.2' } @@ -238,4 +252,5 @@ compileJava { task fullDeploy() { dependsOn tasks.named("calculateTrajectories") dependsOn 'deploy' + dependsOn 'deployInfo' } \ No newline at end of file diff --git a/src/main/resources/yaml/zoffseason.config.yml b/src/main/resources/yaml/zoffseason.config.yml index 604216ed..34fcaaf4 100644 --- a/src/main/resources/yaml/zoffseason.config.yml +++ b/src/main/resources/yaml/zoffseason.config.yml @@ -1,106 +1,106 @@ -subsystems: - drivetrain: - implemented: true - talons: - FLAz: 2 - FRAz: 12 - RLAz: 10 - RRAz: 15 - falcons: - FLDr: 3 - FRDr: 13 - RLDr: 1 - RRDr: 14 - invertMotor: - - FRDr - - RRDr - - FLAz - - FRAz - - RLAz - - RRAz - invertSensorPhase: - - FLAz - - FRAz - - RLAz - - RRAz - swerveModules: - modules: - frontLeft: - drive: FLDr - azimuth: FLAz - constants: - encoderOffset: 2445 - frontRight: - drive: FRDr - azimuth: FRAz - constants: - encoderOffset: 1479 - backLeft: - drive: RLDr - azimuth: RLAz - constants: - encoderOffset: 539 - backRight: - drive: RRDr - azimuth: RRAz - constants: - encoderOffset: 224 - drivePID: - slot0: - kP: .11 - kI: 0.0001 - kD: 4.0 - kF: 0.048077 - allowableError: 40 - azimuthPID: - slot0: - kP: 6.0 - kI: 0.0001 - kD: 15.0 - kF: 0 - allowableError: 5 - constants: - maxVelTicks100ms: 12275.7 # ticks per 100ms - openLoopRampRate: 0.8 - isSwerve: 1 - encPPR: 12855.51 #Use for distance calibration - azimuthEncPPR: 4096 - maxRotVel: 14 - trackWidth: 21 #inches - wheelbaseLength: 21.5 #inches - wheelDiameter: 4 - kTrackScrubFactor: 1.0 - maxVelOpenLoop: 1 # meters/s - maxVelPathFollowing: 1 # meters/s (2.2) - maxAccel: .5 # meters/s^ 2 - isDemoMode: 0 - autoBalanceDivider: 30 - pitchRollMaxFlat: 2.5 -infrastructure: - canivoreBusName: rio - # power distribution - pdId: 9 - pdIsRev: false - # pneumatics control - compressorEnabled: false - pcmIsRev: false - pcmId: 2 - # pigeon - pigeonId: 32 - isPigeon2: true -controlboard: example -constants: - # drivetrain misc - maxAllowablePoseError: 0.1 - ## Logging ## - logTeleOp: 1 # 0 or 1 - logAuto: 1 # 0 or 1 - badLogEnabled: 1 # 0 or 1 - configStatusFrames: 0 # 0 or 1 - verbose: 0 # 0 or 1 - ## General ## - EnableBucketTuning: 0 - teleopFieldCentric: 1 # 0 or 1 - kLooperDt: .050 # seconds - resetFactoryDefaults: 0 # whether motors get reset to factory default - doesn't affect motor encoder offset +subsystems: + drivetrain: + implemented: true + talons: + FLAz: 2 + FRAz: 12 + RLAz: 10 + RRAz: 15 + falcons: + FLDr: 3 + FRDr: 13 + RLDr: 1 + RRDr: 14 + invertMotor: + - FRDr + - RRDr + - FLAz + - FRAz + - RLAz + - RRAz + invertSensorPhase: + - FLAz + - FRAz + - RLAz + - RRAz + swerveModules: + modules: + frontLeft: + drive: FLDr + azimuth: FLAz + constants: + encoderOffset: 2445 + frontRight: + drive: FRDr + azimuth: FRAz + constants: + encoderOffset: 1479 + backLeft: + drive: RLDr + azimuth: RLAz + constants: + encoderOffset: 539 + backRight: + drive: RRDr + azimuth: RRAz + constants: + encoderOffset: 224 + drivePID: + slot0: + kP: .11 + kI: 0.0001 + kD: 4.0 + kF: 0.048077 + allowableError: 40 + azimuthPID: + slot0: + kP: 6.0 + kI: 0.0001 + kD: 15.0 + kF: 0 + allowableError: 5 + constants: + maxVelTicks100ms: 12275.7 # ticks per 100ms + openLoopRampRate: 0.8 + isSwerve: 1 + encPPR: 12855.51 #Use for distance calibration + azimuthEncPPR: 4096 + maxRotVel: 14 + trackWidth: 21 #inches + wheelbaseLength: 21.5 #inches + wheelDiameter: 4 + kTrackScrubFactor: 1.0 + maxVelOpenLoop: 1 # meters/s + maxVelPathFollowing: 1 # meters/s (2.2) + maxAccel: .5 # meters/s^ 2 + isDemoMode: 0 + autoBalanceDivider: 30 + pitchRollMaxFlat: 2.5 +infrastructure: + canivoreBusName: rio + # power distribution + pdId: 9 + pdIsRev: false + # pneumatics control + compressorEnabled: false + pcmIsRev: false + pcmId: 2 + # pigeon + pigeonId: 32 + isPigeon2: true +controlboard: example +constants: + # drivetrain misc + maxAllowablePoseError: 0.1 + ## Logging ## + logTeleOp: 1 # 0 or 1 + logAuto: 1 # 0 or 1 + badLogEnabled: 1 # 0 or 1 + configStatusFrames: 0 # 0 or 1 + verbose: 0 # 0 or 1 + ## General ## + EnableBucketTuning: 0 + teleopFieldCentric: 1 # 0 or 1 + kLooperDt: .050 # seconds + resetFactoryDefaults: 0 # whether motors get reset to factory default - doesn't affect motor encoder offset # - if motors reset, rebooting the robot mid-match would kill the turret \ No newline at end of file From 8741f1eece9220ce0a9ef612ce8ccd49290fd10e Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Fri, 10 Feb 2023 00:05:01 -0600 Subject: [PATCH 28/31] added statements in path util for loading --- src/main/java/com/team1816/lib/auto/paths/PathUtil.java | 6 ++++++ src/main/java/com/team1816/season/Robot.java | 1 + 2 files changed, 7 insertions(+) diff --git a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java index d67e1f3f..1fe632a6 100644 --- a/src/main/java/com/team1816/lib/auto/paths/PathUtil.java +++ b/src/main/java/com/team1816/lib/auto/paths/PathUtil.java @@ -58,7 +58,10 @@ public static Trajectory generateTrajectory( if (loadTrajectories) { var trajectory = TrajectoryCalculator.loadTrajectory(pathName); if (!trajectory.equals(new Trajectory())) { + System.out.println("Loaded Trajectory: " + pathName); return trajectory; + } else { + System.out.println("PathUtil Failed to Load Trajectory: " + pathName); } } /* Inch to meter conversions for waypoints for trajectory calculations */ @@ -134,7 +137,10 @@ public static List generateHeadings( if (loadTrajectories) { var trajectoryHeadings = TrajectoryCalculator.loadTrajectoryHeadings(name); if (trajectoryHeadings.size() > 0) { + System.out.println("Loaded Trajectory Headings: " + name); return trajectoryHeadings; + } else { + System.out.println("PathUtil Failed to Load Trajectory Headings: " + name); } } if (waypoints == null || swerveHeadings == null) { diff --git a/src/main/java/com/team1816/season/Robot.java b/src/main/java/com/team1816/season/Robot.java index 8313f100..8af6d0f4 100644 --- a/src/main/java/com/team1816/season/Robot.java +++ b/src/main/java/com/team1816/season/Robot.java @@ -18,6 +18,7 @@ import com.team1816.season.states.Orchestrator; import com.team1816.season.states.RobotState; import com.team1816.season.subsystems.LedManager; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.*; import java.nio.file.Files; From 0f81736e2a31e2a13e04939a3dab1852abe370e8 Mon Sep 17 00:00:00 2001 From: miapirobotics <34904974+miapirobotics@users.noreply.github.com> Date: Fri, 24 Feb 2023 18:48:31 -0600 Subject: [PATCH 29/31] Fix stuff --- .../lib/auto/paths/DriveStraightPath.java | 5 - .../lib/subsystems/drive/ISwerveModule.java | 56 +- .../lib/subsystems/drive/SwerveModule.java | 55 +- .../lib/subsystems/drive/TankDrive.java | 1156 ++++++++--------- .../auto/paths/DriveToChargeStationPath.java | 9 +- .../auto/paths/TrajectoryToTargetPath.java | 474 ++++--- 6 files changed, 868 insertions(+), 887 deletions(-) diff --git a/src/main/java/com/team1816/lib/auto/paths/DriveStraightPath.java b/src/main/java/com/team1816/lib/auto/paths/DriveStraightPath.java index 4e85b79a..930133bd 100644 --- a/src/main/java/com/team1816/lib/auto/paths/DriveStraightPath.java +++ b/src/main/java/com/team1816/lib/auto/paths/DriveStraightPath.java @@ -51,9 +51,4 @@ public List getReflectedWaypoints() { public List getReflectedWaypointHeadings() { return null; } - - @Override - public boolean usingApp() { - return false; - } } diff --git a/src/main/java/com/team1816/lib/subsystems/drive/ISwerveModule.java b/src/main/java/com/team1816/lib/subsystems/drive/ISwerveModule.java index 5e1b78e1..4a028b46 100644 --- a/src/main/java/com/team1816/lib/subsystems/drive/ISwerveModule.java +++ b/src/main/java/com/team1816/lib/subsystems/drive/ISwerveModule.java @@ -1,29 +1,27 @@ -package com.team1816.lib.subsystems.drive; - -/** - * Base interface for a swerve module with isolated components for a drive motor and azimuth motor with feedback control - */ -public interface ISwerveModule { - String getModuleName(); - - // angle degrees - double getActualAzimuth(); - - double getAzimuthError(); - - double getDesiredAzimuth(); - - double getDesiredNormalizedAzimuth(); - - double getActualNormalizedAzimuth(); - - // velocity ticks/100ms - double getActualDrive(); - - double getDesiredDrive(); - - double getDriveError(); - - // Temperature C - double getMotorTemp(); -} +package com.team1816.lib.subsystems.drive; + +/** + * Base interface for a swerve module with isolated components for a drive motor and azimuth motor with feedback control + */ +public interface ISwerveModule { + String getModuleName(); + + // angle degrees + double getActualAzimuth(); + + double getAzimuthError(); + + double getDesiredAzimuth(); + + // velocity ticks/100ms + double getActualDrive(); + + double getDesiredDrive(); + + double getDrivePosition(); + + double getDriveError(); + + // Temperature C + double getMotorTemp(); +} \ No newline at end of file diff --git a/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java b/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java index f42b5c8d..f8c4d8d8 100644 --- a/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java +++ b/src/main/java/com/team1816/lib/subsystems/drive/SwerveModule.java @@ -16,7 +16,8 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.Timer; -import static com.team1816.lib.subsystems.drive.Drive.*; +import static com.team1816.lib.subsystems.drive.Drive.NAME; +import static com.team1816.lib.subsystems.drive.Drive.factory; public class SwerveModule implements ISwerveModule { @@ -32,11 +33,11 @@ public class SwerveModule implements ISwerveModule { */ public SwerveModuleState moduleState; public SwerveModulePosition modulePosition; - public double driveDemand; // ticks - public double driveActual; // m/s - public double driveDistance; // m - public double azimuthDemand; // ticks - public double azimuthActual; // degrees + public double driveDemand; + public double driveActual; + public double drivePosition; + public double azimuthDemand; + public double azimuthActual; public double motorTemp; // Drive Motor Temperature /** @@ -106,6 +107,7 @@ public SwerveModule( ); allowableError = 5; // TODO this is a dummy value for checkSystem + drivePosition = 0; moduleState = new SwerveModuleState(); modulePosition = new SwerveModulePosition(); @@ -148,11 +150,16 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) * @see this#getActualPosition() */ public void update() { - driveActual = driveMotor.getSelectedSensorVelocity(0); - azimuthActual = azimuthMotor.getSelectedSensorPosition(0); + driveActual = + DriveConversions.ticksToMeters(driveMotor.getSelectedSensorVelocity(0)) * 10; + azimuthActual = + DriveConversions.convertTicksToDegrees( + azimuthMotor.getSelectedSensorPosition(0) - + mModuleConfig.azimuthEncoderHomeOffset + ); - moduleState.speedMetersPerSecond = DriveConversions.ticksPer100msToMetersPerSecond(driveActual); - moduleState.angle = Rotation2d.fromDegrees(DriveConversions.convertTicksToDegrees(azimuthActual - mModuleConfig.azimuthEncoderHomeOffset)); + moduleState.speedMetersPerSecond = driveActual; + moduleState.angle = Rotation2d.fromDegrees(azimuthActual); drivePosition += driveActual * Robot.dt / 1000; modulePosition.distanceMeters = drivePosition; @@ -252,31 +259,23 @@ public double getActualDrive() { } /** - * Returns the closed loop error of the Drive motor (in-built) + * Returns the "position" of the Drive motor * - * @return driveError + * @return drivePosition */ @Override - public double getDriveError() { - return driveMotor.getClosedLoopError(0); + public double getDrivePosition() { + return drivePosition; } /** - * Returns the normalized azimuth demand in ticks with respect to the zero offset - * Used for logging only - */ - @Override - public double getDesiredNormalizedAzimuth() { - return azimuthDemand - mModuleConfig.azimuthEncoderHomeOffset; - } - - /** - * Returns the normalized azimuth position in ticks - * Used for logging only + * Returns the closed loop error of the Drive motor (in-built) + * + * @return driveError */ @Override - public double getActualNormalizedAzimuth() { - return azimuthActual - mModuleConfig.azimuthEncoderHomeOffset; + public double getDriveError() { + return driveMotor.getClosedLoopError(0); } /** @@ -389,4 +388,4 @@ public ModuleConfig() { 4096 ); } -} +} \ No newline at end of file diff --git a/src/main/java/com/team1816/lib/subsystems/drive/TankDrive.java b/src/main/java/com/team1816/lib/subsystems/drive/TankDrive.java index ac411510..cbc8e822 100644 --- a/src/main/java/com/team1816/lib/subsystems/drive/TankDrive.java +++ b/src/main/java/com/team1816/lib/subsystems/drive/TankDrive.java @@ -1,578 +1,578 @@ -package com.team1816.lib.subsystems.drive; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.IMotorController; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; -import com.google.inject.Inject; -import com.google.inject.Singleton; -import com.team1816.lib.Infrastructure; -import com.team1816.lib.hardware.PIDSlotConfiguration; -import com.team1816.lib.hardware.components.motor.IGreenMotor; -import com.team1816.lib.subsystems.LedManager; -import com.team1816.lib.util.EnhancedMotorChecker; -import com.team1816.lib.util.team254.CheesyDriveHelper; -import com.team1816.lib.util.team254.DriveSignal; -import com.team1816.lib.util.team254.SwerveDriveSignal; -import com.team1816.season.configuration.Constants; -import com.team1816.season.states.RobotState; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; -import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; -import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; -import edu.wpi.first.wpilibj.RobotBase; - -import static com.team1816.lib.util.driveUtil.DriveConversions.*; - -@Singleton -public class TankDrive extends Drive implements DifferentialDrivetrain { - - /** - * Components - */ - private final IGreenMotor leftMain, rightMain; - private final IGreenMotor leftFollowerA, rightFollowerA, leftFollowerB, rightFollowerB; - - /** - * Odometry - */ - private DifferentialDriveOdometry tankOdometry; - private static final DifferentialDriveKinematics tankKinematics = new DifferentialDriveKinematics( - kDriveWheelTrackWidthMeters - ); - private final CheesyDriveHelper driveHelper = new CheesyDriveHelper(); - private final double tickRatioPerLoop = Constants.kLooperDt / .1d; // Convert Ticks/100MS into Ticks/Robot Loop - - /** - * States - */ - public double leftPowerDemand, rightPowerDemand; // % Output (-1 to 1) - used in OPEN_LOOP - public double leftVelDemand, rightVelDemand; // Velocity (Ticks/100MS) - used in TRAJECTORY_FOLLOWING - - private double leftActualDistance = 0, rightActualDistance = 0; // Meters - - private double leftActualVelocity, rightActualVelocity; // Ticks/100MS - - double leftErrorClosedLoop; - double rightErrorClosedLoop; - - /** - * Instantiates a swerve drivetrain from base subsystem parameters - * - * @param lm LEDManager - * @param inf Infrastructure - * @param rs RobotState - * @see Drive#Drive(LedManager, Infrastructure, RobotState) - */ - @Inject - public TankDrive(LedManager lm, Infrastructure inf, RobotState rs) { - super(lm, inf, rs); - // configure motors - leftMain = factory.getMotor(NAME, "leftMain"); - leftFollowerA = factory.getFollowerMotor(NAME, "leftFollower", leftMain); - leftFollowerB = factory.getFollowerMotor(NAME, "leftFollowerTwo", leftMain); - rightMain = factory.getMotor(NAME, "rightMain"); - rightFollowerA = factory.getFollowerMotor(NAME, "rightFollower", rightMain); - rightFollowerB = factory.getFollowerMotor(NAME, "rightFollowerTwo", rightMain); - - // configure follower motor currentLimits - var currentLimitConfig = new SupplyCurrentLimitConfiguration( - true, - factory.getConstant(NAME, "currentLimit", 40), - 0, - 0 - ); - leftMain.configSupplyCurrentLimit( - currentLimitConfig, - Constants.kLongCANTimeoutMs - ); - leftFollowerA.configSupplyCurrentLimit( - currentLimitConfig, - Constants.kLongCANTimeoutMs - ); - leftFollowerB.configSupplyCurrentLimit( - currentLimitConfig, - Constants.kLongCANTimeoutMs - ); - rightMain.configSupplyCurrentLimit( - currentLimitConfig, - Constants.kLongCANTimeoutMs - ); - rightFollowerA.configSupplyCurrentLimit( - currentLimitConfig, - Constants.kLongCANTimeoutMs - ); - rightFollowerB.configSupplyCurrentLimit( - currentLimitConfig, - Constants.kLongCANTimeoutMs - ); - - setOpenLoop(DriveSignal.NEUTRAL); - - tankOdometry = - new DifferentialDriveOdometry( - getActualHeading(), - leftActualDistance, - rightActualDistance - ); - } - - /** - * Read/Write Periodic - */ - - /** - * Writes outputs / demands to hardware on the drivetrain such as motors and handles the desired state of the left - * and right sides. Directly writes to the motors. - * - * @see IGreenMotor - */ - @Override - public synchronized void writeToHardware() { // sets the demands for hardware from the inputs provided - if (controlState == ControlState.OPEN_LOOP) { - leftMain.set( - ControlMode.PercentOutput, - isSlowMode ? (leftPowerDemand * 0.5) : leftPowerDemand - ); - rightMain.set( - ControlMode.PercentOutput, - isSlowMode ? (rightPowerDemand * 0.5) : rightPowerDemand - ); - } else { - leftMain.set(ControlMode.Velocity, leftVelDemand); - rightMain.set(ControlMode.Velocity, rightVelDemand); - } - } - - /** - * Reads outputs from hardware on the drivetrain such as sensors and handles the actual state the wheels and - * drivetrain speeds. Used to update odometry and other related data. - * - * @see Infrastructure - * @see RobotState - */ - @Override - public synchronized void readFromHardware() { - // update current motor velocities and distance traveled - leftActualVelocity = leftMain.getSelectedSensorVelocity(0); - rightActualVelocity = rightMain.getSelectedSensorVelocity(0); - leftActualDistance += ticksToMeters(leftActualVelocity * tickRatioPerLoop); - rightActualDistance += ticksToMeters(rightActualVelocity * tickRatioPerLoop); - - // update error (only if in closed loop where knowing it is useful) - if (controlState == ControlState.TRAJECTORY_FOLLOWING) { - leftErrorClosedLoop = leftMain.getClosedLoopError(0); - rightErrorClosedLoop = rightMain.getClosedLoopError(0); - } - - // update current movement of the whole drivetrain - chassisSpeed = - tankKinematics.toChassisSpeeds( - new DifferentialDriveWheelSpeeds(getLeftMPSActual(), getRightMPSActual()) - ); - - // update actual heading from gyro (pigeon) - if (RobotBase.isSimulation()) { - simulateGyroOffset(); - } - actualHeading = Rotation2d.fromDegrees(infrastructure.getYaw()); - - tankOdometry.update(actualHeading, leftActualDistance, rightActualDistance); - updateRobotState(); - } - - /** Config */ - - /** - * Zeroes the encoders and odometry based on a certain pose - * - * @param pose Pose2d - * @see Drive#zeroSensors() - */ - @Override - public void zeroSensors(Pose2d pose) { - System.out.println("Zeroing drive sensors!"); - - actualHeading = Rotation2d.fromDegrees(infrastructure.getYaw()); - resetEncoders(); - resetOdometry(pose); - startingPose = pose; - - chassisSpeed = new ChassisSpeeds(); - isBraking = false; - } - - /** - * Stops the drivetrain - * - * @see Drive#stop() - */ - @Override - public synchronized void stop() { - setOpenLoop( - controlState == ControlState.OPEN_LOOP - ? DriveSignal.NEUTRAL - : DriveSignal.BRAKE - ); - setBraking(controlState == ControlState.TRAJECTORY_FOLLOWING); - } - - /** - * Resets the encoders to hold the zero value - * - * @see this#zeroSensors(Pose2d) - */ - public synchronized void resetEncoders() { - leftMain.setSelectedSensorPosition(0, 0, 0); - rightMain.setSelectedSensorPosition(0, 0, 0); - leftActualDistance = 0; - rightActualDistance = 0; - } - - /** - * Resets the odometry to a certain pose - * - * @param pose Pose2d - */ - @Override - public void resetOdometry(Pose2d pose) { - tankOdometry.resetPosition( - getActualHeading(), - leftActualDistance, - rightActualDistance, - pose - ); - tankOdometry.update(actualHeading, leftActualDistance, rightActualDistance); - updateRobotState(); - } - - /** - * Updates robotState based on values from odometry and sensor readings in readFromHardware - * - * @see RobotState - */ - @Override - public void updateRobotState() { - robotState.fieldToVehicle = tankOdometry.getPoseMeters(); - - var cs = new ChassisSpeeds( - chassisSpeed.vxMetersPerSecond, - chassisSpeed.vyMetersPerSecond, - chassisSpeed.omegaRadiansPerSecond - ); - robotState.calculatedVehicleAccel = - new ChassisSpeeds( - (cs.vxMetersPerSecond - robotState.deltaVehicle.vxMetersPerSecond) / - (timestamp-prevTimestamp), - (cs.vyMetersPerSecond - robotState.deltaVehicle.vyMetersPerSecond) / - (timestamp-prevTimestamp), - cs.omegaRadiansPerSecond - robotState.deltaVehicle.omegaRadiansPerSecond - ); - robotState.deltaVehicle = cs; - - robotState.vehicleToFloorProximityCentimeters = infrastructure.getMaximumProximity(); - } - - /** Open loop control */ - - /** - * Sets open loop percent output commands based on the DriveSignal from setTeleOpInputs() - * - * @param signal DriveSignal - * @see Drive#setOpenLoop(DriveSignal) - */ - @Override - public synchronized void setOpenLoop(DriveSignal signal) { - if (controlState != ControlState.OPEN_LOOP) { - System.out.println("switching to open loop"); - controlState = ControlState.OPEN_LOOP; - leftErrorClosedLoop = 0; - rightErrorClosedLoop = 0; - } - leftPowerDemand = signal.getLeft(); - rightPowerDemand = signal.getRight(); - - leftVelDemand = leftPowerDemand * maxVelTicks100ms; - rightVelDemand = rightPowerDemand * maxVelTicks100ms; - } - - /** - * Translates teleoperated inputs into a DriveSignal to be used in setTeleOpInputs() - * - * @param forward forward demand - * @param strafe strafe demand - * @param rotation rotation demand - * @see this#setOpenLoop(DriveSignal) - * @see Drive#setTeleopInputs(double, double, double) - * @see SwerveDriveSignal - */ - @Override - public void setTeleopInputs(double forward, double strafe, double rotation) { - DriveSignal driveSignal = driveHelper.cheesyDrive( - (isDemoMode ? forward * demoModeMultiplier : forward), - (isDemoMode ? rotation * demoModeMultiplier : rotation), - false, - false - ); - - // To avoid overriding brake command - if (!isBraking) { - setOpenLoop(driveSignal); - } - } - - /** - * Adapts a DriveSignal for closed loop PID controlled motion - * - * @param signal DriveSignal - */ - public synchronized void setVelocity(DriveSignal signal) { - if (controlState == ControlState.OPEN_LOOP) { - leftMain.selectProfileSlot(0, 0); - rightMain.selectProfileSlot(0, 0); - - leftMain.configNeutralDeadband(0.0, 0); - rightMain.configNeutralDeadband(0.0, 0); - } - - leftVelDemand = signal.getLeft(); - rightVelDemand = signal.getRight(); - } - - /** - * Autobalances while in Tankdrive manual control TODO redo description - */ - @Override - public void autoBalance(ChassisSpeeds fieldRelativeChassisSpeeds) { - double pitch = infrastructure.getPitch(); - double roll = infrastructure.getRoll(); - double throttle = 0; - double strafe = 0; - var heading = Constants.EmptyRotation2d; - - double maxFlatRange = Constants.autoBalanceThresholdDegrees; - double correction = (getInitialYaw() - infrastructure.getYaw()) / 1440; - - if (Math.abs(pitch) > maxFlatRange || Math.abs(roll) > maxFlatRange) { - throttle = pitch / 4; - strafe = roll / 4; - - ChassisSpeeds chassisSpeeds = new ChassisSpeeds(throttle, strafe, correction); - - - DifferentialDriveWheelSpeeds wheelSpeeds = tankKinematics.toWheelSpeeds(chassisSpeeds); - DriveSignal driveSignal = new DriveSignal(wheelSpeeds.leftMetersPerSecond / TankDrive.kPathFollowingMaxVelMeters, wheelSpeeds.rightMetersPerSecond / TankDrive.kPathFollowingMaxVelMeters); - setVelocity(driveSignal); - } else { - - heading = Rotation2d.fromDegrees(90).minus(robotState.fieldToVehicle.getRotation()); - //TODO tankdrive jolt align - } - - } - - /** - * Utilizes a DriveSignal to adapt Trajectory demands for TRAJECTORY_FOLLOWING and closed loop control - * - * @param leftVel left velocity - * @param rightVel right velocity - */ - public void updateTrajectoryVelocities(Double leftVel, Double rightVel) { - // Velocities are in m/sec comes from trajectory command - var signal = new DriveSignal( - metersPerSecondToTicksPer100ms(leftVel), - metersPerSecondToTicksPer100ms(rightVel) - ); - setVelocity(signal); - } - - /** - * General getters and setters - */ - - /** - * Sets whether the drivetrain is braking - * - * @param braking boolean - * @see Drive#setBraking(boolean) - */ - @Override - public synchronized void setBraking(boolean braking) { - if (isBraking != braking) { - System.out.println("braking: " + braking); - isBraking = braking; - - if (braking) { - leftMain.set(ControlMode.Velocity, 0); - rightMain.set(ControlMode.Velocity, 0); - } - // TODO ensure that changing neutral modes won't backfire while we're using brushless motors - NeutralMode mode = braking ? NeutralMode.Brake : NeutralMode.Coast; - - rightMain.setNeutralMode(mode); - rightFollowerA.setNeutralMode(mode); - rightFollowerB.setNeutralMode(mode); - - leftMain.setNeutralMode(mode); - leftFollowerA.setNeutralMode(mode); - leftFollowerB.setNeutralMode(mode); - } - } - - /** - * Returns the actual velocity of the left side in meters per second - * - * @return left velocity (m/s) - * @see com.team1816.lib.util.driveUtil.DriveConversions#ticksPer100MSToMPS(double) - */ - public double getLeftMPSActual() { - return ticksPer100msToMetersPerSecond(leftActualVelocity); - } - - /** - * Returns the actual velocity of the right side in meters per second - * - * @return right velocity (m/s) - * @see com.team1816.lib.util.driveUtil.DriveConversions#ticksPer100MSToMPS(double) - */ - public double getRightMPSActual() { - return ticksPer100msToMetersPerSecond(rightActualVelocity); - } - - /** - * Returns the left velocity demand in ticks per 100ms - * - * @return leftVelDemand - */ - @Override - public double getLeftVelocityTicksDemand() { - return leftVelDemand; - } - - /** - * Returns the right velocity demand in ticks per 100ms - * - * @return rightVelDemand - */ - @Override - public double getRightVelocityTicksDemand() { - return rightVelDemand; - } - - /** - * Returns the actual left velocity in ticks per 100ms - * - * @return leftVelActual - * @see IMotorController - * @see IGreenMotor - */ - @Override - public double getLeftVelocityTicksActual() { - return leftMain.getSelectedSensorVelocity(0); - } - - /** - * Returns the actual right velocity in ticks per 100ms - * - * @return rightVelActual - * @see IMotorController - * @see IGreenMotor - */ - @Override - public double getRightVelocityTicksActual() { - return rightMain.getSelectedSensorVelocity(0); - } - - /** - * Returns the total distance (not displacement) traveled by the left side of the drivetrain - * - * @return leftActualDistance - */ - @Override - public double getLeftDistance() { - return leftActualDistance; - } - - /** - * Returns the total distance (not displacement) traveled by the right side of the drivetrain - * - * @return rightActualDistance - */ - @Override - public double getRightDistance() { - return rightActualDistance; - } - - /** - * Returns the left side closed loop error (in-built) - * - * @return leftErrorClosedLoop - */ - @Override - public double getLeftError() { - return leftErrorClosedLoop; - } - - /** - * Returns the right side closed loop error (in-built) - * - * @return rightErrorClosedLoop - */ - @Override - public double getRightError() { - return rightErrorClosedLoop; - } - - /** config and tests */ - - /** - * Tests the drivetrain by seeing if each side can go back and forth - * - * @return true if tests passed - * @see Drive#testSubsystem() - */ - @Override - public boolean testSubsystem() { - boolean leftSide = EnhancedMotorChecker.checkMotor(this, leftMain); - boolean rightSide = EnhancedMotorChecker.checkMotor(this, rightMain); - - boolean checkPigeon = infrastructure.getPigeon() == null; - - System.out.println(leftSide && rightSide && checkPigeon); - if (leftSide && rightSide && checkPigeon) { - ledManager.indicateStatus(LedManager.RobotStatus.ENABLED); - } else { - ledManager.indicateStatus(LedManager.RobotStatus.ERROR); - } - return leftSide && rightSide; - } - - /** - * Returns the pid configuration of the motors - * - * @return PIDSlotConfiguration - * @see Drive#getPIDConfig() - */ - @Override - public PIDSlotConfiguration getPIDConfig() { - PIDSlotConfiguration defaultPIDConfig = new PIDSlotConfiguration(); - defaultPIDConfig.kP = 0.0; - defaultPIDConfig.kI = 0.0; - defaultPIDConfig.kD = 0.0; - defaultPIDConfig.kF = 0.0; - return (factory.getSubsystem(NAME).implemented) - ? factory.getSubsystem(NAME).pidConfig.getOrDefault("slot0", defaultPIDConfig) - : defaultPIDConfig; - } - - /** - * Returns the associated kinematics with the drivetrain - * - * @return tankKinematics - */ - public DifferentialDriveKinematics getKinematics() { - return tankKinematics; - } -} +package com.team1816.lib.subsystems.drive; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.IMotorController; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; +import com.google.inject.Inject; +import com.google.inject.Singleton; +import com.team1816.lib.Infrastructure; +import com.team1816.lib.hardware.PIDSlotConfiguration; +import com.team1816.lib.hardware.components.motor.IGreenMotor; +import com.team1816.lib.subsystems.LedManager; +import com.team1816.lib.util.EnhancedMotorChecker; +import com.team1816.lib.util.team254.CheesyDriveHelper; +import com.team1816.lib.util.team254.DriveSignal; +import com.team1816.lib.util.team254.SwerveDriveSignal; +import com.team1816.season.configuration.Constants; +import com.team1816.season.states.RobotState; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.RobotBase; + +import static com.team1816.lib.util.driveUtil.DriveConversions.*; + +@Singleton +public class TankDrive extends Drive implements DifferentialDrivetrain { + + /** + * Components + */ + private final IGreenMotor leftMain, rightMain; + private final IGreenMotor leftFollowerA, rightFollowerA, leftFollowerB, rightFollowerB; + + /** + * Odometry + */ + private DifferentialDriveOdometry tankOdometry; + private static final DifferentialDriveKinematics tankKinematics = new DifferentialDriveKinematics( + kDriveWheelTrackWidthMeters + ); + private final CheesyDriveHelper driveHelper = new CheesyDriveHelper(); + private final double tickRatioPerLoop = Constants.kLooperDt / .1d; // Convert Ticks/100MS into Ticks/Robot Loop + + /** + * States + */ + public double leftPowerDemand, rightPowerDemand; // % Output (-1 to 1) - used in OPEN_LOOP + public double leftVelDemand, rightVelDemand; // Velocity (Ticks/100MS) - used in TRAJECTORY_FOLLOWING + + private double leftActualDistance = 0, rightActualDistance = 0; // Meters + + private double leftActualVelocity, rightActualVelocity; // Ticks/100MS + + double leftErrorClosedLoop; + double rightErrorClosedLoop; + + /** + * Instantiates a swerve drivetrain from base subsystem parameters + * + * @param lm LEDManager + * @param inf Infrastructure + * @param rs RobotState + * @see Drive#Drive(LedManager, Infrastructure, RobotState) + */ + @Inject + public TankDrive(LedManager lm, Infrastructure inf, RobotState rs) { + super(lm, inf, rs); + // configure motors + leftMain = factory.getMotor(NAME, "leftMain"); + leftFollowerA = factory.getFollowerMotor(NAME, "leftFollower", leftMain); + leftFollowerB = factory.getFollowerMotor(NAME, "leftFollowerTwo", leftMain); + rightMain = factory.getMotor(NAME, "rightMain"); + rightFollowerA = factory.getFollowerMotor(NAME, "rightFollower", rightMain); + rightFollowerB = factory.getFollowerMotor(NAME, "rightFollowerTwo", rightMain); + + // configure follower motor currentLimits + var currentLimitConfig = new SupplyCurrentLimitConfiguration( + true, + factory.getConstant(NAME, "currentLimit", 40), + 0, + 0 + ); + leftMain.configSupplyCurrentLimit( + currentLimitConfig, + Constants.kLongCANTimeoutMs + ); + leftFollowerA.configSupplyCurrentLimit( + currentLimitConfig, + Constants.kLongCANTimeoutMs + ); + leftFollowerB.configSupplyCurrentLimit( + currentLimitConfig, + Constants.kLongCANTimeoutMs + ); + rightMain.configSupplyCurrentLimit( + currentLimitConfig, + Constants.kLongCANTimeoutMs + ); + rightFollowerA.configSupplyCurrentLimit( + currentLimitConfig, + Constants.kLongCANTimeoutMs + ); + rightFollowerB.configSupplyCurrentLimit( + currentLimitConfig, + Constants.kLongCANTimeoutMs + ); + + setOpenLoop(DriveSignal.NEUTRAL); + + tankOdometry = + new DifferentialDriveOdometry( + getActualHeading(), + leftActualDistance, + rightActualDistance + ); + } + + /** + * Read/Write Periodic + */ + + /** + * Writes outputs / demands to hardware on the drivetrain such as motors and handles the desired state of the left + * and right sides. Directly writes to the motors. + * + * @see IGreenMotor + */ + @Override + public synchronized void writeToHardware() { // sets the demands for hardware from the inputs provided + if (controlState == ControlState.OPEN_LOOP) { + leftMain.set( + ControlMode.PercentOutput, + isSlowMode ? (leftPowerDemand * 0.5) : leftPowerDemand + ); + rightMain.set( + ControlMode.PercentOutput, + isSlowMode ? (rightPowerDemand * 0.5) : rightPowerDemand + ); + } else { + leftMain.set(ControlMode.Velocity, leftVelDemand); + rightMain.set(ControlMode.Velocity, rightVelDemand); + } + } + + /** + * Reads outputs from hardware on the drivetrain such as sensors and handles the actual state the wheels and + * drivetrain speeds. Used to update odometry and other related data. + * + * @see Infrastructure + * @see RobotState + */ + @Override + public synchronized void readFromHardware() { + // update current motor velocities and distance traveled + leftActualVelocity = leftMain.getSelectedSensorVelocity(0); + rightActualVelocity = rightMain.getSelectedSensorVelocity(0); + leftActualDistance += ticksToMeters(leftActualVelocity * tickRatioPerLoop); + rightActualDistance += ticksToMeters(rightActualVelocity * tickRatioPerLoop); + + // update error (only if in closed loop where knowing it is useful) + if (controlState == ControlState.TRAJECTORY_FOLLOWING) { + leftErrorClosedLoop = leftMain.getClosedLoopError(0); + rightErrorClosedLoop = rightMain.getClosedLoopError(0); + } + + // update current movement of the whole drivetrain + chassisSpeed = + tankKinematics.toChassisSpeeds( + new DifferentialDriveWheelSpeeds(getLeftMPSActual(), getRightMPSActual()) + ); + + // update actual heading from gyro (pigeon) + if (RobotBase.isSimulation()) { + simulateGyroOffset(); + } + actualHeading = Rotation2d.fromDegrees(infrastructure.getYaw()); + + tankOdometry.update(actualHeading, leftActualDistance, rightActualDistance); + updateRobotState(); + } + + /** Config */ + + /** + * Zeroes the encoders and odometry based on a certain pose + * + * @param pose Pose2d + * @see Drive#zeroSensors() + */ + @Override + public void zeroSensors(Pose2d pose) { + System.out.println("Zeroing drive sensors!"); + + actualHeading = Rotation2d.fromDegrees(infrastructure.getYaw()); + resetEncoders(); + resetOdometry(pose); + startingPose = pose; + + chassisSpeed = new ChassisSpeeds(); + isBraking = false; + } + + /** + * Stops the drivetrain + * + * @see Drive#stop() + */ + @Override + public synchronized void stop() { + setOpenLoop( + controlState == ControlState.OPEN_LOOP + ? DriveSignal.NEUTRAL + : DriveSignal.BRAKE + ); + setBraking(controlState == ControlState.TRAJECTORY_FOLLOWING); + } + + /** + * Resets the encoders to hold the zero value + * + * @see this#zeroSensors(Pose2d) + */ + public synchronized void resetEncoders() { + leftMain.setSelectedSensorPosition(0, 0, 0); + rightMain.setSelectedSensorPosition(0, 0, 0); + leftActualDistance = 0; + rightActualDistance = 0; + } + + /** + * Resets the odometry to a certain pose + * + * @param pose Pose2d + */ + @Override + public void resetOdometry(Pose2d pose) { + tankOdometry.resetPosition( + getActualHeading(), + leftActualDistance, + rightActualDistance, + pose + ); + tankOdometry.update(actualHeading, leftActualDistance, rightActualDistance); + updateRobotState(); + } + + /** + * Updates robotState based on values from odometry and sensor readings in readFromHardware + * + * @see RobotState + */ + @Override + public void updateRobotState() { + robotState.fieldToVehicle = tankOdometry.getPoseMeters(); + + var cs = new ChassisSpeeds( + chassisSpeed.vxMetersPerSecond, + chassisSpeed.vyMetersPerSecond, + chassisSpeed.omegaRadiansPerSecond + ); + robotState.calculatedVehicleAccel = + new ChassisSpeeds( + (cs.vxMetersPerSecond - robotState.deltaVehicle.vxMetersPerSecond) / + Constants.kLooperDt, + (cs.vyMetersPerSecond - robotState.deltaVehicle.vyMetersPerSecond) / + Constants.kLooperDt, + cs.omegaRadiansPerSecond - robotState.deltaVehicle.omegaRadiansPerSecond + ); + robotState.deltaVehicle = cs; + + robotState.vehicleToFloorProximityCentimeters = infrastructure.getMaximumProximity(); + } + + /** Open loop control */ + + /** + * Sets open loop percent output commands based on the DriveSignal from setTeleOpInputs() + * + * @param signal DriveSignal + * @see Drive#setOpenLoop(DriveSignal) + */ + @Override + public synchronized void setOpenLoop(DriveSignal signal) { + if (controlState != ControlState.OPEN_LOOP) { + System.out.println("switching to open loop"); + controlState = ControlState.OPEN_LOOP; + leftErrorClosedLoop = 0; + rightErrorClosedLoop = 0; + } + leftPowerDemand = signal.getLeft(); + rightPowerDemand = signal.getRight(); + + leftVelDemand = leftPowerDemand * maxVelTicks100ms; + rightVelDemand = rightPowerDemand * maxVelTicks100ms; + } + + /** + * Translates teleoperated inputs into a DriveSignal to be used in setTeleOpInputs() + * + * @param forward forward demand + * @param strafe strafe demand + * @param rotation rotation demand + * @see this#setOpenLoop(DriveSignal) + * @see Drive#setTeleopInputs(double, double, double) + * @see SwerveDriveSignal + */ + @Override + public void setTeleopInputs(double forward, double strafe, double rotation) { + DriveSignal driveSignal = driveHelper.cheesyDrive( + (isDemoMode ? forward * demoModeMultiplier : forward), + (isDemoMode ? rotation * demoModeMultiplier : rotation), + false, + false + ); + + // To avoid overriding brake command + if (!isBraking) { + setOpenLoop(driveSignal); + } + } + + /** + * Adapts a DriveSignal for closed loop PID controlled motion + * + * @param signal DriveSignal + */ + public synchronized void setVelocity(DriveSignal signal) { + if (controlState == ControlState.OPEN_LOOP) { + leftMain.selectProfileSlot(0, 0); + rightMain.selectProfileSlot(0, 0); + + leftMain.configNeutralDeadband(0.0, 0); + rightMain.configNeutralDeadband(0.0, 0); + } + + leftVelDemand = signal.getLeft(); + rightVelDemand = signal.getRight(); + } + + /** + * Autobalances while in Tankdrive manual control TODO redo description + */ + @Override + public void autoBalance(ChassisSpeeds fieldRelativeChassisSpeeds) { + double pitch = infrastructure.getPitch(); + double roll = infrastructure.getRoll(); + double throttle = 0; + double strafe = 0; + var heading = Constants.EmptyRotation2d; + + double maxFlatRange = Constants.autoBalanceThresholdDegrees; + double correction = (getInitialYaw() - infrastructure.getYaw()) / 1440; + + if (Math.abs(pitch) > maxFlatRange || Math.abs(roll) > maxFlatRange) { + throttle = pitch / 4; + strafe = roll / 4; + + ChassisSpeeds chassisSpeeds = new ChassisSpeeds(throttle, strafe, correction); + + + DifferentialDriveWheelSpeeds wheelSpeeds = tankKinematics.toWheelSpeeds(chassisSpeeds); + DriveSignal driveSignal = new DriveSignal(wheelSpeeds.leftMetersPerSecond / TankDrive.kPathFollowingMaxVelMeters, wheelSpeeds.rightMetersPerSecond / TankDrive.kPathFollowingMaxVelMeters); + setVelocity(driveSignal); + } else { + + heading = Rotation2d.fromDegrees(90).minus(robotState.fieldToVehicle.getRotation()); + //TODO tankdrive jolt align + } + + } + + /** + * Utilizes a DriveSignal to adapt Trajectory demands for TRAJECTORY_FOLLOWING and closed loop control + * + * @param leftVel left velocity + * @param rightVel right velocity + */ + public void updateTrajectoryVelocities(Double leftVel, Double rightVel) { + // Velocities are in m/sec comes from trajectory command + var signal = new DriveSignal( + metersPerSecondToTicksPer100ms(leftVel), + metersPerSecondToTicksPer100ms(rightVel) + ); + setVelocity(signal); + } + + /** + * General getters and setters + */ + + /** + * Sets whether the drivetrain is braking + * + * @param braking boolean + * @see Drive#setBraking(boolean) + */ + @Override + public synchronized void setBraking(boolean braking) { + if (isBraking != braking) { + System.out.println("braking: " + braking); + isBraking = braking; + + if (braking) { + leftMain.set(ControlMode.Velocity, 0); + rightMain.set(ControlMode.Velocity, 0); + } + // TODO ensure that changing neutral modes won't backfire while we're using brushless motors + NeutralMode mode = braking ? NeutralMode.Brake : NeutralMode.Coast; + + rightMain.setNeutralMode(mode); + rightFollowerA.setNeutralMode(mode); + rightFollowerB.setNeutralMode(mode); + + leftMain.setNeutralMode(mode); + leftFollowerA.setNeutralMode(mode); + leftFollowerB.setNeutralMode(mode); + } + } + + /** + * Returns the actual velocity of the left side in meters per second + * + * @return left velocity (m/s) + * @see com.team1816.lib.util.driveUtil.DriveConversions#ticksPer100MSToMPS(double) + */ + public double getLeftMPSActual() { + return ticksPer100MSToMPS(leftActualVelocity); + } + + /** + * Returns the actual velocity of the right side in meters per second + * + * @return right velocity (m/s) + * @see com.team1816.lib.util.driveUtil.DriveConversions#ticksPer100MSToMPS(double) + */ + public double getRightMPSActual() { + return ticksPer100MSToMPS(rightActualVelocity); + } + + /** + * Returns the left velocity demand in ticks per 100ms + * + * @return leftVelDemand + */ + @Override + public double getLeftVelocityTicksDemand() { + return leftVelDemand; + } + + /** + * Returns the right velocity demand in ticks per 100ms + * + * @return rightVelDemand + */ + @Override + public double getRightVelocityTicksDemand() { + return rightVelDemand; + } + + /** + * Returns the actual left velocity in ticks per 100ms + * + * @return leftVelActual + * @see IMotorController + * @see IGreenMotor + */ + @Override + public double getLeftVelocityTicksActual() { + return leftMain.getSelectedSensorVelocity(0); + } + + /** + * Returns the actual right velocity in ticks per 100ms + * + * @return rightVelActual + * @see IMotorController + * @see IGreenMotor + */ + @Override + public double getRightVelocityTicksActual() { + return rightMain.getSelectedSensorVelocity(0); + } + + /** + * Returns the total distance (not displacement) traveled by the left side of the drivetrain + * + * @return leftActualDistance + */ + @Override + public double getLeftDistance() { + return leftActualDistance; + } + + /** + * Returns the total distance (not displacement) traveled by the right side of the drivetrain + * + * @return rightActualDistance + */ + @Override + public double getRightDistance() { + return rightActualDistance; + } + + /** + * Returns the left side closed loop error (in-built) + * + * @return leftErrorClosedLoop + */ + @Override + public double getLeftError() { + return leftErrorClosedLoop; + } + + /** + * Returns the right side closed loop error (in-built) + * + * @return rightErrorClosedLoop + */ + @Override + public double getRightError() { + return rightErrorClosedLoop; + } + + /** config and tests */ + + /** + * Tests the drivetrain by seeing if each side can go back and forth + * + * @return true if tests passed + * @see Drive#testSubsystem() + */ + @Override + public boolean testSubsystem() { + boolean leftSide = EnhancedMotorChecker.checkMotor(this, leftMain); + boolean rightSide = EnhancedMotorChecker.checkMotor(this, rightMain); + + boolean checkPigeon = infrastructure.getPigeon() == null; + + System.out.println(leftSide && rightSide && checkPigeon); + if (leftSide && rightSide && checkPigeon) { + ledManager.indicateStatus(LedManager.RobotStatus.ENABLED); + } else { + ledManager.indicateStatus(LedManager.RobotStatus.ERROR); + } + return leftSide && rightSide; + } + + /** + * Returns the pid configuration of the motors + * + * @return PIDSlotConfiguration + * @see Drive#getPIDConfig() + */ + @Override + public PIDSlotConfiguration getPIDConfig() { + PIDSlotConfiguration defaultPIDConfig = new PIDSlotConfiguration(); + defaultPIDConfig.kP = 0.0; + defaultPIDConfig.kI = 0.0; + defaultPIDConfig.kD = 0.0; + defaultPIDConfig.kF = 0.0; + return (factory.getSubsystem(NAME).implemented) + ? factory.getSubsystem(NAME).pidConfig.getOrDefault("slot0", defaultPIDConfig) + : defaultPIDConfig; + } + + /** + * Returns the associated kinematics with the drivetrain + * + * @return tankKinematics + */ + public DifferentialDriveKinematics getKinematics() { + return tankKinematics; + } +} \ No newline at end of file diff --git a/src/main/java/com/team1816/season/auto/paths/DriveToChargeStationPath.java b/src/main/java/com/team1816/season/auto/paths/DriveToChargeStationPath.java index ddb4931a..5ec87ead 100644 --- a/src/main/java/com/team1816/season/auto/paths/DriveToChargeStationPath.java +++ b/src/main/java/com/team1816/season/auto/paths/DriveToChargeStationPath.java @@ -17,7 +17,7 @@ public DriveToChargeStationPath(Color color) { } @Override - protected List getWaypoints() { + public List getWaypoints() { return List.of( new Pose2d(1.7, 3.38, Rotation2d.fromDegrees(0)), new Pose2d(3.9, 2.78, Rotation2d.fromDegrees(0)) @@ -25,15 +25,10 @@ protected List getWaypoints() { } @Override - protected List getWaypointHeadings() { + public List getWaypointHeadings() { return List.of( Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(180) ); } - - @Override - protected boolean usingApp() { - return true; - } } diff --git a/src/main/java/com/team1816/season/auto/paths/TrajectoryToTargetPath.java b/src/main/java/com/team1816/season/auto/paths/TrajectoryToTargetPath.java index 51193c9e..32f8ffdb 100644 --- a/src/main/java/com/team1816/season/auto/paths/TrajectoryToTargetPath.java +++ b/src/main/java/com/team1816/season/auto/paths/TrajectoryToTargetPath.java @@ -1,240 +1,234 @@ -package com.team1816.season.auto.paths; - -import com.team1816.lib.Injector; -import com.team1816.lib.auto.Color; -import com.team1816.lib.auto.paths.AutoPath; -import com.team1816.lib.auto.paths.PathUtil; -import com.team1816.season.configuration.Constants; -import com.team1816.season.states.RobotState; -import edu.wpi.first.math.geometry.Pose2d; -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.trajectory.Trajectory; - -import java.util.ArrayList; -import java.util.List; - -public class TrajectoryToTargetPath extends AutoPath { - - public static RobotState robotState; - private static Pose2d target; - - public TrajectoryToTargetPath(Pose2d pose) { - robotState = Injector.get(RobotState.class); - target = pose; - } - - public TrajectoryToTargetPath() { - robotState = Injector.get(RobotState.class); - new TrajectoryToTargetPath(robotState.target); - } - - @Override - protected List getWaypoints() { - List waypoints = new ArrayList<>(); - if ( - (target.getY() > Constants.chargeStationThresholdYMin && target.getY() < Constants.chargeStationThresholdYMax) && - (robotState.fieldToVehicle.getY() > Constants.chargeStationThresholdYMin && robotState.fieldToVehicle.getY() < Constants.chargeStationThresholdYMax) - ) { // add two waypoints around the charging station so the path is as safe as possible - if (robotState.allianceColor == Color.RED) { // red side - if (robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMinRed) { // rectangular minimization - // average the y values and determine which vertices to choose - double avg = (robotState.fieldToVehicle.getY() + target.getY()) / 2; - if (avg > (Constants.chargeStationThresholdYMin + Constants.chargeStationThresholdYMax) / 2) { // upper case - var angle = new Rotation2d(Constants.chargeStationThresholdXMinRed - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMax - robotState.fieldToVehicle.getY()); - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinRed, Constants.chargeStationThresholdYMax), target.getRotation())); // upper bounding box - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMax), target.getRotation())); // upper bounding box - } else { // lower case - var angle = new Rotation2d(Constants.chargeStationThresholdXMinRed - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMin - robotState.fieldToVehicle.getY()); - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinRed, Constants.chargeStationThresholdYMin), target.getRotation())); // lower bounding box - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMin), target.getRotation())); // lower bounding box - } - } else { // on the charge station or beyond - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); // straight line - } - } else { // blue side - if (robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMaxBlue) { // rectangular minimization - double avg = (robotState.fieldToVehicle.getY() + target.getY()) / 2; - if (avg > (Constants.chargeStationThresholdYMin + Constants.chargeStationThresholdYMax) / 2) { // upper case - var angle = new Rotation2d(Constants.chargeStationThresholdXMaxBlue - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMax - robotState.fieldToVehicle.getY()); - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxBlue, Constants.chargeStationThresholdYMax), target.getRotation())); // upper bounding box - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMax), target.getRotation())); // upper bounding box - } else { // lower case - var angle = new Rotation2d(Constants.chargeStationThresholdXMaxBlue - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMin - robotState.fieldToVehicle.getY()); - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxBlue, Constants.chargeStationThresholdYMin), target.getRotation())); // lower bounding box - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMin), target.getRotation())); // lower bounding box - } - } else { // on the charge station or beyond - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); // straight line - } - } - } else if ( - target.getY() > Constants.chargeStationThresholdYMin && target.getY() < Constants.chargeStationThresholdYMax - ) { // target is in the middle but robot is not or path goes across the null box, so add waypoint closer to the scoring nodes - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); - if (robotState.fieldToVehicle.getY() > Constants.chargeStationThresholdYMax) { // upper bounding box - if (robotState.allianceColor == Color.RED) { - if (!(robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMaxRed)) { - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMax), target.getRotation())); - } - } else { - if (!(robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMinBlue)) { - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMax), target.getRotation())); - } - } - } else { // lower bounding box - if (robotState.allianceColor == Color.RED) { - if (!(robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMaxRed)) { - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMin), target.getRotation())); - } - } else { - if (!(robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMinBlue)) { - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMin), target.getRotation())); - } - } - } - } else if (robotState.fieldToVehicle.getY() > Constants.chargeStationThresholdYMin && robotState.fieldToVehicle.getY() < Constants.chargeStationThresholdYMax) { // target is not in the middle but robot is, so add waypoint farther from the scoring nodes - if (target.getY() > Constants.chargeStationThresholdYMax) { // upper bounding box - if (robotState.allianceColor == Color.RED) { - if (robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMinRed) { - var angle = new Rotation2d(Constants.chargeStationThresholdXMinRed - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMax - robotState.fieldToVehicle.getY()); - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinRed, Constants.chargeStationThresholdYMax), target.getRotation())); // upper bounding box - } else if (robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMaxRed) { - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); // initial - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, robotState.fieldToVehicle.getY()), target.getRotation())); // get off charging station - } else { - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); - } - } else { - if (robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMaxBlue) { - var angle = new Rotation2d(Constants.chargeStationThresholdXMaxBlue - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMax - robotState.fieldToVehicle.getY()); - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxBlue, Constants.chargeStationThresholdYMax), target.getRotation())); // upper bounding box - } else if (robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMinBlue) { - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); // initial - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, robotState.fieldToVehicle.getY()), target.getRotation())); // get off charging station - } else { - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); - } - } - } else { // lower bounding box - if (robotState.allianceColor == Color.RED) { - if (robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMinRed) { - var angle = new Rotation2d(Constants.chargeStationThresholdXMinRed - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMin - robotState.fieldToVehicle.getY()); - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinRed, Constants.chargeStationThresholdYMin), target.getRotation())); // upper bounding box - } else if (robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMaxRed) { - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); // initial - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, robotState.fieldToVehicle.getY()), target.getRotation())); // get off charging station - } else { - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); - } - } else { - if (robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMaxBlue) { - var angle = new Rotation2d(Constants.chargeStationThresholdXMaxBlue - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMin - robotState.fieldToVehicle.getY()); - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxBlue, Constants.chargeStationThresholdYMin), target.getRotation())); // upper bounding box - } else if (robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMinBlue) { - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); // initial - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, robotState.fieldToVehicle.getY()), target.getRotation())); // get off charging station - } else { - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); - } - } - } - } else if ( - (target.getY() > Constants.chargeStationThresholdYMax && robotState.fieldToVehicle.getY() < Constants.chargeStationThresholdYMin) || - (target.getY() < Constants.chargeStationThresholdYMin && robotState.fieldToVehicle.getY() > Constants.chargeStationThresholdYMax)) { // target and robot are on opposite sides of the charge station - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); - if (robotState.fieldToVehicle.getY() > Constants.chargeStationThresholdYMax) { // upper bounding box - if (robotState.allianceColor == Color.RED) { - if (!(robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMaxRed)) { - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMax), target.getRotation())); - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMin), Rotation2d.fromDegrees(-90))); - } - } else { - if (!(robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMinBlue)) { - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMax), target.getRotation())); - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMin), Rotation2d.fromDegrees(-90))); - } - } - } else { // lower bounding box - if (robotState.allianceColor == Color.RED) { - if (!(robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMaxRed)) { - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMin), target.getRotation())); - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMax), Rotation2d.fromDegrees(90))); - } - } else { - if (!(robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMinBlue)) { - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMin), target.getRotation())); - waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMax), Rotation2d.fromDegrees(90))); - } - } - } - } else { - waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); - } - waypoints.add(target); - return waypoints; - } - - @Override - protected List getWaypointHeadings() { - List headings = new ArrayList<>(); - headings.add(robotState.fieldToVehicle.getRotation()); - if (robotState.allianceColor == Color.BLUE && robotState.fieldToVehicle.getRotation().getDegrees() < 0) { - for (int i = 1; i < getWaypoints().size(); i++) { - headings.add(target.getRotation().times(-1)); // optimizes blue side wraparound - } - } else { - for (int i = 1; i < getWaypoints().size(); i++) { - headings.add(target.getRotation()); - } - } - return headings; - } - - @Override - public Trajectory getAsTrajectory() { - var translatedVelocity = new Translation2d( - robotState.deltaVehicle.vxMetersPerSecond, - robotState.deltaVehicle.vyMetersPerSecond).rotateBy(robotState.fieldToVehicle.getRotation().unaryMinus() - ); - var translatedChassisSpeeds = new ChassisSpeeds( - translatedVelocity.getX(), - translatedVelocity.getY(), - robotState.deltaVehicle.omegaRadiansPerSecond - ); - return PathUtil.generateTrajectory(usingApp(), translatedChassisSpeeds, getWaypoints()); - } - - @Override - public List getAsTrajectoryHeadings() { - var translatedVelocity = new Translation2d( - robotState.deltaVehicle.vxMetersPerSecond, - robotState.deltaVehicle.vyMetersPerSecond).rotateBy(robotState.fieldToVehicle.getRotation().unaryMinus() - ); - var translatedChassisSpeeds = new ChassisSpeeds( - translatedVelocity.getX(), - translatedVelocity.getY(), - robotState.deltaVehicle.omegaRadiansPerSecond - ); - return PathUtil.generateHeadings( - usingApp(), - getWaypoints(), - getWaypointHeadings(), - translatedChassisSpeeds - ); - } - - @Override - protected boolean usingApp() { - return true; - } -} +package com.team1816.season.auto.paths; + +import com.team1816.lib.Injector; +import com.team1816.lib.auto.Color; +import com.team1816.lib.auto.paths.AutoPath; +import com.team1816.lib.auto.paths.PathUtil; +import com.team1816.season.configuration.Constants; +import com.team1816.season.states.RobotState; +import edu.wpi.first.math.geometry.Pose2d; +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.trajectory.Trajectory; + +import java.util.ArrayList; +import java.util.List; + +public class TrajectoryToTargetPath extends AutoPath { + + public static RobotState robotState; + private static Pose2d target; + + public TrajectoryToTargetPath(Pose2d pose) { + robotState = Injector.get(RobotState.class); + target = pose; + } + + public TrajectoryToTargetPath() { + robotState = Injector.get(RobotState.class); + new TrajectoryToTargetPath(robotState.target); + } + + @Override + public List getWaypoints() { + List waypoints = new ArrayList<>(); + if ( + (target.getY() > Constants.chargeStationThresholdYMin && target.getY() < Constants.chargeStationThresholdYMax) && + (robotState.fieldToVehicle.getY() > Constants.chargeStationThresholdYMin && robotState.fieldToVehicle.getY() < Constants.chargeStationThresholdYMax) + ) { // add two waypoints around the charging station so the path is as safe as possible + if (robotState.allianceColor == Color.RED) { // red side + if (robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMinRed) { // rectangular minimization + // average the y values and determine which vertices to choose + double avg = (robotState.fieldToVehicle.getY() + target.getY()) / 2; + if (avg > (Constants.chargeStationThresholdYMin + Constants.chargeStationThresholdYMax) / 2) { // upper case + var angle = new Rotation2d(Constants.chargeStationThresholdXMinRed - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMax - robotState.fieldToVehicle.getY()); + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinRed, Constants.chargeStationThresholdYMax), target.getRotation())); // upper bounding box + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMax), target.getRotation())); // upper bounding box + } else { // lower case + var angle = new Rotation2d(Constants.chargeStationThresholdXMinRed - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMin - robotState.fieldToVehicle.getY()); + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinRed, Constants.chargeStationThresholdYMin), target.getRotation())); // lower bounding box + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMin), target.getRotation())); // lower bounding box + } + } else { // on the charge station or beyond + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); // straight line + } + } else { // blue side + if (robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMaxBlue) { // rectangular minimization + double avg = (robotState.fieldToVehicle.getY() + target.getY()) / 2; + if (avg > (Constants.chargeStationThresholdYMin + Constants.chargeStationThresholdYMax) / 2) { // upper case + var angle = new Rotation2d(Constants.chargeStationThresholdXMaxBlue - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMax - robotState.fieldToVehicle.getY()); + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxBlue, Constants.chargeStationThresholdYMax), target.getRotation())); // upper bounding box + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMax), target.getRotation())); // upper bounding box + } else { // lower case + var angle = new Rotation2d(Constants.chargeStationThresholdXMaxBlue - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMin - robotState.fieldToVehicle.getY()); + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxBlue, Constants.chargeStationThresholdYMin), target.getRotation())); // lower bounding box + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMin), target.getRotation())); // lower bounding box + } + } else { // on the charge station or beyond + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); // straight line + } + } + } else if ( + target.getY() > Constants.chargeStationThresholdYMin && target.getY() < Constants.chargeStationThresholdYMax + ) { // target is in the middle but robot is not or path goes across the null box, so add waypoint closer to the scoring nodes + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); + if (robotState.fieldToVehicle.getY() > Constants.chargeStationThresholdYMax) { // upper bounding box + if (robotState.allianceColor == Color.RED) { + if (!(robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMaxRed)) { + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMax), target.getRotation())); + } + } else { + if (!(robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMinBlue)) { + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMax), target.getRotation())); + } + } + } else { // lower bounding box + if (robotState.allianceColor == Color.RED) { + if (!(robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMaxRed)) { + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMin), target.getRotation())); + } + } else { + if (!(robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMinBlue)) { + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMin), target.getRotation())); + } + } + } + } else if (robotState.fieldToVehicle.getY() > Constants.chargeStationThresholdYMin && robotState.fieldToVehicle.getY() < Constants.chargeStationThresholdYMax) { // target is not in the middle but robot is, so add waypoint farther from the scoring nodes + if (target.getY() > Constants.chargeStationThresholdYMax) { // upper bounding box + if (robotState.allianceColor == Color.RED) { + if (robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMinRed) { + var angle = new Rotation2d(Constants.chargeStationThresholdXMinRed - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMax - robotState.fieldToVehicle.getY()); + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinRed, Constants.chargeStationThresholdYMax), target.getRotation())); // upper bounding box + } else if (robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMaxRed) { + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); // initial + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, robotState.fieldToVehicle.getY()), target.getRotation())); // get off charging station + } else { + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); + } + } else { + if (robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMaxBlue) { + var angle = new Rotation2d(Constants.chargeStationThresholdXMaxBlue - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMax - robotState.fieldToVehicle.getY()); + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxBlue, Constants.chargeStationThresholdYMax), target.getRotation())); // upper bounding box + } else if (robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMinBlue) { + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); // initial + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, robotState.fieldToVehicle.getY()), target.getRotation())); // get off charging station + } else { + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); + } + } + } else { // lower bounding box + if (robotState.allianceColor == Color.RED) { + if (robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMinRed) { + var angle = new Rotation2d(Constants.chargeStationThresholdXMinRed - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMin - robotState.fieldToVehicle.getY()); + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinRed, Constants.chargeStationThresholdYMin), target.getRotation())); // upper bounding box + } else if (robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMaxRed) { + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); // initial + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, robotState.fieldToVehicle.getY()), target.getRotation())); // get off charging station + } else { + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); + } + } else { + if (robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMaxBlue) { + var angle = new Rotation2d(Constants.chargeStationThresholdXMaxBlue - robotState.fieldToVehicle.getX(), Constants.chargeStationThresholdYMin - robotState.fieldToVehicle.getY()); + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), angle)); // straighter segment to the first edge + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxBlue, Constants.chargeStationThresholdYMin), target.getRotation())); // upper bounding box + } else if (robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMinBlue) { + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); // initial + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, robotState.fieldToVehicle.getY()), target.getRotation())); // get off charging station + } else { + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); + } + } + } + } else if ( + (target.getY() > Constants.chargeStationThresholdYMax && robotState.fieldToVehicle.getY() < Constants.chargeStationThresholdYMin) || + (target.getY() < Constants.chargeStationThresholdYMin && robotState.fieldToVehicle.getY() > Constants.chargeStationThresholdYMax)) { // target and robot are on opposite sides of the charge station + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); + if (robotState.fieldToVehicle.getY() > Constants.chargeStationThresholdYMax) { // upper bounding box + if (robotState.allianceColor == Color.RED) { + if (!(robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMaxRed)) { + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMax), target.getRotation())); + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMin), Rotation2d.fromDegrees(-90))); + } + } else { + if (!(robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMinBlue)) { + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMax), target.getRotation())); + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMin), Rotation2d.fromDegrees(-90))); + } + } + } else { // lower bounding box + if (robotState.allianceColor == Color.RED) { + if (!(robotState.fieldToVehicle.getX() > Constants.chargeStationThresholdXMaxRed)) { + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMin), target.getRotation())); + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMaxRed, Constants.chargeStationThresholdYMax), Rotation2d.fromDegrees(90))); + } + } else { + if (!(robotState.fieldToVehicle.getX() < Constants.chargeStationThresholdXMinBlue)) { + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMin), target.getRotation())); + waypoints.add(new Pose2d(new Translation2d(Constants.chargeStationThresholdXMinBlue, Constants.chargeStationThresholdYMax), Rotation2d.fromDegrees(90))); + } + } + } + } else { + waypoints.add(new Pose2d(robotState.fieldToVehicle.getTranslation(), target.getRotation())); + } + waypoints.add(target); + return waypoints; + } + + @Override + public List getWaypointHeadings() { + List headings = new ArrayList<>(); + headings.add(robotState.fieldToVehicle.getRotation()); + if (robotState.allianceColor == Color.BLUE && robotState.fieldToVehicle.getRotation().getDegrees() < 0) { + for (int i = 1; i < getWaypoints().size(); i++) { + headings.add(target.getRotation().times(-1)); // optimizes blue side wraparound + } + } else { + for (int i = 1; i < getWaypoints().size(); i++) { + headings.add(target.getRotation()); + } + } + return headings; + } + + @Override + public Trajectory getAsTrajectory() { + var translatedVelocity = new Translation2d( + robotState.deltaVehicle.vxMetersPerSecond, + robotState.deltaVehicle.vyMetersPerSecond).rotateBy(robotState.fieldToVehicle.getRotation().unaryMinus() + ); + var translatedChassisSpeeds = new ChassisSpeeds( + translatedVelocity.getX(), + translatedVelocity.getY(), + robotState.deltaVehicle.omegaRadiansPerSecond + ); + return PathUtil.generateTrajectory(translatedChassisSpeeds, getWaypoints()); + } + + @Override + public List getAsTrajectoryHeadings() { + var translatedVelocity = new Translation2d( + robotState.deltaVehicle.vxMetersPerSecond, + robotState.deltaVehicle.vyMetersPerSecond).rotateBy(robotState.fieldToVehicle.getRotation().unaryMinus() + ); + var translatedChassisSpeeds = new ChassisSpeeds( + translatedVelocity.getX(), + translatedVelocity.getY(), + robotState.deltaVehicle.omegaRadiansPerSecond + ); + return PathUtil.generateHeadings( + getWaypoints(), + getWaypointHeadings(), + translatedChassisSpeeds + ); + } +} From 94b71409fca94e382e5521db708f568461697a34 Mon Sep 17 00:00:00 2001 From: Tag Date: Wed, 8 Mar 2023 19:11:33 -0600 Subject: [PATCH 30/31] drivestraight 15->5 (testing) --- .../java/com/team1816/lib/auto/paths/DriveStraightPath.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/team1816/lib/auto/paths/DriveStraightPath.java b/src/main/java/com/team1816/lib/auto/paths/DriveStraightPath.java index 930133bd..56261fe8 100644 --- a/src/main/java/com/team1816/lib/auto/paths/DriveStraightPath.java +++ b/src/main/java/com/team1816/lib/auto/paths/DriveStraightPath.java @@ -21,7 +21,7 @@ public DriveStraightPath(int driveDistance) { } public DriveStraightPath() { - this(15); + this(5); } @Override From 75194b33bbe92d94713a139f5a9f66614ae90210 Mon Sep 17 00:00:00 2001 From: Keerthi Kaashyap Date: Wed, 29 Mar 2023 15:02:34 -0500 Subject: [PATCH 31/31] optimizing calculation process --- build.gradle | 2 +- .../trajectoryUtil/TrajectoryCalculator.java | 17 +++++++++++++---- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/build.gradle b/build.gradle index 6b0e6dde..eb831c1d 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ import org.gradle.nativeplatform.platform.internal.DefaultNativePlatform plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.4.2" + id "edu.wpi.first.GradleRIO" version "2023.4.3" id 'org.jsonschema2dataclass' version '4.0.1' } diff --git a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java index 4656b44e..26a42d15 100644 --- a/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java +++ b/src/main/java/com/team1816/lib/util/trajectoryUtil/TrajectoryCalculator.java @@ -2,12 +2,15 @@ import com.fasterxml.jackson.core.type.TypeReference; import com.fasterxml.jackson.databind.ObjectMapper; +import com.team1816.lib.auto.Symmetry; import com.team1816.lib.auto.paths.AutoPath; import com.team1816.lib.auto.paths.DriveStraightPath; import com.team1816.lib.auto.paths.LivingRoomPath; import com.team1816.lib.auto.paths.PathUtil; +import com.team1816.lib.controlboard.Controller; import com.team1816.lib.hardware.factory.RobotFactory; import com.team1816.season.auto.paths.*; +import com.team1816.season.configuration.Constants; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.Trajectory; @@ -93,13 +96,19 @@ public static void calcAllTrajectoryFormats(String robotName, AutoPath path) { String name = formatClassName(path.getClass().getName()); calcTrajectory(robotName, name, path.getWaypoints()); - calcTrajectory(robotName, name + "_Reflected", path.getReflectedWaypoints()); - calcTrajectory(robotName, name + "_Rotated", path.getRotatedWaypoints()); + if (Constants.fieldSymmetry == Symmetry.AXIS) { + calcTrajectory(robotName, name + "_Reflected", path.getReflectedWaypoints()); + } else if (Constants.fieldSymmetry == Symmetry.ORIGIN) { + calcTrajectory(robotName, name + "_Rotated", path.getRotatedWaypoints()); + } if (path.getWaypointHeadings() != null) { calcHeadings(robotName, name + "Headings", path.getWaypoints(), path.getWaypointHeadings()); - calcHeadings(robotName, name + "Headings_Reflected", path.getReflectedWaypoints(), path.getReflectedWaypointHeadings()); - calcHeadings(robotName, name + "Headings_Rotated", path.getRotatedWaypoints(), path.getRotatedWaypointHeadings()); + if (Constants.fieldSymmetry == Symmetry.AXIS) { + calcHeadings(robotName, name + "Headings_Reflected", path.getReflectedWaypoints(), path.getReflectedWaypointHeadings()); + } else if (Constants.fieldSymmetry == Symmetry.ORIGIN) { + calcHeadings(robotName, name + "Headings_Rotated", path.getRotatedWaypoints(), path.getRotatedWaypointHeadings()); + } } }