Skip to content

Commit

Permalink
Got the cases where points are going in a positive direction to a neg…
Browse files Browse the repository at this point in the history 
…ative direction working and vice verca. Also added a final setpoint checker and put the next velocity from last velocity + last acceleration checker back in
  • Loading branch information
Sean Doyle authored and Sean Doyle committed Aug 27, 2017
1 parent a690cac commit e86a74b
Show file tree
Hide file tree
Showing 13 changed files with 110 additions and 70 deletions.
Binary file modified eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidAccelerationException.class
View file
Binary file not shown.
Binary file modified eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidFinalPosition.class
View file
Binary file not shown.
Binary file modified ...kspace/MotionProfiling/bin/KinematicsTester$InvalidNextVelocityFromLastAcceleration.class
View file
Binary file not shown.
Binary file modified eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidVelocityException.class
View file
Binary file not shown.
Binary file modified eclipse-workspace/MotionProfiling/bin/KinematicsTester.class
View file
Binary file not shown.
Binary file modified ...orkspace/MotionProfiling/bin/kinematics/KinematicsSimpler$InvalidDimentionException.class
View file
Binary file not shown.
Binary file modified eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler$Path.class
View file
Binary file not shown.
Binary file modified eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler$Point.class
View file
Binary file not shown.
Binary file modified eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler$TrajectoryPaths.class
View file
Binary file not shown.
Binary file modified eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler$TrajectoryPoint.class
View file
Binary file not shown.
Binary file modified eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler.class
View file
Binary file not shown.
157 changes: 97 additions & 60 deletions eclipse-workspace/MotionProfiling/src/KinematicsTester.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,46 +11,15 @@ public static void main(String[] args) {

createPositiveTrajectoryGreaterThanTheDistanceCoveredWhileAcceleratingCase();

createPositiveTrajectoryLessThanTheDistanceCoveredWhileAcceleratingCase();
createPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase();

createNegativeTrajectoryGreaterThanDistanceCoveredWhileAcceleratingCase();

createNegativeTrajectoryLessThanTheDistanceCoveredWhileAcceleratingCase();
}

public class InvalidVelocityException extends Exception {
public InvalidVelocityException(String errMessage) {
super(errMessage);
}

private static final long serialVersionUID = 1L;

}

public class InvalidAccelerationException extends Exception {
public InvalidAccelerationException(String errMessage) {
super(errMessage);
}

private static final long serialVersionUID = 1L;

}

public class InvalidNextVelocityFromLastAcceleration extends Exception {
public InvalidNextVelocityFromLastAcceleration(String errMessage) {
super(errMessage);
}

private static final long serialVersionUID = 1L;
}

public class InvalidFinalPosition extends Exception {
public InvalidFinalPosition(String errMessage) {
super(errMessage);
}
createNegativeTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase();

private static final long serialVersionUID = 1L;
createPositiveTrajectoryToNegativeTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase();

createNegativeTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase();
}

private static void createPositiveTrajectoryGreaterThanTheDistanceCoveredWhileAcceleratingCase() {
Expand All @@ -62,17 +31,17 @@ private static void createPositiveTrajectoryGreaterThanTheDistanceCoveredWhileAc
} catch (InvalidDimentionException e) {
e.printStackTrace();
}

try {
checkTrajectoryPath(myPath, kinematicsTester);
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException e) {
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException
| InvalidFinalPosition e) {
// TODO Auto-generated catch block
e.printStackTrace();
}

}

private static void createPositiveTrajectoryLessThanTheDistanceCoveredWhileAcceleratingCase() {
private static void createPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() {
Path myPath1 = m_kinematicsSmallSteps.new Path();
KinematicsTester kinematicsTester1 = new KinematicsTester();
try {
Expand All @@ -81,10 +50,11 @@ private static void createPositiveTrajectoryLessThanTheDistanceCoveredWhileAccel
} catch (InvalidDimentionException e) {
e.printStackTrace();
}

try {
checkTrajectoryPath(myPath1, kinematicsTester1);
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException e) {
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException
| InvalidFinalPosition e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
Expand All @@ -101,13 +71,14 @@ private static void createNegativeTrajectoryGreaterThanDistanceCoveredWhileAccel
}
try {
checkTrajectoryPath(myPath, kinematicsTester);
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException e) {
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException
| InvalidFinalPosition e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}

private static void createNegativeTrajectoryLessThanTheDistanceCoveredWhileAcceleratingCase() {
private static void createNegativeTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() {
Path myPath1 = m_kinematicsSmallSteps.new Path();
KinematicsTester kinematicsTester1 = new KinematicsTester();
try {
Expand All @@ -118,84 +89,98 @@ private static void createNegativeTrajectoryLessThanTheDistanceCoveredWhileAccel
}
try {
checkTrajectoryPath(myPath1, kinematicsTester1);
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException e) {
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException
| InvalidFinalPosition e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}

private static void createPositiveTrajectoryCaseWith2Points() {
private static void createPositiveTrajectoryToNegativeTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase() {
Path myPath = m_kinematicsSmallSteps.new Path();
KinematicsTester kinematicsTester = new KinematicsTester();
try {
m_kinematicsSmallSteps.addPointToPath(myPath, m_kinematicsSmallSteps.new Point(10));
m_kinematicsSmallSteps.addPointToPath(myPath, m_kinematicsSmallSteps.new Point(20));
m_kinematicsSmallSteps.addPointToPath(myPath, m_kinematicsSmallSteps.new Point(-2));
m_kinematicsSmallSteps.createTrajectory(myPath, 2.0, 0.5);
} catch (InvalidDimentionException e) {
e.printStackTrace();
}
try {
checkTrajectoryPath(myPath, kinematicsTester);
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException e) {
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException
| InvalidFinalPosition e) {
// TODO Auto-generated catch block
e.printStackTrace();
}

}

private void createPositiveTrajectoryToNegativeTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() {
Path myPath1 = m_kinematicsSmallSteps.new Path();
KinematicsTester kinematicsTester1 = new KinematicsTester();
try {
m_kinematicsSmallSteps.addPointToPath(myPath1, m_kinematicsSmallSteps.new Point(10));
m_kinematicsSmallSteps.addPointToPath(myPath1, m_kinematicsSmallSteps.new Point(12));
m_kinematicsSmallSteps.addPointToPath(myPath1, m_kinematicsSmallSteps.new Point(8));
m_kinematicsSmallSteps.createTrajectory(myPath1, 2.0, 0.5);
} catch (InvalidDimentionException e) {
e.printStackTrace();
}

try {
checkTrajectoryPath(myPath1, kinematicsTester1);
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException e) {
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException
| InvalidFinalPosition e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}

private static void createNegativeTrajectoryCaseWith2Points() {
private static void createNegativeTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase() {
Path myPath = m_kinematicsSmallSteps.new Path();
KinematicsTester kinematicsTester = new KinematicsTester();
try {
m_kinematicsSmallSteps.addPointToPath(myPath, m_kinematicsSmallSteps.new Point(-10));
m_kinematicsSmallSteps.addPointToPath(myPath, m_kinematicsSmallSteps.new Point(-20));
m_kinematicsSmallSteps.addPointToPath(myPath, m_kinematicsSmallSteps.new Point(2));
m_kinematicsSmallSteps.createTrajectory(myPath, 2.0, 0.5);
} catch (InvalidDimentionException e) {
e.printStackTrace();
}
try {
checkTrajectoryPath(myPath, kinematicsTester);
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException e) {
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException
| InvalidFinalPosition e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}

private void createNegativeTrajectoryToPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() {
Path myPath1 = m_kinematicsSmallSteps.new Path();
KinematicsTester kinematicsTester1 = new KinematicsTester();
try {
m_kinematicsSmallSteps.addPointToPath(myPath1, m_kinematicsSmallSteps.new Point(-10));
m_kinematicsSmallSteps.addPointToPath(myPath1, m_kinematicsSmallSteps.new Point(-12));
m_kinematicsSmallSteps.addPointToPath(myPath1, m_kinematicsSmallSteps.new Point(-8));
m_kinematicsSmallSteps.createTrajectory(myPath1, 2.0, 0.5);
} catch (InvalidDimentionException e) {
e.printStackTrace();
}
printTrajectory(myPath1);
try {
checkTrajectoryPath(myPath1, kinematicsTester1);
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException e) {
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException
| InvalidFinalPosition e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}

private static void checkTrajectoryPath(Path Key, KinematicsTester kinematicsTester)
throws InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException {
checkVelocity(Key, kinematicsTester);
throws InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException,
InvalidFinalPosition {
checkAcceleration(Key, kinematicsTester);
checkVelocity(Key, kinematicsTester);
checkFinalPosition(Key, kinematicsTester);
}

private static void checkVelocity(Path Key, KinematicsTester kinematicsTester)
Expand All @@ -212,7 +197,7 @@ private static void checkVelocity(Path Key, KinematicsTester kinematicsTester)
throw invalidVelocityException;
}
}
// checkNextVelocityFromLastAcceleration(Key, kinematicsTester);
checkNextVelocityFromLastAcceleration(Key, kinematicsTester);
}

private static void checkAcceleration(Path Key, KinematicsTester kinematicsTester)
Expand All @@ -221,8 +206,10 @@ 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()
|| currentPoint.m_acceleration < -Key.getmaxAcceleration()) {
if (currentPoint.m_acceleration > (Key.getmaxAcceleration()
* KinematicsSimpler.getTrajectoryPointInterval())
|| currentPoint.m_acceleration < (-Key.getmaxAcceleration()
* KinematicsSimpler.getTrajectoryPointInterval())) {
errMessage = "The Current Acceleration at time: " + currentPoint.m_timestamp
+ " is above the maximum acceleration!";
invalidAccelerationException = kinematicsTester.new InvalidAccelerationException(errMessage);
Expand All @@ -231,6 +218,19 @@ private static void checkAcceleration(Path Key, KinematicsTester kinematicsTeste
}
}

private static void checkFinalPosition(Path Key, KinematicsTester kinematicsTester) throws InvalidFinalPosition {
InvalidFinalPosition invalidFinalPosition;
String errMesage;
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;
}
}

private static void checkNextVelocityFromLastAcceleration(Path Key, KinematicsTester kinematicsTester)
throws InvalidNextVelocityFromLastAcceleration {
for (int i = 0; i < Key.getTrajectoryVector().size(); i++) {
Expand All @@ -243,7 +243,8 @@ private static void checkNextVelocityFromLastAcceleration(Path Key, KinematicsTe
} catch (ArrayIndexOutOfBoundsException a) {
continue;
}
if (lastPoint.m_currentVelocity + lastPoint.m_acceleration != currentPoint.m_currentVelocity) {
if (lastPoint.m_currentVelocity + lastPoint.m_acceleration - currentPoint.m_currentVelocity > 0.5
|| lastPoint.m_currentVelocity + lastPoint.m_acceleration - currentPoint.m_currentVelocity < -0.5) {
errMessage = "The Current Acceleration added to the Current Velocity at time: " + lastPoint.m_timestamp
+ " Do not equal the next Velocity at time: " + currentPoint.m_timestamp;
invalidNextVelocityFromLastAcceleration = kinematicsTester.new InvalidNextVelocityFromLastAcceleration(
Expand All @@ -253,6 +254,41 @@ private static void checkNextVelocityFromLastAcceleration(Path Key, KinematicsTe
}
}

public class InvalidVelocityException extends Exception {
public InvalidVelocityException(String errMessage) {
super(errMessage);
}

private static final long serialVersionUID = 1L;

}

public class InvalidAccelerationException extends Exception {
public InvalidAccelerationException(String errMessage) {
super(errMessage);
}

private static final long serialVersionUID = 1L;

}

public class InvalidNextVelocityFromLastAcceleration extends Exception {
public InvalidNextVelocityFromLastAcceleration(String errMessage) {
super(errMessage);
}

private static final long serialVersionUID = 1L;
}

public class InvalidFinalPosition extends Exception {
public InvalidFinalPosition(String errMessage) {
super(errMessage);
}

private static final long serialVersionUID = 1L;

}

private static void printTrajectory(Path Key) {
System.out.println("Trajectory Point: [vel, acc, pos, time]");
for (int i = 0; i < Key.getTrajectoryVector().size(); i++) {
Expand All @@ -261,7 +297,8 @@ private static void printTrajectory(Path Key) {
+ Key.getTrajectoryVector().get(i).m_position + ", " + Key.getTrajectoryVector().get(i).m_timestamp
+ "]");
}
System.out.println("The final Setpoint was: " + Key.getSetpointVector().get(Key.getSetpointVector().size()-1).getm_X());
System.out.println(
"The final Setpoint was: " + Key.getSetpointVector().get(Key.getSetpointVector().size() - 1).getm_X());
System.out.println("");
}
}
23 changes: 13 additions & 10 deletions eclipse-workspace/MotionProfiling/src/kinematics/KinematicsSimpler.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,11 @@ public class KinematicsSimpler {

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS

private double m_trajectoryPointInterval = 0.5;

private static double m_trajectoryPointInterval = 0.5;

public static double getTrajectoryPointInterval() {
return m_trajectoryPointInterval;
}
public class InvalidDimentionException extends Exception {

private static final long serialVersionUID = 1L;
Expand Down Expand Up @@ -313,14 +316,16 @@ private void setTrajectoryVector(Path Key) {
for (int i1 = 0; i1 < setpointVector.size(); i1++) {
// Gets the next point
Point setpoint = setpointVector.get(i1);
Point lastSepoint = new Point(0, 0);
Point previousSetpoint = new Point(0, 0);
try {
lastSepoint = setpointVector.get(i1 - 1);
previousSetpoint = setpointVector.get(i1 - 1);
} catch (ArrayIndexOutOfBoundsException a) {

}
if (lastSepoint.m_x > setpoint.m_x) {
if (previousSetpoint.m_x > setpoint.m_x) {
directionConstant = -1;
}else {
directionConstant = 1;
}

// This is where the last setpoint's m_x is stored so that later on when there
Expand All @@ -347,9 +352,6 @@ private void setTrajectoryVector(Path Key) {
// than the endDeltatTime of the last setpoint however if i11 is set to 0 it
// will not be greater than the last setpoint's end delta time at all.

// previousTime is added to endDeltaTime so that the times can be kept on the
// same scale

for (double i11 = currentTime; currentTime < setpoint.endDeltaTime
+ previousTime; i11 += m_trajectoryPointInterval) {

Expand Down Expand Up @@ -497,7 +499,7 @@ private void setTrajectoryVector(Path Key) {
&& nextTime < setpoint.endCruisingDeltaTime + previousTime) {
nextVelocity = setpoint.maxVelocity;
} else if (nextTime < setpoint.startCruisingDeltaTime + previousTime) {
nextVelocity = setpoint.vi + Key.maxAcceleration * nextTime;
nextVelocity = setpoint.vi + Key.maxAcceleration * (nextTime - previousTime);
} else {
nextVelocity = setpoint.vf
+ Key.maxAcceleration * (setpoint.endDeltaTime + previousTime - nextTime);
Expand All @@ -507,7 +509,7 @@ private void setTrajectoryVector(Path Key) {
// velocity
trajectoryPoint.m_acceleration = (nextVelocity * directionConstant) - trajectoryPoint.m_currentVelocity;

if (trajectoryPoint.m_timestamp == -1) {
if (trajectoryPoint.m_timestamp == -1.0) {
System.out.println("");
System.out.println("trajectoryPoint.m_timestamp: " + trajectoryPoint.m_timestamp);
System.out.println("setpoint.startCruisingDeltaTime: " + setpoint.startCruisingDeltaTime);
Expand All @@ -530,6 +532,7 @@ private void setTrajectoryVector(Path Key) {
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("");
}

Expand Down

0 comments on commit e86a74b

Please sign in to comment.