From 7831a23aa54ba7681c8c967fb12c6bfd85941b41 Mon Sep 17 00:00:00 2001 From: Joop Schilder Date: Thu, 14 May 2020 00:19:39 +0200 Subject: [PATCH] WIP circular interpolation --- bin/console_sim.php | 113 ------------------ bin/simulate | 203 +++++++++++++++++++++++++++++++++ src/Machine/InstructionSet.php | 6 +- 3 files changed, 205 insertions(+), 117 deletions(-) delete mode 100644 bin/console_sim.php create mode 100755 bin/simulate diff --git a/bin/console_sim.php b/bin/console_sim.php deleted file mode 100644 index 91a1f2e..0000000 --- a/bin/console_sim.php +++ /dev/null @@ -1,113 +0,0 @@ -TM->write(new ToolData(1, 5500, 2)); -$nc->TM->write(new ToolData(2, 3000, 61379)); -(new ProgramLoader)->loadPrograms($nc); - -$nc->loadProgram(9002); - -$xBuffer = []; -$zBuffer = []; -while ($nc->PPMC->ready()) { - $startingPosition = new Vector2D($nc->DR->X, $nc->DR->Z); - $centre = new Vector2D($nc->DR->I, $nc->DR->K); - $nc->step(); - $endingPosition = new Vector2D($nc->DR->X, $nc->DR->Z); - switch ($nc->DR->G_TRAVERSE) { - case 2: - case 3: - // Circular - $arc = new Arc2D($startingPosition, $endingPosition, $centre); - dump($arc); - readline(); - default: - - } - - $diff = $endingPosition->diff($startingPosition); - - readline(); - p($endingPosition); - usleep(100000); -} - -function p(Vector2D $v): void -{ - system('clear'); - $x = map($v->x, -1000 * W, 1000 * W, 0, W); - $z = map($v->y, -1000 * H, 1000 * H, 0, H); - print('╔' . str_repeat('═', W) . '╗' . PHP_EOL); // Top - print(str_repeat("║" . str_repeat(' ', W) . "║\n", $z - 1)); - print('║' . str_repeat(' ', $x - 1) . 'X' . 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( - sqrt(($this->x - $other->x) ** 2), - sqrt(($this->y - $other->y) ** 2) - ); - } -} - -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(); - } -} diff --git a/bin/simulate b/bin/simulate new file mode 100755 index 0000000..3dd61f4 --- /dev/null +++ b/bin/simulate @@ -0,0 +1,203 @@ +#!/usr/bin/env php7.4 +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); +// while ($angleStart < 0) { +// $angleStart += TWO_PI; +// } + $angleEnd = atan2($posEnd->y - $posCenter->y, $posEnd->x - $posCenter->x); +// while ($angleEnd < 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.001; + if ($nSteps === 0) { + break; + } + + $interval = 1_000_000 * $duration / $nSteps; + $delta = $afterStep->G_TRAVERSE === 2 ? 0.001 : -0.001; // 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.001; $a += $delta) { + if ($a > TWO_PI) { + $a -= TWO_PI; + } else if ($a < -TWO_PI) { + $a += TWO_PI; + } + $pos = new Vector2D($posCenter->x + $r * cos($a), $posCenter->y + $r * sin($a)); + p($pos); + print("Alpha: $a\n"); + 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(); + } +} diff --git a/src/Machine/InstructionSet.php b/src/Machine/InstructionSet.php index 71fa012..bc226ce 100644 --- a/src/Machine/InstructionSet.php +++ b/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,8 +42,6 @@ class InstructionSet print("WARNING: No method or register for word '{$word}'\n"); } } - dump($this->CNC6600->DR); - readline(); }