private void OnPositionChanged(PanTiltPosition position)
{
lock (this._syncLock)
{
if (this._lastRefresh.AddMilliseconds(200) >= DateTime.Now)
{
return;
}
this._lastRefresh = DateTime.Now;
this.labelPositionPan.Invoke(o => {
o.ForeColor = Color.Black;
o.Text = $"Pan: {position.Pan}";
});
this.labelPositionTilt.Invoke(o => o.Text = $"Tilt: {position.Tilt}");
if (this._deviceConfiguration.CameraActive)
{
return;
}
if (this._panTiltControl != null)
{
//TODO:Optimize render image does not block position changed event
this.Redraw(position);
}
}
}
public void DrawText(PanTiltPosition position, string text, Brush brush, bool centerText)
{
var point = this.ConvertPanTilt2Point(position);
using (var graphics = Graphics.FromImage(this._drawImage))
{
graphics.TextRenderingHint = TextRenderingHint.SingleBitPerPixelGridFit;
var font = new Font("Verdana", 4 * this._multiplier, FontStyle.Bold);
float x, y;
if (centerText)
{
var textSize = graphics.MeasureString(text, font);
x = point.X - (textSize.Width / 2);
y = point.Y - (textSize.Height / 2);
}
else
{
x = point.X;
y = point.Y;
}
graphics.DrawString(text, font, brush, x, y);
}
}
private void OnPositionChanged(PanTiltPosition position)
{
if (this._lastRefresh.AddMilliseconds(200) >= DateTime.Now)
{
return;
}
this._lastRefresh = DateTime.Now;
this.labelPositionPan.Invoke(o => o.Text = $"Pan: {position.Pan}");
this.labelPositionTilt.Invoke(o => o.Text = $"Tilt: {position.Tilt}");
if (this._deviceConfiguration.CameraActive)
{
return;
}
if (this._panTiltControl != null)
{
//TODO:Optimize render image does not block position changed event
this._cameraDrawEngine.Clear();
this._cameraDrawEngine.DrawPtHeadLimits(this._panTiltControl.GetLimits());
this._cameraDrawEngine.DrawCrossHair(position, Brushes.Black);
this.UpdateCurrentImage();
}
}
private PointF ConvertPanTilt2Point(PanTiltPosition item)
{
var x = (item.Pan + this._originalWidth) * this._multiplier;
var y = -(item.Tilt - this._originalHeight) * this._multiplier;
return(new PointF((float)x, (float)y));
}
public static PanTiltPosition AddRelativePosition(this PanTiltPosition currentPanTiltPosition, PanTiltPosition panTiltPosition, int seconds)
{
var pan = currentPanTiltPosition.Pan + panTiltPosition.Pan * seconds;
var tilt = currentPanTiltPosition.Tilt + panTiltPosition.Tilt * seconds;
return(new PanTiltPosition(pan, tilt));
}
private void OnPositionChanged(PanTiltPosition pt)
{
if (this._active && pt.Pan > this._stop)
{
this._resetEvent.Set();
}
}
public void DrawLineWithArrow(PanTiltPosition pt1, PanTiltPosition pt2, Color color, int width)
{
var pen = new Pen(color, width);
var point1 = this.ConvertPanTilt2Point(pt1);
var point2 = this.ConvertPanTilt2Point(pt2);
this.DrawLineWithArrow(this._drawImage, point1, point2, pen);
}
protected bool Equals(PanTiltPosition other)
{
if (other == null)
{
return(false);
}
return(this.Pan == other.Pan && this.Tilt == other.Tilt);
}
public void DrawCircle(PanTiltPosition centerPt, float dimension, Brush brush)
{
var point = this.ConvertPanTilt2Point(centerPt);
using (var graphics = Graphics.FromImage(this._drawImage))
{
graphics.SmoothingMode = SmoothingMode.HighQuality;
graphics.DrawEllipse(new Pen(brush, 3), point.X - (dimension / 2), point.Y - (dimension / 2), dimension, dimension);
}
}
public static bool IsEqual(PanTiltPosition position1, PanTiltPosition position2, double panTolerance = 0.5, double tiltTolerance = 0.5)
{
var panDifference = Math.Abs(position1.Pan - position2.Pan);
var tiltDifference = Math.Abs(position1.Tilt - position2.Tilt);
if (panDifference <= panTolerance && tiltDifference <= tiltTolerance)
{
return(true);
}
return(false);
}
private void Redraw(PanTiltPosition position)
{
if (this._cameraDrawEngine == null)
{
return;
}
this._cameraDrawEngine.Clear();
this._cameraDrawEngine.DrawPtHeadLimits(this._panTiltControl.GetLimits());
this._cameraDrawEngine.DrawCrossHair(position, Brushes.Black);
this.UpdateCurrentImage();
}
private void OnPositionChanged(PanTiltPosition pt)
{
switch (this._axisType)
{
case AxisType.Pan:
this._lastPosition = pt.Pan;
break;
case AxisType.Tilt:
this._lastPosition = pt.Tilt;
break;
}
}
public void DrawPtHeadLimits(PanTiltLimit panTiltLimit)
{
var leftUp = new PanTiltPosition(panTiltLimit.PanMin, panTiltLimit.TiltMin);
var leftDown = new PanTiltPosition(panTiltLimit.PanMin, panTiltLimit.TiltMax);
var rightUp = new PanTiltPosition(panTiltLimit.PanMax, panTiltLimit.TiltMin);
var rightDown = new PanTiltPosition(panTiltLimit.PanMax, panTiltLimit.TiltMax);
var positions = new PanTiltPosition[] { leftUp, rightUp, rightDown, leftDown };
var textPos = positions.OrderBy(o => this.Distance(o, new PanTiltPosition(-180, 90))).FirstOrDefault();
this.DrawText(textPos, "PT Unit Limits", Brushes.Crimson, false);
this.DrawPolygon(positions, Color.Crimson);
}
public ActionResult MoveToPosition([FromQuery] string ipAddress, [FromBody] PanTiltPosition position)
{
this._logger.LogDebug($"MoveToPosition {ipAddress} {position}");
using (ICommunication communication = new UdpNetworkCommunication(IPAddress.Parse(ipAddress), 5555))
using (IPanTiltControl panTiltControl = new AlturosPanTiltControl(communication))
{
if (panTiltControl.PanTiltAbsolute(position))
{
return(StatusCode(StatusCodes.Status200OK));
}
return(StatusCode(StatusCodes.Status400BadRequest));
}
}
private void DrawMovement(PanTiltPosition start, PanTiltPosition end, int number, bool isLast)
{
var brush = Brushes.DarkGreen;
var dimension = isLast ? 50 : 25;
lock (this._syncObject)
{
this._drawEngine.DrawLineWithArrow(start, end, new Pen(brush).Color, 4);
this._drawEngine.DrawText(new PanTiltPosition(end.Pan - 10, end.Tilt), number.ToString(), brush, true);
this._drawEngine.DrawCircle(new PanTiltPosition(end.Pan, end.Tilt), 5, brush);
this._drawEngine.DrawCircle(new PanTiltPosition(end.Pan - 10, end.Tilt), dimension, brush);
}
this.UpdateCurrentImage();
}
private async Task RunTest()
{
this._movementInfos.Clear();
this.PrepareDrawing();
this._lastPosition = null;
this._currentPtIndices = -1;
this._ptUpdateTimer.Enabled = true;
this._ptUpdateTimer.Start();
var timeoutCommands = (int)this.numericUpDownTime.Value;
foreach (var panTiltPosition in this._panTiltPositions)
{
this._currentPtIndices++;
if (this._currentPtIndices == 0)
{
this.DrawMovement(this._panTiltPositions.First(), this._panTiltPositions.First(), this._currentPtIndices, false);
}
else
{
var isLast = this._currentPtIndices == this._panTiltPositions.Count - 1;
this.DrawMovement(this._panTiltPositions[_currentPtIndices - 1], this._panTiltPositions[_currentPtIndices], this._currentPtIndices, isLast);
}
var info = new FastMovementInfo(DateTime.Now, this._currentPtIndices, "NewPosition", panTiltPosition.Pan, panTiltPosition.Tilt);
this.AddMovementInfo(info);
await Task.Run(() =>
{
this._panTiltControl?.PanTiltAbsolute(panTiltPosition.Pan, panTiltPosition.Tilt);
this._waitMovementResetEvent.WaitOne(timeoutCommands);
});
}
await Task.Run(() =>
{
this._positionChecker?.ComparePosition(this._panTiltPositions.Last());
});
this._ptUpdateTimer.Stop();
this._ptUpdateTimer.Enabled = false;
this.RefreshGrid();
}
public EneoPanTiltControl(ICommunication communication, bool debug = false)
{
this._communication = communication;
this._communication.ReceiveData += PackageReceived;
this._debug = debug;
this._positionConverter = new PositionConverter();
this._position = null;
this._limits = new PanTiltLimit();
this._handler = new FeedbackHandler();
this._resetEvent = new ManualResetEvent(false);
this._stopHeartbeat = true;
this.InitializeSpeedTranslationTable();
}
public FastMovementControl()
{
this.InitializeComponent();
this._drawEngine = new DrawEngine(this._drawEngineMultiplier);
this.labelError.Text = string.Empty;
this._lastPosition = new PanTiltPosition();
this._currentPtIndices = 0;
this._movementInfos = new List <FastMovementInfo>();
this._bindingSource.DataSource = this._movementInfos;
this.dataGridViewFastMovement.AutoGenerateColumns = false;
this.dataGridViewFastMovement.DataSource = this._bindingSource;
this._ptUpdateTimer = new Timer();
this._ptUpdateTimer.Elapsed += UpdateTimerElapsed;
this._ptUpdateTimer.Interval = 250;
}
public void DrawCrossHair(PanTiltPosition position, Brush brush)
{
var point = this.ConvertPanTilt2Point(position);
var size1 = 7 * this._multiplier;
var size2 = 4 * this._multiplier;
var length = 5 * this._multiplier;
using (var graphics = Graphics.FromImage(this._drawImage))
{
graphics.SmoothingMode = SmoothingMode.HighQuality;
graphics.DrawEllipse(new Pen(brush, 3), point.X - (size1 / 2), point.Y - (size1 / 2), size1, size1);
graphics.DrawEllipse(new Pen(brush, 2), point.X - (size2 / 2), point.Y - (size2 / 2), size2, size2);
graphics.DrawLine(new Pen(brush, 2), point.X + 5, point.Y, point.X + length, point.Y);
graphics.DrawLine(new Pen(brush, 2), point.X - 5, point.Y, point.X - length, point.Y);
graphics.DrawLine(new Pen(brush, 2), point.X, point.Y + 5, point.X, point.Y + length);
graphics.DrawLine(new Pen(brush, 2), point.X, point.Y - 5, point.X, point.Y - length);
}
}
private void ProveDeriviation(PanTiltPosition absoluteCurrentPt, double allowedDeriviation)
{
if (this._stoppedManually)
{
return;
}
var deviationOk = this._positionChecker.ComparePosition(absoluteCurrentPt, allowedDeriviation);
this._deviationOverrunDetected = !this._deviationOverrunDetected && !deviationOk;
if (this._deviationOverrunDetected)
{
this.labelError.Invoke(o => o.Text = $"{DateTime.Now.ToShortTimeString()} - Too high deviation detected");
}
else
{
this.labelError.Invoke(o => o.Text = string.Empty);
}
}
private void PackageReceived(byte[] data)
{
if (this._debug)
{
var hex = BitConverter.ToString(data);
Log.Debug($"{nameof(PackageReceived)} - {hex}");
}
Interlocked.Increment(ref this._receiveCount);
var message = Encoding.ASCII.GetString(data);
if (message.Length == 14)
{
int.TryParse(message.Substring(2, 6), out var tempPan);
int.TryParse(message.Substring(8, 6), out var tempTilt);
this._position = new PanTiltPosition(tempPan / 100.0, tempTilt / 100.0);
this.PositionChanged?.Invoke(this._position);
}
}
public async Task <bool> ComparePositionAsync(PanTiltPosition position, double tolerance = 0.5, int retry = 5, int timeout = 500, CancellationToken cancellationToken = default(CancellationToken))
{
if (this._debug)
{
Log.Debug($"{nameof(ComparePositionAsync)} - Pan:{position.Pan} Tilt:{position.Tilt} Tolerance: {tolerance}");
}
while (retry > 0 && !cancellationToken.IsCancellationRequested)
{
var ptHeadPosition = this._panTiltControl.GetPosition();
if (ptHeadPosition == null)
{
await Task.Delay(timeout, cancellationToken);
retry--;
continue;
}
var panDifference = Math.Abs(position.Pan - ptHeadPosition.Pan);
var tiltDifference = Math.Abs(position.Tilt - ptHeadPosition.Tilt);
if (panDifference <= tolerance && tiltDifference <= tolerance)
{
return(true);
}
if (this._debug)
{
Log.Debug($"{nameof(ComparePositionAsync)} - Difference, Pan:{panDifference} Tilt:{tiltDifference} Retry:{retry}");
}
await Task.Delay(timeout, cancellationToken);
retry--;
}
return(false);
}
public void GoToStartPosition(int position)
{
PanTiltPosition panTiltPosition = null;
switch (this._axisType)
{
case AxisType.Pan:
panTiltPosition = new PanTiltPosition(position, 0);
this._panTiltControl.PanTiltAbsolute(position, 0);
break;
case AxisType.Tilt:
panTiltPosition = new PanTiltPosition(0, position);
this._panTiltControl.PanTiltAbsolute(0, position);
break;
}
while (!this._positionChecker.ComparePosition(panTiltPosition, tolerance: 0.2))
{
Thread.Sleep(100);
}
}
private void UpdateTimerElapsed(object sender, ElapsedEventArgs e)
{
try
{
if (this._panTiltControl == null || this.buttonStartFast.Enabled)
{
return;
}
var currentPosition = this._currentPosition;
if (currentPosition == null)
{
return;
}
var info = new FastMovementInfo(DateTime.Now, this._currentPtIndices, "CurrentPosition", currentPosition.Pan, currentPosition.Tilt);
this.AddMovementInfo(info);
if (this._lastPosition != null)
{
lock (this._syncObject)
{
this._drawEngine.DrawLine(currentPosition, this._lastPosition, new Pen(Brushes.HotPink).Color, 4);
this._drawEngine.DrawCircle(new PanTiltPosition(currentPosition.Pan, currentPosition.Tilt), 5, Brushes.HotPink);
}
this.UpdateCurrentImage();
}
this._lastPosition = currentPosition;
this.RefreshGrid();
}
catch (Exception exception)
{
MessageBox.Show("Error", exception.Message);
}
}
public bool ComparePosition(PanTiltPosition position, double tolerance = 0.5, int retry = 5, int timeout = 500)
{
if (this._debug)
{
Log.Debug($"{nameof(ComparePosition)} - Pan:{position.Pan} Tilt:{position.Tilt} Tolerance: {tolerance}");
}
while (retry > 0)
{
var ptHeadPosition = this._panTiltControl.GetPosition();
if (ptHeadPosition == null)
{
Thread.Sleep(timeout);
retry--;
continue;
}
var panDifference = Math.Abs(position.Pan - ptHeadPosition.Pan);
var tiltDifference = Math.Abs(position.Tilt - ptHeadPosition.Tilt);
if (panDifference <= tolerance && tiltDifference <= tolerance)
{
return(true);
}
if (this._debug)
{
Log.Debug($"{nameof(ComparePosition)} - Difference, Pan:{panDifference} Tilt:{tiltDifference} Retry:{retry}");
}
Thread.Sleep(timeout);
retry--;
}
return(false);
}
private async Task MoveRectangleRelative(double degreePerSeconds, int seconds)
{
if (this._stoppedManually)
{
return;
}
this.PrepareDrawing();
await Task.Run(() =>
{
this._panTiltControl.PanTiltAbsolute(0, 0);
this._waitMovementResetEvent.WaitOne(1000);
this._positionChecker.ComparePosition(new PanTiltPosition(0, 0), 0.1, 20, 40);
});
this._currentPosition = _panTiltControl.GetPosition();
this._deviationOverrunDetected = false;
var items = new List <PanTiltPosition>();
items.Add(new PanTiltPosition(0, 0));
items.Add(new PanTiltPosition(0, degreePerSeconds / 2));
items.Add(new PanTiltPosition(degreePerSeconds / 2, 0));
items.Add(new PanTiltPosition(0, -degreePerSeconds));
items.Add(new PanTiltPosition(-degreePerSeconds, 0));
items.Add(new PanTiltPosition(0, degreePerSeconds));
items.Add(new PanTiltPosition(degreePerSeconds / 2, 0));
var absoluteCurrentPt = this._currentPosition;
var absolutePreviousPt = this._currentPosition;
var allowedDeriviation = 0.5 * degreePerSeconds;
this._lastPosition = null;
int i = 1;
while (!_stoppedManually && i < items.Count)
{
var relativePreviousPt = items[i - 1];
var relativeCurrentPt = items[i];
absolutePreviousPt = absolutePreviousPt.AddRelativePosition(relativePreviousPt, seconds);
absoluteCurrentPt = absoluteCurrentPt.AddRelativePosition(relativeCurrentPt, seconds);
this._drawEngine.DrawLine(absolutePreviousPt, absoluteCurrentPt, Color.DarkGreen, 4);
this.labelPanTiltPosition.Text = $"Pan:{this._currentPosition.Pan:0.00} Tilt:{this._currentPosition.Tilt:0.00}";
if (this._lastPosition != null)
{
this._drawEngine.DrawLine(this._lastPosition, this._currentPosition, Color.HotPink, 3);
}
this.UpdateCurrentImage();
this._lastPosition = this._currentPosition;
await Task.Run(() =>
{
this._panTiltControl.PanTiltRelative(relativeCurrentPt.Pan, relativeCurrentPt.Tilt);
this._waitMovementResetEvent.WaitOne(seconds * 1000);
this._panTiltControl.StopMoving();
this.ProveDeriviation(absoluteCurrentPt, allowedDeriviation);
});
i++;
}
if (this._lastPosition != null)
{
this._drawEngine.DrawLine(this._lastPosition, this._currentPosition, Color.HotPink, 3);
}
this.UpdateCurrentImage();
this.SaveRectangleTestImageToFile(degreePerSeconds, seconds);
}
private async Task MoveRectangleRelative(double degreePerSeconds, double seconds)
{
if (this._stoppedManually)
{
return;
}
this.PrepareDrawing();
await Task.Run(() =>
{
this._panTiltControl.PanTiltAbsolute(0, 0);
this._positionChecker.ComparePosition(new PanTiltPosition(0, 0), 0.1, 100, 100);
});
this._currentPosition = this._panTiltControl.GetPosition();
this._lastPosition = new PanTiltPosition(this._currentPosition.Pan, this._currentPosition.Tilt);
this._deviationOverrunDetected = false;
var items = new List <PanTiltPosition>
{
new PanTiltPosition(0, 0),
new PanTiltPosition(0, degreePerSeconds / 2),
new PanTiltPosition(degreePerSeconds / 2, 0),
new PanTiltPosition(0, -degreePerSeconds),
new PanTiltPosition(-degreePerSeconds, 0),
new PanTiltPosition(0, degreePerSeconds),
new PanTiltPosition(degreePerSeconds / 2, 0)
};
var absoluteCurrentPt = this._currentPosition;
var absolutePreviousPt = this._currentPosition;
var allowedDeriviation = 0.5 * degreePerSeconds;
this._lastPosition = null;
int i = 1;
while (!this._stoppedManually && i < items.Count)
{
var relativePreviousPt = items[i - 1];
var relativeCurrentPt = items[i];
//Draw expected route
absolutePreviousPt = absolutePreviousPt.AddRelativePosition(relativePreviousPt, seconds);
absoluteCurrentPt = absoluteCurrentPt.AddRelativePosition(relativeCurrentPt, seconds);
this._drawEngine.DrawLine(absolutePreviousPt, absoluteCurrentPt, Color.DarkGreen, 4);
//Draw actual route
if (this._lastPosition != null)
{
this._drawEngine.DrawLine(this._lastPosition, this._currentPosition, Color.HotPink, 3);
}
this.UpdateCurrentImage();
this.labelPanTiltPosition.Text = $"Pan:{this._currentPosition.Pan:0.00} Tilt:{this._currentPosition.Tilt:0.00}";
this._lastPosition = new PanTiltPosition(this._currentPosition.Pan, this._currentPosition.Tilt);
this._panTiltControl.PanTiltRelative(relativeCurrentPt.Pan, relativeCurrentPt.Tilt);
var delay = seconds * 1000.0;
await Task.Delay((int)delay, this._cancellationTokenSource.Token).ContinueWith(tsk => { /* TaskCanceledException */ });
this._panTiltControl.StopMoving();
await Task.Delay((int)Math.Abs((this._currentPosition.Pan + this._currentPosition.Tilt) * 35));
this.ProveDeriviation(absoluteCurrentPt, allowedDeriviation);
i++;
}
if (this._lastPosition != null)
{
this._drawEngine.DrawLine(this._lastPosition, this._currentPosition, Color.HotPink, 3);
}
this.UpdateCurrentImage();
this.SaveRectangleTestImageToFile(degreePerSeconds, seconds);
}
private void PanTiltPositionChanged(PanTiltPosition position)
{
this._currentPosition = position;
}
private async Task MoveCircle(int radius, int detailLevel = 8, bool reverse = false)
{
if (this._stoppedManually)
{
return;
}
this.PrepareDrawing();
await Task.Run(() =>
{
this._panTiltControl.PanTiltAbsolute(0, 0);
this._waitMovementResetEvent.WaitOne(1000);
this._positionChecker.ComparePosition(new PanTiltPosition(0, 0), 0.2, 20, 40);
});
var circleLogic = new CircleLogic();
this._currentPosition = new PanTiltPosition(0, 0);
this._deviationOverrunDetected = false;
var items = new List <PanTiltPosition>();
for (var r = radius; r <= 180; r += radius)
{
var positions = circleLogic.CalculatePtPositions(r, detailLevel);
if (reverse)
{
positions.Reverse();
}
items.AddRange(positions);
}
this._lastPosition = null;
for (var i = 1; i < items.Count; i++)
{
if (this._stoppedManually)
{
return;
}
var previousPt = items[i - 1];
var currentPt = items[i];
this._drawEngine.DrawLine(previousPt, currentPt, Color.DarkGreen, 4);
if (this._lastPosition != null)
{
this._drawEngine.DrawLine(this._lastPosition, this._currentPosition, Color.HotPink, 2);
}
this._lastPosition = new PanTiltPosition(this._currentPosition.Pan, this._currentPosition.Tilt);
this.labelPanTiltPosition.Text = $"Pan:{currentPt.Pan:0.00} Tilt:{currentPt.Tilt:0.00}";
this.UpdateCurrentImage();
await Task.Run(() =>
{
this._panTiltControl.PanTiltAbsolute(currentPt.Pan, currentPt.Tilt);
this._positionChecker.ComparePosition(new PanTiltPosition(currentPt.Pan, currentPt.Tilt), 0.1, 50, 40);
});
}
this.UpdateCurrentImage();
this.SaveCircleTestImageToFile(radius, detailLevel);
}
private double Distance(PanTiltPosition position1, PanTiltPosition position2)
{
return(Math.Pow(position1.Pan - position2.Pan, 2) + Math.Pow(position1.Tilt - position2.Tilt, 2));
}
