diff --git a/TA.NexDome.Rotator/CommandProcessor.cpp b/TA.NexDome.Rotator/CommandProcessor.cpp index 8e6adb7..cde6d24 100644 --- a/TA.NexDome.Rotator/CommandProcessor.cpp +++ b/TA.NexDome.Rotator/CommandProcessor.cpp @@ -105,6 +105,7 @@ Response CommandProcessor::HandleCommand(Command& command) const if (command.Verb == "FR") return HandleFR(command); // Read firmware version if (command.Verb == "GA") return HandleGA(command); // Goto Azimuth (rotator only) if (command.Verb == "GH") return HandleGH(command); // Goto Home Sensor (rotator only) + if (command.Verb == "GS") return HandleGS(command); // Goto step position (rotator only) if (command.Verb == "HR") return HandleHR(command); // Home position Read (rotator only) if (command.Verb == "HW") return HandleHW(command); // Home position Write (rotator only) if (command.Verb == "SW") return HandleSW(command); // Stop motor (emergency stop) @@ -134,19 +135,30 @@ Response CommandProcessor::HandleAR(Command& command) const } +void CommandProcessor::rotateToMicrostepPosition(const int32_t target) const { +const auto currentPosition = rotator.getCurrentPosition(); +const auto delta = target - currentPosition; +const auto direction = sgn(delta); +if (abs(delta) >= settings.deadZone) + { + HomeSensor::cancelHoming(); + sendDirection(direction); + rotator.moveToPosition(target); + } +} + Response CommandProcessor::HandleGA(Command& command) const { const auto microstepsPerDegree = settings.home.microstepsPerRotation / 360.0; const auto target = targetStepPosition(command.StepPosition * microstepsPerDegree); - const auto currentPosition = rotator.getCurrentPosition(); - const auto delta = target - currentPosition; - const auto direction = sgn(delta); - if (abs(delta) >= settings.deadZone) - { - HomeSensor::cancelHoming(); - sendDirection(direction); - rotator.moveToPosition(target); - } + rotateToMicrostepPosition(target); + return Response::FromSuccessfulCommand(command); + } + +Response CommandProcessor::HandleGS(Command& command) const + { + const auto target = targetStepPosition(stepsToMicrosteps(command.StepPosition)); + rotateToMicrostepPosition(target); return Response::FromSuccessfulCommand(command); } diff --git a/TA.NexDome.Rotator/CommandProcessor.h b/TA.NexDome.Rotator/CommandProcessor.h index d526d6b..667fb06 100644 --- a/TA.NexDome.Rotator/CommandProcessor.h +++ b/TA.NexDome.Rotator/CommandProcessor.h @@ -39,6 +39,8 @@ class CommandProcessor Response HandleAW(Command& command) const; // AW - Acceleration ramp time write Response HandleFR(Command& command) const; // Firmware version read Response HandleGA(Command& command) const; // GA - GoTo Azimuth (in degrees). + void rotateToMicrostepPosition(int32_t target) const; + Response HandleGS(Command& command) const; // GS - GoTo Step Position Response HandleGH(Command& command) const; // Go to home sensor Response HandleHR(Command& command) const; // Read Home position (steps clockwise from true north) Response HandleHW(Command& command) const; // Write home position (steps clockwise from true north)