Skip to content

Commit

Permalink
Got the test cases woking where the first point is less than the dist…
Browse files Browse the repository at this point in the history 
…ance it takes to accelerate up to max velocity. The other positive and negative test cases still work
  • Loading branch information
Sean Doyle authored and Sean Doyle committed Aug 27, 2017
1 parent 83a3a53 commit a690cac
Show file tree
Hide file tree
Showing 8 changed files with 139 additions and 44 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 eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler.class
View file
Binary file not shown.
127 changes: 119 additions & 8 deletions eclipse-workspace/MotionProfiling/src/KinematicsTester.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,14 @@ public class KinematicsTester {
static KinematicsSimpler m_kinematicsSmallSteps = new KinematicsSimpler();

public static void main(String[] args) {
createPositiveTrajectoryCase();
createNegativeTrajectoryCase();


createPositiveTrajectoryGreaterThanTheDistanceCoveredWhileAcceleratingCase();

createPositiveTrajectoryLessThanTheDistanceCoveredWhileAcceleratingCase();

createNegativeTrajectoryGreaterThanDistanceCoveredWhileAcceleratingCase();

createNegativeTrajectoryLessThanTheDistanceCoveredWhileAcceleratingCase();
}

public class InvalidVelocityException extends Exception {
Expand Down Expand Up @@ -48,7 +53,7 @@ public InvalidFinalPosition(String errMessage) {

}

private static void createPositiveTrajectoryCase() {
private static void createPositiveTrajectoryGreaterThanTheDistanceCoveredWhileAcceleratingCase() {
Path myPath = m_kinematicsSmallSteps.new Path();
KinematicsTester kinematicsTester = new KinematicsTester();
try {
Expand All @@ -57,15 +62,35 @@ private static void createPositiveTrajectoryCase() {
} catch (InvalidDimentionException e) {
e.printStackTrace();
}

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

}

private static void createNegativeTrajectoryCase() {

private static void createPositiveTrajectoryLessThanTheDistanceCoveredWhileAcceleratingCase() {
Path myPath1 = m_kinematicsSmallSteps.new Path();
KinematicsTester kinematicsTester1 = new KinematicsTester();
try {
m_kinematicsSmallSteps.addPointToPath(myPath1, m_kinematicsSmallSteps.new Point(2));
m_kinematicsSmallSteps.createTrajectory(myPath1, 2.0, 0.5);
} catch (InvalidDimentionException e) {
e.printStackTrace();
}

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

private static void createNegativeTrajectoryGreaterThanDistanceCoveredWhileAcceleratingCase() {
Path myPath = m_kinematicsSmallSteps.new Path();
KinematicsTester kinematicsTester = new KinematicsTester();
try {
Expand All @@ -74,7 +99,6 @@ private static void createNegativeTrajectoryCase() {
} catch (InvalidDimentionException e) {
e.printStackTrace();
}
printTrajectory(myPath);
try {
checkTrajectoryPath(myPath, kinematicsTester);
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException e) {
Expand All @@ -83,6 +107,91 @@ private static void createNegativeTrajectoryCase() {
}
}

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

private static void createPositiveTrajectoryCaseWith2Points() {
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.createTrajectory(myPath, 2.0, 0.5);
} catch (InvalidDimentionException e) {
e.printStackTrace();
}
try {
checkTrajectoryPath(myPath, kinematicsTester);
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}

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.createTrajectory(myPath1, 2.0, 0.5);
} catch (InvalidDimentionException e) {
e.printStackTrace();
}
try {
checkTrajectoryPath(myPath1, kinematicsTester1);
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}

private static void createNegativeTrajectoryCaseWith2Points() {
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.createTrajectory(myPath, 2.0, 0.5);
} catch (InvalidDimentionException e) {
e.printStackTrace();
}
try {
checkTrajectoryPath(myPath, kinematicsTester);
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}

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.createTrajectory(myPath1, 2.0, 0.5);
} catch (InvalidDimentionException e) {
e.printStackTrace();
}
try {
checkTrajectoryPath(myPath1, kinematicsTester1);
} catch (InvalidVelocityException | InvalidNextVelocityFromLastAcceleration | InvalidAccelerationException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}

private static void checkTrajectoryPath(Path Key, KinematicsTester kinematicsTester)
throws InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException {
checkVelocity(Key, kinematicsTester);
Expand All @@ -103,7 +212,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 Down Expand Up @@ -152,5 +261,7 @@ 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("");
}
}
56 changes: 20 additions & 36 deletions eclipse-workspace/MotionProfiling/src/kinematics/KinematicsSimpler.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -255,6 +255,7 @@ public void createTrajectory(Path Key, double maxVelocity, double maxAcceleratio
setpoint.endCruisingDeltaTime = Math
.abs((distanceCruising / Key.maxVelocity) + setpoint.startCruisingDeltaTime);
setpoint.endDeltaTime = setpoint.endCruisingDeltaTime + endCruisingDeltaTimeFromEnd;
setpoint.maxVelocity = Key.maxVelocity;

} else {
setpoint.startCruisingDeltaTime = Math.abs(halfWayTime);
Expand All @@ -263,21 +264,10 @@ public void createTrajectory(Path Key, double maxVelocity, double maxAcceleratio
double endCruisingDeltaTimeFromEnd = Math
.abs((setpoint.vf - theoreticalMaxVelocity) / Key.maxAcceleration);
setpoint.endDeltaTime = setpoint.startCruisingDeltaTime + endCruisingDeltaTimeFromEnd;
setpoint.maxVelocity = theoreticalMaxVelocity;
}

System.out.println("");
System.out.println("setpoint.vi: " + setpoint.vi);
System.out.println("setpoint.vf: " + setpoint.vf);
System.out.println("Key.maxVelocity: " + Key.maxVelocity);
System.out.println("Key.maxAcceleration: " + Key.maxAcceleration);
System.out.println("startCruisingDeltaTime: " + setpoint.startCruisingDeltaTime);
System.out.println("End Cruising Delta Time: " + setpoint.endCruisingDeltaTime);
System.out.println("End Delta Time: " + setpoint.endDeltaTime);
System.out.println("theoreticalMaxVelocityDistance: " + theoreticalMaxVelocityDistance);
System.out.println("theoreticalMaxVelocity: " + theoreticalMaxVelocity);
System.out.println("setpoint.maxVelocity: " + setpoint.maxVelocity);
System.out.println("maxVelocity: " + maxVelocity);
System.out.println("");


// Needs to do this so that the last time through the code the max velocity is
// not left at some obscure value
Expand Down Expand Up @@ -359,14 +349,11 @@ private void setTrajectoryVector(Path Key) {

// previousTime is added to endDeltaTime so that the times can be kept on the
// same scale
System.out.println("Gone THruLoop1");

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

// Sets the current time for the trajectory point being created
System.out.println("currentTime: " + currentTime);
System.out.println("i11: " + i11);
System.out.println("setpoint.endDeltaTime + previousTime: " + setpoint.endDeltaTime + previousTime);
TrajectoryPoint trajectoryPoint = new TrajectoryPoint(0, 0, 0, currentTime);

// If currentTime is between startCruisingDeltaTime and endCruisingDeltaTime set
Expand All @@ -378,16 +365,16 @@ private void setTrajectoryVector(Path Key) {
// Else the currentTime is after endCruisingDeltaTime
if (setpoint.startCruisingDeltaTime + previousTime <= currentTime
&& i11 < setpoint.endCruisingDeltaTime + previousTime) {
trajectoryPoint.m_currentVelocity = Key.maxVelocity;
trajectoryPoint.m_currentVelocity = setpoint.maxVelocity;
trajectoryPoint.m_currentVelocity *= directionConstant;

// Gets the distance traveled to accelerate up to the max velocity from the
// initial velocity of the setpoint
double distanceTraveledWhileAccelerating = getDistanceTraveledWhileAccelerating(setpoint.vi,
Key.maxVelocity, Key.maxAcceleration);
setpoint.maxVelocity, Key.maxAcceleration);

// Gets the distance covered from the startCruisingDeltaTime to currentTime
double distanceTraveledWhileCruising = Key.maxVelocity
double distanceTraveledWhileCruising = setpoint.maxVelocity
* (currentTime - (setpoint.startCruisingDeltaTime + previousTime));

// Calculate the delta position from the starting position of the setpoint by
Expand Down Expand Up @@ -419,18 +406,19 @@ private void setTrajectoryVector(Path Key) {
// Finds the distance traveled while accelerating up to max velocity from the
// initial velocity
double distanceTraveledWhileAccelerating = getDistanceTraveledWhileAccelerating(setpoint.vi,
Key.maxVelocity, Key.maxAcceleration);
setpoint.maxVelocity, Key.maxAcceleration);

// Finds the distance traveled while cruising
double distanceTraveledWhileCruising = Key.maxVelocity
double distanceTraveledWhileCruising = setpoint.maxVelocity
* ((setpoint.endCruisingDeltaTime + previousTime)
- (setpoint.startCruisingDeltaTime + previousTime));



// Finds the distance covered while decelerating from the cruising
// velocity(maxVelocity) to the current velocity
double distanceTraveledWhileDecelerating = getDistanceTraveledWhileAccelerating(
Key.maxVelocity, trajectoryPoint.m_currentVelocity, Key.maxAcceleration);

setpoint.maxVelocity, trajectoryPoint.m_currentVelocity, Key.maxAcceleration);
// Calculate the delta position from the starting position of the setpoint by
// adding the
// distanceTraveledWhileAccelerating to the distanceTraveledWhileCruising to the
Expand Down Expand Up @@ -473,10 +461,6 @@ private void setTrajectoryVector(Path Key) {
// This for loop goes though all of the rest of the setpoints after the current
// setpoint
for (int a = i1 + 1; a < Key.setpointVector.size(); a += m_trajectoryPointInterval) {
System.out.println("a: " + a);
System.out.println("setpointVector.get(a).endDeltaTime: " + setpointVector.get(a).endDeltaTime);
System.out.println("previousTime: " + previousTime);

// If the end time of the setpoint being looked at is greater than the nextTime
// set it to lastSetpoint and break out of the for loop
if (setpointVector.get(a).endDeltaTime + previousTime + nextSetpointsEndTimes > nextTime) {
Expand All @@ -499,19 +483,19 @@ private void setTrajectoryVector(Path Key) {
nextVelocity = 0.0;
} else if (lastSetpoint.startCruisingDeltaTime + previousTime + nextSetpointsEndTimes <= nextTime
&& nextTime < lastSetpoint.endCruisingDeltaTime + previousTime + nextSetpointsEndTimes) {
System.out.println("1");

nextVelocity = lastSetpoint.maxVelocity;
} else if (nextTime < lastSetpoint.startCruisingDeltaTime + previousTime + nextSetpointsEndTimes) {
System.out.println("2");

nextVelocity = lastSetpoint.vi + Key.maxAcceleration * nextTime;
} else {
System.out.println("3");

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

if (trajectoryPoint.m_timestamp == 17.0) {
if (trajectoryPoint.m_timestamp == -1) {
System.out.println("");
System.out.println("trajectoryPoint.m_timestamp: " + trajectoryPoint.m_timestamp);
System.out.println("setpoint.startCruisingDeltaTime: " + setpoint.startCruisingDeltaTime);
Expand All @@ -548,7 +532,7 @@ private void setTrajectoryVector(Path Key) {
System.out.println("previousTime: " + previousTime);
System.out.println("");
}
System.out.println("Gone Thru loop");


// Add this trajectory point to the trajectory vector
trajectoryVector.add(trajectoryPoint);
Expand Down

0 comments on commit a690cac

Please sign in to comment.