feat: implement maths module

maths.cpp

@@ -0,0 +1,39 @@
+#include "math.h"
+
+//rollH atan((22.5 - 18.5) / 236) ~0.97°
+//rollV ~1°
+
+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 = 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;
+}
+
+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
+  double dX = x + offsetX;
+  double dY = y + offsetY;
+  double dZ = z + distance;
+
+  double rho = sqrt(pow(dX, 2.0) + pow(dY, 2.0) + pow(dZ, 2.0));
+  double angleY = M_PI / 2.0 - acos(dY / rho);
+  double angleX = atan2(dX, dZ);
+
+  angleToStep(stepX, stepY, angleX, angleY);    
+}