private List <ScannerResult> GetAngleFromMessage(string readed)
{
List <ScannerResult> scannerResults = new List <ScannerResult>();
if (!string.IsNullOrEmpty(readed))
{
bool canRead = false;
ScannerResult scannerResult = new ScannerResult();
foreach (var line in readed.Split('|'))
{
if (line.Contains("Kat czujnika: "))
{
if (canRead)
{
var tempLine = line.Replace("Kat czujnika: ", "");
scannerResult.ultrasonicAngle = int.Parse(tempLine) - ULTRASONIC_ZERO_AMGLE;
}
}
else if (line.Contains("Czujnik ultradzwiekowy: "))
{
if (canRead)
{
var tempLine = line.Replace("Czujnik ultradzwiekowy: ", "");
scannerResult.ulstrasonicDistanceCm = int.Parse(tempLine);
scannerResults.Add(scannerResult);
canRead = false;
}
}
else if (line.Contains("Czujnik radarowy: "))
{
if (canRead)
{
var tempLine = line.Replace("Czujnik radarowy: ", "");
scannerResult.radarDistanceMm = int.Parse(tempLine);
}
}
else
{
int tempval;
if (int.TryParse(line, out tempval))
{
scannerResult = new ScannerResult();
scannerResult.id = tempval;
canRead = true;
}
}
}
}
return(scannerResults);
}
private void GraphicWrite(ScannerResult scannerResults, int colorID)
{
degree = scannerResults.ultrasonicAngle;
p = new Pen(Color.Green, 1f);
g = Graphics.FromImage(bmp);
int tu = (degree - lim) % 360;
if (degree >= 0 && degree <= 180)
{
x = cx + (int)(scannerResults.ulstrasonicDistanceCm * Math.Sin(Math.PI * degree / 180));
y = cy - (int)(scannerResults.ulstrasonicDistanceCm * Math.Cos(Math.PI * degree / 180));
}
else
{
x = cx - (int)(scannerResults.ulstrasonicDistanceCm * -Math.Sin(Math.PI * degree / 180));
y = cy - (int)(scannerResults.ulstrasonicDistanceCm * Math.Cos(Math.PI * degree / 180));
}
if (tu >= 0 && tu <= 180)
{
tx = cx + (int)(scannerResults.ulstrasonicDistanceCm * Math.Sin(Math.PI * tu / 180));
ty = cy - (int)(scannerResults.ulstrasonicDistanceCm * Math.Cos(Math.PI * tu / 180));
}
else
{
tx = cx - (int)(scannerResults.ulstrasonicDistanceCm * -Math.Sin(Math.PI * tu / 180));
ty = cy - (int)(scannerResults.ulstrasonicDistanceCm * Math.Cos(Math.PI * tu / 180));
}
//g.DrawEllipse(p, 0, 0, WIDTH, HEIGHT);
//g.DrawEllipse(p, 80, 80, WIDTH - 160, HEIGHT - 160);
g.DrawLine(p, new Point(cx, 0), new Point(cx, HEIGHT));
g.DrawLine(p, new Point(0, cy), new Point(WIDTH, cy));
//g.DrawLine(new Pen(Color.Black, 1f), new Point(cx, cy), new Point(tx, ty));
//g.DrawLine(p, new Point(cx, cy), new Point(x, y));
Brush brush;
switch (colorID)
{
case 0:
brush = Brushes.White;
break;
case 1:
brush = Brushes.Yellow;
break;
case 2:
brush = Brushes.GreenYellow;
break;
case 3:
brush = Brushes.MediumOrchid;
break;
case 4:
brush = Brushes.Green;
break;
case 5:
brush = Brushes.Orange;
break;
case 6:
brush = Brushes.DarkViolet;
break;
case 7:
brush = Brushes.Firebrick;
break;
case 8:
brush = Brushes.DodgerBlue;
break;
case 9:
brush = Brushes.DeepPink;
break;
default:
brush = Brushes.White;
break;
}
g.FillRectangle(brush, x, y, 1, 1);
pictureBox1.Invoke(new Action(() =>
{
pictureBox1.Image = bmp;
}));
p.Dispose();
g.Dispose();
}
