34 lines
966 B
C++
34 lines
966 B
C++
#include "math.h"
|
|
|
|
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;
|
|
|
|
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
|
|
|
|
// x = -x; // natural axis direction
|
|
y = -y; // natural axis direction
|
|
double dX = x + offsetX;
|
|
double dY = y + offsetY;
|
|
double dZ = z + distance;
|
|
|
|
double angleY = atan2(dZ, dY) - M_PI / 2;
|
|
double angleX = atan2(dZ, dX) - M_PI / 2;
|
|
|
|
angleToStep(stepX, stepY, angleX, angleY);
|
|
}
|