public VPoint(VShape shape, VPoint camera) { ID = shape.ID; healthy = true; if (shape._isSquare) tag = "Square"; if (shape._isTriangle) tag = "Triangle"; Point camXY = shape.FindCenter(); //Right so this is where things get ugly. Camera X and Y are divided by the total to get a percentage. //50% is subtracted to make the coordinant a percentage of total width relative to middle. //This is multiplied by the Camera constant (viewing angle) times the height of the double xToCam = ((double)camXY.X / 480 - 0.5) * (EveDef.CameraWidthOverZ * 1.00000 * (camera.z + EveDef.WristToEnd_Z - EveDef.WristToCam_Z)); double yToCam = ((double)camXY.Y / 640 - 0.5) * (EveDef.CameraWidthOverZ * 1.33333 * (camera.z + EveDef.WristToEnd_Z - EveDef.WristToCam_Z)); double rToCam = Geometry.XYtoR(xToCam, yToCam); double thetaToCam = Geometry.XYtoTheta(xToCam, yToCam); //these are the distance from the center of camera to the object in correctly oriented mm int dX = (int)Math.Round(Geometry.PolarToX(thetaToCam + camera.Eve.A, rToCam)); int dY = -(int)Math.Round(Geometry.PolarToY(thetaToCam + camera.Eve.A, rToCam)); //the real camera position x and y, plus the dX and dY to the object, minus the extra x and y to get to the end-effector from the camera x = camera.x + dX - (int)Math.Round(Geometry.PolarToX(camera.Eve.A + Math.PI / 2, EveDef.WristToCam_L)); y = camera.y + dY - (int)Math.Round(Geometry.PolarToY(camera.Eve.A + Math.PI / 2, EveDef.WristToCam_L)); y = (int)(1.11*y - 30); if(y > 250 && y < 500) { z = (int)-((y - 250) / 30); } else z = 0; TransformXYZtoEVE(); TransformXYZtoMARTY(); Eve.D = Math.PI / 2; //NEEDS MORE Marty.D = 0; }
private void QueuePointMarty(VPoint point) { //NEEDS CONTENT //Make sure to know if you are using radians or degrees //suggestion: commands2.Enqueue("C:" + Math.Round(point.Marty.C, 0).ToString()); commands2.Enqueue(VCommand.Wait); commands2.Enqueue("2"); commands2.Enqueue("B:" + Math.Round(point.Marty.B, 0).ToString()); commands2.Enqueue(VCommand.Wait); commands2.Enqueue("2"); commands2.Enqueue("A:" + Math.Round(point.Marty.A, 0).ToString()); commands2.Enqueue(VCommand.Wait); commands2.Enqueue("2"); }
private void QueuePointEve(VPoint point, Boolean down) { commands1.Enqueue("D" + Math.Round(Geometry.RtD(point.Eve.D), 2).ToString()); if(down) { commands1.Enqueue("B" + Math.Round(Geometry.RtD(point.Eve.B), 2).ToString()); commands1.Enqueue("C" + Math.Round(Geometry.RtD(point.Eve.C), 2).ToString()); } else { commands1.Enqueue("C" + Math.Round(Geometry.RtD(point.Eve.C), 2).ToString()); commands1.Enqueue("B" + Math.Round(Geometry.RtD(point.Eve.B), 2).ToString()); } commands1.Enqueue("A" + Math.Round(Geometry.RtD(point.Eve.A), 2).ToString()); }
public MainWindow() { InitializeComponent(); statusEve = Connection.Unknown; statusMarty = Connection.Unknown; statusCam = Connection.Unknown; _autoCommand = true; EvePort = new SerialPort(); MartyPort = new SerialPort(); EvePort.DataReceived += new SerialDataReceivedEventHandler(DataReceived); MartyPort.DataReceived += new SerialDataReceivedEventHandler(DataReceived); MartyPort.BaudRate = 115200; /* if(MessageBox.Show("Attempt communication with " + EveDef.name + "?", "Establishing Connection...", MessageBoxButton.YesNo, MessageBoxImage.Question) == MessageBoxResult.No) { statusEve = Connection.Disconnected; } if (MessageBox.Show("Attempt communication with " + MartyDef.name + "?", "Establishing Connection...", MessageBoxButton.YesNo, MessageBoxImage.Question) == MessageBoxResult.No) { statusMarty = Connection.Disconnected; } */ string evePortName = string.Empty; string martyPortName = string.Empty; while(statusEve == Connection.Unknown || statusMarty == Connection.Unknown) { string[] ports = SerialPort.GetPortNames(); cmbSelect1.ItemsSource = ports; switch (ports.Length) { case 1: StartEveComm(ports[0]); statusMarty = Connection.Disconnected; if (!ProbeEve()) { StartMartyComm(ports[0]); martyPortName = ports[0]; statusEve = Connection.Disconnected; } else { evePortName = ports[0]; } break; case 2: StartEveComm(ports[0]); if (ProbeEve()) { StartMartyComm(ports[1]); evePortName = ports[0]; martyPortName = ports[1]; } else { StartEveComm(ports[1]); StartMartyComm(ports[0]); evePortName = ports[1]; martyPortName = ports[0]; } break; default: if (MessageBox.Show("No robots found. Refresh?", "", MessageBoxButton.YesNo) == MessageBoxResult.No) { statusEve = Connection.Disconnected; statusMarty = Connection.Disconnected; } else { statusEve = Connection.Unknown; statusMarty = Connection.Unknown; } break; } } cmbSelect1.ItemsSource = SerialPort.GetPortNames(); cmbSelect2.ItemsSource = SerialPort.GetPortNames(); cmbSelect1.Text = evePortName; cmbSelect2.Text = martyPortName; lblStatus1.Content = statusEve.ToString(); lblStatus2.Content = statusMarty.ToString(); if (statusEve == Connection.Connected) _eveNext = true; else _eveNext = false; while(statusCam == Connection.Unknown) { try { _cap = new Capture(1); statusCam = Connection.Connected; } catch { if (MessageBox.Show("No camera found. Refresh?", "", MessageBoxButton.YesNo) == MessageBoxResult.No) { statusCam = Connection.Disconnected; _cap = new Capture(0); } } } PrepareTimers(); shapeDetector = new ShapeDetection(); eveCapturePoint = new VPoint(Geometry.DtoR(0), Geometry.DtoR(90), Geometry.DtoR(180), Geometry.DtoR(90)); eveCapturePoint.ID = new Bgr(64, 64, 64); eveCapturePoint.tag = "Camera capture position"; eveCapturePoint.immune = true; eveHomePoint = new VPoint(Geometry.DtoR(0), Geometry.DtoR(90), Geometry.DtoR(150), Geometry.DtoR(90)); eveHomePoint.ID = new Bgr(198, 64, 64); eveHomePoint.tag = "Home Position"; eveHomePoint.immune = true; //squarePoint = new VPoint((int)Geometry.PolarToX(Geometry.DtoR(150), 350), (int)Geometry.PolarToY(Geometry.DtoR(150), 350), 30); squarePoint = new VPoint(-260, 475, 50); squarePoint.ID = new Bgr(64, 64, 198); squarePoint.tag = "Destination for sorted squares"; squarePoint.immune = true; //trianglePoint = new VPoint((int)Geometry.PolarToX(Geometry.DtoR(30), 350), (int)Geometry.PolarToY(Geometry.DtoR(30), 350), 30); trianglePoint = new VPoint(-250, 500, 60); trianglePoint.ID = new Bgr(64, 198, 64); trianglePoint.tag = "Destination for sorted triangles"; trianglePoint.immune = true; testPoint = new VPoint(0, 400, 0); testPoint.ID = new Bgr(200, 100, 200); testPoint.tag = "Triangle"; testPoint2 = new VPoint(0, 400, 0); testPoint2.ID = new Bgr(100, 200, 200); testPoint2.tag = "Square"; points.Add(eveCapturePoint); points.Add(eveHomePoint); points.Add(squarePoint); points.Add(trianglePoint); //points.Add(testPoint); //points.Add(testPoint2); DisplayPoints(); }
private void ScanStop() { scanTimer.Stop(); SetProgress(0); _scanning = false; foreach(VPoint point in points) { if (point.immune == false) point.healthy = false; } foreach (VShape shape in shapeDetector.shapes) { int i = points.FindIndex(x => x.ID.ToString() == shape.ID.ToString()); if (i == -1) { points.Add(new VPoint(shape, eveCapturePoint)); } else { points[i] = new VPoint(shape, eveCapturePoint); } } List<VPoint> damnedPoints = new List<VPoint>(); foreach (VPoint point in points) { if (point.healthy == false) { damnedPoints.Add(point); continue; } } foreach (VPoint damnedPoint in damnedPoints) { try { if (damnedPoint.tag == "Triangle") EvePort.WriteLine("T"); else { EvePort.WriteLine("S"); } } catch { } finally { points.Remove(damnedPoint); } } if(shapeDetector.shapes.Count == 0) { commands1.Clear(); commands2.Clear(); DisplayCommands(); try { EvePort.WriteLine("M0"); } catch { } progTimer.Stop(); SetStatus("Done!"); MessageBox.Show("All shapes sorted"); } DisplayPoints(); }
private void SortAShape() { //FIX ME VPoint shape; if (_eveNext) { shape = points.Find(x => x.tag == "Square"); if (shape == null) { shape = points.Find(x => x.tag == "Triangle"); if (shape == null) { ProgramStart(); DisplayCommands(); return; } } } else { shape = points.Find(x => x.tag == "Triangle"); if (shape == null) { shape = points.Find(x => x.tag == "Square"); if (shape == null) { ProgramStart(); DisplayCommands(); return; } else { _eveNext = true; } } else { if (shape.invalid) _eveNext = true; } } if (_eveNext) { VPoint on_shape = new VPoint(shape.x, shape.y, shape.z); on_shape.tag = shape.tag; VPoint above_shape = new VPoint(shape.x, shape.y, 30); VPoint near_shape = new VPoint(shape.x, shape.y, 15); VPoint far_above_shape = new VPoint(shape.x, shape.y, 200); VPoint square_cap_a = new VPoint(squarePoint.Eve.A, far_above_shape.Eve.B, far_above_shape.Eve.C, far_above_shape.Eve.D); QueuePointEve(above_shape, false); commands1.Enqueue(VCommand.WaitForStop); commands1.Enqueue("2"); QueuePointEve(near_shape, false); commands1.Enqueue(VCommand.WaitForStop); commands1.Enqueue("1"); commands1.Enqueue("P1"); QueuePointEve(on_shape, false); commands1.Enqueue(VCommand.WaitForStop); commands1.Enqueue("1"); commands1.Enqueue(VCommand.Wait); commands1.Enqueue("0.5"); QueuePointEve(far_above_shape, false); commands1.Enqueue(VCommand.WaitForStop); commands1.Enqueue("2"); commands1.Enqueue(VCommand.Wait); commands1.Enqueue("0.5"); QueuePointEve(square_cap_a, false); commands1.Enqueue(VCommand.WaitForStop); commands1.Enqueue("2"); if (shape.tag == "Square") QueuePointEve(squarePoint, true); if (shape.tag == "Triangle") QueuePointEve(trianglePoint, true); commands1.Enqueue(VCommand.WaitForStop); commands1.Enqueue("2"); commands1.Enqueue(VCommand.Wait); commands1.Enqueue("0.5"); commands1.Enqueue("P0"); commands1.Enqueue(VCommand.Wait); commands1.Enqueue("1"); QueuePointEve(eveCapturePoint, false); commands1.Enqueue(VCommand.WaitForStop); commands1.Enqueue("2"); commands1.Enqueue(VCommand.Wait); commands1.Enqueue("0.5"); commands1.Enqueue(VCommand.DoAScan); commands1.Enqueue(VCommand.Wait); commands1.Enqueue("0.5"); DisplayPoints(); } else { //These four lines create a shape to process. //If you need to shift the position (xy) of the percieved shape, do it here VPoint on_shape = new VPoint(shape.x, shape.y); //ex. new VPoint(shape2.x - 30, shape2.y, 10) on_shape.tag = shape.tag; QueuePointMarty(on_shape); //Go to ahape commands2.Enqueue("M:1"); //Electromagnet on commands2.Enqueue(VCommand.Wait); commands2.Enqueue("2"); commands2.Enqueue("A:100"); //Lift commands2.Enqueue(VCommand.Wait); commands2.Enqueue("2"); commands2.Enqueue("C:0"); commands2.Enqueue(VCommand.Wait); commands2.Enqueue("2"); commands2.Enqueue("A:50"); commands2.Enqueue(VCommand.Wait); commands2.Enqueue("2"); commands2.Enqueue("B:120"); commands2.Enqueue(VCommand.Wait); commands2.Enqueue("2"); commands2.Enqueue("M:0"); //Electromagnet off commands2.Enqueue(VCommand.Wait); commands2.Enqueue("2"); commands2.Enqueue("A:142"); //Pounce position commands2.Enqueue(VCommand.Wait); commands2.Enqueue("2"); commands2.Enqueue("B:20"); commands2.Enqueue(VCommand.Wait); commands2.Enqueue("2"); commands2.Enqueue("C:90"); commands2.Enqueue(VCommand.Wait); commands2.Enqueue("2"); commands2.Enqueue(VCommand.DoAScan); DisplayCommands(); DisplayPoints(); } if (statusEve == Connection.Connected && statusMarty == Connection.Disconnected) _eveNext = true; else if (statusEve == Connection.Disconnected && statusMarty == Connection.Connected) _eveNext = false; else _eveNext = !_eveNext; }