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))); }