doc: improve config.h constants description

This commit close this branch with WIP implementation of a stack based
task pool system to handle long running action (motor move) of
the kissStepper lib (that need a long `while`). The goal was to
allow handling of incomming serrial messages and motion cancellation
during stepper control loop.

.gitignore

@@ -0,0 +1 @@
+.clangd

README.md

@@ -3,9 +3,12 @@
 Code de la tourelle de pointage laser du projet bouchons EDF.
 
 > [!WARNING]
-> Code de debuggage uniquement.
+> Code de débogage uniquement.
 
-# Usage
+> [!IMPORTANT]
+> Nouvelle mécanique, les axes H/x et V/y sont découplés.
+
+## Usage
 
 Compile through Arduino IDE or equivalent.
 
@@ -13,6 +16,51 @@ Compile through Arduino IDE or equivalent.
 > Add [kissStepper](https://github.com/risitt/kissStepper) to your project
 > librairies.
 
+## Conventions
+
+- **x** is the horizontal axis centered at 0 and going from left (min value) to
+  right (max value).
+- **y** is the vertical axis centered at 0 and going from bottom (min value) to
+  top (max value).
+- **z** is the distance between the turret and the target.
+
+```mermaid
+xychart-beta
+  title "Turret coordinate system"
+  x-axis "X axis [m]" -3 --> 3 
+  y-axis "Y axis [m]" -3 --> 3
+  bar [-4]
+```
+
+## Development
+
+Required dependencies if not using
+[Arduino IDE](https://www.arduino.cc/en/software/):
+
+- [Arduino CLI](https://docs.arduino.cc/arduino-cli/):
+  - Install:
+    ```sh
+    # linux
+    curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | sh
+
+    # macos
+    brew update
+    brew install arduino-cli
+
+    # windows
+    winget install --id=ArduinoSA.CLI  -e
+    ```
+  - Configure:
+    ```sh
+    arduino-cli config init
+    arduino-cli core update-index
+    arduino-cli core install arduino:avr
+    ```
+
+If you use `clangd` as lsp run `python ./generate_clangd.py` to load arduino
+config and libraries.
+
 ## Contributing
 
-Before `git commit`, run `git clang-format` to format stagged files.
+Before `git commit`, run `git clang-format --staged` to format staged files and
+then `git add` to commit formatting.

config.h

@@ -1,6 +1,8 @@
 #ifndef CONFIG_H
 #define CONFIG_H
 
+#include "math.h"
+
 #define BAUDRATE (9600)
 
 // Pins
@@ -14,16 +16,33 @@
 #define PIN_PD2 (2)
 #define PIN_MOSI (11)
 
-#define PIN_X_PULSE (PIN_PB0)
+#define PIN_Y_PULSE (PIN_PB0)
-#define PIN_X_ENABLE (PIN_PD6)
+#define PIN_Y_ENABLE (PIN_PD6)
-#define PIN_X_DIRECTION (PIN_PD7)
+#define PIN_Y_DIRECTION (PIN_PD7)
-#define PIN_X_HOME (PIN_PD3)
+#define PIN_Y_HOME (PIN_PD3)
 
-#define PIN_Y_PULSE (PIN_PB2)
+#define PIN_X_PULSE (PIN_PB2)
-#define PIN_Y_ENABLE (PIN_PB1)
+#define PIN_X_ENABLE (PIN_PB1)
-#define PIN_Y_DIRECTION (PIN_MOSI)
+#define PIN_X_DIRECTION (PIN_MOSI)
-#define PIN_Y_HOME (PIN_PD4)
+#define PIN_X_HOME (PIN_PD4)
 
 #define PIN_LASER (PIN_PD2)
 
+// Stepper ratio [step/rad]
+#define STEP_RATIO_X (6432 / M_PI)
+#define STEP_RATIO_Y (6432 / M_PI)
+
+// Offset between home and zero [step]
+#define ZERO_OFFSET_X (0)
+#define ZERO_OFFSET_Y (2500)
+
+// Offset between turret and lidar [cm]
+// Distance between the lidar optical center and the vertical laser (should be 0
+// unless mechanical constraints)
+#define OFFSET_X (0)
+// Distance between the lidar optical center and the horizontal laser
+#define OFFSET_Y (21)
+// Distance between the surface and the turret motors axis
+#define OFFSET_Z (166)
+
 #endif

generate_clangd.py

@@ -0,0 +1,48 @@
+#!/usr/bin/env python3
+
+import os
+import platform
+import yaml
+
+def write_clangd_file(flags):
+    clangd_config = {
+        "CompileFlags": {
+            "Add": flags
+        }
+    }
+
+    with open(".clangd", "w") as f:
+        yaml.dump(clangd_config, f, default_flow_style=False)
+
+def main():
+    arduino_base_path = os.path.expanduser("~/AppData/Local") if platform.system() == "Windows" else os.path.expanduser('~')
+    
+    flags = [
+        "-x", "c++",
+        "-std=gnu++11",
+        "-fpermissive",
+        "-fno-exceptions",
+        "-ffunction-sections",
+        "-fdata-sections",
+        "-fno-threadsafe-statics",
+        "-Wno-error=narrowing",
+        "-flto",
+        "-E",
+        "-CC",
+        "-D__AVR__",
+        "-D__AVR_ATmega328P__",
+        "-DF_CPU=16000000L",
+        "-DARDUINO=10607",
+        "-DARDUINO_AVR_UNO",
+        "-DARDUINO_ARCH_AVR",
+        "-I./include",
+        f"-I{arduino_base_path}/Arduino15/packages/arduino/hardware/avr/1.8.6/cores/arduino",
+        f"-I{arduino_base_path}/Arduino15/packages/arduino/hardware/avr/1.8.6/variants/standard",
+        f"-I{arduino_base_path}/Arduino15/packages/arduino/tools/avr-gcc/7.3.0-atmel3.6.1-arduino7/avr/include",
+        f"-I{arduino_base_path}/Arduino15/packages/arduino/tools/avr-gcc/7.3.0-atmel3.6.1-arduino7/lib/gcc/avr/7.3.0/include",
+    ]
+    
+    write_clangd_file(flags)
+
+if __name__ == "__main__":
+    main()

include/kissStepper.h

@@ -0,0 +1,312 @@
+/*
+kissStepper - a lightweight library for the Easy Driver, Big Easy Driver, Allegro stepper motor drivers and others that use a Step/Dir interface
+Written by Rylee Isitt. September 21, 2015
+License: GNU Lesser General Public License (LGPL) V2.1
+
+Despite the existence of several excellent libraries for driving stepper motors, I created this one to fulfill the following needs:
+- Simplicity
+- Handling of enable, step, and dir pins
+- Based around an external loop
+- Approximately linear acceleration using a fast algorithm
+- High step frequency (or reasonably so, given the overhead involved)
+- Use AVR/ARM libraries and port access to increase performance while keeping the API Arduino-friendly
+- Teensy (Teensyduino) compatibility
+
+Acceleration approximation math is based on Aryeh Eiderman's "Real Time Stepper Motor Linear Ramping Just by Addition and Multiplication", available at http://hwml.com/LeibRamp.pdf
+*/
+
+#ifndef kissStepper_H
+#define kissStepper_H
+
+#include <Arduino.h>
+
+// determine port register size
+#if defined(__AVR__) || defined(__avr__)
+	typedef uint8_t regint;
+#elif defined(TEENSYDUINO)
+	#if defined(__AVR_ATmega32U4__) || defined(__AVR_AT90USB1286__) || defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MKL26Z64__) || defined(__MK64FX512__) || defined(__MK66FX1M0__)
+		typedef uint8_t regint;
+	#else
+		typedef uint32_t regint;
+	#endif
+#else
+	typedef uint32_t regint;
+#endif
+
+
+// the order of enums allows some simple tests:
+// if > STATE_STARTING, motor is in motion
+// if > STATE_RUN, motor is accelerating or decelerating
+enum kissState_t: uint8_t
+{
+    STATE_STOPPED = 0,
+    STATE_STARTING = 1,
+    STATE_RUN = 2,
+    STATE_ACCEL = 3,
+    STATE_DECEL = 4
+};
+
+// ----------------------------------------------------------------------------------------------------
+// ----------------------------------------------------------------------------------------------------
+// ----------------------------------------------------------------------------------------------------
+// kissStepper without acceleration
+// ----------------------------------------------------------------------------------------------------
+// ----------------------------------------------------------------------------------------------------
+// ----------------------------------------------------------------------------------------------------
+
+class kissStepperNoAccel
+{
+public:
+    kissStepperNoAccel(uint8_t PIN_DIR, uint8_t PIN_STEP, uint8_t PIN_ENABLE = 255, bool invertDir = false);
+    kissStepperNoAccel(uint8_t PIN_DIR, uint8_t PIN_STEP, bool invertDir = false);
+    ~kissStepperNoAccel(void) {};
+
+    bool prepareMove(int32_t target);
+    kissState_t move(void);
+    void stop(void);
+
+    uint16_t getCurSpeed(void)
+    {
+        if (m_kissState == STATE_RUN)
+            return m_maxSpeed;
+        else
+            return 0;
+    }
+    kissState_t getState(void)
+    {
+        return m_kissState;
+    }
+    int32_t getPos(void)
+    {
+        if (m_forwards)
+            return m_pos + m_distMoved;
+        else
+            return m_pos - m_distMoved;
+    }
+    bool isEnabled(void)
+    {
+        return m_enabled;
+    }
+    bool isMovingForwards(void)
+    {
+        return m_forwards;
+    }
+    void begin(void);
+    void enable(void);
+    void disable(void);
+
+    void setPos(int32_t pos)
+    {
+        if (m_kissState == STATE_STOPPED)
+            m_pos = constrain(pos, m_reverseLimit, m_forwardLimit);
+    }
+    int32_t getTarget(void)
+    {
+        if (m_kissState == STATE_STOPPED)
+            return m_pos;
+        else if (m_forwards)
+            return m_pos + m_distTotal;
+        else
+            return m_pos - m_distTotal;
+    }
+    uint32_t getDistRemaining(void)
+    {
+        return m_distTotal - m_distMoved;
+    }
+    void setForwardLimit(int32_t forwardLimit)
+    {
+        m_forwardLimit = forwardLimit;
+    }
+    void setReverseLimit(int32_t reverseLimit)
+    {
+        m_reverseLimit = reverseLimit;
+    }
+    int32_t getForwardLimit(void)
+    {
+        return m_forwardLimit;
+    }
+    int32_t getReverseLimit(void)
+    {
+        return m_reverseLimit;
+    }
+    void setMaxSpeed(uint16_t maxSpeed)
+    {
+        if (m_kissState == STATE_STOPPED) m_maxSpeed = maxSpeed;
+    }
+    uint16_t getMaxSpeed(void)
+    {
+        return m_maxSpeed;
+    }
+
+protected:
+    void setDir(bool forwards)
+    {
+        m_forwards = forwards;
+        digitalWrite(PIN_DIR, forwards == m_invertDir);
+    }
+    void updatePos(void)
+    {
+        if (m_forwards)
+            m_pos += m_distMoved;
+        else
+            m_pos -= m_distMoved;
+        m_distMoved = 0;
+    }
+    static const uint32_t ONE_SECOND = 1000000UL;
+    static const uint8_t PULSE_WIDTH_US = 2; // desired width of step pulse (high) in us
+    static const int32_t DEFAULT_FORWARD_LIMIT = 2147483647L;
+    static const int32_t DEFAULT_REVERSE_LIMIT = -2147483648L;
+    static const uint16_t DEFAULT_SPEED = 1600;
+    static const uint16_t INTERVAL_CORRECTION_INCREMENT = 255;
+
+    int32_t m_forwardLimit;
+    int32_t m_reverseLimit;
+    uint16_t m_maxSpeed;
+
+    const uint8_t PIN_DIR;
+    const uint8_t PIN_STEP;
+    const uint8_t PIN_ENABLE;
+
+    kissState_t m_kissState;
+    uint32_t m_distTotal, m_distMoved;
+    bool m_forwards;
+    int32_t m_pos;
+
+	const regint m_stepBit;
+    regint volatile * const m_stepOut;
+	
+    uint32_t m_stepIntervalWhole;
+    uint16_t m_stepIntervalRemainder;
+    uint16_t m_stepIntervalCorrectionCounter;
+    bool m_enabled;
+    uint32_t m_lastStepTime;
+    bool m_invertDir;
+    bool m_init;
+};
+
+// ----------------------------------------------------------------------------------------------------
+// ----------------------------------------------------------------------------------------------------
+// ----------------------------------------------------------------------------------------------------
+// kissStepper WITH acceleration
+// ----------------------------------------------------------------------------------------------------
+// ----------------------------------------------------------------------------------------------------
+// ----------------------------------------------------------------------------------------------------
+
+class kissStepper: public kissStepperNoAccel
+{
+public:
+    kissStepper(uint8_t PIN_DIR, uint8_t PIN_STEP, uint8_t PIN_ENABLE = 255, bool invertDir = false);
+    kissStepper(uint8_t PIN_DIR, uint8_t PIN_STEP, bool invertDir = false);
+    ~kissStepper(void) {};
+    bool prepareMove(int32_t target);
+    kissState_t move(void);
+    void stop(void);
+    
+    uint16_t getCurSpeed(void)
+    {
+        if (m_kissState == STATE_RUN)
+            return m_maxSpeed;
+        else if (m_kissState > STATE_STARTING)
+        {
+            uint32_t curSpeed = ONE_SECOND / m_stepIntervalWhole;
+            if (curSpeed > m_maxSpeed) curSpeed = m_maxSpeed;
+            return curSpeed;
+        }
+        else
+            return 0;
+    }
+    
+    void decelerate(void);
+    uint32_t calcMaxAccelDist(void)
+    {
+        if (m_accel > 0)
+            return ((uint32_t)m_maxSpeed * m_maxSpeed) / (2UL * m_accel);
+        else
+            return 0;
+    }
+    uint32_t getAccelDist(void)
+    {
+        return m_distAccel;
+    }
+    uint32_t getRunDist(void)
+    {
+        return m_distRun - m_distAccel;
+    }
+    uint32_t getDecelDist(void)
+    {
+        return m_distTotal - m_distRun;
+    }
+    void setAccel(uint16_t accel)
+    {
+        if (m_kissState == STATE_STOPPED) m_accel = accel;
+    }
+    uint16_t getAccel(void)
+    {
+        return m_accel;
+    }
+    uint16_t getTopSpeed(void);
+
+protected:
+
+    static const uint16_t DEFAULT_ACCEL = 1600;
+    uint32_t m_distAccel, m_distRun;
+    uint32_t m_topSpeedStepInterval;
+    uint32_t m_minSpeedStepInterval;
+    float m_stepInterval;
+    float m_constMult;
+    uint16_t m_accel;
+
+private:
+
+    /*
+       ----------------------------------------------------------------------------------------------------
+
+           To strike a balance between accuracy and performance, this library uses a set of approximations
+           for calculating stepInterval when accelerating/decelerating. Although this does use floating point
+           math, it is a drastic improvement over exact calculations and better than anything else I've tried.
+
+           There is probably room for further improvement (fixed point or integer math?) but this is good enough.
+
+           exact:
+               stepInterval = ONE_SECOND / newSpeed
+               curSpeed = ONE_SECOND / stepInterval
+               newSpeed = sqrt(curSpeed^2 + 2a)
+               stepInterval = ONE_SECOND / sqrt(curSpeed^2 + 2a)
+
+           approximations:
+               constMult = accel / (ONE_SECOND * ONE_SECOND)
+               q = constMult*stepInterval*stepInterval
+               set q to negative if accelerating
+
+               good precision, fast: stepInterval *= 1.0 + q
+               better precision, slower: stepInterval *= 1.0 + q + q*q
+               best precision, slowest: stepInterval *= 1.0 + q + 1.5*q*q
+
+       ----------------------------------------------------------------------------------------------------
+       */
+
+    float accelStep(float stepInterval, float constMult)
+    {
+        float newStepInterval;
+        float q = -constMult*stepInterval*stepInterval;
+        newStepInterval = stepInterval * (1.0 + q);
+        // newStepInterval = stepInterval * (1.0 + q + q*q); // better accuracy
+        // newStepInterval = stepInterval * (1.0 + q + 1.5*q*q); // best accuracy
+        if (newStepInterval < m_topSpeedStepInterval) newStepInterval = m_topSpeedStepInterval;
+        return newStepInterval;
+    }
+
+    float decelStep(float stepInterval, float constMult)
+    {
+        float newStepInterval;
+        float q = constMult*stepInterval*stepInterval;
+        newStepInterval = stepInterval * (1.0 + q);
+        // newStepInterval = stepInterval * (1.0 + q + q*q); // better accuracy
+        // newStepInterval = stepInterval * (1.0 + q + 1.5*q*q); // best accuracy
+        if (newStepInterval > m_minSpeedStepInterval) newStepInterval = m_minSpeedStepInterval;
+        return newStepInterval;
+    }
+
+};
+
+#endif

maths.cpp

@@ -1,48 +1,46 @@
 #include "math.h"
-
+#include "turret.h"
-// rollH atan((22.5 - 18.5) / 236) ~0.97°
-// rollV ~1°
 
 double degToRad(double deg) { return deg * M_PI / 180.0; }
 
 double radToDeg(double rad) { return rad * 180.0 / M_PI; }
 
-void angleToStep(long &stepX, long &stepY, double angleX, double angleY) {
+void radToStep(Turret::StepRatio stepRatio, vec2<double> angle,
-  // TODO remove magic numbers
+               vec2<long> &step) {
-  double stepRatioX = M_PI / ((51.2 / 15.0) * (100.0 * 64.0));
+  step.x = angle.x * stepRatio.x;
-  double stepRatioY = M_PI / ((48.0 / 15.0) * (100.0 * 64.0));
+  step.y = angle.y * stepRatio.y;
-
-  stepX = angleX / stepRatioX;
-  stepY = angleY / stepRatioY;
 }
 
-double rollCorrection(double x, double max, double width) {
+void stepToRad(Turret::StepRatio stepRatio, vec2<long> step,
-  double correction = max * (1.0 - cos((10.0 * x) / (width * M_PI))) / 2.0;
+               vec2<double> &angle) {
-  return x - correction;
+  angle.x = step.x / stepRatio.x;
+  angle.y = step.y / stepRatio.y;
 }
 
-void cartesianToPolar(long &stepX, long &stepY, double zeroX, double zeroY,
+void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset,
-                      double x, double y, double z) {
+                      vec2<long> zero, vec3<double> position,
-  // TODO remove magic numbers
+                      vec2<long> &step) {
-  double distance = 166.0; // turret to screen
+  // x = -x; // natural axis direction
-  double offsetY = 21.0;   // laser to stand
+  position.y = -position.y; // natural axis direction
-  double offsetX = 0.0;    // unused
+  double dX = position.x + offset.x;
+  double dY = position.y + offset.y;
+  double dZ = position.z + offset.z;
 
-  x = -x; // natural axis direction
+  vec2<double> angle(atan2(dZ, dY) - M_PI / 2, atan2(dZ, dX) - M_PI / 2);
-  double dX = x + offsetX;
-  double dY = rollCorrection(y + offsetY, 0.2, 400.0);
-  double dZ = z + distance;
 
-  double roll = atan2(4.0, 236.0);
+  radToStep(stepRatio, angle, step);
-  double rollY = dX * tan(roll);
+}
-  double rollX = dY * tan(roll);
+
-
+void polarToCartesian(Turret::StepRatio stepRatio, Turret::Offset offset,
-  double dXr = dX + rollX;
+                      vec3<long> current, vec2<long> zero, vec2<long> step,
-  double dYr = dY + rollY;
+                      vec3<double> &position) {
-
+
-  double rho = sqrt(pow(dXr, 2.0) + pow(dYr, 2.0) + pow(dZ, 2.0));
+  vec2<double> angle;
-  double angleY = M_PI / 2.0 - acos(dYr / rho);
+  stepToRad(stepRatio, step, angle);
-  double angleX = atan2(dXr, dZ);
+
-
+  vec2<double> dXYZ(tan(angle.x) * current.z, tan(angle.y) * current.z);
-  angleToStep(stepX, stepY, angleX, angleY);
+
+  position.x = dXYZ.x - offset.x;
+  position.y = -(dXYZ.y - offset.y); // natural axis direction
+  position.z = current.z - offset.z;
 }

maths.h

@@ -1,5 +1,18 @@
+#ifndef MATHS_H
+#define MATHS_H
+
+#include "turret.h"
+
 double degToRad(double deg);
 double radToDeg(double rad);
-void angleToStep(long &stepX, long &stepY, double angleX, double angleY);
+void radToStep(Turret::StepRatio stepRatio, vec2<double> angle,
-void cartesianToPolar(long &stepX, long &stepY, double zeroX, double zeroY,
+               vec2<long> &step);
-                      double x, double y, double z);
+void stepToRad(Turret::StepRatio stepRatio, vec2<long> step,
+               vec2<double> &angle);
+void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset,
+                      vec2<long> zero, vec3<double> position, vec2<long> &step);
+void polarToCartesian(Turret::StepRatio stepRatio, Turret::Offset offset,
+                      vec3<long> current, vec2<long> zero, vec2<long> step,
+                      vec3<double> &position);
+
+#endif

serial_stream.cpp

@@ -0,0 +1,30 @@
+#include "serial_stream.h"
+
+SerialStream &SerialStream::connect(HardwareSerial &serial) {
+  _serial = &serial;
+  return *this;
+}
+
+SerialStream &SerialStream::disconnect() {
+  _serial = nullptr;
+  return *this;
+}
+
+SerialStream &SerialStream::bind(Turret &turret) {
+  _turret = &turret;
+  return *this;
+}
+
+SerialStream &SerialStream::unbind() {
+  _turret = nullptr;
+  return *this;
+}
+
+SerialStream &SerialStream::loop() {
+  if (_serial == nullptr) return *this;
+  if (_serial->available() == 0) return *this;
+
+  _serial->read();
+
+  return *this;
+}

serial_stream.h

@@ -0,0 +1,28 @@
+#ifndef SERIAL_STREAM_H
+#define SERIAL_STREAM_H
+
+#include "HardwareSerial.h"
+#include "turret.h"
+
+class SerialStream {
+public:
+  SerialStream();
+  ~SerialStream();
+
+  SerialStream &connect(HardwareSerial &serial);
+  SerialStream &disconnect();
+
+  SerialStream &bind(Turret &turret);
+  SerialStream &unbind();
+
+  SerialStream &loop();
+
+private:
+  // TEMP to keep this in mind
+  int _internal_buffer;
+
+  HardwareSerial *_serial = nullptr;
+  Turret *_turret = nullptr;
+};
+
+#endif

turret.cpp

@@ -1,218 +1,432 @@
 #include "turret.h"
-#include "config.h"
+#include "Arduino.h"
+#include "math.h"
 #include "maths.h"
 #include <kissStepper.h>
 
-Turret::Turret(int pin_x_direction, int pin_x_pulse, int pin_x_enable,
+Turret::Turret(StepRatio step_ratio, Offset offset, ZeroOffset zero_offset,
-               int pin_x_home, int pin_y_direction, int pin_y_pulse,
+               PinMap pin_map_x, PinMap pin_map_y)
-               int pin_y_enable, int pin_y_home, int pin_laser)
+    : _stepper(kissStepper(static_cast<uint8_t>(pin_map_x.direction),
-    : _stepperX(pin_x_direction, pin_x_pulse, pin_x_enable),
+                           static_cast<uint8_t>(pin_map_x.pulse),
-      _stepperY(pin_y_direction, pin_y_pulse, pin_y_enable) {
+                           static_cast<uint8_t>(pin_map_x.enable)),
+               kissStepper(static_cast<uint8_t>(pin_map_y.direction),
+                           static_cast<uint8_t>(pin_map_y.pulse),
+                           static_cast<uint8_t>(pin_map_y.enable))) {
 
-  _pin_x_direction = pin_x_direction;
+  _pin.x = pin_map_x;
-  _pin_x_pulse = pin_x_pulse;
+  _pin.y = pin_map_y;
-  _pin_x_enable = pin_x_enable;
+  _step_ratio = step_ratio;
+  _offset = offset;
+  _zero_offset = zero_offset;
+}
 
-  _pin_y_direction = pin_y_direction;
+Turret::~Turret() {
-  _pin_y_pulse = pin_y_pulse;
+  _stepper.x.stop();
-  _pin_y_enable = pin_y_enable;
+  _stepper.x.disable();
-
+  _stepper.y.stop();
-  _pin_x_home = pin_x_home;
+  _stepper.y.disable();
-  _pin_y_home = pin_y_home;
+  digitalWrite(_pin.x.laser, LOW);
-  _pin_laser = pin_laser;
+  digitalWrite(_pin.y.laser, LOW);
 }
 
 Turret &Turret::init() {
-  pinMode(_pin_laser, OUTPUT);
+  pinMode(_pin.x.laser, OUTPUT);
+  pinMode(_pin.y.laser, OUTPUT);
 
-  pinMode(_pin_x_direction, OUTPUT);
+  pinMode(_pin.x.direction, OUTPUT);
-  pinMode(_pin_x_pulse, OUTPUT);
+  pinMode(_pin.x.pulse, OUTPUT);
-  pinMode(_pin_x_enable, OUTPUT);
+  pinMode(_pin.x.enable, OUTPUT);
-  pinMode(_pin_x_home, INPUT_PULLUP);
+  pinMode(_pin.x.home, INPUT_PULLUP);
 
-  pinMode(_pin_y_direction, OUTPUT);
+  pinMode(_pin.y.direction, OUTPUT);
-  pinMode(_pin_y_pulse, OUTPUT);
+  pinMode(_pin.y.pulse, OUTPUT);
-  pinMode(_pin_y_enable, OUTPUT);
+  pinMode(_pin.y.enable, OUTPUT);
-  pinMode(_pin_y_home, INPUT_PULLUP);
+  pinMode(_pin.y.home, INPUT_PULLUP);
 
-  _stepperX.begin();
+  _stepper.x.begin();
-  _stepperY.begin();
+  _stepper.y.begin();
 
   return *this;
 }
 
-Turret &Turret::gotoHome() {
+bool Turret::_enqueue_tick(Turret::TickHandler handler) {
-  long xStop = 0;
+  size_t next = (_tick_queue_tail + 1) % Turret::TICK_QUEUE_LENGTH;
-  long yStop = 0;
+  if (next == _tick_queue_head) return false;  // queue full
+  _tick_queue[_tick_queue_tail] = handler;
+  _tick_queue_tail = next;
+  return true;
+}
 
-  _stepperX.prepareMove(-1000000l);
+bool Turret::_dequeue_tick() {
-  _stepperY.prepareMove(-1000000l);
+  if (_tick_queue_head == _tick_queue_tail) return false;  // queue is empty
+  _tick_queue_head = (_tick_queue_head + 1) % Turret::TICK_QUEUE_LENGTH;
+  return true;
+}
 
-  while (true) {
+Turret::TickHandler Turret::_current_tick() {
-    _stepperX.move();
+  if (_tick_queue_head == _tick_queue_tail) return nullptr;
-    _stepperY.move();
+  return _tick_queue[_tick_queue_head];
+}
 
-    if (xStop == 2 && yStop == 2) {
+Turret &Turret::wait(long ms, bool blocking) {
-      _homeX = _stepperX.getPos();
+  if (blocking) {
-      _homeY = _stepperY.getPos();
+    while(_wait_tick());
-      break;
+  } else {
-    }
+    _next_tick = &Turret::_wait_tick;
+  }
+}
 
-    if (xStop < 2 && digitalRead(_pin_x_home)) {
+bool Turret::_wait_tick() {
-      xStop++;
+  if (delay + start >= now) return false;
-      _stepperX.stop();
+  return true
-    }
+}
 
-    if (yStop < 2 && digitalRead(_pin_y_home)) {
+Turret &Turret::gotoHome(bool blocking) {
-      yStop++;
+  if (blocking) {
-      _stepperY.stop();
+    while(_goto_home_tick());
-    }
+  } else {
+    _next_tick = &Turret::_goto_home_tick;
+  }
+  return *this;
+}
+
+bool Turret::_goto_home_tick() {
+  _stepper.x.prepareMove(-1000000l);
+  _stepper.y.prepareMove(-1000000l);
+  
+  bool xStop = _stepper.x.getState();
+  bool yStop = _stepper.y.getState();
+
+  _stepper.x.move();
+  _stepper.y.move();
+
+  if (xStop && yStop ) {
+    _home.x = _stepper.x.getPos();
+    _home.y = _stepper.y.getPos();
+    return false;
   }
 
-  return *this;
+  if (!xStop && digitalRead(_pin.x.home)) {
-}
+    _stepper.x.stop();
-
-Turret &Turret::gotoZero() {
-  // TODO remove magic numbers
-  _stepperX.prepareMove(_homeX + 9300);
-  _stepperY.prepareMove(_homeY + 3500);
-
-  bool xStop = false;
-  bool yStop = false;
-
-  while (true) {
-    if (xStop && yStop)
-      break;
-    if (!xStop)
-      xStop = _stepperY.move() == STATE_STOPPED;
-    if (!yStop)
-      yStop = _stepperX.move() == STATE_STOPPED;
   }
 
-  _zeroX = _stepperX.getPos();
+  if (!yStop && digitalRead(_pin.y.home)) {
-  _zeroY = _stepperY.getPos();
+    _stepper.y.stop();
+  }
 
+  return true;
+}
+
+Turret &Turret::gotoZero(bool blocking) {
+  if (blocking) {
+    while(_goto_zero_tick());
+  } else {
+    _next_tick = &Turret::_goto_zero_tick;
+  }
   return *this;
 }
 
-Turret &Turret::calibrate() {
+bool Turret::_goto_zero_tick() {
-  gotoHome();
+  _stepper.x.prepareMove(_home.x + _zero_offset.x);
-  gotoZero();
+  _stepper.y.prepareMove(_home.y + _zero_offset.y);
+  
+  bool xStop = _stepper.x.getState();
+  bool yStop = _stepper.y.getState();
+
+  if (xStop && yStop) {
+    _zero.x = _stepper.x.getPos();
+    _zero.y = _stepper.y.getPos();
+    return false;
+  }
+
+  if (!xStop)
+    xStop = _stepper.y.move() == STATE_STOPPED;
+  if (!yStop)
+    yStop = _stepper.x.move() == STATE_STOPPED;
+
+  return true; 
+}
+
+Turret &Turret::calibrate(bool blocking) {
+  gotoHome(blocking);
+  gotoZero(blocking);
   return *this;
 }
 
-Turret &Turret::moveTo(double x, double y, double z, Unit unit) {
+Turret &Turret::moveTo(double x, double y, double z, Unit unit, bool blocking) {
-  long stepX;
+  vec2<long> step;
-  long stepY;
 
   if (unit == Unit::MM) {
-    cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x / 10.0, y / 10.0,
+    vec3<double> position(x / 10.0, y / 10.0, z / 10.0);
-                     z / 10.0);
+    cartesianToPolar(_step_ratio, _offset, _zero, position, step);
   }
 
   if (unit == Unit::CM) {
-    cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x, y, z);
+    vec3<double> position(x, y, z);
+    cartesianToPolar(_step_ratio, _offset, _zero, position, step);
   }
 
   if (unit == Unit::M) {
-    cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x * 100.0, y * 100.0,
+    vec3<double> position(x * 100.0, y * 100.0, z * 100.0);
-                     z * 100.0);
+    cartesianToPolar(_step_ratio, _offset, _zero, position, step);
   }
 
   if (unit == Unit::STEP) {
-    stepX = x;
+    step.x = x;
-    stepY = y;
+    step.y = y;
   }
 
   if (unit == Unit::RAD) {
-    angleToStep(stepX, stepY, x, y);
+    radToStep(_step_ratio, vec2<double>(x, y), step);
   }
 
   if (unit == Unit::DEG) {
-    angleToStep(stepX, stepY, degToRad(x), degToRad(y));
+    radToStep(_step_ratio, vec2<double>(degToRad(x), degToRad(y)), step);
   }
 
-  // TODO min(valueI, -_homeI);
+  long maxDeltaStepX = _step_ratio.x * M_PI_2;
-  _stepperX.prepareMove(_zeroX + stepX);
+  long maxDeltaStepY = _step_ratio.y * M_PI_2;
-  _stepperY.prepareMove(_zeroY + stepY);
 
-  bool xStop = false;
+  // force move from -90° to 90° avoiding backside,
-  bool yStop = false;
+  // 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));
 
-  while (true) {
+  if (blocking) {
-    if (xStop && yStop)
+    while(_move_to_tick());
-      break;
+  } else {
-    if (!xStop)
+    _next_tick = &Turret::_move_to_tick;
-      xStop = _stepperX.move() == STATE_STOPPED;
-    if (!yStop)
-      yStop = _stepperY.move() == STATE_STOPPED;
   }
+  // _stepper.x.prepareMove(_zero.x + stepX);
+  // _stepper.y.prepareMove(_zero.y + stepY);
 
-  _currentX = x;
+  // bool xStop = false;
-  _currentY = y;
+  // bool yStop = false;
-  _currentZ = z;
+
+  // while (true) {
+  //   if (xStop && yStop)
+  //     break;
+  //   if (!xStop)
+  //     xStop = _stepper.x.move() == STATE_STOPPED;
+  //   if (!yStop)
+  //     yStop = _stepper.y.move() == STATE_STOPPED;
+  // }
+
+  // _current.x = x;
+  // _current.y = y;
+  // _current.z = z;
 
   return *this;
 }
 
-Turret &Turret::moveBy(double x, double y, double z, Unit unit) {
+bool Turret::_move_to_tick() {
-  long zeroXStored = _zeroX;
+  stepX;
-  long zeroYStored = _zeroY;
+  stepY;
 
-  _zeroX += _stepperX.getPos();
+  _stepper.x.prepareMove(_zero.x + stepX);
-  _zeroY += _stepperY.getPos();
+  _stepper.y.prepareMove(_zero.y + stepY);
 
-  moveTo(x, y, z, unit);
+  bool xStop = _stepper.x.getState() == STATE_STOPPED;
+  bool yStop = _stepper.y.getState() == STATE_STOPPED;
 
-  _zeroX = zeroXStored;
+  if (xStop && yStop) {
-  _zeroY = zeroYStored;
+  _current.x = x;
+  _current.y = y;
+  _current.z = z;
+    return false;
+  }
+  
+  if (!xStop) xStop = _stepper.x.move() == STATE_STOPPED;
+  if (!yStop) yStop = _stepper.y.move() == STATE_STOPPED;
+}
 
-  _currentX = x;
+Turret &Turret::moveBy(double x, double y, double z, Unit unit, bool blocking) {
-  _currentY = y;
+  long zeroXStored = _zero.x;
-  _currentZ = z;
+  long zeroYStored = _zero.y;
+
+  _zero.x += _stepper.x.getPos();
+  _zero.y += _stepper.y.getPos();
+
+  moveTo(x, y, z, unit, blocking);
+
+  _zero.x = zeroXStored;
+  _zero.y = zeroYStored;
+
+  _current.x = x;
+  _current.y = y;
+  _current.z = z;
 
   return *this;
 }
 
-Turret &Turret::moveToX(double x, Unit unit) {
+Turret &Turret::moveToX(double x, Unit unit, bool blocking) {
-  return moveTo(x, _currentY, _currentZ, unit);
+  return moveTo(x, _current.y, _current.z, unit, blocking);
 }
 
-Turret &Turret::moveToY(double y, Unit unit) {
+Turret &Turret::moveToY(double y, Unit unit, bool blocking) {
-  return moveTo(_currentX, y, _currentZ, unit);
+  return moveTo(_current.x, y, _current.z, unit, blocking);
 }
 
-Turret &Turret::moveToZ(double z, Unit unit) {
+Turret &Turret::moveToZ(double z, Unit unit, bool blocking) {
-  return moveTo(_currentX, _currentY, z, unit);
+  return moveTo(_current.x, _current.y, z, unit, blocking);
 }
 
-Turret &Turret::moveByX(double x, Unit unit) { return moveBy(x, 0, 0, unit); }
+Turret &Turret::moveByX(double x, Unit unit, bool blocking) { return moveBy(x, 0, 0, unit, blocking); }
 
-Turret &Turret::moveByY(double y, Unit unit) { return moveTo(0, y, 0, unit); }
+Turret &Turret::moveByY(double y, Unit unit, bool blocking) { return moveTo(0, y, 0, unit, blocking); }
 
-Turret &Turret::moveByZ(double z, Unit unit) { return moveTo(0, 0, z, unit); }
+Turret &Turret::moveByZ(double z, Unit unit, bool blocking) { return moveTo(0, 0, z, unit, blocking); }
+
+Turret &Turret::nextTick() {
+  TickHandler tick = _current_tick();
+  if (tick == nullptr) return *this;
+  if (!(this->*tick)()) _dequeue_tick();
+  return *this;
+}
+
+Turret &Turret::flushTick() {
+  while (_dequeue_tick());
+  return *this;
+}
+
+bool Turret::hasTick() {
+  TickHandler tick = _current_tick();
+  return tick != nullptr;
+}
 
 Turret &Turret::getPosition(double &x, double &y, double &z, Unit unit) {
-  // TODO implement
+  if (unit == Unit::MM) {
+    vec3<double> position;
+    vec2<long> xy(_current.x, _current.y);
+    polarToCartesian(_step_ratio, _offset, _current, _zero, xy, position);
+    x = position.x / 10;
+    y = position.y / 10;
+    z = position.z / 10;
+  }
+
+  if (unit == Unit::CM) {
+    vec3<double> position;
+    vec2<long> xy(_current.x, _current.y);
+    polarToCartesian(_step_ratio, _offset, _current, _zero, xy, position);
+    x = position.x;
+    y = position.y;
+    z = position.z;
+  }
+
+  if (unit == Unit::M) {
+    vec3<double> position;
+    vec2<long> xy(_current.x, _current.y);
+    polarToCartesian(_step_ratio, _offset, _current, _zero, xy, position);
+    x = position.x * 100;
+    y = position.y * 100;
+    z = position.z * 100;
+  }
+
+  if (unit == Unit::STEP) {
+    x = _home.x;
+    y = _home.y;
+  }
+
+  if (unit == Unit::RAD) {
+    vec2<double> angle;
+    stepToRad(_step_ratio, _home, angle);
+    x = angle.x;
+    y = angle.y;
+  }
+
+  if (unit == Unit::DEG) {
+    vec2<double> angle;
+    stepToRad(_step_ratio, _home, angle);
+    x = radToDeg(angle.x);
+    y = radToDeg(angle.y);
+  }
+
   return *this;
 }
 
-Turret &Turret::getHome(double &x, double &y, double &z, Unit unit) {
+Turret &Turret::getHome(double &x, double &y, Unit unit) {
-  // TODO implement
+  if (unit == Unit::MM) {
+    vec3<double> position;
+    polarToCartesian(_step_ratio, _offset, _current, _zero, _home, position);
+    x = position.x / 10;
+    y = position.y / 10;
+  }
+
+  if (unit == Unit::CM) {
+    vec3<double> position;
+    polarToCartesian(_step_ratio, _offset, _current, _zero, _home, position);
+    x = position.x;
+    y = position.y;
+  }
+
+  if (unit == Unit::M) {
+    vec3<double> position;
+    polarToCartesian(_step_ratio, _offset, _current, _zero, _home, position);
+    x = position.x * 100;
+    y = position.y * 100;
+  }
+
+  if (unit == Unit::STEP) {
+    x = _home.x;
+    y = _home.y;
+  }
+
+  if (unit == Unit::RAD) {
+    vec2<double> angle;
+    stepToRad(_step_ratio, _home, angle);
+    x = angle.x;
+    y = angle.y;
+  }
+
+  if (unit == Unit::DEG) {
+    vec2<double> angle;
+    stepToRad(_step_ratio, _home, angle);
+    x = radToDeg(angle.x);
+    y = radToDeg(angle.y);
+  }
+
   return *this;
 }
 
-Turret &Turret::getZero(double &x, double &y, double &z, Unit unit) {
+Turret &Turret::getZero(double &x, double &y, Unit unit) {
-  // TODO implement
+  if ((unit == Unit::MM) || (unit == Unit::CM) || (unit == Unit::MM)) {
+    x = 0;
+    y = 0;
+  }
+
+  if (unit == Unit::STEP) {
+    x = _zero.x;
+    y = _zero.y;
+  }
+
+  if (unit == Unit::RAD) {
+    vec2<double> angle;
+    stepToRad(_step_ratio, _zero, angle);
+    x = angle.x;
+    y = angle.y;
+  }
+
+  if (unit == Unit::DEG) {
+    vec2<double> angle;
+    stepToRad(_step_ratio, _zero, angle);
+    x = radToDeg(angle.x);
+    y = radToDeg(angle.y);
+  }
+
   return *this;
 }
 
 Turret &Turret::laserOn() {
-  digitalWrite(_pin_laser, HIGH);
+  digitalWrite(_pin.x.laser, HIGH);
+  digitalWrite(_pin.y.laser, HIGH);
   return *this;
 }
 
 Turret &Turret::laserOff() {
-  digitalWrite(_pin_laser, LOW);
+  digitalWrite(_pin.x.laser, LOW);
+  digitalWrite(_pin.y.laser, LOW);
   return *this;
 }

turret.h

@@ -3,63 +3,102 @@
 
 #include <kissStepper.h>
 
+template <typename T> struct vec3 {
+  T x;
+  T y;
+  T z;
+
+  vec3() : x(T()), y(T()), z(T()) {}
+  vec3(T _x, T _y, T _z) : x(_x), y(_y), z(_z) {}
+};
+
+template <typename T> struct vec2 {
+  T x;
+  T y;
+
+  vec2() : x(T()), y(T()) {}
+  vec2(T _x, T _y) : x(_x), y(_y) {}
+};
+
 class Turret {
 public:
   enum Unit { MM, CM, M, RAD, DEG, STEP };
 
-  Turret(int pin_x_direction, int pin_x_pulse, int pin_x_enable, int pin_x_home,
+  using StepRatio = vec2<double>;
-         int pin_y_direction, int pin_y_pulse, int pin_y_enable, int pin_y_home,
+  using Offset = vec3<double>;
-         int pin_laser);
+  // x -> turret to screen
+  // y -> laser to stand
+  // z -> unused
+
+  using ZeroOffset = vec2<long>;
+
+  struct PinMap {
+    int home;
+    int direction;
+    int pulse;
+    int enable;
+    int laser;
+  };
+
+  Turret(StepRatio step_ratio, Offset offset, ZeroOffset zero_offset,
+         PinMap pin_map_x, PinMap pin_map_y);
+  ~Turret();
 
   Turret &init();
-  Turret &gotoHome();
+  Turret &gotoHome(bool blocking = false);
-  Turret &gotoZero();
+  Turret &gotoZero(bool blocking = false);
 
-  Turret &moveTo(double x, double y, double z, Unit unit = Turret::Unit::CM);
+  Turret &moveTo(double x, double y, double z, Unit unit = Turret::Unit::CM, bool blocking = false);
-  Turret &moveBy(double x, double y, double z, Unit unit = Turret::Unit::CM);
+  Turret &moveBy(double x, double y, double z, Unit unit = Turret::Unit::CM, bool blocking = false);
 
-  Turret &moveToX(double x, Unit unit = Turret::Unit::CM);
+  Turret &moveToX(double x, Unit unit = Turret::Unit::CM, bool blocking = false);
-  Turret &moveToY(double y, Unit unit = Turret::Unit::CM);
+  Turret &moveToY(double y, Unit unit = Turret::Unit::CM, bool blocking = false);
-  Turret &moveToZ(double z, Unit unit = Turret::Unit::CM);
+  Turret &moveToZ(double z, Unit unit = Turret::Unit::CM, bool blocking = false);
 
-  Turret &moveByX(double x, Unit unit = Turret::Unit::CM);
+  Turret &moveByX(double x, Unit unit = Turret::Unit::CM, bool blocking = false);
-  Turret &moveByY(double y, Unit unit = Turret::Unit::CM);
+  Turret &moveByY(double y, Unit unit = Turret::Unit::CM, bool blocking = false);
-  Turret &moveByZ(double z, Unit unit = Turret::Unit::CM);
+  Turret &moveByZ(double z, Unit unit = Turret::Unit::CM, bool blocking = false);
 
   Turret &getPosition(double &x, double &y, double &z, Unit unit);
-  Turret &getHome(double &x, double &y, double &z, Unit unit);
+  Turret &getHome(double &x, double &y, Unit unit);
-  Turret &getZero(double &x, double &y, double &z, Unit unit);
+  Turret &getZero(double &x, double &y, Unit unit);
 
-  Turret &calibrate();
+  Turret &calibrate(bool blocking = false);
 
   Turret &laserOn();
   Turret &laserOff();
 
+  Turret &nextTick();
+  Turret &flushTick();
+  bool hasTick();
+
 private:
-  kissStepper _stepperX;
+  vec2<kissStepper> _stepper;
-  kissStepper _stepperY;
+  vec2<long> _home;
+  vec2<long> _zero;
+  vec3<long> _current;
+  vec2<PinMap> _pin;
+  StepRatio _step_ratio;
+  Offset _offset;
+  ZeroOffset _zero_offset;
 
-  int _homeX;
+  // tick queue related
-  int _homeY;
+  static constexpr size_t TICK_QUEUE_LENGTH = 32;
+  using TickHandler = bool (Turret::*)();
 
-  int _zeroX;
+  TickHandler _tick_queue[TICK_QUEUE_LENGTH];
-  int _zeroY;
+  size_t _tick_queue_head = 0;
+  size_t _tick_queue_tail = 0;
 
-  int _currentX;
+  bool _enqueue_tick(TickHandler fn);
-  int _currentY;
+  bool _dequeue_tick();
-  int _currentZ;
+  TickHandler _current_tick();
 
-  int _pin_x_direction;
+  bool (Turret::*_next_tick)() = nullptr;
-  int _pin_x_pulse;
-  int _pin_x_enable;
 
-  int _pin_y_direction;
+  // tick handlers
-  int _pin_y_pulse;
+  bool _goto_zero_tick();
-  int _pin_y_enable;
+  bool _goto_home_tick();
-
-  int _pin_x_home;
-  int _pin_y_home;
-  int _pin_laser;
 };
 
 #endif

turret_debug.ino

@@ -1,17 +1,34 @@
 #include "config.h"
 #include "turret.h"
 
-Turret turret(PIN_X_DIRECTION, PIN_X_PULSE, PIN_X_ENABLE, PIN_X_HOME,
+Turret::PinMap pinX = {
-              PIN_Y_DIRECTION, PIN_Y_PULSE, PIN_Y_ENABLE, PIN_Y_HOME,
+    .home = PIN_X_HOME,
-              PIN_LASER);
+    .direction = PIN_X_DIRECTION,
+    .pulse = PIN_X_PULSE,
+    .enable = PIN_X_ENABLE,
+    .laser = PIN_LASER,
+};
+
+Turret::PinMap pinY = {
+    .home = PIN_Y_HOME,
+    .direction = PIN_Y_DIRECTION,
+    .pulse = PIN_Y_PULSE,
+    .enable = PIN_Y_ENABLE,
+    .laser = PIN_LASER,
+};
+
+Turret::StepRatio stepRatio(STEP_RATIO_X, STEP_RATIO_Y);
+Turret::Offset offset(OFFSET_X, OFFSET_Y, OFFSET_Z);
+Turret::ZeroOffset zeroOffset(ZERO_OFFSET_X, ZERO_OFFSET_Y);
+
+Turret turret(stepRatio, offset, zeroOffset, pinX, pinY);
 
 void setup() {
   turret.init();
 
   Serial.begin(BAUDRATE);
 
-  turret.laserOn().calibrate();
+  turret.laserOn().calibrate().moveTo(0, 0, 0);
-  turret.moveTo(0, 0, 0);
   delay(5000);
 
   // test pointage
@@ -20,74 +37,22 @@ void setup() {
   int panelHCount = 3;
   int panelVCount = 2;
 
-  // double zeroOffsetV = -31.5;
+  double zeroOffsetV = -21;
-  double zeroOffsetV = -20.8;
+  double zeroOffsetH = -163;
-  // double zeroOffsetH = -184.5;
+  // double zeroOffsetH = -147;
-  double zeroOffsetH = -195.5;
 
   // Test constant step h,v
-  Serial.println("Start demo_0");
+  for (double step = 0; step < 160; step += 10) {
-  for (int i = 0; i < 20; i++) {
+    Serial.print("[step]: ");
-    Serial.print("> ");
+    Serial.println(step);
-    Serial.println(i);
+    turret.moveTo(step, 0, 0);
-    for (double step = -198.5; step < 176.0; step += 10) {
-      Serial.print("[step]: ");
-      Serial.println(step);
-      turret.moveTo(step, 87, 0);
-      // turret.moveBy(20.0, 0.0, 0.0);
-      delay(500);
-    }
-
-    for (double step = 0; step < 200.0; step += 10) {
-      Serial.print("[step]: ");
-      Serial.println(step);
-      turret.moveTo(0, step, 0);
-      // turret.moveBy(20.0, 0.0, 0.0);
-      delay(500);
-    }
-
-    delay(5000);
-    turret.moveTo(0, 0, 0);
     delay(5000);
   }
-
+  for (double step = 0; step > -160; step -= 10) {
-  // Test rollX
+    Serial.print("[step]: ");
-  turret.moveTo(0 * panelWidth + zeroOffsetH, 0, 0);
+    Serial.println(step);
-  delay(5000);
+    turret.moveTo(step, 0, 0);
-  turret.moveTo((panelHCount * panelWidth) / 2 + zeroOffsetH, 0, 0);
+    delay(5000);
-  delay(5000);
-  turret.moveTo(panelHCount * panelWidth + zeroOffsetH, 0, 0);
-  delay(5000);
-  turret.gotoZero();
-
-  // Test rollY
-  turret.moveTo(0, 0 * panelHeight + zeroOffsetV, 0);
-  delay(5000);
-  turret.moveTo(0, (panelVCount * panelHeight) / 2 + zeroOffsetV, 0);
-  delay(5000);
-  turret.moveTo(0, panelVCount * panelHeight + zeroOffsetV, 0);
-  delay(5000);
-  turret.gotoZero();
-
-  // Test align to panels
-  for (double panelH = 0; panelH < panelHCount + 1; panelH += 0.5) {
-    for (double panelV = 0; panelV < panelVCount + 1; panelV += 0.5) {
-      double x = panelH * panelWidth + zeroOffsetH;
-      double y = panelV * panelHeight + zeroOffsetV;
-      double z = 0;
-
-      Serial.print("Goto (x, y, z) => (");
-      Serial.print(x);
-      Serial.print(", ");
-      Serial.print(y);
-      Serial.print(", ");
-      Serial.print(z);
-      Serial.println(")");
-
-      turret.moveTo(x, y, z);
-
-      delay(2000);
-    }
   }
 
   turret.gotoZero();