void ScanPollThread()
{
try
{
Motor xmotor = Motors[RobotSetup.XPort];
Motor ymotor = Motors[RobotSetup.YPort];
float xratio = RatioSettings[RobotSetup.XPort];
float yratio = RatioSettings[RobotSetup.YPort];
int prevx = -1;
int prevy = -1;
while (true)
{
_scanWait.WaitOne();
// capture motor positions
int x = xmotor.GetTachoCount();
int y = ymotor.GetTachoCount();
if (prevx != x || prevy != y)
{
// send a data point
RGBColor color = _colorSensor.ReadRGB();
// a bit of formatting
SendData(string.Format("PIX;{0};{1};{2};{3:#.##};{4:#.##}\0", new object[] { color.Red, color.Green, color.Blue, x / xratio, y / yratio }));
//
// LcdConsole.WriteLine(string.Format("{0:#.##}:{1:#.##} = {2}", x / xratio, y / yratio, color.ToString()));
prevx = x;
prevy = y;
}
if (_scanDelay > 0)
{
Thread.Sleep(_scanDelay);
}
}
}
catch (OperationCanceledException)
{
// normal exception when thread is stopped
}
catch (Exception ex)
{
//
while (ex != null)
{
LcdConsole.WriteLine(ex.Message);
ex = ex.InnerException;
}
throw;
}
}
public static RGBColor[] CountFacelets(Motor a, EV3ColorSensor sensor)
{
RGBColor[] colors = new RGBColor[8];
a.ResetTacho();
for (int count = 0; count < 8;)
{
if (count == a.GetTachoCount() / 45 % 8)
{
colors [count++] = sensor.ReadRGB();
}
}
return(colors);
}
/*
* This method controls the movements of the robot to scan the facelets
* of any single face of the cube.
*/
public static void BuildFace(Face face, Motor motorA, Motor motorB, EV3ColorSensor sensor)
{
RGBColor[] colors = null;
RGBColor color = null;
Thread count = new Thread(() => colors = CountFacelets(motorA, sensor));
MoveCube.Move(motorB, MoveCube.SensorMid, 15);
color = sensor.ReadRGB();
MoveCube.Move(motorB, MoveCube.SensorSide, 15);
count.Start();
while (!count.IsAlive)
{
;
}
WaitHandle handle = motorA.SpeedProfile((sbyte)100, 0, (uint)360, 0, true);
handle.WaitOne();
count.Join();
MoveCube.Move(motorB, MoveCube.SensorRest, 15);
char colorvalue = ' ';
for (int i = 0; i < 9; i++)
{
switch (i)
{
case 0:
colorvalue = getColorValue(color);
face.square[1, 1] = colorvalue;
break;
case 1:
color = colors[0];
colorvalue = getColorValue(color);
face.square[2, 1] = colorvalue;
break;
case 3:
color = colors[2];
colorvalue = getColorValue(color);
face.square[1, 2] = colorvalue;
break;
case 5:
color = colors[4];
colorvalue = getColorValue(color);
face.square[0, 1] = colorvalue;
break;
case 7:
color = colors[6];
colorvalue = getColorValue(color);
face.square[1, 0] = colorvalue;
break;
case 2:
color = colors[1];
colorvalue = getColorValue(color);
face.square[2, 2] = colorvalue;
break;
case 4:
color = colors[3];
colorvalue = getColorValue(color);
face.square[0, 2] = colorvalue;
break;
case 6:
color = colors[5];
colorvalue = getColorValue(color);
face.square[0, 0] = colorvalue;
break;
case 8:
color = colors[7];
colorvalue = getColorValue(color);
face.square[2, 0] = colorvalue;
break;
}
Console.WriteLine("Line: {0} Color: {1} RGB code: {2}", i, colorvalue, color);
}
}
private string ScanColor()
{
WaitForSensorReady();
RGBColor rgbColor = colorSensor.ReadRGB();
int max = Math.Max(Math.Max(rgbColor.Red, rgbColor.Green), rgbColor.Blue);
if (max > 255) // Normalize to max 255
{
rgbColor = new RGBColor(
(UInt16)((int)rgbColor.Red * 255 / max),
(UInt16)((int)rgbColor.Green * 255 / max),
(UInt16)((int)rgbColor.Blue * 255 / max));
}
Color color = Color.FromArgb(rgbColor.Red, rgbColor.Green, rgbColor.Blue);
var hue = (int)color.GetHue();
var bright = (int)(color.GetBrightness() * 100);
if ((bright < 15 && hue < 50) || (bright <= 22 && hue < 30) || bright < 15)
{
LcdConsole.WriteLine("Color scan error");
return("ERR");
}
//int sat = (int) (color.GetSaturation()*100);
string colorStr = string.Format("R:{0} G:{1} B:{2} H:{3} B:{4} ", rgbColor.Red, rgbColor.Green, rgbColor.Blue, hue, bright);
string c = " ";
if ((bright > 56) ||
(hue > 42 && hue < 90 && (float)rgbColor.Red / rgbColor.Blue < 1.97) ||
(hue >= 90 && hue < 105 && bright > 40))
{
c = "U - WHITE";
}
else if (hue > 80 && hue < 135)
{
c = "B - GREEN";
}
else if (hue > 135 && hue < 260)
{
c = "F - BLUE";
if (((float)rgbColor.Blue) / rgbColor.Red < 1.2f)
{
c = "U - WHITE";
}
}
else if (hue > 33 && hue < 70)
{
c = "D - YELLOW";
}
else if (hue > 15 && hue < 30)
{
/*if (rgbColor.Blue >= 15 && hue > 15)
* {
* if (((float) rgbColor.Red)/rgbColor.Blue > 6.10f)
* c = "R - ORANGE";
* else
* c = "L - RED";
* }
* else
* c = "R - ORANGE";*/
c = "R - ORANGE";
}
else if (hue <= 15)
{
c = "L - RED";
}
c += " - " + colorStr;
return(c);
}
