diff --git a/maths.cpp b/maths.cpp index 4acb7a5..508b9c4 100644 --- a/maths.cpp +++ b/maths.cpp @@ -4,30 +4,24 @@ 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) { - // TODO remove magic numbers - double stepRatioX = 6000 / M_PI; - double stepRatioY = 6500 / M_PI; - +void angleToStep(double stepRatioX, double stepRatioY, long &stepX, long &stepY, + double angleX, double angleY) { stepX = angleX * stepRatioX; stepY = angleY * stepRatioY; } -void cartesianToPolar(long &stepX, long &stepY, double zeroX, double zeroY, - double x, double y, double z) { - // TODO remove magic numbers - double distance = 166.0; // turret to screen - double offsetY = 21.0; // laser to stand - double offsetX = 0.0; // unused - +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) { // x = -x; // natural axis direction y = -y; // natural axis direction double dX = x + offsetX; double dY = y + offsetY; - double dZ = z + distance; + double dZ = z + offsetZ; double angleY = atan2(dZ, dY) - M_PI / 2; double angleX = atan2(dZ, dX) - M_PI / 2; - angleToStep(stepX, stepY, angleX, angleY); + angleToStep(stepRatioX, stepRatioY, stepX, stepY, angleX, angleY); } diff --git a/maths.h b/maths.h index 9ed3197..d58de39 100644 --- a/maths.h +++ b/maths.h @@ -1,5 +1,7 @@ double degToRad(double deg); double radToDeg(double rad); -void angleToStep(long &stepX, long &stepY, double angleX, double angleY); -void cartesianToPolar(long &stepX, long &stepY, double zeroX, double zeroY, - double x, double y, double z); +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); diff --git a/turret.cpp b/turret.cpp index 4fb37d1..64f8799 100644 --- a/turret.cpp +++ b/turret.cpp @@ -3,9 +3,11 @@ #include "maths.h" #include -Turret::Turret(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) +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(pin_x_direction, pin_x_pulse, pin_x_enable), _stepperY(pin_y_direction, pin_y_pulse, pin_y_enable) { @@ -20,6 +22,13 @@ Turret::Turret(int pin_x_direction, int pin_x_pulse, int pin_x_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() { @@ -106,17 +115,20 @@ Turret &Turret::moveTo(double x, double y, double z, Unit unit) { long stepY; if (unit == Unit::MM) { - cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x / 10.0, y / 10.0, - z / 10.0); + 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); } if (unit == Unit::CM) { - cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x, y, z); + cartesianToPolar(_step_ratio_x, _step_ratio_y, _offset_x, _offset_y, + _offset_z, stepX, stepY, _zeroX, _zeroY, x, y, z); } if (unit == Unit::M) { - cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x * 100.0, y * 100.0, - z * 100.0); + 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); } if (unit == Unit::STEP) { @@ -125,11 +137,12 @@ Turret &Turret::moveTo(double x, double y, double z, Unit unit) { } if (unit == Unit::RAD) { - angleToStep(stepX, stepY, x, y); + angleToStep(_step_ratio_x, _step_ratio_y, stepX, stepY, x, y); } if (unit == Unit::DEG) { - angleToStep(stepX, stepY, degToRad(x), degToRad(y)); + angleToStep(_step_ratio_x, _step_ratio_y, stepX, stepY, degToRad(x), + degToRad(y)); } // TODO min(valueI, -_homeI); diff --git a/turret.h b/turret.h index 042a4be..4284743 100644 --- a/turret.h +++ b/turret.h @@ -7,9 +7,10 @@ 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, - int pin_y_direction, int pin_y_pulse, int pin_y_enable, int pin_y_home, - int pin_laser); + 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); Turret &init(); Turret &gotoHome(); @@ -60,6 +61,13 @@ private: 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