public static void BuildUp(TestContext testContext)
{
Encoder = new SimulationAbsoluteEncoder(12);
ProfileNegligible = SimulationMCUTrajectoryProfile.ForceLinearInstance(
Encoder,
0.0,
0.0,
MCUConstants.SIMULATION_MCU_PEAK_VELOCITY,
MCUConstants.SIMULATION_MCU_PEAK_ACCELERATION,
MiscellaneousConstants.NEGLIGIBLE_POSITION_CHANGE_DEGREES / 2
);
ProfileSCurveTriangularFull = SimulationMCUTrajectoryProfile.ForceLinearInstance(
Encoder,
0.0,
0.0,
MCUConstants.SIMULATION_MCU_PEAK_VELOCITY,
MCUConstants.SIMULATION_MCU_PEAK_ACCELERATION,
1.85
);
ProfileSCurveTriangularPartial = SimulationMCUTrajectoryProfile.ForceLinearInstance(
Encoder,
60.0,
0.0,
MCUConstants.SIMULATION_MCU_PEAK_VELOCITY,
MCUConstants.SIMULATION_MCU_PEAK_ACCELERATION,
60.0 + (MiscellaneousConstants.NEGLIGIBLE_POSITION_CHANGE_DEGREES * 2)
);
ProfileSCurveTrapezoidal = SimulationMCUTrajectoryProfile.ForceLinearInstance(
Encoder,
0.0,
0.0,
MCUConstants.SIMULATION_MCU_PEAK_VELOCITY,
MCUConstants.SIMULATION_MCU_PEAK_ACCELERATION,
MiscellaneousConstants.NEGLIGIBLE_POSITION_CHANGE_DEGREES / 2
);
ProfileLinearFull = SimulationMCUTrajectoryProfile.ForceLinearInstance(
Encoder,
0.0,
0.0,
MCUConstants.SIMULATION_MCU_PEAK_VELOCITY,
MCUConstants.SIMULATION_MCU_PEAK_ACCELERATION,
Encoder.GetEquivalentDegreesFromEncoderTicks(20)
);
ProfileLinearPartial = SimulationMCUTrajectoryProfile.ForceLinearInstance(
Encoder,
0.0,
0.0,
MCUConstants.SIMULATION_MCU_PEAK_VELOCITY,
MCUConstants.SIMULATION_MCU_PEAK_ACCELERATION,
Encoder.GetEquivalentDegreesFromEncoderTicks(10)
);
}
public void TestDegreesToEncoderTicksCalculationGetter()
{
Assert.AreEqual(0.0, Encoder0.GetEquivalentDegreesFromEncoderTicks(0), 360.0 / Encoder0.NumberOfEncoderTickPositions);
Assert.AreEqual(90.0, Encoder0.GetEquivalentDegreesFromEncoderTicks(256), 360.0 / Encoder0.NumberOfEncoderTickPositions);
Assert.AreEqual(135.2, Encoder0.GetEquivalentDegreesFromEncoderTicks(384), 360.0 / Encoder0.NumberOfEncoderTickPositions);
Assert.AreEqual(359.9, Encoder0.GetEquivalentDegreesFromEncoderTicks(1023), 360.0 / Encoder0.NumberOfEncoderTickPositions);
Assert.AreEqual(0.0, Encoder0.GetEquivalentDegreesFromEncoderTicks(0), 360.0 / Encoder0.NumberOfEncoderTickPositions);
Assert.AreEqual(96.7, Encoder0.GetEquivalentDegreesFromEncoderTicks(275), 360.0 / Encoder0.NumberOfEncoderTickPositions);
Assert.AreEqual(339.42, Encoder0.GetEquivalentDegreesFromEncoderTicks(965), 360.0 / Encoder0.NumberOfEncoderTickPositions);
Assert.AreEqual(270.0, Encoder0.GetEquivalentDegreesFromEncoderTicks(768), 360.0 / Encoder0.NumberOfEncoderTickPositions);
Assert.AreEqual(0.0, Encoder1.GetEquivalentDegreesFromEncoderTicks(0), 360.0 / Encoder1.NumberOfEncoderTickPositions);
Assert.AreEqual(90.0, Encoder1.GetEquivalentDegreesFromEncoderTicks(1024), 360.0 / Encoder1.NumberOfEncoderTickPositions);
Assert.AreEqual(135.2, Encoder1.GetEquivalentDegreesFromEncoderTicks(1538), 360.0 / Encoder1.NumberOfEncoderTickPositions);
Assert.AreEqual(359.9, Encoder1.GetEquivalentDegreesFromEncoderTicks(4094), 360.0 / Encoder1.NumberOfEncoderTickPositions);
Assert.AreEqual(0.0, Encoder1.GetEquivalentDegreesFromEncoderTicks(0), 360.0 / Encoder1.NumberOfEncoderTickPositions);
Assert.AreEqual(96.7, Encoder1.GetEquivalentDegreesFromEncoderTicks(1100), 360.0 / Encoder1.NumberOfEncoderTickPositions);
Assert.AreEqual(339.42, Encoder1.GetEquivalentDegreesFromEncoderTicks(3861), 360.0 / Encoder1.NumberOfEncoderTickPositions);
Assert.AreEqual(270.0, Encoder1.GetEquivalentDegreesFromEncoderTicks(3072), 360.0 / Encoder1.NumberOfEncoderTickPositions);
Assert.AreEqual(0.0, Encoder2.GetEquivalentDegreesFromEncoderTicks(0), 360.0 / Encoder2.NumberOfEncoderTickPositions);
Assert.AreEqual(90.0, Encoder2.GetEquivalentDegreesFromEncoderTicks(16384), 360.0 / Encoder2.NumberOfEncoderTickPositions);
Assert.AreEqual(135.2, Encoder2.GetEquivalentDegreesFromEncoderTicks(24612), 360.0 / Encoder2.NumberOfEncoderTickPositions);
Assert.AreEqual(359.9, Encoder2.GetEquivalentDegreesFromEncoderTicks(65517), 360.0 / Encoder2.NumberOfEncoderTickPositions);
Assert.AreEqual(0.0, Encoder2.GetEquivalentDegreesFromEncoderTicks(0), 360.0 / Encoder2.NumberOfEncoderTickPositions);
Assert.AreEqual(96.7, Encoder2.GetEquivalentDegreesFromEncoderTicks(17603), 360.0 / Encoder2.NumberOfEncoderTickPositions);
Assert.AreEqual(339.42, Encoder2.GetEquivalentDegreesFromEncoderTicks(61789), 360.0 / Encoder2.NumberOfEncoderTickPositions);
Assert.AreEqual(270.0, Encoder2.GetEquivalentDegreesFromEncoderTicks(49152), 360.0 / Encoder2.NumberOfEncoderTickPositions);
}
// 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 double InterpretDegreesAt(SimulationAbsoluteEncoder motorEncoder, DateTime startTime, DateTime evaluationTime)
{
return(motorEncoder.GetEquivalentDegreesFromEncoderTicks(InterpretEncoderTicksAt(startTime, evaluationTime)));
}
