From ff568306d802cae198c170818ae447f549872c7d Mon Sep 17 00:00:00 2001 From: Julien Oculi Date: Tue, 10 Jun 2025 17:08:02 +0200 Subject: [PATCH] refactor!: use some structs to simplify and reduce code --- maths.cpp | 29 +++++----- maths.h | 9 ++- turret.cpp | 166 ++++++++++++++++++++++++----------------------------- turret.h | 76 +++++++++++++----------- 4 files changed, 133 insertions(+), 147 deletions(-) diff --git a/maths.cpp b/maths.cpp index 508b9c4..a0c7acd 100644 --- a/maths.cpp +++ b/maths.cpp @@ -1,27 +1,26 @@ #include "math.h" +#include "turret.h" double degToRad(double deg) { return deg * M_PI / 180.0; } double radToDeg(double rad) { return rad * 180.0 / M_PI; } -void angleToStep(double stepRatioX, double stepRatioY, long &stepX, long &stepY, - double angleX, double angleY) { - stepX = angleX * stepRatioX; - stepY = angleY * stepRatioY; +void angleToStep(Turret::StepRatio stepRatio, vec2 angle, vec2 &step) { + step.x = angle.x * stepRatio.x; + step.y = angle.y * stepRatio.y; } -void cartesianToPolar(double stepRatioX, double stepRatioY, double offsetX, - double offsetY, double offsetZ, long &stepX, long &stepY, - double zeroX, double zeroY, double x, double y, - double z) { +void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset, vec2 zero, vec3 position, vec2 &step) { // x = -x; // natural axis direction - y = -y; // natural axis direction - double dX = x + offsetX; - double dY = y + offsetY; - double dZ = z + offsetZ; + position.y = -position.y; // natural axis direction + double dX = position.x + offset.x; + double dY = position.y + offset.y; + double dZ = position.z + offset.z; - double angleY = atan2(dZ, dY) - M_PI / 2; - double angleX = atan2(dZ, dX) - M_PI / 2; + vec2 angle( + atan2(dZ, dY) - M_PI / 2, + atan2(dZ, dX) - M_PI / 2 + ); - angleToStep(stepRatioX, stepRatioY, stepX, stepY, angleX, angleY); + angleToStep(stepRatio, angle, step); } diff --git a/maths.h b/maths.h index d58de39..a6eb16c 100644 --- a/maths.h +++ b/maths.h @@ -1,7 +1,6 @@ +#include "turret.h" + double degToRad(double deg); double radToDeg(double rad); -void angleToStep(double stepRatioX, double stepRatioY, long &stepX, long &stepY, - double angleX, double angleY); -void cartesianToPolar(double stepRatioX, double stepRatioY, double offsetX, - double offsetY, double offsetZ, long &stepX, long &stepY, - double zeroX, double zeroY, double x, double y, double z); +void angleToStep(Turret::StepRatio stepRatio, vec2 angle, vec2 &step); +void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset, vec2 zero, vec3 position, vec2 &step); diff --git a/turret.cpp b/turret.cpp index 1b27638..7c7098e 100644 --- a/turret.cpp +++ b/turret.cpp @@ -2,53 +2,37 @@ #include "maths.h" #include -Turret::Turret(double stepRatioX, double stepRatioY, double offsetX, - double offsetY, double offsetZ, int pin_x_direction, - int pin_x_pulse, int pin_x_enable, int pin_x_home, - int pin_y_direction, int pin_y_pulse, int pin_y_enable, - int pin_y_home, int pin_laser) - : _stepperX(static_cast(pin_x_direction), - static_cast(pin_x_pulse), - static_cast(pin_x_enable)), - _stepperY(static_cast(pin_y_direction), - static_cast(pin_y_pulse), - static_cast(pin_y_enable)) { +Turret::Turret(StepRatio step_ratio, Offset offset, PinMap pin_map_x, PinMap pin_map_y) + : _stepper( + kissStepper(static_cast(pin_map_x.direction), + static_cast(pin_map_x.pulse), + static_cast(pin_map_x.enable)), + kissStepper(static_cast(pin_map_y.direction), + static_cast(pin_map_y.pulse), + static_cast(pin_map_y.enable))) { - _pin_x_direction = pin_x_direction; - _pin_x_pulse = pin_x_pulse; - _pin_x_enable = pin_x_enable; - - _pin_y_direction = pin_y_direction; - _pin_y_pulse = pin_y_pulse; - _pin_y_enable = pin_y_enable; - - _pin_x_home = pin_x_home; - _pin_y_home = pin_y_home; - _pin_laser = pin_laser; - - _step_ratio_x = stepRatioX; - _step_ratio_y = stepRatioY; - - _offset_x = offsetX; - _offset_y = offsetY; - _offset_z = offsetZ; + _pin.x = pin_map_x; + _pin.y = pin_map_y; + _step_ratio = step_ratio; + _offset = offset; } 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_pulse, OUTPUT); - pinMode(_pin_x_enable, OUTPUT); - pinMode(_pin_x_home, INPUT_PULLUP); + pinMode(_pin.x.direction, OUTPUT); + pinMode(_pin.x.pulse, OUTPUT); + pinMode(_pin.x.enable, OUTPUT); + pinMode(_pin.x.home, INPUT_PULLUP); - pinMode(_pin_y_direction, OUTPUT); - pinMode(_pin_y_pulse, OUTPUT); - pinMode(_pin_y_enable, OUTPUT); - pinMode(_pin_y_home, INPUT_PULLUP); + pinMode(_pin.y.direction, OUTPUT); + pinMode(_pin.y.pulse, OUTPUT); + pinMode(_pin.y.enable, OUTPUT); + pinMode(_pin.y.home, INPUT_PULLUP); - _stepperX.begin(); - _stepperY.begin(); + _stepper.x.begin(); + _stepper.y.begin(); return *this; } @@ -57,27 +41,27 @@ Turret &Turret::gotoHome() { long xStop = 0; long yStop = 0; - _stepperX.prepareMove(-1000000l); - _stepperY.prepareMove(-1000000l); + _stepper.x.prepareMove(-1000000l); + _stepper.y.prepareMove(-1000000l); while (true) { - _stepperX.move(); - _stepperY.move(); + _stepper.x.move(); + _stepper.y.move(); if (xStop == 2 && yStop == 2) { - _homeX = _stepperX.getPos(); - _homeY = _stepperY.getPos(); + _home.x = _stepper.x.getPos(); + _home.y = _stepper.y.getPos(); break; } - if (xStop < 2 && digitalRead(_pin_x_home)) { + if (xStop < 2 && digitalRead(_pin.x.home)) { xStop++; - _stepperX.stop(); + _stepper.x.stop(); } - if (yStop < 2 && digitalRead(_pin_y_home)) { + if (yStop < 2 && digitalRead(_pin.y.home)) { yStop++; - _stepperY.stop(); + _stepper.y.stop(); } } @@ -86,8 +70,8 @@ Turret &Turret::gotoHome() { Turret &Turret::gotoZero() { // TODO remove magic numbers - _stepperX.prepareMove(_homeX + 0); - _stepperY.prepareMove(_homeY + 2500); + _stepper.x.prepareMove(_home.x + 0); + _stepper.y.prepareMove(_home.y + 2500); bool xStop = false; bool yStop = false; @@ -96,13 +80,13 @@ Turret &Turret::gotoZero() { if (xStop && yStop) break; if (!xStop) - xStop = _stepperY.move() == STATE_STOPPED; + xStop = _stepper.y.move() == STATE_STOPPED; if (!yStop) - yStop = _stepperX.move() == STATE_STOPPED; + yStop = _stepper.x.move() == STATE_STOPPED; } - _zeroX = _stepperX.getPos(); - _zeroY = _stepperY.getPos(); + _zero.x = _stepper.x.getPos(); + _zero.y = _stepper.y.getPos(); return *this; } @@ -114,43 +98,39 @@ Turret &Turret::calibrate() { } Turret &Turret::moveTo(double x, double y, double z, Unit unit) { - long stepX; - long stepY; + vec2 step; if (unit == Unit::MM) { - cartesianToPolar(_step_ratio_x, _step_ratio_y, _offset_x, _offset_y, - _offset_z, stepX, stepY, _zeroX, _zeroY, x / 10.0, - y / 10.0, z / 10.0); + vec3 position(x / 10.0, y / 10.0, z / 10.0); + cartesianToPolar(_step_ratio, _offset, _zero, position, step); } if (unit == Unit::CM) { - cartesianToPolar(_step_ratio_x, _step_ratio_y, _offset_x, _offset_y, - _offset_z, stepX, stepY, _zeroX, _zeroY, x, y, z); + vec3 position(x, y, z); + cartesianToPolar(_step_ratio, _offset, _zero, position, step); } if (unit == Unit::M) { - cartesianToPolar(_step_ratio_x, _step_ratio_y, _offset_x, _offset_y, - _offset_z, stepX, stepY, _zeroX, _zeroY, x * 100.0, - y * 100.0, z * 100.0); + vec3 position(x * 100.0, y * 100.0, z * 100.0); + cartesianToPolar(_step_ratio, _offset, _zero, position, step); } if (unit == Unit::STEP) { - stepX = x; - stepY = y; + step.x = x; + step.y = y; } if (unit == Unit::RAD) { - angleToStep(_step_ratio_x, _step_ratio_y, stepX, stepY, x, y); + angleToStep(_step_ratio, vec2 (x, y), step); } if (unit == Unit::DEG) { - angleToStep(_step_ratio_x, _step_ratio_y, stepX, stepY, degToRad(x), - degToRad(y)); + angleToStep(_step_ratio, vec2 (degToRad(x), degToRad(y)), step); } // TODO min(valueI, -_homeI); - _stepperX.prepareMove(_zeroX + stepX); - _stepperY.prepareMove(_zeroY + stepY); + _stepper.x.prepareMove(_zero.x + step.x); + _stepper.y.prepareMove(_zero.y + step.y); bool xStop = false; bool yStop = false; @@ -159,47 +139,47 @@ Turret &Turret::moveTo(double x, double y, double z, Unit unit) { if (xStop && yStop) break; if (!xStop) - xStop = _stepperX.move() == STATE_STOPPED; + xStop = _stepper.x.move() == STATE_STOPPED; if (!yStop) - yStop = _stepperY.move() == STATE_STOPPED; + yStop = _stepper.y.move() == STATE_STOPPED; } - _currentX = x; - _currentY = y; - _currentZ = z; + _current.x = x; + _current.y = y; + _current.z = z; return *this; } Turret &Turret::moveBy(double x, double y, double z, Unit unit) { - long zeroXStored = _zeroX; - long zeroYStored = _zeroY; + long zeroXStored = _zero.x; + long zeroYStored = _zero.y; - _zeroX += _stepperX.getPos(); - _zeroY += _stepperY.getPos(); + _zero.x += _stepper.x.getPos(); + _zero.y += _stepper.y.getPos(); moveTo(x, y, z, unit); - _zeroX = zeroXStored; - _zeroY = zeroYStored; + _zero.x = zeroXStored; + _zero.y = zeroYStored; - _currentX = x; - _currentY = y; - _currentZ = z; + _current.x = x; + _current.y = y; + _current.z = z; return *this; } Turret &Turret::moveToX(double x, Unit unit) { - return moveTo(x, _currentY, _currentZ, unit); + return moveTo(x, _current.y, _current.z, unit); } Turret &Turret::moveToY(double y, Unit unit) { - return moveTo(_currentX, y, _currentZ, unit); + return moveTo(_current.x, y, _current.z, unit); } Turret &Turret::moveToZ(double z, Unit unit) { - return moveTo(_currentX, _currentY, z, unit); + return moveTo(_current.x, _current.y, z, unit); } Turret &Turret::moveByX(double x, Unit unit) { return moveBy(x, 0, 0, unit); } @@ -224,11 +204,13 @@ Turret &Turret::getZero(double &x, double &y, double &z, Unit unit) { } 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; } diff --git a/turret.h b/turret.h index 4284743..b50ba69 100644 --- a/turret.h +++ b/turret.h @@ -3,14 +3,44 @@ #include +template +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 +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(double stepRatioX, double stepRatioY, double offsetX, double offsetY, - double offsetZ, int pin_x_direction, int pin_x_pulse, int pin_x_enable, - int pin_x_home, int pin_y_direction, int pin_y_pulse, int pin_y_enable, - int pin_y_home, int pin_laser); + using StepRatio = vec2; + using Offset = vec3; + //x -> turret to screen + //y -> laser to stand + // z -> unused + + struct PinMap { + int home; + int direction; + int pulse; + int enable; + int laser; + }; + + Turret(StepRatio step_ratio, Offset offset, PinMap pin_map_x, PinMap pin_map_y); Turret &init(); Turret &gotoHome(); @@ -37,37 +67,13 @@ public: Turret &laserOff(); private: - kissStepper _stepperX; - kissStepper _stepperY; - - long _homeX; - long _homeY; - - long _zeroX; - long _zeroY; - - long _currentX; - long _currentY; - long _currentZ; - - int _pin_x_direction; - int _pin_x_pulse; - int _pin_x_enable; - - int _pin_y_direction; - int _pin_y_pulse; - int _pin_y_enable; - - int _pin_x_home; - int _pin_y_home; - int _pin_laser; - - double _step_ratio_x; - double _step_ratio_y; - - double _offset_x; // turret to screen - double _offset_y; // laser to stand - double _offset_z; // unused + vec2 _stepper; + vec2 _home; + vec2 _zero; + vec3 _current; + vec2 _pin; + StepRatio _step_ratio; + Offset _offset; }; #endif