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)
);
}
// 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
));
}
}
/// <summary>
///
/// </summary>
///
public SimulationMCU(SimulationAbsoluteEncoder azEncoder, SimulationAbsoluteEncoder elEncoder)
{
AzEncoder = azEncoder;
ElEncoder = elEncoder;
ActiveObjectiveOrientationMoveStart = DateTime.MinValue;
ActiveObjectiveOrientation = null;
ActiveObjectiveAzimuthProfile = null;
ActiveObjectiveElevationProfile = null;
RequestedStopType = SimulationStopTypeEnum.NONE;
}
// Because the order of execution of tests is not guaranteed, have this method called whenever the values are changed
private static void Reset()
{
Encoder0 = new SimulationAbsoluteEncoder(10);
Encoder1 = new SimulationAbsoluteEncoder(12, 47.5);
Encoder2 = new SimulationAbsoluteEncoder(16, 185.12);
}
public double InterpretDegreesAt(SimulationAbsoluteEncoder motorEncoder, DateTime startTime, DateTime evaluationTime)
{
return(motorEncoder.GetEquivalentDegreesFromEncoderTicks(InterpretEncoderTicksAt(startTime, evaluationTime)));
}
// This factory function utilizes the equations on page 20 and 21 of the 2-axis MCU documentation to calculate the type of
// trajectory profile associated with the possible move type, as well as the characteristics of said profile
public static SimulationMCUTrajectoryProfile CalculateInstance(
SimulationAbsoluteEncoder motorEncoder,
double initialDegrees,
double initialVelocity,
double peakVelocity,
double peakAcceleration,
double objectiveDegrees)
{
// int initialSteps = motorEncoder.GetEquivalentEncoderTicksFromDegrees(initialDegrees);
// int objectiveSteps = motorEncoder.GetEquivalentEncoderTicksFromDegrees(objectiveDegrees);
// int changeInSteps = objectiveSteps - initialSteps;
// SimulationMCUTrajectoryProfile resultingProfile;
// // See if this move is even worth the effort of TRYING to move
// if (Math.Abs(motorEncoder.GetEquivalentDegreesFromEncoderTicks(changeInSteps)) < HardwareConstants.NEGLIGIBLE_POSITION_CHANGE_DEGREES)
// {
// resultingProfile = new SimulationMCUTrajectoryProfile(SimulationMCUTrajectoryProfileTypeEnum.NEGLIGIBLE, initialVelocity, peakVelocity, peakAcceleration, 0.0, 0.0, initialSteps, objectiveSteps);
// }
// else
// {
// // The move is substantial enough for us to want to move...
// // Try to make the most optimal types of trajectories work
// double timeRequiredForAcceleration = 4 * (peakVelocity - initialVelocity) / (3 * peakAcceleration);
// double distanceRequiredForAcceleration = timeRequiredForAcceleration * (peakVelocity + initialVelocity) / 2;
// int actualDistanceRequired = (int)(2 * distanceRequiredForAcceleration);
// // See if it can be a trapezoidal s-curve
// if (actualDistanceRequired < changeInSteps)
// {
// double remainingDistanceForConstantVelocity = changeInSteps - (2 * distanceRequiredForAcceleration);
// double remainingTimeForConstantVelocity = peakVelocity / remainingDistanceForConstantVelocity;
// resultingProfile = new SimulationMCUTrajectoryProfile(
// SimulationMCUTrajectoryProfileTypeEnum.S_CURVE_TRAPEZOIDAL,
// initialVelocity,
// peakVelocity,
// peakAcceleration,
// timeRequiredForAcceleration,
// (2 * timeRequiredForAcceleration) + remainingTimeForConstantVelocity,
// initialSteps,
// objectiveSteps
// );
// }
// // Did not fit the full trapezoidal s-curve requirements, try a triangular s-curve
// else if (changeInSteps == actualDistanceRequired)
// {
// resultingProfile = new SimulationMCUTrajectoryProfile(
// SimulationMCUTrajectoryProfileTypeEnum.S_CURVE_TRIANGULAR_FULL,
// initialVelocity,
// peakVelocity,
// peakAcceleration,
// timeRequiredForAcceleration,
// 2 * timeRequiredForAcceleration,
// initialSteps,
// objectiveSteps
// );
// }
// // Did not fit the full trapezoidal or triangular s-curve requirements, it's a partial triangular s-curve
// else
// {
// double fixedPeakVelocity = Math.Sqrt((0.75 * distanceRequiredForAcceleration * peakAcceleration) + (initialVelocity * initialVelocity));
// double fixedTimeRequiredForAcceleration = 4 * (fixedPeakVelocity - initialVelocity) / (3 * peakAcceleration);
// resultingProfile = new SimulationMCUTrajectoryProfile(
// SimulationMCUTrajectoryProfileTypeEnum.S_CURVE_TRIANGULAR_PARTIAL,
// initialVelocity,
// fixedPeakVelocity,
// peakAcceleration,
// fixedTimeRequiredForAcceleration,
// 2 * fixedTimeRequiredForAcceleration,
// initialSteps,
// objectiveSteps
// );
// }
// }
// return resultingProfile;
throw new NotImplementedException("Logic is here, but tests are still failing, so more work needs to be done - use linear instance for now.");
}
