53 lines
1.4 KiB
C++
53 lines
1.4 KiB
C++
#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;
|
|
}
|
|
|
|
double rollCorrection(double x, double max, double width) {
|
|
double correction = max * (1.0 - cos((10.0 * x) / (width * M_PI))) / 2.0;
|
|
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
|
|
|
|
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);
|
|
}
|