julien.oculi/technoshop.laser_turret_debug
1
0
Fork 0
Code Issues Pull requests Actions Packages Projects Releases Wiki Activity

feat: try implement roll correction for maths

Browse source
This commit is contained in:
Julien Oculi 2025-06-05 11:05:36 +02:00
parent 222fc2b172
commit b093b54598
1 changed files with 17 additions and 4 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

21
maths.cpp
View file

@ -20,6 +20,11 @@ void angleToStep(long &stepX, long &stepY, double angleX, double angleY) {
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
@ -28,12 +33,20 @@ void cartesianToPolar(long &stepX, long &stepY, double zeroX, double zeroY, doub
x = -x; //natural axis direction
double dX = x + offsetX;
double dY = y + offsetY;
double dY = rollCorrection(y + offsetY, 0.2, 400.0);
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);
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);
}