Implemented M67

bin/simulate

@@ -1,203 +0,0 @@
-#!/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');
-//		print("\n$block\n\n");
+		system('clear');
+		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,6 +42,8 @@ class InstructionSet
 				print("WARNING: No method or register for word '{$word}'\n");
 			}
 		}
+		dump($this->CNC6600->DR);
+		readline();
 	}
 
 
@@ -324,6 +326,9 @@ class InstructionSet
 
 	protected function M67(Block $block): void
 	{
-		throw new RuntimeException('[Enter new tool information without machine stop] is not implemented');
+		$T = $block->pop('T');
+		$this->CNC6600->TM->read($T);
+		$this->CNC6600->DR->T = $T;
 	}
+
 }