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); }