diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidAccelerationException.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidAccelerationException.class index 8e4f8d5..c515dc3 100644 Binary files a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidAccelerationException.class and b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidAccelerationException.class differ diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidFinalPosition.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidFinalPosition.class index 31fc643..a8bbf80 100644 Binary files a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidFinalPosition.class and b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidFinalPosition.class differ diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidNextVelocityFromLastAcceleration.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidNextVelocityFromLastAcceleration.class index 913757b..5059e46 100644 Binary files a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidNextVelocityFromLastAcceleration.class and b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidNextVelocityFromLastAcceleration.class differ diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidTrajectoryLogic.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidTrajectoryLogic.class index 68f2346..7e0cf6c 100644 Binary files a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidTrajectoryLogic.class and b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidTrajectoryLogic.class differ diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidVelocityException.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidVelocityException.class index 72b422c..23ab3a1 100644 Binary files a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidVelocityException.class and b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidVelocityException.class differ diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester.class index d50cfb2..69e9c1b 100644 Binary files a/eclipse-workspace/MotionProfiling/bin/KinematicsTester.class and b/eclipse-workspace/MotionProfiling/bin/KinematicsTester.class differ diff --git a/eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler.class b/eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler.class index 5821cd9..58fdb52 100644 Binary files a/eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler.class and b/eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler.class differ diff --git a/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/RobotMap.java~ b/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/RobotMap.java~ index 47f84a8..3d013ff 100644 --- a/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/RobotMap.java~ +++ b/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/RobotMap.java~ @@ -25,8 +25,8 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindow; */ public class RobotMap { // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS - public static CANTalon driveTrainUpLeft; - public static CANTalon driveTrainUpRight; + public static CANTalon driveTrainFrontLeft; + public static CANTalon driveTrainFrontRight; public static CANTalon driveTrainBackLeft; public static CANTalon driveTrainBackRight; @@ -34,11 +34,11 @@ public class RobotMap { public static void init() { // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS - driveTrainUpLeft = new CANTalon(0); - LiveWindow.addActuator("DriveTrain", "UpLeft", driveTrainUpLeft); + driveTrainFrontLeft = new CANTalon(0); + LiveWindow.addActuator("DriveTrain", "FrontLeft", driveTrainFrontLeft); - driveTrainUpRight = new CANTalon(1); - LiveWindow.addActuator("DriveTrain", "UpRight", driveTrainUpRight); + driveTrainFrontRight = new CANTalon(1); + LiveWindow.addActuator("DriveTrain", "FrontRight", driveTrainFrontRight); driveTrainBackLeft = new CANTalon(2); LiveWindow.addActuator("DriveTrain", "BackLeft", driveTrainBackLeft); diff --git a/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/commands/MoveWithEncoder.java~ b/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/commands/MoveWithEncoder.java~ index 5d4b005..79a6091 100644 --- a/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/commands/MoveWithEncoder.java~ +++ b/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/commands/MoveWithEncoder.java~ @@ -69,7 +69,7 @@ public class MoveWithEncoder extends Command { Robot.driveTrain.moveToEncoderRevolutions(trajectoryPoint.m_position * m_positionToEncoderRevolutionsRatio); m_PIDOut = Robot.driveTrain.getEncoderPIDOutput(); - Robot.driveTrain.setDriveControllers(trajectoryPoint.m_currentVelocity * m_velocityToMotorOutputRatio + Robot.driveTrain.setAllDriveControllers(trajectoryPoint.m_currentVelocity * m_velocityToMotorOutputRatio + trajectoryPoint.m_acceleration * m_accelerationToMotorOutputRatio + m_PIDOut); } diff --git a/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/commands/RunWheelsAndLog.java~ b/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/commands/RunWheelsAndLog.java~ new file mode 100644 index 0000000..a4e1384 --- /dev/null +++ b/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/commands/RunWheelsAndLog.java~ @@ -0,0 +1,94 @@ +// RobotBuilder Version: 2.0 +// +// This file was generated by RobotBuilder. It contains sections of +// code that are automatically generated and assigned by robotbuilder. +// These sections will be updated in the future when you export to +// Java from RobotBuilder. Do not put any code or make any change in +// the blocks indicating autogenerated code or it will be lost on an +// update. Deleting the comments indicating the section will prevent +// it from being updated in the future. + +package org.usfirst.frc4905.MotionProfiling.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; + +import java.util.Vector; + +import org.usfirst.frc4905.MotionProfiling.Robot; + +import Utilities.Trace; + +/** + * + */ +public class RunWheelsAndLog extends Command { + Vector m_header = new Vector(); + double m_initialTimeStamp = 0.0; + double m_previousVelocity = 0.0; + double m_previousTimeStamp = 0.0; + // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS + + // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS + + // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR + public RunWheelsAndLog() { + + // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR + // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING + + // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING + // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES + requires(Robot.driveTrain); + + // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES + } + + // Called just before this Command runs the first time + protected void initialize() { + Vector header = new Vector(); + header.add(new String("Velocities")); + header.add(new String("Acceleration")); + header.addElement(new String("Time Between Runs")); + Trace.getInstance().addTrace("Motion Profiling Data", header); + m_header = header; + m_initialTimeStamp = Timer.getFPGATimestamp(); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Robot.driveTrain.setAllDriveControllers(1.0); + + Vector entry = new Vector(); + double currentTimeStamp = Timer.getFPGATimestamp(); + double currentDeltaTimeStampFromStart = currentTimeStamp - m_initialTimeStamp; + double deltaTimeFromLastExecute = currentTimeStamp - m_previousTimeStamp; + + double currentPosition = Robot.driveTrain.getAllEncoderPosition(); + double currentVelocity = currentPosition / currentDeltaTimeStampFromStart; + double currentAcceleration = currentVelocity - m_previousVelocity; + + entry.add(currentVelocity); + entry.add(currentAcceleration); + entry.add(deltaTimeFromLastExecute); + + Trace.getInstance().addEntry("Motion Profiling Data", entry); + + m_previousTimeStamp = currentTimeStamp; + m_previousVelocity = currentVelocity; + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } +} diff --git a/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain.java~ b/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain.java~ index af0add8..9d7c88f 100644 --- a/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain.java~ +++ b/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain.java~ @@ -32,8 +32,8 @@ public class DriveTrain extends Subsystem { // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS - private final CANTalon upLeft = RobotMap.driveTrainUpLeft; - private final CANTalon upRight = RobotMap.driveTrainUpRight; + private final CANTalon frontLeft = RobotMap.driveTrainFrontLeft; + private final CANTalon frontRight = RobotMap.driveTrainFrontRight; private final CANTalon backLeft = RobotMap.driveTrainBackLeft; private final CANTalon backRight = RobotMap.driveTrainBackRight; @@ -58,21 +58,27 @@ public class DriveTrain extends Subsystem { double frontRight; double frontLeft; } + public double getEncoderPIDOutput() { return m_encoderPIDOutput; } - public void setDriveControllers(double value) { - + + public void setAllDriveControllers(double value) { + backLeft.set(value); + backRight.set(value); + frontLeft.set(value); + frontRight.set(value); } - private double getEncoderPosition() { - return 0.0; + public double getAllEncoderPosition() { + return backLeft.get() + backRight.get() + frontLeft.get() + frontRight.get(); } + private void resetEncoderPosition(){ - m_resetEncoderPositions.backLeft = 0.0; - m_resetEncoderPositions.backRight = 0.0; - m_resetEncoderPositions.frontRight = 0.0; - m_resetEncoderPositions.frontLeft = 0.0; + m_resetEncoderPositions.backLeft = backLeft.get(); + m_resetEncoderPositions.backRight = backRight.get(); + m_resetEncoderPositions.frontRight = frontRight.get(); + m_resetEncoderPositions.frontLeft = frontLeft.get(); } // Encoder PID controller private PIDController m_moveWithEncoderPID; @@ -95,7 +101,7 @@ public class DriveTrain extends Subsystem { @Override public double pidGet() { - return getEncoderPosition(); + return getAllEncoderPosition(); } } diff --git a/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/subsystems/Kinematics.class b/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/subsystems/Kinematics.class deleted file mode 100644 index a68976a..0000000 Binary files a/eclipse-workspace/MotionProfiling/bin/org/usfirst/frc4905/MotionProfiling/subsystems/Kinematics.class and /dev/null differ diff --git a/eclipse-workspace/MotionProfiling/build/KinematicsTester$InvalidAccelerationException.class b/eclipse-workspace/MotionProfiling/build/KinematicsTester$InvalidAccelerationException.class new file mode 100644 index 0000000..a33a410 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/KinematicsTester$InvalidAccelerationException.class differ diff --git a/eclipse-workspace/MotionProfiling/build/KinematicsTester$InvalidFinalPosition.class b/eclipse-workspace/MotionProfiling/build/KinematicsTester$InvalidFinalPosition.class new file mode 100644 index 0000000..265c52d Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/KinematicsTester$InvalidFinalPosition.class differ diff --git a/eclipse-workspace/MotionProfiling/build/KinematicsTester$InvalidNextVelocityFromLastAcceleration.class b/eclipse-workspace/MotionProfiling/build/KinematicsTester$InvalidNextVelocityFromLastAcceleration.class new file mode 100644 index 0000000..eac9aa7 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/KinematicsTester$InvalidNextVelocityFromLastAcceleration.class differ diff --git a/eclipse-workspace/MotionProfiling/build/KinematicsTester$InvalidTrajectoryLogic.class b/eclipse-workspace/MotionProfiling/build/KinematicsTester$InvalidTrajectoryLogic.class new file mode 100644 index 0000000..293d555 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/KinematicsTester$InvalidTrajectoryLogic.class differ diff --git a/eclipse-workspace/MotionProfiling/build/KinematicsTester$InvalidVelocityException.class b/eclipse-workspace/MotionProfiling/build/KinematicsTester$InvalidVelocityException.class new file mode 100644 index 0000000..0138d03 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/KinematicsTester$InvalidVelocityException.class differ diff --git a/eclipse-workspace/MotionProfiling/build/KinematicsTester.class b/eclipse-workspace/MotionProfiling/build/KinematicsTester.class index 429f30d..8817cae 100644 Binary files a/eclipse-workspace/MotionProfiling/build/KinematicsTester.class and b/eclipse-workspace/MotionProfiling/build/KinematicsTester.class differ diff --git a/eclipse-workspace/MotionProfiling/build/Utilities/MultipleOutputStream.class b/eclipse-workspace/MotionProfiling/build/Utilities/MultipleOutputStream.class new file mode 100644 index 0000000..d909119 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/Utilities/MultipleOutputStream.class differ diff --git a/eclipse-workspace/MotionProfiling/build/Utilities/Pair.class b/eclipse-workspace/MotionProfiling/build/Utilities/Pair.class new file mode 100644 index 0000000..4cb8eb5 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/Utilities/Pair.class differ diff --git a/eclipse-workspace/MotionProfiling/build/Utilities/SideOfField.class b/eclipse-workspace/MotionProfiling/build/Utilities/SideOfField.class new file mode 100644 index 0000000..fb4fee7 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/Utilities/SideOfField.class differ diff --git a/eclipse-workspace/MotionProfiling/build/Utilities/Trace$TraceEntry.class b/eclipse-workspace/MotionProfiling/build/Utilities/Trace$TraceEntry.class new file mode 100644 index 0000000..857ccd6 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/Utilities/Trace$TraceEntry.class differ diff --git a/eclipse-workspace/MotionProfiling/build/Utilities/Trace.class b/eclipse-workspace/MotionProfiling/build/Utilities/Trace.class new file mode 100644 index 0000000..a7e4575 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/Utilities/Trace.class differ diff --git a/eclipse-workspace/MotionProfiling/build/Utilities/TurnDirection$1.class b/eclipse-workspace/MotionProfiling/build/Utilities/TurnDirection$1.class new file mode 100644 index 0000000..a4453d5 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/Utilities/TurnDirection$1.class differ diff --git a/eclipse-workspace/MotionProfiling/build/Utilities/TurnDirection.class b/eclipse-workspace/MotionProfiling/build/Utilities/TurnDirection.class new file mode 100644 index 0000000..f4cfadc Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/Utilities/TurnDirection.class differ diff --git a/eclipse-workspace/MotionProfiling/build/jars/CTRLib.jar b/eclipse-workspace/MotionProfiling/build/jars/CTRLib.jar new file mode 100644 index 0000000..d66b399 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/jars/CTRLib.jar differ diff --git a/eclipse-workspace/MotionProfiling/build/jars/NetworkTables.jar b/eclipse-workspace/MotionProfiling/build/jars/NetworkTables.jar new file mode 100644 index 0000000..3b578a3 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/jars/NetworkTables.jar differ diff --git a/eclipse-workspace/MotionProfiling/build/jars/WPILib.jar b/eclipse-workspace/MotionProfiling/build/jars/WPILib.jar new file mode 100644 index 0000000..fa955eb Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/jars/WPILib.jar differ diff --git a/eclipse-workspace/MotionProfiling/build/jars/cscore.jar b/eclipse-workspace/MotionProfiling/build/jars/cscore.jar new file mode 100644 index 0000000..7b95664 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/jars/cscore.jar differ diff --git a/eclipse-workspace/MotionProfiling/build/jars/opencv.jar b/eclipse-workspace/MotionProfiling/build/jars/opencv.jar new file mode 100644 index 0000000..5edacd2 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/jars/opencv.jar differ diff --git a/eclipse-workspace/MotionProfiling/build/kinematics/Kinematics$Point.class b/eclipse-workspace/MotionProfiling/build/kinematics/Kinematics$Point.class index 29f1fe2..c35924a 100644 Binary files a/eclipse-workspace/MotionProfiling/build/kinematics/Kinematics$Point.class and b/eclipse-workspace/MotionProfiling/build/kinematics/Kinematics$Point.class differ diff --git a/eclipse-workspace/MotionProfiling/build/kinematics/Kinematics.class b/eclipse-workspace/MotionProfiling/build/kinematics/Kinematics.class index 75aaec9..9c3d66d 100644 Binary files a/eclipse-workspace/MotionProfiling/build/kinematics/Kinematics.class and b/eclipse-workspace/MotionProfiling/build/kinematics/Kinematics.class differ diff --git a/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$1.class b/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$1.class new file mode 100644 index 0000000..204ffa0 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$1.class differ diff --git a/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$InvalidDimentionException.class b/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$InvalidDimentionException.class new file mode 100644 index 0000000..00c1fcb Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$InvalidDimentionException.class differ diff --git a/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$Path.class b/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$Path.class new file mode 100644 index 0000000..9665ef9 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$Path.class differ diff --git a/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$Point.class b/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$Point.class new file mode 100644 index 0000000..8585a46 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$Point.class differ diff --git a/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$TrajectoryPaths.class b/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$TrajectoryPaths.class new file mode 100644 index 0000000..783e2b9 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$TrajectoryPaths.class differ diff --git a/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$TrajectoryPoint.class b/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$TrajectoryPoint.class new file mode 100644 index 0000000..caea206 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler$TrajectoryPoint.class differ diff --git a/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler.class b/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler.class new file mode 100644 index 0000000..2b080fe Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/kinematics/KinematicsSimpler.class differ diff --git a/eclipse-workspace/MotionProfiling/build/kinematics/package-info.class b/eclipse-workspace/MotionProfiling/build/kinematics/package-info.class new file mode 100644 index 0000000..af63617 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/kinematics/package-info.class differ diff --git a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/OI.class b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/OI.class index 3671497..4bb7d20 100644 Binary files a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/OI.class and b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/OI.class differ diff --git a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/Robot.class b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/Robot.class index 017990c..8149d8a 100644 Binary files a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/Robot.class and b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/Robot.class differ diff --git a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/RobotMap.class b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/RobotMap.class index 03156ba..274358c 100644 Binary files a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/RobotMap.class and b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/RobotMap.class differ diff --git a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/commands/MotionProfilingTesting.class b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/commands/MotionProfilingTesting.class index 3de6d4a..3b4722f 100644 Binary files a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/commands/MotionProfilingTesting.class and b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/commands/MotionProfilingTesting.class differ diff --git a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/commands/MoveWithEncoder.class b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/commands/MoveWithEncoder.class new file mode 100644 index 0000000..efcfce1 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/commands/MoveWithEncoder.class differ diff --git a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/commands/RunWheelsAndLog.class b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/commands/RunWheelsAndLog.class new file mode 100644 index 0000000..a889d37 Binary files /dev/null and b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/commands/RunWheelsAndLog.class differ diff --git a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain$EncoderPIDin.class b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain$EncoderPIDin.class index 985e376..7369180 100644 Binary files a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain$EncoderPIDin.class and b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain$EncoderPIDin.class differ diff --git a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain$EncoderPIDout.class b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain$EncoderPIDout.class index 1a1bde9..fca1e46 100644 Binary files a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain$EncoderPIDout.class and b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain$EncoderPIDout.class differ diff --git a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain$ResetEncoderPositions.class b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain$ResetEncoderPositions.class index c8c3bac..8adadc0 100644 Binary files a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain$ResetEncoderPositions.class and b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain$ResetEncoderPositions.class differ diff --git a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain.class b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain.class index f9648a6..9c81500 100644 Binary files a/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain.class and b/eclipse-workspace/MotionProfiling/build/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain.class differ diff --git a/eclipse-workspace/MotionProfiling/dist/FRCUserProgram.jar b/eclipse-workspace/MotionProfiling/dist/FRCUserProgram.jar new file mode 100644 index 0000000..c78e3fc Binary files /dev/null and b/eclipse-workspace/MotionProfiling/dist/FRCUserProgram.jar differ diff --git a/eclipse-workspace/MotionProfiling/src/KinematicsTester.java b/eclipse-workspace/MotionProfiling/src/KinematicsTester.java index 7cae848..b120444 100644 --- a/eclipse-workspace/MotionProfiling/src/KinematicsTester.java +++ b/eclipse-workspace/MotionProfiling/src/KinematicsTester.java @@ -9,945 +9,729 @@ public class KinematicsTester { static KinematicsSimpler m_kinematicsSimpler = new KinematicsSimpler(); public static void main(String[] args) { - - createPositiveTrajectoryGreaterThanTheDistanceCoveredWhileAcceleratingCase(); - createPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + try { + createPositiveTrajectoryGreaterThanTheDistanceCoveredWhileAcceleratingCase(); - createNegativeTrajectoryGreaterThanDistanceCoveredWhileAcceleratingCase(); + createPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - createNegativeTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + createNegativeTrajectoryGreaterThanDistanceCoveredWhileAcceleratingCase(); - - createPositiveTrajectoryToNegativeTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + createNegativeTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - createNegativeTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + createPositiveTrajectoryToNegativeTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - createPositiveTrajectoryToNegativeTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + createNegativeTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - createNegativeTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + createPositiveTrajectoryToNegativeTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - createNegativeTrajectoryToPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + createNegativeTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - createPositiveTrajectoryToNegativeTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - - - createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTheDistanceCoveredWhileAcceleratingCase(); - - createPositiveTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - - createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - - createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - - createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTheDistanceCoveredWhileAcceleratingCase(); - - createPositiveTrajectoryToPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - - createPositiveTrajectoryToPositiveTrajectoryLessThanTheDistanceCoveredWhileAcceleratingCase(); - - createPositiveTrajectoryToPositiveTrajectoryLessThanAndGreaterThanTheDistanceCoveredWhileAcceleratingCase(); - - - createNegativeTrajectoryToNegativeTrajectory1stPointLessThanTheDistanceCoveredWhileAcceleratingCase(); - - createNegativeTrajectoryToNegativeTrajectory1stPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - - createNegativeTrajectoryToNegativeTrajectory2ndPointLessThanTheDistanceCoveredWhileAcceleratingCase(); - - createNegativeTrajectoryToNegativeTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - - createNegativeTrajectoryToNegativeTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - - createNegativeTrajectoryToNegativeTrajectoryLessThanAndGreaterThanTheDistanceCoveredWhileAcceleratingCase(); - - createNegativeTrajectoryToNegativeTrajectoryLessThanTheDistanceCoveredWhileAcceleratingCase(); - - createNegativeTrajectoryToNegativeTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - - - createPositiveTrajectoryWithACustomMaxAccelerationGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - - createNegativeTrajectoryWithACustomMaxAccelerationGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - - createPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingWithACustomMaxAccelerationCase(); - - - createPositiveTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase(); - - createPositiveTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase(); - - createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase(); - - createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase(); - + createNegativeTrajectoryToPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + + createPositiveTrajectoryToNegativeTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + + createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTheDistanceCoveredWhileAcceleratingCase(); + + createPositiveTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + + createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + + createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + + createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTheDistanceCoveredWhileAcceleratingCase(); + + createPositiveTrajectoryToPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + + createPositiveTrajectoryToPositiveTrajectoryLessThanTheDistanceCoveredWhileAcceleratingCase(); + + createPositiveTrajectoryToPositiveTrajectoryLessThanAndGreaterThanTheDistanceCoveredWhileAcceleratingCase(); + + createNegativeTrajectoryToNegativeTrajectory1stPointLessThanTheDistanceCoveredWhileAcceleratingCase(); + + createNegativeTrajectoryToNegativeTrajectory1stPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + + createNegativeTrajectoryToNegativeTrajectory2ndPointLessThanTheDistanceCoveredWhileAcceleratingCase(); + + createNegativeTrajectoryToNegativeTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + + createNegativeTrajectoryToNegativeTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + + createNegativeTrajectoryToNegativeTrajectoryLessThanAndGreaterThanTheDistanceCoveredWhileAcceleratingCase(); + + createNegativeTrajectoryToNegativeTrajectoryLessThanTheDistanceCoveredWhileAcceleratingCase(); + + createNegativeTrajectoryToNegativeTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + + createPositiveTrajectoryWithACustomMaxVelocityGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + + createNegativeTrajectoryWithACustomMaxVelocityGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase(); + + createPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingWithACustomMaxVelocityCase(); + + createPositiveTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase(); + + createPositiveTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase(); + + createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase(); + + createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase(); + + createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTwiceTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase(); + + createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase(); + + createPositiveTrajectoryToPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase(); + + createPositiveTrajectoryToPositiveTrajectoryLessThanTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase(); + + createPositiveTrajectoryToPositiveTrajectoryLessThanAndGreaterThanTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase(); + + createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase(); + } catch (InvalidDimentionException | InvalidVelocityException | InvalidNextVelocityFromLastAcceleration + | InvalidAccelerationException | InvalidFinalPosition | InvalidTrajectoryLogic e) { + // TODO Auto-generated catch block + e.printStackTrace(); } - private static void createPositiveTrajectoryGreaterThanTheDistanceCoveredWhileAcceleratingCase() { + } + + private static void createPositiveTrajectoryGreaterThanTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester); } - private static void createPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + private static void createPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(6)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(6)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); } - private static void createNegativeTrajectoryGreaterThanDistanceCoveredWhileAcceleratingCase() { + private static void createNegativeTrajectoryGreaterThanDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-10)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-10)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester); + } - private static void createNegativeTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + private static void createNegativeTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-6)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-6)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createPositiveTrajectoryToNegativeTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + private static void createPositiveTrajectoryToNegativeTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-2)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-2)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester); } - private static void createPositiveTrajectoryToNegativeTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + private static void createPositiveTrajectoryToNegativeTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(8)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(8)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createNegativeTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + private static void createNegativeTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-10)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(2)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-10)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(2)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester); + } - private static void createNegativeTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + private static void createNegativeTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-10)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-8)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-10)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-8)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createNegativeTrajectoryToPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + private static void createNegativeTrajectoryToPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-6)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-2)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-6)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-2)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createPositiveTrajectoryToNegativeTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + private static void createPositiveTrajectoryToNegativeTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(6)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(2)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(6)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(2)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createNegativeTrajectoryToNegativeTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + private static void createNegativeTrajectoryToNegativeTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-10)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-20)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-10)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-20)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createNegativeTrajectoryToNegativeTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + private static void createNegativeTrajectoryToNegativeTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-10)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-16)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-10)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-16)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createNegativeTrajectoryToNegativeTrajectory2ndPointLessThanTheDistanceCoveredWhileAcceleratingCase() { + + private static void createNegativeTrajectoryToNegativeTrajectory2ndPointLessThanTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-10)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-12)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-10)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-12)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createNegativeTrajectoryToNegativeTrajectory1stPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + private static void createNegativeTrajectoryToNegativeTrajectory1stPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-6)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-20)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-6)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-20)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createNegativeTrajectoryToNegativeTrajectory1stPointLessThanTheDistanceCoveredWhileAcceleratingCase() { + + private static void createNegativeTrajectoryToNegativeTrajectory1stPointLessThanTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-2)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-20)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-2)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-20)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createNegativeTrajectoryToNegativeTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + private static void createNegativeTrajectoryToNegativeTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-5)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-7)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-5)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-7)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createNegativeTrajectoryToNegativeTrajectoryLessThanTheDistanceCoveredWhileAcceleratingCase() { + + private static void createNegativeTrajectoryToNegativeTrajectoryLessThanTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-1)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-3)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-1)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-3)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createNegativeTrajectoryToNegativeTrajectoryLessThanAndGreaterThanTheDistanceCoveredWhileAcceleratingCase() { + + private static void createNegativeTrajectoryToNegativeTrajectoryLessThanAndGreaterThanTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-2)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-6)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-2)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-6)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + + private static void createPositiveTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + private static void createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(16)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(16)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTheDistanceCoveredWhileAcceleratingCase() { + + private static void createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(12)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(12)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(6)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(6)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTheDistanceCoveredWhileAcceleratingCase() { + + private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(2)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(2)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createPositiveTrajectoryToPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + private static void createPositiveTrajectoryToPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(5)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(7)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(5)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(7)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryToPositiveTrajectoryLessThanTheDistanceCoveredWhileAcceleratingCase() { + + private static void createPositiveTrajectoryToPositiveTrajectoryLessThanTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(3)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(3)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryToPositiveTrajectoryLessThanAndGreaterThanTheDistanceCoveredWhileAcceleratingCase() { + + private static void createPositiveTrajectoryToPositiveTrajectoryLessThanAndGreaterThanTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(2)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(6)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(2)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(6)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryWithACustomMaxAccelerationGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + + private static void createPositiveTrajectoryWithACustomMaxVelocityGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10), 1.0); - - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10), 1.0); + + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createNegativeTrajectoryWithACustomMaxAccelerationGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase() { + + private static void createNegativeTrajectoryWithACustomMaxVelocityGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-10), 1.0); - - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-10), 1.0); + + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingWithACustomMaxAccelerationCase() { + + private static void createPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingWithACustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1.5), 1.0); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1.5), 1.0); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() { + + private static void createPositiveTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10), 1.0); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10), 1.0); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() { + private static void createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10), 1.0); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(11.5)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10), 1.0); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(11.5)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() { + + private static void createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10),1.0); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10.5)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - printTrajectory(myPath); - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10), 1.0); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10.5)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTwiceTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() { + private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTwiceTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1.5),1.0); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1.5), 1.0); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() { + + private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(0.5),1.0); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(0.25), 1.0); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createPositiveTrajectoryToPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() { + private static void createPositiveTrajectoryToPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1.25),1.0); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1.5)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1.25), 1.0); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1.5)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryToPositiveTrajectoryLessThanTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() { + + private static void createPositiveTrajectoryToPositiveTrajectoryLessThanTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(0.25),1.0); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(0.5)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(0.25), 1.0); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(0.5)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryToPositiveTrajectoryLessThanAndGreaterThanTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() { + + private static void createPositiveTrajectoryToPositiveTrajectoryLessThanAndGreaterThanTheDistanceCoveredWhileAcceleratingFirstPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(0.5),1.0); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1.5)); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(0.5), 1.0); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1.5)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() { + + private static void createPositiveTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20),1.0); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20), 1.0); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() { + private static void createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(16),1.0); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(16), 1.0); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + printTrajectory(myPath); + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() { + + private static void createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(12),1.0); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(10)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(12), 1.0); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTwiceTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() { + private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTwiceTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(6)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20),1.0); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(6)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20), 1.0); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() { + + private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThanTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(2)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20),1.0); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(2)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(20), 1.0); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - private static void createPositiveTrajectoryToPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() { + private static void createPositiveTrajectoryToPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(5)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(7),1.0); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(5)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(7), 1.0); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryToPositiveTrajectoryLessThanTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() { + + private static void createPositiveTrajectoryToPositiveTrajectoryLessThanTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(3),1.0); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(3), 1.0); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - - private static void createPositiveTrajectoryToPositiveTrajectoryLessThanAndGreaterThanTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() { + + private static void createPositiveTrajectoryToPositiveTrajectoryLessThanAndGreaterThanTheDistanceCoveredWhileAcceleratingSecondPointWithCustomMaxVelocityCase() + throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, + InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - try { - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(2)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(6),1.0); - m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); - } catch (InvalidDimentionException e) { - e.printStackTrace(); - } - - try { - checkTrajectoryPath(myPath, kinematicsTester1); - } catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException - | InvalidFinalPosition | InvalidTrajectoryLogic e) { - e.printStackTrace(); - } + + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(2)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(6), 1.0); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5); + + checkTrajectoryPath(myPath, kinematicsTester1); + } - + private static void checkTrajectoryPath(Path Key, KinematicsTester kinematicsTester) throws InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic { - + checkAcceleration(Key, kinematicsTester); - + checkVelocity(Key, kinematicsTester); - + checkVelocityTakingCustomMaxVelocityIntoAccount(Key, kinematicsTester); - + checkFinalPosition(Key, kinematicsTester); - + checkTrajectoryLogic(Key, kinematicsTester); } @@ -974,8 +758,8 @@ private static void checkAcceleration(Path Key, KinematicsTester kinematicsTeste String errMessage; for (int i = 0; i < Key.getTrajectoryVector().size(); i++) { TrajectoryPoint currentPoint = Key.getTrajectoryVector().get(i); - if (currentPoint.m_acceleration > (Key.getmaxAcceleration() - * KinematicsSimpler.getTrajectoryPointInterval() + 0.1) + if (currentPoint.m_acceleration > (Key.getmaxAcceleration() * KinematicsSimpler.getTrajectoryPointInterval() + + 0.1) || currentPoint.m_acceleration < (-Key.getmaxAcceleration() * KinematicsSimpler.getTrajectoryPointInterval() - 0.1)) { errMessage = "The Current Acceleration at time: " + currentPoint.m_timestamp @@ -990,15 +774,15 @@ private static void checkFinalPosition(Path Key, KinematicsTester kinematicsTest InvalidFinalPosition invalidFinalPosition; String errMesage; try { - if (Key.getTrajectoryVector().get(Key.getTrajectoryVector().size() - 1).m_position - - Key.getSetpointVector().get(Key.getSetpointVector().size() - 1).getm_X() > 0.1 - || Key.getTrajectoryVector().get(Key.getTrajectoryVector().size() - 1).m_position - - Key.getSetpointVector().get(Key.getSetpointVector().size() - 1).getm_X() < -0.1) { - errMesage = "The final position of the trajectory path does not match the final position of the setpoint path!"; - invalidFinalPosition = kinematicsTester.new InvalidFinalPosition(errMesage); - throw invalidFinalPosition; - } - }catch(ArrayIndexOutOfBoundsException q) { + if (Key.getTrajectoryVector().get(Key.getTrajectoryVector().size() - 1).m_position + - Key.getSetpointVector().get(Key.getSetpointVector().size() - 1).getm_X() > 0.1 + || Key.getTrajectoryVector().get(Key.getTrajectoryVector().size() - 1).m_position + - Key.getSetpointVector().get(Key.getSetpointVector().size() - 1).getm_X() < -0.1) { + errMesage = "The final position of the trajectory path does not match the final position of the setpoint path!"; + invalidFinalPosition = kinematicsTester.new InvalidFinalPosition(errMesage); + throw invalidFinalPosition; + } + } catch (ArrayIndexOutOfBoundsException q) { errMesage = "There are no points in this path!"; invalidFinalPosition = kinematicsTester.new InvalidFinalPosition(errMesage); throw invalidFinalPosition; @@ -1027,90 +811,99 @@ private static void checkNextVelocityFromLastAcceleration(Path Key, KinematicsTe } } } - - private static void checkVelocityTakingCustomMaxVelocityIntoAccount(Path Key, KinematicsTester kinematicsTester) throws InvalidVelocityException { + + private static void checkVelocityTakingCustomMaxVelocityIntoAccount(Path Key, KinematicsTester kinematicsTester) + throws InvalidVelocityException { InvalidVelocityException invalidVelocityException; String errMessage; int currentTrajectoryIndex = 0; - for(int i=0; i < Key.getSetpointVector().size(); i++) { + for (int i = 0; i < Key.getSetpointVector().size(); i++) { Point setpoint = Key.getSetpointVector().get(i); - for(int i1 = currentTrajectoryIndex; i1 < Key.getTrajectoryVector().size(); i1++) { + for (int i1 = currentTrajectoryIndex; i1 < Key.getTrajectoryVector().size(); i1++) { TrajectoryPoint trajectoryPoint = Key.getTrajectoryVector().get(i1); - - if(Math.abs(setpoint.getMaxVelocity()) < Math.abs(trajectoryPoint.m_currentVelocity)) { - errMessage = "The velocity at time: " + trajectoryPoint.m_timestamp + " is above the maximum velocity specified by the user which is: " + setpoint.getMaxVelocity(); + + if (Math.abs(setpoint.getMaxVelocity()) + 0.2 < Math.abs(trajectoryPoint.m_currentVelocity)) { + errMessage = "The velocity at time: " + trajectoryPoint.m_timestamp + + " is above the maximum velocity specified by the user which is: " + + setpoint.getMaxVelocity(); invalidVelocityException = kinematicsTester.new InvalidVelocityException(errMessage); throw invalidVelocityException; } - if(trajectoryPoint.m_position - setpoint.getm_X() < 0.001 && trajectoryPoint.m_position - setpoint.getm_X() > -0.001) { + if (trajectoryPoint.m_position - setpoint.getm_X() < 0.001 + && trajectoryPoint.m_position - setpoint.getm_X() > -0.001) { break; } currentTrajectoryIndex++; } } } - - private static void checkTrajectoryLogic(Path Key, KinematicsTester kinematicsTester) throws InvalidTrajectoryLogic { + + private static void checkTrajectoryLogic(Path Key, KinematicsTester kinematicsTester) + throws InvalidTrajectoryLogic { InvalidTrajectoryLogic invalidTrajectoryLogic; String errMessage; int currentTrajectoryPointIndex = 0; for (int i = 0; i < Key.getSetpointVector().size(); i++) { Point setpoint = Key.getSetpointVector().get(i); Point nextSetpoint; - Point lastSetpoint = m_kinematicsSimpler.new Point(0,0); + Point lastSetpoint = m_kinematicsSimpler.new Point(0, 0); boolean traveledInAPositiveDirection; boolean willTravelInAPositiveDirection; TrajectoryPoint trajectoryPoint = Key.getTrajectoryVector().get(currentTrajectoryPointIndex); int initialCurrentTrajectoryPointIndex = currentTrajectoryPointIndex; - for(int a = currentTrajectoryPointIndex; a < Key.getTrajectoryVector().size(); a++) { + for (int a = currentTrajectoryPointIndex; a < Key.getTrajectoryVector().size(); a++) { currentTrajectoryPointIndex++; - trajectoryPoint = Key.getTrajectoryVector().get(a); - if(trajectoryPoint.m_position - setpoint.getm_X() < 0.01 && trajectoryPoint.m_position - setpoint.getm_X() > -0.01) { - + trajectoryPoint = Key.getTrajectoryVector().get(a); + if (trajectoryPoint.m_position - setpoint.getm_X() < 0.01 + && trajectoryPoint.m_position - setpoint.getm_X() > -0.01) { + break; } } - - if(currentTrajectoryPointIndex == (Key.getTrajectoryVector().size())) { + + if (currentTrajectoryPointIndex == (Key.getTrajectoryVector().size())) { currentTrajectoryPointIndex = initialCurrentTrajectoryPointIndex; continue; } - - try{ - nextSetpoint = Key.getSetpointVector().get(i+1); - }catch(ArrayIndexOutOfBoundsException a) { + + try { + nextSetpoint = Key.getSetpointVector().get(i + 1); + } catch (ArrayIndexOutOfBoundsException a) { continue; } - try{ - lastSetpoint = Key.getSetpointVector().get(i-1); - }catch(ArrayIndexOutOfBoundsException a) { - + try { + lastSetpoint = Key.getSetpointVector().get(i - 1); + } catch (ArrayIndexOutOfBoundsException a) { + } - - if(lastSetpoint.getm_X() < setpoint.getm_X()) { + + if (lastSetpoint.getm_X() < setpoint.getm_X()) { traveledInAPositiveDirection = true; - }else { + } else { traveledInAPositiveDirection = false; } - - if(setpoint.getm_X() < nextSetpoint.getm_X()) { + + if (setpoint.getm_X() < nextSetpoint.getm_X()) { willTravelInAPositiveDirection = true; - }else { + } else { willTravelInAPositiveDirection = false; } - if(trajectoryPoint.m_timestamp == 0.0) { + if (trajectoryPoint.m_timestamp == 0.0) { continue; } - - if((traveledInAPositiveDirection && willTravelInAPositiveDirection) || (!traveledInAPositiveDirection && !willTravelInAPositiveDirection)) { - if(trajectoryPoint.m_currentVelocity < 0.1 && trajectoryPoint.m_currentVelocity > -0.1) { - errMessage = "The point at time: " + trajectoryPoint.m_timestamp + " is a point in which the direction is not changing however the velocity is 0!"; + + if ((traveledInAPositiveDirection && willTravelInAPositiveDirection) + || (!traveledInAPositiveDirection && !willTravelInAPositiveDirection)) { + if (trajectoryPoint.m_currentVelocity < 0.1 && trajectoryPoint.m_currentVelocity > -0.1) { + errMessage = "The point at time: " + trajectoryPoint.m_timestamp + + " is a point in which the direction is not changing however the velocity is 0!"; invalidTrajectoryLogic = kinematicsTester.new InvalidTrajectoryLogic(errMessage); throw invalidTrajectoryLogic; } - }else { - if(trajectoryPoint.m_currentVelocity > 0.1 || trajectoryPoint.m_currentVelocity < -0.1) { - errMessage = "The point at time: " + trajectoryPoint.m_timestamp + " is a point in which the direction is changing however the velocity is not 0!"; + } else { + if (trajectoryPoint.m_currentVelocity > 0.1 || trajectoryPoint.m_currentVelocity < -0.1) { + errMessage = "The point at time: " + trajectoryPoint.m_timestamp + + " is a point in which the direction is changing however the velocity is not 0!"; invalidTrajectoryLogic = kinematicsTester.new InvalidTrajectoryLogic(errMessage); throw invalidTrajectoryLogic; } @@ -1152,12 +945,12 @@ public InvalidFinalPosition(String errMessage) { private static final long serialVersionUID = 1L; } - + public class InvalidTrajectoryLogic extends Exception { public InvalidTrajectoryLogic(String errMessage) { super(errMessage); } - + private static final long serialVersionUID = 1L; } @@ -1172,14 +965,14 @@ private static void printTrajectory(Path Key) { } System.out.print("The Setpoints are: "); for (int i = 0; i < Key.getSetpointVector().size(); i++) { - if(i == 0) { + if (i == 0) { System.out.print(Key.getSetpointVector().get(i).getm_X()); - }else { + } else { System.out.print(", " + Key.getSetpointVector().get(i).getm_X()); } } System.out.println(""); System.out.println(""); } - + } diff --git a/eclipse-workspace/MotionProfiling/src/kinematics/KinematicsSimpler.java b/eclipse-workspace/MotionProfiling/src/kinematics/KinematicsSimpler.java index 6a84e7a..6b9b101 100644 --- a/eclipse-workspace/MotionProfiling/src/kinematics/KinematicsSimpler.java +++ b/eclipse-workspace/MotionProfiling/src/kinematics/KinematicsSimpler.java @@ -247,8 +247,7 @@ public void createTrajectory(Path Key, double maxVelocity, double maxAcceleratio * cruising to the halfway time which is when the maximum theoretical velocity * will be reached and pretend that you are cruising for 0 seconds */ - - + if (theoreticalMaxVelocity > setpoint.maxVelocity) { double distanceAccelerating = getDistanceTraveledWhileAccelerating(setpoint.vi, setpoint.maxVelocity, Key.maxAcceleration); @@ -257,7 +256,7 @@ public void createTrajectory(Path Key, double maxVelocity, double maxAcceleratio double endCruisingDeltaTimeFromEnd = Math .abs((setpoint.vf - setpoint.maxVelocity) / Key.maxAcceleration); - if(setpoint.maxVelocity < setpoint.vf) { + if (setpoint.maxVelocity < setpoint.vf) { endCruisingDeltaTimeFromEnd = 0.0; } double distanceDecelerating = getDistanceTraveledWhileAccelerating(setpoint.maxVelocity, setpoint.vf, @@ -268,7 +267,7 @@ public void createTrajectory(Path Key, double maxVelocity, double maxAcceleratio setpoint.endCruisingDeltaTime = Math .abs((distanceCruising / setpoint.maxVelocity) + setpoint.startCruisingDeltaTime); setpoint.endDeltaTime = setpoint.endCruisingDeltaTime + endCruisingDeltaTimeFromEnd; - + } else { setpoint.startCruisingDeltaTime = Math.abs(halfWayTime); setpoint.endCruisingDeltaTime = Math.abs(halfWayTime); @@ -277,7 +276,7 @@ public void createTrajectory(Path Key, double maxVelocity, double maxAcceleratio .abs((setpoint.vf - theoreticalMaxVelocity) / Key.maxAcceleration); setpoint.endDeltaTime = setpoint.startCruisingDeltaTime + endCruisingDeltaTimeFromEnd; } - + // Needs to do this so that the last time through the code the max velocity is // not left at some obscure value @@ -376,7 +375,7 @@ private void getFinalVelocityAndInitialVelocity(Vector setpointVector, Pa setpoint.vf = 0.0; } else if ((traveledInAPositiveDirection && willTravelInAPositiveDirection) || (!traveledInAPositiveDirection && !willTravelInAPositiveDirection)) { - + if (setpoint.maxVelocity != Key.maxVelocity) { double distanceCoveredWhileAcceleratingToMaxVelocity = getDistanceTraveledWhileAccelerating(0.0, @@ -384,12 +383,15 @@ private void getFinalVelocityAndInitialVelocity(Vector setpointVector, Pa Point previousPoint = setpoint; Vector possibleFinalVelocityDeterminingSetpoints = new Vector(); Vector possibleFinalVelocityDeterminingSetpointIndexes = new Vector(); - - for (int i = i1 + 1; i < setpointVector.size(); i++) { - + + for (int i = i1; i < setpointVector.size(); i++) { + double directionConstant = 1.0; nextSetpoint = setpointVector.get(i); possibleFinalVelocityDeterminingSetpoints.add(nextSetpoint); + if (i == i1) { + continue; + } if (nextSetpoint.m_x - setpoint.m_x > distanceCoveredWhileAcceleratingToMaxVelocity) { break; } @@ -403,22 +405,37 @@ private void getFinalVelocityAndInitialVelocity(Vector setpointVector, Pa break; } - if (i + 1 == setpointVector.size()) { - possibleFinalVelocityDeterminingSetpointIndexes.addElement(i); - break; - } - if (nextSetpoint.maxVelocity >= Key.maxVelocity || nextSetpoint.maxVelocity <= 0.0) { nextSetpoint.maxVelocity = Key.maxVelocity; } else { possibleFinalVelocityDeterminingSetpointIndexes.addElement(i); } + + if (i + 1 == setpointVector.size()) { + possibleFinalVelocityDeterminingSetpointIndexes.addElement(i); + break; + } previousPoint = nextSetpoint; } + try { + if (Key.setpointVector.get(0).m_x == 10.0 && Key.setpointVector.get(1).m_x == 10.5) { + for (int i = 0; i < possibleFinalVelocityDeterminingSetpointIndexes.size(); i++) { + System.out.println("possibleFinalVelocityDeterminingSetpointIndexe: " + + possibleFinalVelocityDeterminingSetpointIndexes.get(i)); + } + for (int i = 0; i < possibleFinalVelocityDeterminingSetpoints.size(); i++) { + System.out.println("possibleFinalVelocityDeterminingSetpoints: " + + possibleFinalVelocityDeterminingSetpoints.get(i).m_x); + } + } + } catch (ArrayIndexOutOfBoundsException q) { + } for (int i11 = 0; i11 < possibleFinalVelocityDeterminingSetpointIndexes.size(); i11++) { Point possibleFinalVelocityDeterminingSetpoint = new Point(0, 0); + double possibleFinalVelocityDeterminingSetpointMaxVelocity = possibleFinalVelocityDeterminingSetpoints + .elementAt(possibleFinalVelocityDeterminingSetpointIndexes.elementAt(i11)).maxVelocity; try { possibleFinalVelocityDeterminingSetpoint = possibleFinalVelocityDeterminingSetpoints .elementAt(possibleFinalVelocityDeterminingSetpointIndexes.elementAt(i11) - 1); @@ -426,20 +443,19 @@ private void getFinalVelocityAndInitialVelocity(Vector setpointVector, Pa } double deltaDistanceFromPossibleFinalVelocityDeterminingSetpointToStartDecceleratingAt = (Math - .pow(possibleFinalVelocityDeterminingSetpoint.maxVelocity, 2) + .pow(possibleFinalVelocityDeterminingSetpointMaxVelocity, 2) - Math.pow(setpoint.maxVelocity, 2)) / (-2 * Key.maxAcceleration); double deltaDistanceFromSetpointToStartDecceleratingAt = setpoint.m_x - (possibleFinalVelocityDeterminingSetpoint.m_x - deltaDistanceFromPossibleFinalVelocityDeterminingSetpointToStartDecceleratingAt); double tempVf = Math.sqrt(Math.pow(setpoint.maxVelocity, 2) - 2 * Key.maxAcceleration * deltaDistanceFromSetpointToStartDecceleratingAt); - if (setpoint.vf > tempVf) { setpoint.vf = tempVf; } - if (i11 + 1 == possibleFinalVelocityDeterminingSetpoints.size() - && (possibleFinalVelocityDeterminingSetpoint.maxVelocity >= Key.maxVelocity - || possibleFinalVelocityDeterminingSetpoint.maxVelocity <= 0.0)) { + + if (i11 + 1 == possibleFinalVelocityDeterminingSetpointIndexes.size() + ) { deltaDistanceFromPossibleFinalVelocityDeterminingSetpointToStartDecceleratingAt = -Math .pow(setpoint.maxVelocity, 2) / (-2 * Key.maxAcceleration); deltaDistanceFromSetpointToStartDecceleratingAt = setpoint.m_x @@ -459,10 +475,10 @@ private void getFinalVelocityAndInitialVelocity(Vector setpointVector, Pa continue; } - } else { + } else { setpoint.vf = 0.0; } - + } } @@ -731,40 +747,41 @@ private void setTrajectoryVector(Path Key) { // velocity trajectoryPoint.m_acceleration = (nextVelocity * directionConstant) - trajectoryPoint.m_currentVelocity; try { - if (trajectoryPoint.m_timestamp == -10.5 && Key.setpointVector.get(0).m_x == -6.0 && Key.setpointVector.get(1).m_x == -2.0) { - System.out.println(""); - System.out.println("currentTime: " + currentTime); - System.out.println("trajectoryPoint.m_timestamp: " + trajectoryPoint.m_timestamp); - System.out.println("setpoint.startCruisingDeltaTime: " + setpoint.startCruisingDeltaTime); - System.out.println("setpoint.endCruisingDeltaTime: " + setpoint.endCruisingDeltaTime); - System.out.println("setpoint.endDeltaTime: " + setpoint.endDeltaTime); - System.out.println("i11: " + i11); - System.out.println("setpoint.vi: " + setpoint.vi); - System.out.println("setpoint.vf: " + setpoint.vf); - System.out.println(""); - - - System.out.println("nextSetpointsEndTimes: " + +nextSetpointsEndTimes); - System.out.println("lastSetpoint.vf: " + lastSetpoint.vf); - System.out.println("nextTime: " + nextTime); - System.out.println("setpoint.endDeltaTime: " + setpoint.endDeltaTime); - System.out.println("i1: " + i1); - System.out.println("nextVelocity: " + nextVelocity); - System.out.println("trajectoryPoint.m_currentVelocity: " + trajectoryPoint.m_currentVelocity); - System.out.println("trajectoryPoint.m_acceleration: " + trajectoryPoint.m_acceleration); - System.out.println("lastSetpoint.startCruisingDeltaTime: " + lastSetpoint.startCruisingDeltaTime); - System.out.println("lastSetpoint.endCruisingDeltaTime: " + lastSetpoint.endCruisingDeltaTime); - System.out.println("lastSetpoint.endDeltaTime: " + lastSetpoint.endDeltaTime); - System.out.println("(nextTime - setpoint.endDeltaTime): " + (nextTime - previousTime)); - System.out.println("lastSetpoint.maxVelocity: " + lastSetpoint.maxVelocity); - System.out.println("previousTime: " + previousTime); - System.out.println("setpoint.m_x: " + setpoint.m_x); - System.out.println("lastSetpoint.m_x: " + lastSetpoint.m_x); - System.out.println("lastSetpoint.vi:" + lastSetpoint.vi); - System.out.println(""); - } - }catch(ArrayIndexOutOfBoundsException a) { - + if (trajectoryPoint.m_timestamp == -10.5 && Key.setpointVector.get(0).m_x == -6.0 + && Key.setpointVector.get(1).m_x == -2.0) { + System.out.println(""); + System.out.println("currentTime: " + currentTime); + System.out.println("trajectoryPoint.m_timestamp: " + trajectoryPoint.m_timestamp); + System.out.println("setpoint.startCruisingDeltaTime: " + setpoint.startCruisingDeltaTime); + System.out.println("setpoint.endCruisingDeltaTime: " + setpoint.endCruisingDeltaTime); + System.out.println("setpoint.endDeltaTime: " + setpoint.endDeltaTime); + System.out.println("i11: " + i11); + System.out.println("setpoint.vi: " + setpoint.vi); + System.out.println("setpoint.vf: " + setpoint.vf); + System.out.println(""); + + System.out.println("nextSetpointsEndTimes: " + +nextSetpointsEndTimes); + System.out.println("lastSetpoint.vf: " + lastSetpoint.vf); + System.out.println("nextTime: " + nextTime); + System.out.println("setpoint.endDeltaTime: " + setpoint.endDeltaTime); + System.out.println("i1: " + i1); + System.out.println("nextVelocity: " + nextVelocity); + System.out.println("trajectoryPoint.m_currentVelocity: " + trajectoryPoint.m_currentVelocity); + System.out.println("trajectoryPoint.m_acceleration: " + trajectoryPoint.m_acceleration); + System.out + .println("lastSetpoint.startCruisingDeltaTime: " + lastSetpoint.startCruisingDeltaTime); + System.out.println("lastSetpoint.endCruisingDeltaTime: " + lastSetpoint.endCruisingDeltaTime); + System.out.println("lastSetpoint.endDeltaTime: " + lastSetpoint.endDeltaTime); + System.out.println("(nextTime - setpoint.endDeltaTime): " + (nextTime - previousTime)); + System.out.println("lastSetpoint.maxVelocity: " + lastSetpoint.maxVelocity); + System.out.println("previousTime: " + previousTime); + System.out.println("setpoint.m_x: " + setpoint.m_x); + System.out.println("lastSetpoint.m_x: " + lastSetpoint.m_x); + System.out.println("lastSetpoint.vi:" + lastSetpoint.vi); + System.out.println(""); + } + } catch (ArrayIndexOutOfBoundsException a) { + } // Add this trajectory point to the trajectory vector trajectoryVector.add(trajectoryPoint); diff --git a/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/RobotMap.java~ b/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/RobotMap.java~ index 47f84a8..3d013ff 100644 --- a/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/RobotMap.java~ +++ b/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/RobotMap.java~ @@ -25,8 +25,8 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindow; */ public class RobotMap { // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS - public static CANTalon driveTrainUpLeft; - public static CANTalon driveTrainUpRight; + public static CANTalon driveTrainFrontLeft; + public static CANTalon driveTrainFrontRight; public static CANTalon driveTrainBackLeft; public static CANTalon driveTrainBackRight; @@ -34,11 +34,11 @@ public class RobotMap { public static void init() { // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS - driveTrainUpLeft = new CANTalon(0); - LiveWindow.addActuator("DriveTrain", "UpLeft", driveTrainUpLeft); + driveTrainFrontLeft = new CANTalon(0); + LiveWindow.addActuator("DriveTrain", "FrontLeft", driveTrainFrontLeft); - driveTrainUpRight = new CANTalon(1); - LiveWindow.addActuator("DriveTrain", "UpRight", driveTrainUpRight); + driveTrainFrontRight = new CANTalon(1); + LiveWindow.addActuator("DriveTrain", "FrontRight", driveTrainFrontRight); driveTrainBackLeft = new CANTalon(2); LiveWindow.addActuator("DriveTrain", "BackLeft", driveTrainBackLeft); diff --git a/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/commands/MoveWithEncoder.java~ b/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/commands/MoveWithEncoder.java~ index 5d4b005..79a6091 100644 --- a/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/commands/MoveWithEncoder.java~ +++ b/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/commands/MoveWithEncoder.java~ @@ -69,7 +69,7 @@ public class MoveWithEncoder extends Command { Robot.driveTrain.moveToEncoderRevolutions(trajectoryPoint.m_position * m_positionToEncoderRevolutionsRatio); m_PIDOut = Robot.driveTrain.getEncoderPIDOutput(); - Robot.driveTrain.setDriveControllers(trajectoryPoint.m_currentVelocity * m_velocityToMotorOutputRatio + Robot.driveTrain.setAllDriveControllers(trajectoryPoint.m_currentVelocity * m_velocityToMotorOutputRatio + trajectoryPoint.m_acceleration * m_accelerationToMotorOutputRatio + m_PIDOut); } diff --git a/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/commands/RunWheelsAndLog.java b/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/commands/RunWheelsAndLog.java index a4e1384..6d28b13 100644 --- a/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/commands/RunWheelsAndLog.java +++ b/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/commands/RunWheelsAndLog.java @@ -39,9 +39,9 @@ public RunWheelsAndLog() { // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES - requires(Robot.driveTrain); + requires(Robot.driveTrain); - // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES + // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES } // Called just before this Command runs the first time diff --git a/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/commands/RunWheelsAndLog.java~ b/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/commands/RunWheelsAndLog.java~ index 2d25075..a4e1384 100644 --- a/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/commands/RunWheelsAndLog.java~ +++ b/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/commands/RunWheelsAndLog.java~ @@ -8,52 +8,87 @@ // update. Deleting the comments indicating the section will prevent // it from being updated in the future. - package org.usfirst.frc4905.MotionProfiling.commands; + +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; + +import java.util.Vector; + import org.usfirst.frc4905.MotionProfiling.Robot; +import Utilities.Trace; + /** * */ public class RunWheelsAndLog extends Command { + Vector m_header = new Vector(); + double m_initialTimeStamp = 0.0; + double m_previousVelocity = 0.0; + double m_previousTimeStamp = 0.0; + // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS + + // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS + + // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR + public RunWheelsAndLog() { + + // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR + // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING - // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS - - // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS + // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING + // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES + requires(Robot.driveTrain); - // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR - public RunWheelsAndLog() { + // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES + } - // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR - // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING + // Called just before this Command runs the first time + protected void initialize() { + Vector header = new Vector(); + header.add(new String("Velocities")); + header.add(new String("Acceleration")); + header.addElement(new String("Time Between Runs")); + Trace.getInstance().addTrace("Motion Profiling Data", header); + m_header = header; + m_initialTimeStamp = Timer.getFPGATimestamp(); + } - // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING - // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES - requires(Robot.driveTrain); + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Robot.driveTrain.setAllDriveControllers(1.0); - // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES - } + Vector entry = new Vector(); + double currentTimeStamp = Timer.getFPGATimestamp(); + double currentDeltaTimeStampFromStart = currentTimeStamp - m_initialTimeStamp; + double deltaTimeFromLastExecute = currentTimeStamp - m_previousTimeStamp; - // Called just before this Command runs the first time - protected void initialize() { - } + double currentPosition = Robot.driveTrain.getAllEncoderPosition(); + double currentVelocity = currentPosition / currentDeltaTimeStampFromStart; + double currentAcceleration = currentVelocity - m_previousVelocity; - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } + entry.add(currentVelocity); + entry.add(currentAcceleration); + entry.add(deltaTimeFromLastExecute); + + Trace.getInstance().addEntry("Motion Profiling Data", entry); + + m_previousTimeStamp = currentTimeStamp; + m_previousVelocity = currentVelocity; + } - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } - // Called once after isFinished returns true - protected void end() { - } + // Called once after isFinished returns true + protected void end() { + } - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } } diff --git a/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain.java~ b/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain.java~ index af0add8..9d7c88f 100644 --- a/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain.java~ +++ b/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/subsystems/DriveTrain.java~ @@ -32,8 +32,8 @@ public class DriveTrain extends Subsystem { // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS - private final CANTalon upLeft = RobotMap.driveTrainUpLeft; - private final CANTalon upRight = RobotMap.driveTrainUpRight; + private final CANTalon frontLeft = RobotMap.driveTrainFrontLeft; + private final CANTalon frontRight = RobotMap.driveTrainFrontRight; private final CANTalon backLeft = RobotMap.driveTrainBackLeft; private final CANTalon backRight = RobotMap.driveTrainBackRight; @@ -58,21 +58,27 @@ public class DriveTrain extends Subsystem { double frontRight; double frontLeft; } + public double getEncoderPIDOutput() { return m_encoderPIDOutput; } - public void setDriveControllers(double value) { - + + public void setAllDriveControllers(double value) { + backLeft.set(value); + backRight.set(value); + frontLeft.set(value); + frontRight.set(value); } - private double getEncoderPosition() { - return 0.0; + public double getAllEncoderPosition() { + return backLeft.get() + backRight.get() + frontLeft.get() + frontRight.get(); } + private void resetEncoderPosition(){ - m_resetEncoderPositions.backLeft = 0.0; - m_resetEncoderPositions.backRight = 0.0; - m_resetEncoderPositions.frontRight = 0.0; - m_resetEncoderPositions.frontLeft = 0.0; + m_resetEncoderPositions.backLeft = backLeft.get(); + m_resetEncoderPositions.backRight = backRight.get(); + m_resetEncoderPositions.frontRight = frontRight.get(); + m_resetEncoderPositions.frontLeft = frontLeft.get(); } // Encoder PID controller private PIDController m_moveWithEncoderPID; @@ -95,7 +101,7 @@ public class DriveTrain extends Subsystem { @Override public double pidGet() { - return getEncoderPosition(); + return getAllEncoderPosition(); } } diff --git a/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/subsystems/Kinematics.java b/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/subsystems/Kinematics.java deleted file mode 100644 index e826ed6..0000000 --- a/eclipse-workspace/MotionProfiling/src/org/usfirst/frc4905/MotionProfiling/subsystems/Kinematics.java +++ /dev/null @@ -1,47 +0,0 @@ -// RobotBuilder Version: 2.0 -// -// This file was generated by RobotBuilder. It contains sections of -// code that are automatically generated and assigned by robotbuilder. -// These sections will be updated in the future when you export to -// Java from RobotBuilder. Do not put any code or make any change in -// the blocks indicating autogenerated code or it will be lost on an -// update. Deleting the comments indicating the section will prevent -// it from being updated in the future. - - -package org.usfirst.frc4905.MotionProfiling.subsystems; - -import org.usfirst.frc4905.MotionProfiling.RobotMap; -import org.usfirst.frc4905.MotionProfiling.commands.*; - -import edu.wpi.first.wpilibj.command.Subsystem; - - -/** - * - */ -public class Kinematics extends Subsystem { - - // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS - - // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS - - // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS - - // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS - - - // Put methods for controlling this subsystem - // here. Call these from Commands. - - public void initDefaultCommand() { - // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND - - - // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND - - // Set the default command for a subsystem here. - // setDefaultCommand(new MySpecialCommand()); - } -} -