diff --git a/maths.cpp b/maths.cpp index 5309b0d..8cef301 100644 --- a/maths.cpp +++ b/maths.cpp @@ -1,21 +1,17 @@ #include "math.h" -//rollH atan((22.5 - 18.5) / 236) ~0.97° -//rollV ~1° +// rollH atan((22.5 - 18.5) / 236) ~0.97° +// rollV ~1° -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(long &stepX, long &stepY, double angleX, double angleY) { - //TODO remove magic numbers - double stepRatioX = M_PI / ((51.2 / 15.0) * (100.0 * 64.0)); - double stepRatioY = M_PI / ((48.0 / 15.0) * (100.0 * 64.0)); - + // TODO remove magic numbers + double stepRatioX = M_PI / ((51.2 / 15.0) * (100.0 * 64.0)); + double stepRatioY = M_PI / ((48.0 / 15.0) * (100.0 * 64.0)); + stepX = angleX / stepRatioX; stepY = angleY / stepRatioY; } @@ -25,28 +21,28 @@ double rollCorrection(double x, double max, double width) { return x - correction; } -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(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 - x = -x; //natural axis direction + x = -x; // natural axis direction double dX = x + offsetX; double dY = rollCorrection(y + offsetY, 0.2, 400.0); double dZ = z + distance; - double roll = atan2(4.0, 236.0); double rollY = dX * tan(roll); double rollX = dY * tan(roll); double dXr = dX + rollX; double dYr = dY + rollY; - + double rho = sqrt(pow(dXr, 2.0) + pow(dYr, 2.0) + pow(dZ, 2.0)); double angleY = M_PI / 2.0 - acos(dYr / rho); double angleX = atan2(dXr, dZ); - angleToStep(stepX, stepY, angleX, angleY); + angleToStep(stepX, stepY, angleX, angleY); } diff --git a/maths.h b/maths.h index 7bc9e63..9ed3197 100644 --- a/maths.h +++ b/maths.h @@ -1,4 +1,5 @@ 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 cartesianToPolar(long &stepX, long &stepY, double zeroX, double zeroY, + double x, double y, double z); diff --git a/turret.cpp b/turret.cpp index 2ccbad2..fe5d4e3 100644 --- a/turret.cpp +++ b/turret.cpp @@ -1,12 +1,13 @@ -#include -#include "config.h" #include "turret.h" +#include "config.h" #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) : _stepperX(pin_x_direction, pin_x_pulse, pin_x_enable), _stepperY(pin_y_direction, pin_y_pulse, pin_y_enable) { +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) + : _stepperX(pin_x_direction, pin_x_pulse, pin_x_enable), + _stepperY(pin_y_direction, pin_y_pulse, pin_y_enable) { _pin_x_direction = pin_x_direction; _pin_x_pulse = pin_x_pulse; @@ -21,7 +22,7 @@ Turret::Turret( _pin_laser = pin_laser; } -Turret& Turret::init() { +Turret &Turret::init() { pinMode(_pin_laser, OUTPUT); pinMode(_pin_x_direction, OUTPUT); @@ -40,7 +41,7 @@ Turret& Turret::init() { return *this; } -Turret& Turret::gotoHome() { +Turret &Turret::gotoHome() { long xStop = 0; long yStop = 0; @@ -71,8 +72,8 @@ Turret& Turret::gotoHome() { return *this; } -Turret& Turret::gotoZero() { - //TODO remove magic numbers +Turret &Turret::gotoZero() { + // TODO remove magic numbers _stepperX.prepareMove(_homeX + 9300); _stepperY.prepareMove(_homeY + 3500); @@ -80,9 +81,12 @@ Turret& Turret::gotoZero() { bool yStop = false; while (true) { - if (xStop && yStop) break; - if (!xStop) xStop = _stepperY.move() == STATE_STOPPED; - if (!yStop) yStop = _stepperX.move() == STATE_STOPPED; + if (xStop && yStop) + break; + if (!xStop) + xStop = _stepperY.move() == STATE_STOPPED; + if (!yStop) + yStop = _stepperX.move() == STATE_STOPPED; } _zeroX = _stepperX.getPos(); @@ -91,18 +95,19 @@ Turret& Turret::gotoZero() { return *this; } -Turret& Turret::calibrate() { +Turret &Turret::calibrate() { gotoHome(); gotoZero(); return *this; } -Turret& Turret::moveTo(double x, double y, double z, Unit unit) { +Turret &Turret::moveTo(double x, double y, double z, Unit unit) { long stepX; long stepY; if (unit == Unit::MM) { - cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x / 10.0, y / 10.0, z / 10.0); + cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x / 10.0, y / 10.0, + z / 10.0); } if (unit == Unit::CM) { @@ -110,7 +115,8 @@ Turret& Turret::moveTo(double x, double y, double z, Unit unit) { } if (unit == Unit::M) { - cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x * 100.0, y * 100.0, z * 100.0); + cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x * 100.0, y * 100.0, + z * 100.0); } if (unit == Unit::STEP) { @@ -126,7 +132,7 @@ Turret& Turret::moveTo(double x, double y, double z, Unit unit) { angleToStep(stepX, stepY, degToRad(x), degToRad(y)); } - //TODO min(valueI, -_homeI); + // TODO min(valueI, -_homeI); _stepperX.prepareMove(_zeroX + stepX); _stepperY.prepareMove(_zeroY + stepY); @@ -134,11 +140,14 @@ Turret& Turret::moveTo(double x, double y, double z, Unit unit) { bool yStop = false; while (true) { - if (xStop && yStop) break; - if (!xStop) xStop = _stepperX.move() == STATE_STOPPED; - if (!yStop) yStop = _stepperY.move() == STATE_STOPPED; + if (xStop && yStop) + break; + if (!xStop) + xStop = _stepperX.move() == STATE_STOPPED; + if (!yStop) + yStop = _stepperY.move() == STATE_STOPPED; } - + _currentX = x; _currentY = y; _currentZ = z; @@ -146,7 +155,7 @@ Turret& Turret::moveTo(double x, double y, double z, Unit unit) { 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 zeroYStored = _zeroY; @@ -165,51 +174,45 @@ Turret& Turret::moveBy(double x, double y, double z, Unit unit) { return *this; } -Turret& Turret::moveToX(double x, Unit unit) { +Turret &Turret::moveToX(double x, Unit unit) { return moveTo(x, _currentY, _currentZ, unit); } -Turret& Turret::moveToY(double y, Unit unit) { +Turret &Turret::moveToY(double y, Unit unit) { return moveTo(_currentX, y, _currentZ, unit); } -Turret& Turret::moveToZ(double z, Unit unit) { +Turret &Turret::moveToZ(double z, Unit unit) { return moveTo(_currentX, _currentY, 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); } -Turret& Turret::moveByY(double y, Unit unit) { - return moveTo(0, y, 0, unit); -} +Turret &Turret::moveByY(double y, Unit unit) { return moveTo(0, y, 0, unit); } -Turret& Turret::moveByZ(double z, Unit unit) { - return moveTo(0, 0, z, unit); -} +Turret &Turret::moveByZ(double z, Unit unit) { return moveTo(0, 0, z, unit); } -Turret& Turret::getPosition(double &x, double &y, double &z, Unit unit) { - //TODO implement +Turret &Turret::getPosition(double &x, double &y, double &z, Unit unit) { + // TODO implement return *this; } -Turret& Turret::getHome(double &x, double &y, double &z, Unit unit) { - //TODO implement +Turret &Turret::getHome(double &x, double &y, double &z, Unit unit) { + // TODO implement return *this; } -Turret& Turret::getZero(double &x, double &y, double &z, Unit unit) { - //TODO implement +Turret &Turret::getZero(double &x, double &y, double &z, Unit unit) { + // TODO implement return *this; } -Turret& Turret::laserOn() { +Turret &Turret::laserOn() { digitalWrite(_pin_laser, HIGH); return *this; } -Turret& Turret::laserOff() { +Turret &Turret::laserOff() { digitalWrite(_pin_laser, LOW); return *this; } diff --git a/turret.h b/turret.h index 8271913..d2547df 100644 --- a/turret.h +++ b/turret.h @@ -4,49 +4,38 @@ #include class Turret { - public: +public: + enum Unit { MM, CM, M, RAD, DEG, STEP }; - 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( - 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(); + Turret &gotoZero(); - Turret& init(); - Turret& gotoHome(); - Turret& gotoZero(); + 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); - 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); + Turret &moveToX(double x, Unit unit = Turret::Unit::CM); + Turret &moveToY(double y, Unit unit = Turret::Unit::CM); + Turret &moveToZ(double z, Unit unit = Turret::Unit::CM); - Turret& moveToX(double x, Unit unit = Turret::Unit::CM); - Turret& moveToY(double y, Unit unit = Turret::Unit::CM); - Turret& moveToZ(double z, Unit unit = Turret::Unit::CM); + Turret &moveByX(double x, Unit unit = Turret::Unit::CM); + Turret &moveByY(double y, Unit unit = Turret::Unit::CM); + Turret &moveByZ(double z, Unit unit = Turret::Unit::CM); - Turret& moveByX(double x, Unit unit = Turret::Unit::CM); - Turret& moveByY(double y, Unit unit = Turret::Unit::CM); - 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, double &z, Unit unit); + Turret &getZero(double &x, double &y, double &z, Unit unit); - Turret& getPosition(double &x, double &y, double &z, Unit unit); - Turret& getHome(double &x, double &y, double &z, Unit unit); - Turret& getZero(double &x, double &y, double &z, Unit unit); + Turret &calibrate(); - Turret& calibrate(); - - Turret& laserOn(); - Turret& laserOff(); - - private: + Turret &laserOn(); + Turret &laserOff(); +private: kissStepper _stepperX; kissStepper _stepperY; diff --git a/turret_debug.ino b/turret_debug.ino index 528fc7f..f3bcad0 100644 --- a/turret_debug.ino +++ b/turret_debug.ino @@ -1,11 +1,9 @@ #include "config.h" #include "turret.h" -Turret turret( - PIN_X_DIRECTION, PIN_X_PULSE, PIN_X_ENABLE, PIN_X_HOME, - PIN_Y_DIRECTION, PIN_Y_PULSE, PIN_Y_ENABLE, PIN_Y_HOME, - PIN_LASER -); +Turret turret(PIN_X_DIRECTION, PIN_X_PULSE, PIN_X_ENABLE, PIN_X_HOME, + PIN_Y_DIRECTION, PIN_Y_PULSE, PIN_Y_ENABLE, PIN_Y_HOME, + PIN_LASER); void setup() { turret.init(); @@ -70,7 +68,7 @@ void setup() { 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) { @@ -95,6 +93,4 @@ void setup() { turret.gotoZero(); } -void loop() { - -} +void loop() {}