public void TestNegligibleProfileConstructorAndProperties()
{
Assert.AreEqual(SimulationMCUTrajectoryProfileTypeEnum.NEGLIGIBLE, ProfileNegligible.ProfileType);
Assert.AreEqual(0.0, ProfileNegligible.InitialVelocity, DOUBLE_EPSILON);
Assert.AreEqual(MCUConstants.SIMULATION_MCU_PEAK_VELOCITY, ProfileNegligible.TrajectoryPeakVelocity, DOUBLE_EPSILON);
Assert.AreEqual(MCUConstants.SIMULATION_MCU_PEAK_ACCELERATION, ProfileNegligible.TrajectoryPeakAcceleration, DOUBLE_EPSILON);
Assert.AreEqual(0.0, ProfileNegligible.TotalTime, DOUBLE_EPSILON);
Assert.AreEqual(Encoder.GetEquivalentEncoderTicksFromDegrees(0.0), ProfileNegligible.InitialStep);
Assert.AreEqual(Encoder.GetEquivalentEncoderTicksFromDegrees(MiscellaneousConstants.NEGLIGIBLE_POSITION_CHANGE_DEGREES / 2), ProfileNegligible.ObjectiveStep);
}
// This factory function utilizes the equations on page 20 and 21 of the 2-axis MCU documentation to form a trajectory profile
// with a linear acceleration to reach the peak programmed speed
public static SimulationMCUTrajectoryProfile ForceLinearInstance(
SimulationAbsoluteEncoder motorEncoder,
double initialDegrees,
double initialVelocity,
double peakVelocity,
double acceleration,
double objectiveDegrees)
{
int initialSteps = motorEncoder.GetEquivalentEncoderTicksFromDegrees(initialDegrees);
int objectiveSteps = motorEncoder.GetEquivalentEncoderTicksFromDegrees(objectiveDegrees);
int changeInSteps = objectiveSteps - initialSteps;
int absoluteChangeInSteps = Math.Abs(changeInSteps);
int minimumTicksNeededToReachPeakSpeed = (int)(((peakVelocity * peakVelocity) - (initialVelocity * initialVelocity)) / (2 * acceleration));
int minimumTicksNeededForAcceleration = 2 * minimumTicksNeededToReachPeakSpeed;
// See if this move is even worth the effort of TRYING to move
if (Math.Abs(motorEncoder.GetEquivalentDegreesFromEncoderTicks(changeInSteps)) < MiscellaneousConstants.NEGLIGIBLE_POSITION_CHANGE_DEGREES)
{
return(new SimulationMCUTrajectoryProfile(SimulationMCUTrajectoryProfileTypeEnum.NEGLIGIBLE, initialVelocity, peakVelocity, acceleration, 0.0, 0.0, initialSteps, objectiveSteps));
}
else if (absoluteChangeInSteps >= minimumTicksNeededForAcceleration)
{
double timeRequiredForAcceleration = (peakVelocity - initialVelocity) / acceleration;
double totalTime = (2 * timeRequiredForAcceleration) + ((absoluteChangeInSteps - minimumTicksNeededForAcceleration) / peakVelocity);
return(new SimulationMCUTrajectoryProfile(
SimulationMCUTrajectoryProfileTypeEnum.LINEAR_FULL,
initialVelocity,
peakVelocity,
acceleration,
timeRequiredForAcceleration,
totalTime,
initialSteps,
objectiveSteps
));
}
else
{
double actualPeakVelocity = Math.Sqrt((initialVelocity * initialVelocity) + (acceleration * absoluteChangeInSteps));
double actualAccelerationTime = (actualPeakVelocity - initialVelocity) / acceleration;
return(new SimulationMCUTrajectoryProfile(
SimulationMCUTrajectoryProfileTypeEnum.LINEAR_PARTIAL,
initialVelocity,
actualPeakVelocity,
acceleration,
actualAccelerationTime,
2 * actualAccelerationTime,
initialSteps,
objectiveSteps
));
}
}
public void TestEncoderTicksToDegreesCalculationGetter()
{
Assert.AreEqual(0, Encoder0.GetEquivalentEncoderTicksFromDegrees(0.0));
Assert.AreEqual(256, Encoder0.GetEquivalentEncoderTicksFromDegrees(90.0));
Assert.AreEqual(384, Encoder0.GetEquivalentEncoderTicksFromDegrees(135.2));
Assert.AreEqual(1023, Encoder0.GetEquivalentEncoderTicksFromDegrees(359.9));
Assert.AreEqual(0, Encoder0.GetEquivalentEncoderTicksFromDegrees(360.0));
Assert.AreEqual(275, Encoder0.GetEquivalentEncoderTicksFromDegrees(456.7));
Assert.AreEqual(965, Encoder0.GetEquivalentEncoderTicksFromDegrees(-20.58));
Assert.AreEqual(768, Encoder0.GetEquivalentEncoderTicksFromDegrees(-450.0));
Assert.AreEqual(0, Encoder1.GetEquivalentEncoderTicksFromDegrees(0.0));
Assert.AreEqual(1024, Encoder1.GetEquivalentEncoderTicksFromDegrees(90.0));
Assert.AreEqual(1538, Encoder1.GetEquivalentEncoderTicksFromDegrees(135.2));
Assert.AreEqual(4094, Encoder1.GetEquivalentEncoderTicksFromDegrees(359.9));
Assert.AreEqual(0, Encoder1.GetEquivalentEncoderTicksFromDegrees(360.0));
Assert.AreEqual(1100, Encoder1.GetEquivalentEncoderTicksFromDegrees(456.7));
Assert.AreEqual(3861, Encoder1.GetEquivalentEncoderTicksFromDegrees(-20.58));
Assert.AreEqual(3072, Encoder1.GetEquivalentEncoderTicksFromDegrees(-450.0));
Assert.AreEqual(0, Encoder2.GetEquivalentEncoderTicksFromDegrees(0.0));
Assert.AreEqual(16384, Encoder2.GetEquivalentEncoderTicksFromDegrees(90.0));
Assert.AreEqual(24612, Encoder2.GetEquivalentEncoderTicksFromDegrees(135.2));
Assert.AreEqual(65517, Encoder2.GetEquivalentEncoderTicksFromDegrees(359.9));
Assert.AreEqual(0, Encoder2.GetEquivalentEncoderTicksFromDegrees(360.0));
Assert.AreEqual(17603, Encoder2.GetEquivalentEncoderTicksFromDegrees(456.7));
Assert.AreEqual(61789, Encoder2.GetEquivalentEncoderTicksFromDegrees(-20.58));
Assert.AreEqual(49152, Encoder2.GetEquivalentEncoderTicksFromDegrees(-450.0));
}
