julien.oculi/technoshop.laser_turret_debug
1
0
Fork 0
Code Issues Pull requests Actions Packages Projects Releases Wiki Activity

refactor!: use some structs to simplify and reduce code

Browse source
This commit is contained in:
Julien Oculi 2025-06-10 17:08:02 +02:00
parent cf88273869
commit ff568306d8
4 changed files with 133 additions and 147 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

29
maths.cpp
View file

@ -1,27 +1,26 @@
#include "math.h" #include "math.h"
#include "turret.h"
double degToRad(double deg) { return deg * M_PI / 180.0; } double degToRad(double deg) { return deg * M_PI / 180.0; }
double radToDeg(double rad) { return rad * 180.0 / M_PI; } double radToDeg(double rad) { return rad * 180.0 / M_PI; }
void angleToStep(double stepRatioX, double stepRatioY, long &stepX, long &stepY, void angleToStep(Turret::StepRatio stepRatio, vec2<double> angle, vec2<long> &step) {
double angleX, double angleY) { step.x = angle.x * stepRatio.x;
stepX = angleX * stepRatioX; step.y = angle.y * stepRatio.y;
stepY = angleY * stepRatioY;
} }
void cartesianToPolar(double stepRatioX, double stepRatioY, double offsetX, void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset, vec2<long> zero, vec3<double> position, vec2<long> &step) {
double offsetY, double offsetZ, long &stepX, long &stepY,
double zeroX, double zeroY, double x, double y,
double z) {
// x = -x; // natural axis direction // x = -x; // natural axis direction
y = -y; // natural axis direction position.y = -position.y; // natural axis direction
double dX = x + offsetX; double dX = position.x + offset.x;
double dY = y + offsetY; double dY = position.y + offset.y;
double dZ = z + offsetZ; double dZ = position.z + offset.z;
double angleY = atan2(dZ, dY) - M_PI / 2; vec2<double> angle(
double angleX = atan2(dZ, dX) - M_PI / 2; atan2(dZ, dY) - M_PI / 2,
atan2(dZ, dX) - M_PI / 2
);
angleToStep(stepRatioX, stepRatioY, stepX, stepY, angleX, angleY); angleToStep(stepRatio, angle, step);
} }

9
maths.h
View file

@ -1,7 +1,6 @@
#include "turret.h"
double degToRad(double deg); double degToRad(double deg);
double radToDeg(double rad); double radToDeg(double rad);
void angleToStep(double stepRatioX, double stepRatioY, long &stepX, long &stepY, void angleToStep(Turret::StepRatio stepRatio, vec2<double> angle, vec2<long> &step);
double angleX, double angleY); void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset, vec2<long> zero, vec3<double> position, vec2<long> &step);
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);

166
turret.cpp
View file

@ -2,53 +2,37 @@
#include "maths.h" #include "maths.h"
#include <kissStepper.h> #include <kissStepper.h>
Turret::Turret(double stepRatioX, double stepRatioY, double offsetX, Turret::Turret(StepRatio step_ratio, Offset offset, PinMap pin_map_x, PinMap pin_map_y)
double offsetY, double offsetZ, int pin_x_direction, : _stepper(
int pin_x_pulse, int pin_x_enable, int pin_x_home, kissStepper(static_cast<uint8_t>(pin_map_x.direction),
int pin_y_direction, int pin_y_pulse, int pin_y_enable, static_cast<uint8_t>(pin_map_x.pulse),
int pin_y_home, int pin_laser) static_cast<uint8_t>(pin_map_x.enable)),
: _stepperX(static_cast<uint8_t>(pin_x_direction), kissStepper(static_cast<uint8_t>(pin_map_y.direction),
static_cast<uint8_t>(pin_x_pulse), static_cast<uint8_t>(pin_map_y.pulse),
static_cast<uint8_t>(pin_x_enable)), static_cast<uint8_t>(pin_map_y.enable))) {
_stepperY(static_cast<uint8_t>(pin_y_direction),
static_cast<uint8_t>(pin_y_pulse),
static_cast<uint8_t>(pin_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;
_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;
} }
Turret &Turret::init() { 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; return *this;
} }
@ -57,27 +41,27 @@ Turret &Turret::gotoHome() {
long xStop = 0; long xStop = 0;
long yStop = 0; long yStop = 0;
_stepperX.prepareMove(-1000000l); _stepper.x.prepareMove(-1000000l);
_stepperY.prepareMove(-1000000l); _stepper.y.prepareMove(-1000000l);
while (true) { while (true) {
_stepperX.move(); _stepper.x.move();
_stepperY.move(); _stepper.y.move();
if (xStop == 2 && yStop == 2) { if (xStop == 2 && yStop == 2) {
_homeX = _stepperX.getPos(); _home.x = _stepper.x.getPos();
_homeY = _stepperY.getPos(); _home.y = _stepper.y.getPos();
break; break;
} }
if (xStop < 2 && digitalRead(_pin_x_home)) { if (xStop < 2 && digitalRead(_pin.x.home)) {
xStop++; xStop++;
_stepperX.stop(); _stepper.x.stop();
} }
if (yStop < 2 && digitalRead(_pin_y_home)) { if (yStop < 2 && digitalRead(_pin.y.home)) {
yStop++; yStop++;
_stepperY.stop(); _stepper.y.stop();
} }
} }
@ -86,8 +70,8 @@ Turret &Turret::gotoHome() {
Turret &Turret::gotoZero() { Turret &Turret::gotoZero() {
// TODO remove magic numbers // TODO remove magic numbers
_stepperX.prepareMove(_homeX + 0); _stepper.x.prepareMove(_home.x + 0);
_stepperY.prepareMove(_homeY + 2500); _stepper.y.prepareMove(_home.y + 2500);
bool xStop = false; bool xStop = false;
bool yStop = false; bool yStop = false;
@ -96,13 +80,13 @@ Turret &Turret::gotoZero() {
if (xStop && yStop) if (xStop && yStop)
break; break;
if (!xStop) if (!xStop)
xStop = _stepperY.move() == STATE_STOPPED; xStop = _stepper.y.move() == STATE_STOPPED;
if (!yStop) if (!yStop)
yStop = _stepperX.move() == STATE_STOPPED; yStop = _stepper.x.move() == STATE_STOPPED;
} }
_zeroX = _stepperX.getPos(); _zero.x = _stepper.x.getPos();
_zeroY = _stepperY.getPos(); _zero.y = _stepper.y.getPos();
return *this; return *this;
} }
@ -114,43 +98,39 @@ Turret &Turret::calibrate() {
} }
Turret &Turret::moveTo(double x, double y, double z, Unit unit) { Turret &Turret::moveTo(double x, double y, double z, Unit unit) {
long stepX; vec2<long> step;
long stepY;
if (unit == Unit::MM) { if (unit == Unit::MM) {
cartesianToPolar(_step_ratio_x, _step_ratio_y, _offset_x, _offset_y, vec3<double> position(x / 10.0, y / 10.0, z / 10.0);
_offset_z, stepX, stepY, _zeroX, _zeroY, x / 10.0, cartesianToPolar(_step_ratio, _offset, _zero, position, step);
y / 10.0, z / 10.0);
} }
if (unit == Unit::CM) { if (unit == Unit::CM) {
cartesianToPolar(_step_ratio_x, _step_ratio_y, _offset_x, _offset_y, vec3<double> position(x, y, z);
_offset_z, stepX, stepY, _zeroX, _zeroY, x, y, z); cartesianToPolar(_step_ratio, _offset, _zero, position, step);
} }
if (unit == Unit::M) { if (unit == Unit::M) {
cartesianToPolar(_step_ratio_x, _step_ratio_y, _offset_x, _offset_y, vec3<double> position(x * 100.0, y * 100.0, z * 100.0);
_offset_z, stepX, stepY, _zeroX, _zeroY, x * 100.0, cartesianToPolar(_step_ratio, _offset, _zero, position, step);
y * 100.0, z * 100.0);
} }
if (unit == Unit::STEP) { if (unit == Unit::STEP) {
stepX = x; step.x = x;
stepY = y; step.y = y;
} }
if (unit == Unit::RAD) { if (unit == Unit::RAD) {
angleToStep(_step_ratio_x, _step_ratio_y, stepX, stepY, x, y); angleToStep(_step_ratio, vec2<double> (x, y), step);
} }
if (unit == Unit::DEG) { if (unit == Unit::DEG) {
angleToStep(_step_ratio_x, _step_ratio_y, stepX, stepY, degToRad(x), angleToStep(_step_ratio, vec2<double> (degToRad(x), degToRad(y)), step);
degToRad(y));
} }
// TODO min(valueI, -_homeI); // TODO min(valueI, -_homeI);
_stepperX.prepareMove(_zeroX + stepX); _stepper.x.prepareMove(_zero.x + step.x);
_stepperY.prepareMove(_zeroY + stepY); _stepper.y.prepareMove(_zero.y + step.y);
bool xStop = false; bool xStop = false;
bool yStop = false; bool yStop = false;
@ -159,47 +139,47 @@ Turret &Turret::moveTo(double x, double y, double z, Unit unit) {
if (xStop && yStop) if (xStop && yStop)
break; break;
if (!xStop) if (!xStop)
xStop = _stepperX.move() == STATE_STOPPED; xStop = _stepper.x.move() == STATE_STOPPED;
if (!yStop) if (!yStop)
yStop = _stepperY.move() == STATE_STOPPED; yStop = _stepper.y.move() == STATE_STOPPED;
} }
_currentX = x; _current.x = x;
_currentY = y; _current.y = y;
_currentZ = z; _current.z = z;
return *this; return *this;
} }
Turret &Turret::moveBy(double x, double y, double z, Unit unit) { Turret &Turret::moveBy(double x, double y, double z, Unit unit) {
long zeroXStored = _zeroX; long zeroXStored = _zero.x;
long zeroYStored = _zeroY; long zeroYStored = _zero.y;
_zeroX += _stepperX.getPos(); _zero.x += _stepper.x.getPos();
_zeroY += _stepperY.getPos(); _zero.y += _stepper.y.getPos();
moveTo(x, y, z, unit); moveTo(x, y, z, unit);
_zeroX = zeroXStored; _zero.x = zeroXStored;
_zeroY = zeroYStored; _zero.y = zeroYStored;
_currentX = x; _current.x = x;
_currentY = y; _current.y = y;
_currentZ = z; _current.z = z;
return *this; return *this;
} }
Turret &Turret::moveToX(double x, Unit unit) { 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) { 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) { 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); } 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() { Turret &Turret::laserOn() {
digitalWrite(_pin_laser, HIGH); digitalWrite(_pin.x.laser, HIGH);
digitalWrite(_pin.y.laser, HIGH);
return *this; return *this;
} }
Turret &Turret::laserOff() { Turret &Turret::laserOff() {
digitalWrite(_pin_laser, LOW); digitalWrite(_pin.x.laser, LOW);
digitalWrite(_pin.y.laser, LOW);
return *this; return *this;
} }

76
turret.h
View file

@ -3,14 +3,44 @@
#include <kissStepper.h> #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 { class Turret {
public: public:
enum Unit { MM, CM, M, RAD, DEG, STEP }; enum Unit { MM, CM, M, RAD, DEG, STEP };
Turret(double stepRatioX, double stepRatioY, double offsetX, double offsetY, using StepRatio = vec2<double>;
double offsetZ, int pin_x_direction, int pin_x_pulse, int pin_x_enable, using Offset = vec3<double>;
int pin_x_home, int pin_y_direction, int pin_y_pulse, int pin_y_enable, //x -> turret to screen
int pin_y_home, int pin_laser); //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 &init();
Turret &gotoHome(); Turret &gotoHome();
@ -37,37 +67,13 @@ public:
Turret &laserOff(); Turret &laserOff();
private: private:
kissStepper _stepperX; vec2<kissStepper> _stepper;
kissStepper _stepperY; vec2<long> _home;
vec2<long> _zero;
long _homeX; vec3<long> _current;
long _homeY; vec2<PinMap> _pin;
StepRatio _step_ratio;
long _zeroX; Offset _offset;
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
}; };
#endif #endif