diff --git a/turret.cpp b/turret.cpp index bf4c705..665833d 100644 --- a/turret.cpp +++ b/turret.cpp @@ -1,5 +1,6 @@ #include "turret.h" #include "Arduino.h" +#include "math.h" #include "maths.h" #include @@ -138,9 +139,19 @@ Turret &Turret::moveTo(double x, double y, double z, Unit unit) { angleToStep(_step_ratio, vec2(degToRad(x), degToRad(y)), step); } - // TODO min(valueI, -_homeI); - _stepper.x.prepareMove(_zero.x + step.x); - _stepper.y.prepareMove(_zero.y + step.y); + long maxDeltaStepX = _step_ratio.x * M_PI_2; + long maxDeltaStepY = _step_ratio.y * M_PI_2; + + // force move from -90° to 90° avoiding backside, + // if move > 180° skip the full turn a start at -90°, + // same in the other direction + long stepX = constrain(step.x % maxDeltaStepX, -(maxDeltaStepX >> 2), + (maxDeltaStepX >> 2)); + long stepY = constrain(step.y % maxDeltaStepY, -(maxDeltaStepY >> 2), + (maxDeltaStepY >> 2)); + + _stepper.x.prepareMove(_zero.x + stepX); + _stepper.y.prepareMove(_zero.y + stepY); bool xStop = false; bool yStop = false;