joopschilder/philips-cnc6600-interpreter
joopschilder/philips-cnc6600-interpreter
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

atan correction fixed

Browse Source
This commit is contained in:
Joop Schilder 2020-05-14 01:06:27 +02:00
parent 7831a23aa5
commit 334b449ca3
1 changed files with 35 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
bin/simulate
View File

@ -53,6 +53,7 @@ while ($nc->PPMC->ready()) {
usleep($interval); usleep($interval);
} }
break; break;
case 2: case 2:
case 3: case 3:
// Circular interpolation // Circular interpolation
@ -70,56 +71,55 @@ while ($nc->PPMC->ready()) {
// Calculate starting and ending angles [0 - 2pi] // Calculate starting and ending angles [0 - 2pi]
$angleStart = atan2($posStart->y - $posCenter->y, $posStart->x - $posCenter->x); $angleStart = atan2($posStart->y - $posCenter->y, $posStart->x - $posCenter->x);
// while ($angleStart < 0) { if ($posStart->y - $posCenter->y < 0) {
// $angleStart += TWO_PI; $angleStart += TWO_PI;
// } }
$angleEnd = atan2($posEnd->y - $posCenter->y, $posEnd->x - $posCenter->x); $angleEnd = atan2($posEnd->y - $posCenter->y, $posEnd->x - $posCenter->x);
// while ($angleEnd < 0) { if ($posEnd->y - $posCenter->y < 0) {
// $angleEnd += TWO_PI; $angleEnd += TWO_PI;
// } }
// segment length = 2 * pi * r * alpha / (2*pi) = r * alpha // segment length = 2 * pi * r * alpha / (2*pi) = r * alpha
$segmentLength = $r * abs($angleStart - $angleEnd); $segmentLength = $r * abs($angleStart - $angleEnd);
$duration = $segmentLength / $feedPerSecond; $duration = $segmentLength / $feedPerSecond;
$nSteps = abs($angleStart - $angleEnd) / 0.001; $nSteps = abs($angleStart - $angleEnd) / 0.01;
if ($nSteps === 0) { if ($nSteps == 0) {
break; break;
} }
$interval = 1_000_000 * $duration / $nSteps; $interval = 1_000_000 * $duration / $nSteps;
$delta = $afterStep->G_TRAVERSE === 2 ? 0.001 : -0.001; // CW[2] vs CCW[3] $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;
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)); $pos = new Vector2D($posCenter->x + $r * cos($a), $posCenter->y + $r * sin($a));
p($pos); p($pos);
print("Alpha: $a\n"); printf("θ=%1.2fπ\n", $a / pi());
usleep($interval); usleep($interval);
$i++; // $i++;
} }
dump(['took_steps' => $i]); // dump(['took_steps' => $i]);
readline(); // readline();
break; break;
default: default:
break; break;