refactor!: use some structs to simplify and reduce code
This commit is contained in:
parent
cf88273869
commit
ff568306d8
29
maths.cpp
29
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<double> angle, vec2<long> &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<long> zero, vec3<double> position, vec2<long> &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<double> angle(
|
||||
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
9
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<double> angle, vec2<long> &step);
|
||||
void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset, vec2<long> zero, vec3<double> position, vec2<long> &step);
|
||||
|
|
|
|||
166
turret.cpp
166
turret.cpp
|
|
@ -2,53 +2,37 @@
|
|||
#include "maths.h"
|
||||
#include <kissStepper.h>
|
||||
|
||||
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<uint8_t>(pin_x_direction),
|
||||
static_cast<uint8_t>(pin_x_pulse),
|
||||
static_cast<uint8_t>(pin_x_enable)),
|
||||
_stepperY(static_cast<uint8_t>(pin_y_direction),
|
||||
static_cast<uint8_t>(pin_y_pulse),
|
||||
static_cast<uint8_t>(pin_y_enable)) {
|
||||
Turret::Turret(StepRatio step_ratio, Offset offset, PinMap pin_map_x, PinMap pin_map_y)
|
||||
: _stepper(
|
||||
kissStepper(static_cast<uint8_t>(pin_map_x.direction),
|
||||
static_cast<uint8_t>(pin_map_x.pulse),
|
||||
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_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<long> 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<double> 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<double> 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<double> 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<double> (x, y), step);
|
||||
}
|
||||
|
||||
if (unit == Unit::DEG) {
|
||||
angleToStep(_step_ratio_x, _step_ratio_y, stepX, stepY, degToRad(x),
|
||||
degToRad(y));
|
||||
angleToStep(_step_ratio, vec2<double> (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;
|
||||
}
|
||||
|
|
|
|||
76
turret.h
76
turret.h
|
|
@ -3,14 +3,44 @@
|
|||
|
||||
#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(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<double>;
|
||||
using Offset = vec3<double>;
|
||||
//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<kissStepper> _stepper;
|
||||
vec2<long> _home;
|
||||
vec2<long> _zero;
|
||||
vec3<long> _current;
|
||||
vec2<PinMap> _pin;
|
||||
StepRatio _step_ratio;
|
||||
Offset _offset;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in a new issue