public HLInstruction(float height, float xTilt, float yTilt, float moveTime, bool isLevelingInstruction = false, bool isFlexInstruction = false)
{
var tiltMaxMultiplier = isFlexInstruction ? 2f : 1f;
TargetHLMachineState = new HLMachineState(
Mathf.Clamp(height, c.MinPlateHeight, c.MaxPlateHeight),
Mathf.Clamp(xTilt, c.MinTiltAngle * tiltMaxMultiplier, c.MaxTiltAngle * tiltMaxMultiplier),
Mathf.Clamp(yTilt, c.MinTiltAngle * tiltMaxMultiplier, c.MaxTiltAngle * tiltMaxMultiplier));
MoveTime = moveTime;
IsLevelingInstruction = isLevelingInstruction;
}
public void SendInstructions(List <HLInstruction> instructions)
{
if (_elapsedTime < _totalMoveTime)
{
return;
}
// add tilt as HighLevelInstruction in here
var levelingInstructions = instructions.Where(instruction => instruction.IsLevelingInstruction);
foreach (var instruction in levelingInstructions)
{
var levelingOnlyState = instruction.TargetHLMachineState -
new HLInstruction(0.01f, 0f, 0f, 0.2f).TargetHLMachineState;
_levelingOffset += levelingOnlyState;
}
var llInstructions = instructions.Select(instruction =>
{
var i = instruction + new HLInstruction(_levelingOffset, 0f, instruction.IsLevelingInstruction);
return(i.Translate());
}).ToList();
_totalMoveTime = 0f;
_elapsedTime = 0f;
foreach (var instruction in instructions)
{
_totalMoveTime += instruction.MoveTime;
}
//NOTE: we're adding 50ms to make sure that the microcontroller is done before new data gets sent.
_totalMoveTime += 0.050f;
switch (_machineEndPoint)
{
case MachineEndPoint.Model:
_modelMachine.Instruct(llInstructions);
break;
case MachineEndPoint.Real:
_realMachine.Instruct(llInstructions);
break;
case MachineEndPoint.ModelAndReal:
_modelMachine.Instruct(llInstructions);
_realMachine.Instruct(llInstructions);
break;
}
}
public HLInstruction(float height, float xTilt, float yTilt, float moveTime, bool isLevelingInstruction = false,
bool isFlexInstruction = false)
{
var tiltMaxMultiplier = isFlexInstruction ? 2f : 1f;
if (xTilt > c.MaxTiltAngle || xTilt < c.MinTiltAngle)
{
Debug.Log("xTilt: " + xTilt + " was capped");
}
if (yTilt > c.MaxTiltAngle || yTilt < c.MinTiltAngle)
{
Debug.Log("yTilt: " + yTilt + " was capped");
}
TargetHLMachineState = new HLMachineState(
Mathf.Clamp(height, c.MinPlateHeight, c.MaxPlateHeight),
Mathf.Clamp(xTilt, c.MinTiltAngle * tiltMaxMultiplier, c.MaxTiltAngle * tiltMaxMultiplier),
Mathf.Clamp(yTilt, c.MinTiltAngle * tiltMaxMultiplier, c.MaxTiltAngle * tiltMaxMultiplier));
MoveTime = moveTime;
IsLevelingInstruction = isLevelingInstruction;
}
/// <summary>
/// Translates a High Level Machine State into a Low Level Machine State
/// </summary>
public static LLMachineState Translate(this HLMachineState hlState)
{
// NOTE: We are adding the origin height because for all IK calculations we need the height relative to
// motor shaft position and not just the height from origin/rest position of the plate.
var ikHeight = hlState.Height + Constants.HeightOrigin;
var xHeightOffset = MiscMath.HeightDifferenceFromTilt(hlState.XTilt);
var yHeightOffset = MiscMath.HeightDifferenceFromTilt(hlState.YTilt);
var xWidthOffset = MiscMath.WidthDifferenceFromTilt(hlState.XTilt);
var yWidthOffset = MiscMath.WidthDifferenceFromTilt(hlState.YTilt);
var m1Rot = ik.CalculateJoint1RotationFromTargetY(ikHeight + xHeightOffset, xWidthOffset);
var m2Rot = ik.CalculateJoint1RotationFromTargetY(ikHeight - xHeightOffset, xWidthOffset);
var m3Rot = ik.CalculateJoint1RotationFromTargetY(ikHeight + yHeightOffset, yWidthOffset);
var m4Rot = ik.CalculateJoint1RotationFromTargetY(ikHeight - yHeightOffset, yWidthOffset);
var m1J2Rot = ik.CalculateJoint2RotationFromJoint1Rotation(m1Rot, xWidthOffset);
var m2J2Rot = ik.CalculateJoint2RotationFromJoint1Rotation(m2Rot, xWidthOffset);
var m3J2Rot = ik.CalculateJoint2RotationFromJoint1Rotation(m3Rot, yWidthOffset);
var m4J2Rot = ik.CalculateJoint2RotationFromJoint1Rotation(m4Rot, yWidthOffset);
return(new LLMachineState(m1Rot, m2Rot, m3Rot, m4Rot, m1J2Rot, m2J2Rot, m3J2Rot, m4J2Rot));
}
public HLInstruction(HLMachineState targetMachineState, float moveTime, bool isLevelingInstruction = false)
{
TargetHLMachineState = targetMachineState;
MoveTime = moveTime;
IsLevelingInstruction = isLevelingInstruction;
}
