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