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();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
203
bin/simulate
Executable file
203
bin/simulate
Executable file
@ -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
|
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) {
|
foreach ($block->words as $word) {
|
||||||
if (method_exists($this, "$word")) {
|
if (method_exists($this, "$word")) {
|
||||||
// There's a method dedicated to handling the full word (eg G22)
|
// 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");
|
print("WARNING: No method or register for word '{$word}'\n");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
dump($this->CNC6600->DR);
|
|
||||||
readline();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user