Arduino calculating angle accuracy problem? - c

I'm trying to calculate the angle of a point I'm measuring.
Using: https://www.calculator.net/triangle-calculator.html?vc=90&vx=50&vy=50&va=&vz=&vb=&angleunits=d&x=101&y=9
My a = 50, b = 50 and c = unknown (varies along with a), I am currently testing it with a fixed distance of 50.
See the link for visualisation, it will probably help a lot.
I am using the following code:
float calculateAngle(float distance)
{
float a = TSThorizontalDistanceToProjector - (distance + TSTdistanceFromFrontToProjection);
float b = TSTverticalDistanceToProjector;
float C = 90;
float c = sqrt(pow(TSTverticalDistanceToProjector, 2) + pow(a,2) - ((2 * b * a) * cos(C)));
Serial.println("float c is: ");
Serial.println(c);
float A = acos((pow(c,2) + pow(b,2) - pow(a,2)) / (2 * c * b));
Serial.println("float A is: ");
Serial.println(A);
}
I first calculate c so I can calculate the A angle. I only want the A angle however, so if I can calculate it right away please say so. The problem with this code is however that it outputs the wrong numbers.
According to the site linked above angle A should be: 45 degrees and side c should be 70.711. My code outputs angle A = 0.40 and side C = 68.88. Both are wrong, am I using the formula incorrectly? Or does it have to do something with the variables I'm using?
EDIT:
I'm now using the following code:
float calculateAngle(float distance)
{
float a = TSThorizontalDistanceToProjector - (distance + TSTdistanceFromFrontToProjection); //120 - (70 + 20) = 30
Serial.println(a);
float b = TSTverticalDistanceToProjector;
float C = 90;
float c = sqrt(pow(a,2) + pow(b,2));
Serial.println("float c: ");
Serial.println(c);
float tanX = (a / b);
Serial.println("float tanX is: ");
Serial.println(tanX);
float tanXresult = atan(tanX);
Serial.println("float tanXresult: ");
Serial.println(tanXresult);
}
I also saw that a = 30 and not 50 but b still is 50.
I can now calculate c with enough accuracy, the angle of A is still a problem though.

Don't use degrees as input to trigonmic functions. Use radians!
Also if a and b join at 90° and you know a and b, c is simply sqrt(a^2+b^2) as
c^2 = a^2 + b^2
Not sure what that cos is supposed to do here.
As you already know a and b you can simply calculate A via the arcustangens or a/b. You don't need c for that.
I suggest you revisit basic trigonometry.

I had to convert my atan from radians to degrees.
float calculateAngle(float distance)
{
float a = TSThorizontalDistanceToProjector - (distance + TSTdistanceFromFrontToProjection); //120 - (70 + 20) = 30
//Serial.println(a);
float b = TSTverticalDistanceToProjector;
float C = 90;
float c = sqrt(pow(a,2) + pow(b,2));
Serial.println("float c: ");
Serial.println(c);
float tanX = (a / b);
Serial.println("float tanX is: ");
Serial.println(tanX);
float tanXresult = ((atan(tanX)) * 180) / Pi;
Serial.println("float tanXresult: ");
Serial.println(tanXresult);
}
I did that with:
float tanXresult = ((atan(tanX)) * 180) / Pi;
printed result: 30.96 degrees, which is correct!

Related

Roots of equation in C

I am relatively new to C, and am trying to improve myself in it. I made a calculator and added the quadratic equation solver to it, cause i know the formula of finding the roots. But i am faced with two problems.
Code:
#include <stdio.h>
#include <maths.h>
#include <stdlib.h>
#include <windows.h>
main(){
float A1, A2, A, B, C, ans, Z;
printf("Welcome to Quadratic Equation solver. Enter the coefficient of X^2, followed by\nthe coefficient of X, followed by the integer value.\n\nEnter values: ");
scanf("%f%f%f", &A, &B, &C);
CheckF = (B * B - 4 * A * C);
if (CheckF < 0) {
system("COLOR B4");
printf("This calculator HeX, currently cannot handle complex numbers.\nPlease pardon my developer. I will now redirect you to the main menu.\n");
system("pause");
system("cls");
system("COLOR F1");
goto Start;
} else if (CheckF >= 0) {
Z = pow(CheckF, 1/2);
A1 = (-B + Z)/(A+A);
A2 = (-B - Z)/(A+A);
if (A1 == A2) {
ans = A1;
printf("\nRoot of equation is %f (Repeated root)\n", ans);
Sleep(250);
} else if (A1 != A2) {
printf("Roots of equation are %f and %f \n", A1, A2);
Sleep(250);
}
}
}
Problem 1:
When i run the code and input 3 32 2, mathematically the output should be Roots of equation are -0.06287 and -10.6038, that i double checked with my sharp calculator. However, the output that i got was was off: Roots of equation are -5.166667 and -5.500000 i am totally unsure why is it not computing the correct roots of the equation.
Problem 2:
Some roots do not have the coefficient of X^2, for example (2X + 2), which can be solved to get repeated roots of -2, (6X - 3), which gives us that x is 0.5 repeated. However, according to the quadratic equation, which is divided by 2A, will never work, as it is divided by 0. What is the possible way out of this situation? Is it to check if A = 0 then do something else? Any help will be appreciable.
integer division
pow(CheckF, 1/2) is 1.0 as 1/2 is integer division with a quotient of 0.
// Z = pow(CheckF, 1/2);
Z = pow(CheckF, 1.0/2.0);
// or better
Z = sqrt(CheckF);
// Even better when working with `float`.
// Use `float sqrtf(float)` instead of `double sqrt(double)`.
Z = sqrtf(CheckF);
Best - re-write using double instead of float. Scant reason for using float here. double is the C goto floating point type.
Other issue
//#include <maths.h>
#include <math.h>
// main() {
int main(void) {
// CheckF = (B * B - 4 * A * C);
float CheckF = (B * B - 4 * A * C);
// goto Start;
Use an auto formater
I see some problems with the code. First, I suggest you to use double instead of float. They offer much better precision and an ideal calculator needs precision. Secondly, you do:
Z = pow(CheckF, 1/2);
You should use sqrt(CheckF) since there is a dedicated function in C for square roots! The following works for me so if you fix the above two problems, your code will probably work.
int main() {
double A1, A2, A, B, C, ans, Z;
printf("Welcome to Quadratic Equation solver. Enter the coefficient of X^2, followed by\nthe coefficient of X, followed by the integer value.\n\nEnter values: ");
A = 3;
B = 32;
C = 2;
double CheckF = (B * B - 4 * A * C);
if (CheckF >= 0) {
Z = sqrt(CheckF);
A1 = (-B + Z) / (A + A);
A2 = (-B - Z) / (A + A);
if (A1 == A2) {
ans = A1;
printf("\nRoot of equation is %f (Repeated root)\n", ans);
} else if (A1 != A2) {
printf("Roots of equation are %f and %f \n", A1, A2);
}
}
}

Sunset in C language

I want to calculate the sunset for specific localization (latitude, longtitude) in C.
I modeled on: http://edwilliams.org/sunrise_sunset_algorithm.htm
For the given model I tried to calculate the sunrise - this value was good.
I still get the incorrect value for sunset: -11:-22.
Here's some code:
#include <stdio.h>
#include <math.h>
#define PI 3.1415926
#define ZENITH -.83
float calculateSunset(int year,int month,int day,float lat, float lng,int localOffset) {
//1. first calculate the day of the year
float N1 = floor(275 * month / 9);
float N2 = floor((month + 9) / 12);
float N3 = (1 + floor((year - 4 * floor(year / 4) + 2) / 3));
float N = N1 - (N2 * N3) + day - 30;
//2. convert the longitude to hour value and calculate an approximate time
float lngHour = lng / 15;
float t = N + ((18 - lngHour) / 24); //if setting time is desired:
//3. calculate the Sun's mean anomaly
float M = (0.9856 * t) - 3.289;
//4. calculate the Sun's true longitude
float L = fmod(M + (1.916 * sin((PI/180)*M)) + (0.020 * sin(2 *(PI/180) * M)) + 282.634,360.0);
//5a. calculate the Sun's right ascension
float RA = fmod(180/PI*atan(0.91764 * tan((PI/180)*L)),360.0);
//5b. right ascension value needs to be in the same quadrant as L
float Lquadrant = floor( L/90) * 90;
float RAquadrant = floor(RA/90) * 90;
RA = RA + (Lquadrant - RAquadrant);
//5c. right ascension value needs to be converted into hours
RA = RA / 15;
//6. calculate the Sun's declination
float sinDec = 0.39782 * sin((PI/180)*L);
float cosDec = cos(asin(sinDec));
//7a. calculate the Sun's local hour angle
float cosH = (sin((PI/180)*ZENITH) - (sinDec * sin((PI/180)*lat))) / (cosDec * cos((PI/180)*lat));
if (cosH > 1) {
printf("the sun never rises on this location (on the specified date)");
return 0;
}
if (cosH < -1) {
printf("the sun never sets on this location (on the specified date)");
return 0;
}
//7b. finish calculating H and convert into hours
float H = acos(cosH); // if setting time is desired:
H = H / 15;
//8. calculate local mean time of rising/setting
float T = H + RA - (0.06571 * t) - 6.622;
//9. adjust back to UTC
float UT = fmod(T - lngHour,24.0);
//10. convert UT value to local time zone of latitude/longitude
return UT + localOffset;
}
void printSunset() {
float localT = calculateSunset(2018,10,4,51.8446,19.2094,2);
double hours;
float minutes = modf(localT,&hours)*60;
printf("Sunset: ");
printf("%.0f:%.0f",hours,minutes);
printf("\n");
}
int main()
{
printSunset();
return 0;
}
Could anyone help me? What am I doing wrong?
I think your main problem is that you expect fmod to return a positive result - that is not true, fmod preserves the sign i.e. if you are taking fmod of a negative value you'll get a (correct) negative result, if you use it on an positive value the (also correct) result will be positive. If you require an positive result you could for example simply add the divisor if the result is negative i.e.
float a = fmod(T, 360.0);
if(a < 0) { a = a + 360.0; }

I can't tell if this rotation function is working as expected

So I derived a rotation function like this:
I want to rotate (a, b, c) around the x axis
the value of a will not change
this is equivalent to rotating (b, c) around the origin in a 2d map
for a 2d map in polar coordinates, rotating d degrees is as simple as:
θ = θ + d
for a point P(x, y), x = Rcos(θ) and y = Rsin(θ)
so let Q be the point after rotation, then Q = (Rcos(θ + d), Rsin(θ + d))
since R2 = x2 + y2 and θ = arctan(y/x):
Q = (sqrt(x2 + y2) * cos(arctan(y/x) + d, sqrt(x2 + y2) * sin(arctan(y/x) + d)
I then made a C function that given a coordinate: a and rot_amount (usually 1) it would rotate my coordinate for me.
static void xrotate_coor(t_coor *a, int rot_amount)
{
double d;
double e;
d = a->y;
e = a->z;
if (e == 0 && d == 0)
return ;
if (d == 0)
{
a->y = sqrt(d * d + e * e) * cos(atan(INFIN) + rot_amount * M_PI / 50);
a->z = sqrt(d * d + e * e) * sin(atan(INFIN) + rot_amount * M_PI / 50);
return ;
}
a->y = sqrt(d * d + e * e) * cos(atan(e / d) + rot_amount * M_PI / 50);
a->z = sqrt(d * d + e * e) * sin(atan(e / d) + rot_amount * M_PI / 50);
}
INFIN is a macro I set to 999999.
I am not sure if it is correct though since using this formula the shape I am rotating is getting deformed so I feel like there is a flaw in my logic somewhere...
You are experiencing the accumulation of errors in the calculations. This is caused by the nature of how numbers are represented in computers.
The typical way to handle this problem in computer graphics is to keep the object's coordinates fixed and translate them to the position required for the frame being rendered. In your case, this would mean that rather than progressively rotating the object, leave the object in its original position and simply calculate the translation to the current angle around the X-axis based on where it should currently be displayed.
In other words, if you are translating 360 degrees total 20 degrees at a time, display the translated coordinates at 20 degrees in the first iteration and the translated coordinates at 40 degrees in the second iteration rather than actually translating 20 degrees each time.
... the shape I am rotating is getting deformed ...
atan(e / d) loses the 4 quadrant nature of a->y, a->z;. Consider that with OP's code, if the y,z are negated, the same result ensues. #Nominal Animal
d = -(a->y);
e = -(a->z);
...
atan(e / d)
Instead use a 4 quadrant arctangent.
double atan2(double y, double x);
The atan2 functions compute the value of the arc tangent of y/x, using the signs of both arguments to determine the quadrant of the return value. A domain error may occur if both arguments are zero.
Other suggested improvements below too.
#include <math.h>
static void xrotate_coor(t_coor *a, int rot_amount) {
double d = a->y;
double e = a->z;
double r = hypot(d, e); // vs. sqrt(d * d + e * e)
if (r) {
double angle = atan2(e, d);
angle += rot_amount * (M_PI / 50);
a->y = r * cos(angle);
a->z = r * sin(angle);
}
}

C Programming: How to calculate Pi to 4th decimal place?

Okay, I want to make a C program that calculates pi accurately to 4th decimal place (3.1415...). I thought that double is more accurate than float type... Even with a trillion terms (n=trillion), the program cannot go past 3.1414... Can someone help? Am I using an incorrect data type to store my Pi value or is my loops incorrect?
#include <stdio.h>
#include <stdlib.h>
int main(int argc, char** argv) {
int n;
while(1){
printf("Please enter how many terms (n) you wish to add to approximate Pi: ");
scanf("%d", &n);
if(n>=1)
break;
}
int x;
int count =2;
double negSum=0;
double posSum=0;
double pi = 0;
for(x=1;x<=n;x++){
do{
if(x%2==1){
posSum += (4.0)/(2.0*x-1.0);
count++;
}
else{
negSum += (-4.0)/(2.0*x-1.0);
count++;
}
pi = negSum + posSum;
}
while(pi>3.1414999 && pi<3.14160000);
}
//pi = negSum + posSum;
printf("The value of Pi using your approximation is %f, and the iteration was %d", pi, count);
return (EXIT_SUCCESS);
}
Here is some of my sample input/output:
Please enter how many terms (n) you wish to add to approximate Pi: 98713485
The value of Pi using your approximation is 3.141407, and the iteration was 98713488
The series you are using:
pi = 4(1 - 1/3 + 1/5 - 1/7 + 1/9 ...)
converges REALLY slowly to pi. It is the evaluation of a Taylor series for 4arctan(x) at x=1 and converges conditionally (it is right on edge of the interval of convergence). That's not going to be a very numerically efficient way to compute pi.
Beyond that, I haven't carefully checked your implementation, but some others have pointed out problems in the comments.
To compute Pi to 4th decimal place, you could use Gauss-Legendre algorithm:
#include <math.h>
#include <stdio.h>
int main(void) {
const double PI = acos(-1), SQRT2 = sqrt(2.0);
double a = 1, b = 1/SQRT2, t = .25, p = 1;
double an, piold, pi = 1, eps = 1e-6; /* use +2 decimal places */
int iteration_count = 0;
do {
++iteration_count;
an = .5 * (a + b);
b = sqrt(a * b);
t -= p * (a - an) * (a - an);
a = an;
p *= 2;
piold = pi;
pi = (a + b) * (a + b) / (4 * t);
} while (fabs(pi - piold) > eps);
printf("got pi=%f with rel. err=%.2e in %d iterations\n",
pi, (pi - PI) / PI, iteration_count);
return 0;
}
To run it:
$ gcc *.c -lm && ./a.out
Output
got pi=3.141593 with rel. err=2.83e-16 in 3 iterations

Implementation of Goertzel algorithm in C

I am implementing BFSK frequency hopping communication system on a DSP processor. It was suggested by some of the forum members to use Goertzel algorithm for the demodulation of frequency hopping at specific frequencies. I have tried implementing the goertzel algorithm in C. the code is follows:
float goertzel(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
{
int k,i;
float floatnumSamples;
float omega,sine,cosine,coeff,q0,q1,q2,result,real,imag;
floatnumSamples = (float) numSamples;
k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
omega = (2.0 * M_PI * k) / floatnumSamples;
sine = sin(omega);
cosine = cos(omega);
coeff = 2.0 * cosine;
q0=0;
q1=0;
q2=0;
for(i=0; i<numSamples; i++)
{
q0 = coeff * q1 - q2 + data[i];
q2 = q1;
q1 = q0;
}
real = (q1 - q2 * cosine);
imag = (q2 * sine);
result = sqrtf(real*real + imag*imag);
return result;
}
When I use the function to calculate the result at specific frequencies for a given dataset, I am not getting the correct results. However, if I use the same dataset and calculate the goertzel result using MATLAB goertzel() function, then I get the results perfectly. I am implemented the algorithm using C, with the help of some online tutorials that I found over the internet. I just want to get the view of you guys if the function is implementing the goertzel algorithm correctly.
If you are saying that the Matlab implementation is good because its results match the result for that frequency of a DFT or FFT of your data, then it's probably because the Matlab implementation is normalizing the results by a scaling factor as is done with the FFT.
Change your code to take this into account and see if it improves your results. Note that I also changed the function and result names to reflect that your goertzel is calculating the magnitude, not the complete complex result, for clarity:
float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
{
int k,i;
float floatnumSamples;
float omega,sine,cosine,coeff,q0,q1,q2,magnitude,real,imag;
float scalingFactor = numSamples / 2.0;
floatnumSamples = (float) numSamples;
k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
omega = (2.0 * M_PI * k) / floatnumSamples;
sine = sin(omega);
cosine = cos(omega);
coeff = 2.0 * cosine;
q0=0;
q1=0;
q2=0;
for(i=0; i<numSamples; i++)
{
q0 = coeff * q1 - q2 + data[i];
q2 = q1;
q1 = q0;
}
// calculate the real and imaginary results
// scaling appropriately
real = (q1 - q2 * cosine) / scalingFactor;
imag = (q2 * sine) / scalingFactor;
magnitude = sqrtf(real*real + imag*imag);
return magnitude;
}
Often you can just use the square of the magnitude in your computations,
for example for tone detection.
Some excellent examples of Goertzels are in the Asterisk PBX DSP code
Asterisk DSP code (dsp.c)
and in the spandsp library SPANDSP DSP Library
Consider two input sample wave-forms:
1) a sine wave with amplitude A and frequency W
2) a cosine wave with the same amplitude and frequency A and W
Goertzel algorithm should yield the same results for two mentioned input wave-forms but the provided code results in different return values. I think the code should be revised as follows:
float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
{
int k,i;
float floatnumSamples;
float omega,sine,cosine,coeff,q0,q1,q2,magnitude,real,imag;
float scalingFactor = numSamples / 2.0;
floatnumSamples = (float) numSamples;
k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
omega = (2.0 * M_PI * k) / floatnumSamples;
sine = sin(omega);
cosine = cos(omega);
coeff = 2.0 * cosine;
q0=0;
q1=0;
q2=0;
for(i=0; i<numSamples; i++)
{
q2 = q1;
q1 = q0;
q0 = coeff * q1 - q2 + data[i];
}
// calculate the real and imaginary results
// scaling appropriately
real = (q0 - q1 * cosine) / scalingFactor;
imag = (-q1 * sine) / scalingFactor;
magnitude = sqrtf(real*real + imag*imag);
return magnitude;
}

Resources