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

Added functionality to get time for trajectory

Browse Source
This commit is contained in:
Aarav Shah
2020-03-09 19:17:44 -06:00
parent afff5226c1
commit 7a0d9e4cda
4 changed files with 15 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/paths/FirstPath2.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/java/frc4388/robot/RobotContainer.java
+10
View File
@@ -92,6 +92,7 @@ public class RobotContainer {
/* Autos */
SixBallAutoMiddle m_sixBallAutoMiddle;
double m_totalTimeAuto;
/**
@@ -260,6 +261,7 @@ public class RobotContainer {
// Run path following command, then stop at the end.
try {
SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto);
return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
} catch (Exception e) {
System.err.println("ERROR");
@@ -304,7 +306,9 @@ public class RobotContainer {
public RamseteCommand[] buildPaths(String[] paths) {
RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length];
double[] times = new double[paths.length];
Trajectory initialTrajectory;
m_totalTimeAuto = 0;
try {
if (true) {
@@ -315,6 +319,7 @@ public class RobotContainer {
initialTrajectory = trajectory;
RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose()));
ramseteCommands[0] = ramseteCommand;
times[0] = initialTrajectory.getTotalTimeSeconds();
}
for(int i = 1; i < paths.length; i++) {
@@ -324,11 +329,16 @@ public class RobotContainer {
Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose()));
ramseteCommands[i] = ramseteCommand;
times[i] = trajectory.getTotalTimeSeconds();
}
} catch (Exception e) {
DriverStation.reportError("Unable to open trajectory", e.getStackTrace());
}
for (int i = 0; i < times.length; i++) {
m_totalTimeAuto += times[i];
}
return ramseteCommands;
}