public void Start()
{
runStopTokenSource = new CancellationTokenSource();
runStopToken = runStopTokenSource.Token;
var action = new RTAction(rtSender, new RTLockCommand(false));
action.Run();
moveThread = new Thread(new ThreadStart(Loop));
moveThread.Start();
RunState = State.Running;
}
public Vector3 ReadHardwareCoordinates()
{
Logger.Instance.Debug(this, "readhw", "wait for lock");
int retry = 0;
Logger.Instance.Debug(this, "readhw", "read coordinates");
while (true)
{
RTAction action = new RTAction(rtSender, new RTGetPositionCommand());
// action.ReadyToRun.WaitOne();
action.Run();
action.Finished.WaitOne(timeoutT);
if (action.ActionResult == null ||
!action.ActionResult.ContainsKey("X") ||
!action.ActionResult.ContainsKey("Y") ||
!action.ActionResult.ContainsKey("Z"))
{
action.Dispose();
Logger.Instance.Warning(this, "readhw", String.Format("Can not read coordinates, retry."));
retry++;
if (retry >= maxretry)
{
Logger.Instance.Error(this, "readhw", String.Format("Max retry exceed"));
throw new TimeoutException();
}
Thread.Sleep(300);
continue;
}
var xs = action.ActionResult["X"];
var ys = action.ActionResult["Y"];
var zs = action.ActionResult["Z"];
action.Dispose();
var pos = new Vector3(decimal.Parse(xs, CultureInfo.InvariantCulture),
decimal.Parse(ys, CultureInfo.InvariantCulture),
decimal.Parse(zs, CultureInfo.InvariantCulture));
pos.x /= config.X_axis.steps_per_mm;
pos.y /= config.Y_axis.steps_per_mm;
pos.z /= config.Z_axis.steps_per_mm;
return(pos);
}
}
private IAction BuildStepAction(decimal fx, decimal fy, decimal fz, decimal dt)
{
Vector3 comp = new Vector3();
var feed = (decimal)(Math.Sqrt((double)(fx * fx + fy * fy + fz * fz)));
var delta = new Vector3(fx * dt, fy * dt, fz * dt);
if (delta.Length() == 0)
{
return(null);
}
var opts = new RTMovementOptions
{
acceleration = config.max_acceleration,
Feed = feed,
FeedStart = feed,
FeedEnd = feed
};
var command = new RTLineMoveCommand(delta, comp, opts, config);
var action = new RTAction(rtSender, command);
return(action);
}
public (bool ex, bool ey, bool ez, bool ep) ReadCurrentEndstops()
{
Logger.Instance.Debug(this, "readhw", "read endstops");
int retry = 0;
while (true)
{
RTAction action = null;
try
{
action = new RTAction(rtSender, new RTGetEndstopsCommand());
// action.ReadyToRun.WaitOne();
action.Run();
action.Finished.WaitOne(timeoutT);
var result = (action.ActionResult["EX"] == "1",
action.ActionResult["EY"] == "1",
action.ActionResult["EZ"] == "1",
action.ActionResult["EP"] == "1");
action.Dispose();
return(result);
}
catch (Exception e)
{
if (action != null)
{
action.Dispose();
}
Logger.Instance.Warning(this, "readhw", String.Format("Can not read position, retry. {0}", e));
retry++;
if (retry >= maxretry)
{
Logger.Instance.Warning(this, "readhw", String.Format("Max retry exceed"));
throw e;
}
Thread.Sleep(300);
}
}
}
