atan correction fixed
This commit is contained in:
parent
7831a23aa5
commit
334b449ca3
70
bin/simulate
70
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;
|
||||
|
Loading…
Reference in New Issue
Block a user