Rev 1490 | Rev 1793 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1490 | Rev 1523 | ||
---|---|---|---|
1 | #include <stdlib.h> |
1 | #include <stdlib.h> |
2 | #include <avr/pgmspace.h> |
2 | #include <avr/pgmspace.h> |
3 | #include "mymath.h" |
3 | #include "mymath.h" |
4 | 4 | ||
5 | // discrete mathematics |
5 | // discrete mathematics |
6 | 6 | ||
7 | // Sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
7 | // Sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
8 | const uint16_t pgm_sinlookup[91] PROGMEM = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192}; |
8 | const uint16_t pgm_sinlookup[91] PROGMEM = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192}; |
9 | 9 | ||
10 | int16_t c_sin_8192(int16_t angle) |
10 | int16_t c_sin_8192(int16_t angle) |
11 | { |
11 | { |
12 | int8_t m,n; |
12 | int8_t m,n; |
13 | int16_t sinus; |
13 | int16_t sinus; |
14 | 14 | ||
15 | // avoid negative angles |
15 | // avoid negative angles |
16 | if (angle < 0) |
16 | if (angle < 0) |
17 | { |
17 | { |
18 | m = -1; |
18 | m = -1; |
19 | angle = abs(angle); |
19 | angle = abs(angle); |
20 | } |
20 | } |
21 | else m = +1; |
21 | else m = +1; |
22 | 22 | ||
23 | // fold angle to intervall 0 to 359 |
23 | // fold angle to intervall 0 to 359 |
24 | angle %= 360; |
24 | angle %= 360; |
25 | 25 | ||
26 | // check quadrant |
26 | // check quadrant |
27 | if (angle <= 90) n=1; // first quadrant |
27 | if (angle <= 90) n=1; // first quadrant |
28 | else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
28 | else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant |
29 | else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
29 | else if ((angle > 180) && (angle <= 270)) {angle = angle - 180; n = -1;} // third quadrant |
30 | else {angle = 360 - angle; n = -1;} //fourth quadrant |
30 | else {angle = 360 - angle; n = -1;} //fourth quadrant |
31 | // get lookup value |
31 | // get lookup value |
32 | sinus = pgm_read_word(&pgm_sinlookup[angle]); |
32 | sinus = pgm_read_word(&pgm_sinlookup[angle]); |
33 | // calculate sinus value |
33 | // calculate sinus value |
34 | return (sinus * m * n); |
34 | return (sinus * m * n); |
35 | } |
35 | } |
36 | 36 | ||
37 | // Cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
37 | // Cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
38 | int16_t c_cos_8192(int16_t angle) |
38 | int16_t c_cos_8192(int16_t angle) |
39 | { |
39 | { |
40 | return (c_sin_8192(90 - angle)); |
40 | return (c_sin_8192(90 - angle)); |
41 | } |
- | |
42 | 41 | } |
|
- | 42 |