diff --git a/bin/simulate b/bin/simulate index 3dd61f4..acdbc8f 100755 --- a/bin/simulate +++ b/bin/simulate @@ -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;