philips-cnc6600-interpreter/bin/simulate

204 lines
4.9 KiB
Plaintext
Executable File
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#!/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);
// 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();
}
}