atan correction fixed

bin/simulate

@@ -0,0 +1,203 @@
+#!/usr/bin/env php7.4
+<?php
+
+/*
+ * ▓▒░
+ * »«º×■·
+ * ╔ ╚ ╝ ╗ ║ ═
+ */
+
+use Machine\CNC6600;
+
+require_once __DIR__ . '/../vendor/autoload.php';
+
+const W = 164;
+const H = 60;
+const TWO_PI = 6.28318530717958647692528676655900576;
+
+/** @var CNC6600 $nc */
+$nc = require __DIR__ . '/bootstrap.php';
+$nc->loadProgram(9002);
+
+while ($nc->PPMC->ready()) {
+	$beforeStep = clone $nc->DR;
+	$nc->step();
+	$afterStep = clone $nc->DR;
+	$afterStep->F = 10000.0;
+
+	$feedPerSecond = ($afterStep->F / 60.0) * 100.0;
+	switch ($afterStep->G_TRAVERSE) {
+		case 0:
+		case 1:
+			$posStart = new Vector2D($beforeStep->X, $beforeStep->Z);
+			$posEnd = new Vector2D($afterStep->X, $afterStep->Z);
+			$toolPath = $posEnd->diff($posStart);
+			$segmentLength = $toolPath->length();
+			if ($segmentLength < 2) {
+				break;
+			}
+			$duration = $segmentLength / $feedPerSecond;
+
+			$stepSize = 100.0; // 0.1 mm
+			$nSteps = $segmentLength / $stepSize;
+			$interval = 1_000_000 * $duration / $nSteps;
+
+			$xDir = $posEnd->x < $posStart->x ? -1.0 : 1.0;
+			$zDir = $posEnd->y < $posStart->y ? -1.0 : 1.0;
+			for ($i = 0; $i < $nSteps; $i++) {
+				$fraction = $i / $nSteps;
+				$x = $posStart->x + $fraction * $toolPath->x * $xDir;
+				$y = $posStart->y + $fraction * $toolPath->y * $zDir;
+				$vector = new Vector2D($x, $y);
+				p($vector);
+				usleep($interval);
+			}
+			break;
+
+		case 2:
+		case 3:
+			// Circular interpolation
+			$posCenter = new Vector2D($afterStep->I, $afterStep->K);
+			$posStart = new Vector2D($beforeStep->X, $beforeStep->Z);
+			$posEnd = new Vector2D($afterStep->X, $afterStep->Z);
+
+			// Calculate radius
+			$radiusStart = $posStart->diff($posCenter)->length();
+			$radiusEnd = $posEnd->diff($posCenter)->length();
+			if (abs($radiusStart - $radiusEnd) > 2) {
+				die("ERROR PERFORMING CIRCULAR INTERPOLATION: INCONSISTENT RADIUS\n");
+			}
+			$r = $radiusStart;
+
+			// Calculate starting and ending angles [0 - 2pi]
+			$angleStart = atan2($posStart->y - $posCenter->y, $posStart->x - $posCenter->x);
+			if ($posStart->y - $posCenter->y < 0) {
+				$angleStart += TWO_PI;
+			}
+
+			$angleEnd = atan2($posEnd->y - $posCenter->y, $posEnd->x - $posCenter->x);
+			if ($posEnd->y - $posCenter->y < 0) {
+				$angleEnd += TWO_PI;
+			}
+
+			// segment length = 2 * pi * r * alpha / (2*pi) = r * alpha
+			$segmentLength = $r * abs($angleStart - $angleEnd);
+
+			$duration = $segmentLength / $feedPerSecond;
+			$nSteps = abs($angleStart - $angleEnd) / 0.01;
+			if ($nSteps == 0) {
+				break;
+			}
+
+			$interval = 1_000_000 * $duration / $nSteps;
+			$delta = $afterStep->G_TRAVERSE === 2 ? 0.01 : -0.01; // CW[2] vs CCW[3]
+
+//			dump([
+//				"G{$afterStep->G_TRAVERSE}",
+//				'from'     => $posStart,
+//				'to'       => $posEnd,
+//				'center'   => $posCenter,
+//				'r'        => $r,
+//				'a_from'   => ($angleStart / pi()) . 'π',
+//				'a_end'    => ($angleEnd / pi()) . 'π',
+//				'length'   => $segmentLength,
+//				'steps'    => $nSteps,
+//				'duration' => $duration,
+//				'interval' => $interval,
+//				'delta'    => $delta,
+//			]);
+//			readline();
+//			$i = 0;
+			for ($a = $angleStart; abs($a - $angleEnd) > 0.01; $a += $delta) {
+				$a += $a < 0 ? TWO_PI : 0;
+				$a -= $a > TWO_PI ? TWO_PI : 0;
+
+				$pos = new Vector2D($posCenter->x + $r * cos($a), $posCenter->y + $r * sin($a));
+				p($pos);
+				printf("θ=%1.2fπ\n", $a / pi());
+				usleep($interval);
+//				$i++;
+			}
+//			dump(['took_steps' => $i]);
+//			readline();
+			break;
+		default:
+			break;
+	}
+	p(new Vector2D($afterStep->X, $afterStep->Z));
+}
+
+function p(Vector2D $v): void
+{
+	static $previous = null;
+	$x = (int)map($v->x, 350 * W, -350 * W, 0, W);
+	$z = (int)map($v->y, 700 * H, -750 * H, 0, H);
+
+	$previous ??= new Vector2D($x - 1, $z);
+
+	if ($previous->x === $x && $previous->y === $z) {
+		return;
+	}
+	$previous->x = $x;
+	$previous->y = $z;
+	system('clear');
+	print('╔' . str_repeat('═', W) . '╗' . PHP_EOL);    // Top
+	print(str_repeat("║" . str_repeat(' ', W) . "║\n", $z - 1));
+	print('║' . str_repeat(' ', $x - 1) . '×' . str_repeat(' ', W - $x) . "║\n");
+	print(str_repeat("║" . str_repeat(' ', W) . "║\n", H - $z));
+	print('╚' . str_repeat('═', W) . '╝' . PHP_EOL); // Bottom
+}
+
+function map(int $x, int $in_min, int $in_max, int $out_min, int $out_max): int
+{
+	return ($x - $in_min) * ($out_max - $out_min) / ($in_max - $in_min) + $out_min;
+}
+
+class Vector2D
+{
+	public float $x;
+	public float $y;
+
+
+	public function __construct(float $x, float $y)
+	{
+		$this->x = $x;
+		$this->y = $y;
+	}
+
+
+	public function length(): float
+	{
+		return sqrt($this->x ** 2 + $this->y ** 2);
+	}
+
+
+	public function diff(self $other): self
+	{
+		return new self(
+			abs($this->x - $other->x),
+			abs($this->y - $other->y)
+		);
+	}
+}
+
+class Arc2D
+{
+	public Vector2D $start;
+	public Vector2D $end;
+	public Vector2D $centre;
+
+
+	public function __construct(Vector2D $start, Vector2D $end, Vector2D $centre)
+	{
+		$this->start = $start;
+		$this->end = $end;
+		$this->centre = $centre;
+	}
+
+
+	public function radius(): float
+	{
+		return $this->start->diff($this->centre)->length();
+	}
+}

src/Machine/InstructionSet.php

@@ -21,8 +21,8 @@ class InstructionSet
 
 	public function apply(Block $block): void
 	{
-		system('clear');
+//		system('clear');
-		print("\n$block\n\n");
+//		print("\n$block\n\n");
 		foreach ($block->words as $word) {
 			if (method_exists($this, "$word")) {
 				// There's a method dedicated to handling the full word (eg G22)
@@ -42,8 +42,6 @@ class InstructionSet
 				print("WARNING: No method or register for word '{$word}'\n");
 			}
 		}
-		dump($this->CNC6600->DR);
-		readline();
 	}