WIP circular interpolation
This commit is contained in:
parent
ff7f9d148f
commit
7831a23aa5
|
|
@ -1,113 +0,0 @@
|
|||
<?php
|
||||
|
||||
/*
|
||||
* ▓▒░
|
||||
* »«º×■·
|
||||
* ╔ ╚ ╝ ╗ ║ ═
|
||||
*/
|
||||
|
||||
use Automata\NC;
|
||||
use Automata\ToolData;
|
||||
|
||||
require_once __DIR__ . '/../vendor/autoload.php';
|
||||
|
||||
const W = 128;
|
||||
const H = 96;
|
||||
|
||||
$nc = new NC();
|
||||
$nc->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();
|
||||
}
|
||||
}
|
||||
|
|
@ -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);
|
||||
// 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();
|
||||
}
|
||||
}
|
||||
|
|
@ -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();
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue