.gitignore

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

README.md

@@ -5,10 +5,7 @@ Code de la tourelle de pointage laser du projet bouchons EDF.
 > [!WARNING]
 > Code de debuggage uniquement.
 
-> [!IMPORTANT]
+# Usage
-> Nouvelle méca, les axes H/x et V/y sont découplés.
-
-## Usage
 
 Compile through Arduino IDE or equivalent.
 
@@ -16,51 +13,6 @@ 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 turrent 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 dependeinces 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 --staged` to format stagged files and
+Before `git commit`, run `git clang-format` to format stagged files.
-then `git add` to commit formatting.

config.h

@@ -1,8 +1,6 @@
 #ifndef CONFIG_H
 #define CONFIG_H
 
-#include "math.h"
-
 #define BAUDRATE (9600)
 
 // Pins
@@ -16,29 +14,16 @@
 #define PIN_PD2 (2)
 #define PIN_MOSI (11)
 
-#define PIN_Y_PULSE (PIN_PB0)
+#define PIN_X_PULSE (PIN_PB0)
-#define PIN_Y_ENABLE (PIN_PD6)
+#define PIN_X_ENABLE (PIN_PD6)
-#define PIN_Y_DIRECTION (PIN_PD7)
+#define PIN_X_DIRECTION (PIN_PD7)
-#define PIN_Y_HOME (PIN_PD3)
+#define PIN_X_HOME (PIN_PD3)
 
-#define PIN_X_PULSE (PIN_PB2)
+#define PIN_Y_PULSE (PIN_PB2)
-#define PIN_X_ENABLE (PIN_PB1)
+#define PIN_Y_ENABLE (PIN_PB1)
-#define PIN_X_DIRECTION (PIN_MOSI)
+#define PIN_Y_DIRECTION (PIN_MOSI)
-#define PIN_X_HOME (PIN_PD4)
+#define PIN_Y_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]
-#define OFFSET_X (0)
-#define OFFSET_Y (21)
-#define OFFSET_Z (166)
-
 #endif

generate_clangd.py

@@ -1,48 +0,0 @@
-#!/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

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

@@ -1,30 +0,0 @@
-#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

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

turret.h

@@ -3,102 +3,63 @@
 
 #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 };
 
-  using StepRatio = vec2<double>;
+  Turret(int pin_x_direction, int pin_x_pulse, int pin_x_enable, int pin_x_home,
-  using Offset = vec3<double>;
+         int pin_y_direction, int pin_y_pulse, int pin_y_enable, int pin_y_home,
-  // x -> turret to screen
+         int pin_laser);
-  // 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(bool blocking = false);
+  Turret &gotoHome();
-  Turret &gotoZero(bool blocking = false);
+  Turret &gotoZero();
 
-  Turret &moveTo(double x, double y, double z, Unit unit = Turret::Unit::CM, bool blocking = false);
+  Turret &moveTo(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 &moveBy(double x, double y, double z, Unit unit = Turret::Unit::CM);
 
-  Turret &moveToX(double x, Unit unit = Turret::Unit::CM, bool blocking = false);
+  Turret &moveToX(double x, Unit unit = Turret::Unit::CM);
-  Turret &moveToY(double y, Unit unit = Turret::Unit::CM, bool blocking = false);
+  Turret &moveToY(double y, Unit unit = Turret::Unit::CM);
-  Turret &moveToZ(double z, Unit unit = Turret::Unit::CM, bool blocking = false);
+  Turret &moveToZ(double z, Unit unit = Turret::Unit::CM);
 
-  Turret &moveByX(double x, Unit unit = Turret::Unit::CM, bool blocking = false);
+  Turret &moveByX(double x, Unit unit = Turret::Unit::CM);
-  Turret &moveByY(double y, Unit unit = Turret::Unit::CM, bool blocking = false);
+  Turret &moveByY(double y, Unit unit = Turret::Unit::CM);
-  Turret &moveByZ(double z, Unit unit = Turret::Unit::CM, bool blocking = false);
+  Turret &moveByZ(double z, Unit unit = Turret::Unit::CM);
 
   Turret &getPosition(double &x, double &y, double &z, Unit unit);
-  Turret &getHome(double &x, double &y, Unit unit);
+  Turret &getHome(double &x, double &y, double &z, Unit unit);
-  Turret &getZero(double &x, double &y, Unit unit);
+  Turret &getZero(double &x, double &y, double &z, Unit unit);
 
-  Turret &calibrate(bool blocking = false);
+  Turret &calibrate();
 
   Turret &laserOn();
   Turret &laserOff();
 
-  Turret &nextTick();
-  Turret &flushTick();
-  bool hasTick();
-
 private:
-  vec2<kissStepper> _stepper;
+  kissStepper _stepperX;
-  vec2<long> _home;
+  kissStepper _stepperY;
-  vec2<long> _zero;
-  vec3<long> _current;
-  vec2<PinMap> _pin;
-  StepRatio _step_ratio;
-  Offset _offset;
-  ZeroOffset _zero_offset;
 
-  // tick queue related
+  int _homeX;
-  static constexpr size_t TICK_QUEUE_LENGTH = 32;
+  int _homeY;
-  using TickHandler = bool (Turret::*)();
 
-  TickHandler _tick_queue[TICK_QUEUE_LENGTH];
+  int _zeroX;
-  size_t _tick_queue_head = 0;
+  int _zeroY;
-  size_t _tick_queue_tail = 0;
 
-  bool _enqueue_tick(TickHandler fn);
+  int _currentX;
-  bool _dequeue_tick();
+  int _currentY;
-  TickHandler _current_tick();
+  int _currentZ;
 
-  bool (Turret::*_next_tick)() = nullptr;
+  int _pin_x_direction;
+  int _pin_x_pulse;
+  int _pin_x_enable;
 
-  // tick handlers
+  int _pin_y_direction;
-  bool _goto_zero_tick();
+  int _pin_y_pulse;
-  bool _goto_home_tick();
+  int _pin_y_enable;
+
+  int _pin_x_home;
+  int _pin_y_home;
+  int _pin_laser;
 };
 
 #endif

turret_debug.ino

@@ -1,34 +1,17 @@
 #include "config.h"
 #include "turret.h"
 
-Turret::PinMap pinX = {
+Turret turret(PIN_X_DIRECTION, PIN_X_PULSE, PIN_X_ENABLE, PIN_X_HOME,
-    .home = PIN_X_HOME,
+              PIN_Y_DIRECTION, PIN_Y_PULSE, PIN_Y_ENABLE, PIN_Y_HOME,
-    .direction = PIN_X_DIRECTION,
+              PIN_LASER);
-    .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().moveTo(0, 0, 0);
+  turret.laserOn().calibrate();
+  turret.moveTo(0, 0, 0);
   delay(5000);
 
   // test pointage
@@ -37,22 +20,74 @@ void setup() {
   int panelHCount = 3;
   int panelVCount = 2;
 
-  double zeroOffsetV = -21;
+  // double zeroOffsetV = -31.5;
-  double zeroOffsetH = -163;
+  double zeroOffsetV = -20.8;
-  // double zeroOffsetH = -147;
+  // double zeroOffsetH = -184.5;
+  double zeroOffsetH = -195.5;
 
   // Test constant step h,v
-  for (double step = 0; step < 160; step += 10) {
+  Serial.println("Start demo_0");
+  for (int i = 0; i < 20; i++) {
+    Serial.print("> ");
+    Serial.println(i);
+    for (double step = -198.5; step < 176.0; step += 10) {
       Serial.print("[step]: ");
       Serial.println(step);
-    turret.moveTo(step, 0, 0);
+      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) {
+
-    Serial.print("[step]: ");
+  // Test rollX
-    Serial.println(step);
+  turret.moveTo(0 * panelWidth + zeroOffsetH, 0, 0);
-    turret.moveTo(step, 0, 0);
   delay(5000);
+  turret.moveTo((panelHCount * panelWidth) / 2 + zeroOffsetH, 0, 0);
+  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();