public ArcInterpolation(Point start, Point end, float radius, bool clockwise)
{
// PhlatScript uses the radius format, so we need to support it.
Start = start;
End = end;
Radius = radius;
Clockwise = !clockwise;
// Calculate center. Best explanation found here:
// http://mathforum.org/library/drmath/view/53027.html
float xs = Start.X;
float ys = Start.Y;
float xe = End.X;
float ye = End.Y;
float r = Radius;
// Distance between start and end
float q = (float)System.Math.Sqrt((xe - xs) * (xe - xs) + (ye - ys) * (ye - ys));
// middle ploint between both points
float xc = (xs + xe) / 2;
float yc = (ys + ye) / 2;
if (!Clockwise)
{
Center = new Point(
(float)(xc - System.Math.Sqrt(r * r - (q / 2) * (q / 2)) * (ys - ye) / q),
(float)(yc - System.Math.Sqrt(r * r - (q / 2) * (q / 2)) * (xe - xs) / q)
);
}
else
{
Center = new Point(
(float)(xc + System.Math.Sqrt(r * r - (q / 2) * (q / 2)) * (ys - ye) / q),
(float)(yc + System.Math.Sqrt(r * r - (q / 2) * (q / 2)) * (xe - xs) / q)
);
}
const string E_NO_ARC_CENTER = "Could not find a suitable center for arc";
if (Center.X == float.MinValue)
{
GCodeMachine.Error(E_NO_ARC_CENTER);
}
if (Center.Y == float.MinValue)
{
GCodeMachine.Error(E_NO_ARC_CENTER);
}
Initialize();
}
public static void Main()
{
var device = new XYZGantryDevice();
device.SetStepX((steps) => { });
device.SetStepY((steps) => { });
device.SetStepZ((steps) => { });
device.SetStopX(() => { });
device.SetStopX(() => { });
device.SetStopX(() => { });
var machine = new GCodeMachine(device);
var parser = new GCodeParser(machine);
//parser.ParseLine("G01 X1 Y1");
parser.ParseLine("G02 X1 Y0 I1 J0");
}
private static void InitializeGCodeParser()
{
settings = new Settings();
settings.OneStepZ_Angle_Positive = 30;
settings.OneStepZ_Angle_Negative = 15;
var servo = new Servo(
PWMChannels.PWM_PIN_D10,
new ServoConfig(0, 180, 500, 2500, 50));
servo.RotateTo(15);
Thread.Sleep(500);
servo.RotateTo(35);
var motorShield = new AdafruitMotorShield();
IDeviceDelegateOneStep oneStepX = PrepareOneStepDelegate(motorShield, 1, OperationMode.FullStep);
IDeviceDelegateOneStep oneStepY = PrepareOneStepDelegate(motorShield, 2, OperationMode.FullStep);
IDeviceDelegateOneStep oneStepZ = (steps) =>
{
var a = servo.Angle;
if (steps >= 0)
{
servo.RotateTo(settings.OneStepZ_Angle_Negative);
}
else
{
servo.RotateTo(settings.OneStepZ_Angle_Positive);
}
if (a != servo.Angle)
{
Thread.Sleep(500);
}
};
IDeviceDelegateStop startX = () => { };
IDeviceDelegateStop startY = () => { };
IDeviceDelegateStop startZ = () => {
servo.RotateTo(settings.OneStepZ_Angle_Negative);
Thread.Sleep(500);
servo.RotateTo(settings.OneStepZ_Angle_Positive);
Thread.Sleep(500);
servo.RotateTo(settings.OneStepZ_Angle_Negative);
Thread.Sleep(500);
servo.RotateTo(settings.OneStepZ_Angle_Positive);
};
IDeviceDelegateStop stopX = () => { motorShield.GetStepper(2).ReleaseHoldingTorque(); };
IDeviceDelegateStop stopY = () => { motorShield.GetStepper(1).ReleaseHoldingTorque(); };
IDeviceDelegateStop stopZ = () => { servo.RotateTo(settings.OneStepZ_Angle_Negative); };
var device = new XYZGantryDevice();
device.SetStepX(oneStepX);
device.SetStepY(oneStepY);
device.SetStepZ(oneStepZ);
device.SetStartX(startX);
device.SetStartY(startY);
device.SetStartZ(startZ);
device.SetStopX(stopX);
device.SetStopY(stopY);
device.SetStopZ(stopZ);
device.Calibrate(25f, 25f, 1);
var machine = new GCodeMachine(device);
parser = new GCodeParser(machine);
}
