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
PathWeaver/Groups/FirstPathGroup
+1
View File
@@ -1,2 +1,3 @@
FirstPath2
FirstPath0 FirstPath0
FirstPath1 FirstPath1
PathWeaver/Paths/FirstPath2
+3
View File
@@ -0,0 +1,3 @@
X,Y,Tangent X,Tangent Y,Fixed Theta,Name
7.2,-0.715,1.5,0.0,true,
6.8264744033479925,-2.6310096463529717,-0.5948912429093554,-0.29744562145467723,true,
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 */ /* Autos */
SixBallAutoMiddle m_sixBallAutoMiddle; SixBallAutoMiddle m_sixBallAutoMiddle;
double m_totalTimeAuto;
/** /**
@@ -260,6 +261,7 @@ public class RobotContainer {
// Run path following command, then stop at the end. // Run path following command, then stop at the end.
try { try {
SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto);
return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
} catch (Exception e) { } catch (Exception e) {
System.err.println("ERROR"); System.err.println("ERROR");
@@ -304,7 +306,9 @@ public class RobotContainer {
public RamseteCommand[] buildPaths(String[] paths) { public RamseteCommand[] buildPaths(String[] paths) {
RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length]; RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length];
double[] times = new double[paths.length];
Trajectory initialTrajectory; Trajectory initialTrajectory;
m_totalTimeAuto = 0;
try { try {
if (true) { if (true) {
@@ -315,6 +319,7 @@ public class RobotContainer {
initialTrajectory = trajectory; initialTrajectory = trajectory;
RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose()));
ramseteCommands[0] = ramseteCommand; ramseteCommands[0] = ramseteCommand;
times[0] = initialTrajectory.getTotalTimeSeconds();
} }
for(int i = 1; i < paths.length; i++) { for(int i = 1; i < paths.length; i++) {
@@ -324,11 +329,16 @@ public class RobotContainer {
Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose()));
ramseteCommands[i] = ramseteCommand; ramseteCommands[i] = ramseteCommand;
times[i] = trajectory.getTotalTimeSeconds();
} }
} catch (Exception e) { } catch (Exception e) {
DriverStation.reportError("Unable to open trajectory", e.getStackTrace()); DriverStation.reportError("Unable to open trajectory", e.getStackTrace());
} }
for (int i = 0; i < times.length; i++) {
m_totalTimeAuto += times[i];
}
return ramseteCommands; return ramseteCommands;
} }