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

atan correction fixed

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);
}
break;
case 2:
case 3:
// Circular interpolation
@ -70,56 +71,55 @@ while ($nc->PPMC->ready()) {
// Calculate starting and ending angles [0 - 2pi]
$angleStart = atan2($posStart->y - $posCenter->y, $posStart->x - $posCenter->x);
// while ($angleStart < 0) {
// $angleStart += TWO_PI;
// }
if ($posStart->y - $posCenter->y < 0) {
$angleStart += TWO_PI;
}
$angleEnd = atan2($posEnd->y - $posCenter->y, $posEnd->x - $posCenter->x);
// while ($angleEnd < 0) {
// $angleEnd += TWO_PI;
// }
if ($posEnd->y - $posCenter->y < 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) {
$nSteps = abs($angleStart - $angleEnd) / 0.01;
if ($nSteps == 0) {
break;
}
$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));
p($pos);
print("Alpha: $a\n");
printf("θ=%1.2fπ\n", $a / pi());
usleep($interval);
$i++;
// $i++;
}
dump(['took_steps' => $i]);
readline();
// dump(['took_steps' => $i]);
// readline();
break;
default:
break;