added speed tests

Author: jhhbrown1

src/main/java/frc/robot/CommandComposer.java

@@ -688,4 +688,16 @@ private static Command getScoringTest(int stationTagID, int level, double distan
 		return c;
 	}
 
+	public static Command forwardBackwardSpeedTest(int iterations, double displacement, double distanceTolerance,
+			double angleToleranceInDegrees) {
+		SequentialCommandGroup g = new SequentialCommandGroup();
+		for (int i = 0; i < iterations; i++)
+			g.addCommands(
+					CommandComposer.moveStraight(displacement, distanceTolerance, angleToleranceInDegrees),
+					new WaitCommand(1.0),
+					CommandComposer.moveStraight(-displacement, distanceTolerance, angleToleranceInDegrees),
+					new WaitCommand(1.0));
+		return g;
+	}
+
 }

src/main/java/frc/robot/Robot.java

@@ -278,10 +278,7 @@ public void addTestingCommands() {
 						m_driveSubsystem.testCommand(kDriveMinSpeed, kTurnMinAngularSpeed, 1.0));
 		m_testingChooser
 				.addOption(
-						"Fastest Forward/Backward Movement Test (5m)",
-						sequence(
-								CommandComposer.moveStraight(5, 0.01, 1),
-								CommandComposer.moveStraight(-5, 0.01, 1)));
+						"Fastest Forward/Backward Movement Test (5m)", forwardBackwardSpeedTest(5, 5, 0.01, 1));
 		m_testingChooser
 				.addOption(
 						"Fastest Rotation Test (5 rotations)",

src/main/java/frc/robot/SwerveModule.java

@@ -166,8 +166,6 @@ public SwerveModuleState getModuleState() {
 	public void setModuleState(SwerveModuleState state) {
 		m_driveMotor.setVoltage(state.speedMetersPerSecond);
 		setAngle(state.angle.getDegrees());
-		if (RobotBase.isSimulation())
-			update();
 	}
 
 	/**