213 lines
47 KiB
C
213 lines
47 KiB
C
|
// fixed.c
|
||
|
|
// fixed-point trig functions
|
||
|
|
// Uses table lookup for speed
|
||
|
|
// TI-RSLK MAX
|
||
|
|
// Daniel Valvano and Jonathan Valvano
|
||
|
|
// April 10, 2019
|
||
|
|
|
||
|
|
/* This example accompanies the book
|
||
|
|
"Embedded Systems: Introduction to Robotics,
|
||
|
|
Jonathan W. Valvano, ISBN: 9781074544300, copyright (c) 2019
|
||
|
|
For more information about my classes, my research, and my books, see
|
||
|
|
http://users.ece.utexas.edu/~valvano/
|
||
|
|
|
||
|
|
Simplified BSD License (FreeBSD License)
|
||
|
|
Copyright (c) 2019, Jonathan Valvano, All rights reserved.
|
||
|
|
|
||
|
|
Redistribution and use in source and binary forms, with or without modification,
|
||
|
|
are permitted provided that the following conditions are met:
|
||
|
|
|
||
|
|
1. Redistributions of source code must retain the above copyright notice,
|
||
|
|
this list of conditions and the following disclaimer.
|
||
|
|
2. Redistributions in binary form must reproduce the above copyright notice,
|
||
|
|
this list of conditions and the following disclaimer in the documentation
|
||
|
|
and/or other materials provided with the distribution.
|
||
|
|
|
||
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||
|
|
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||
|
|
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||
|
|
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||
|
|
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||
|
|
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||
|
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||
|
|
AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||
|
|
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
|
||
|
|
USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||
|
|
|
||
|
|
The views and conclusions contained in the software and documentation are
|
||
|
|
those of the authors and should not be interpreted as representing official
|
||
|
|
policies, either expressed or implied, of the FreeBSD Project.
|
||
|
|
*/
|
||
|
|
|
||
|
|
|
||
|
|
#include <stdint.h>
|
||
|
|
// index is angle in 0.001 radians
|
||
|
|
// index goes from 0 to 1571 (pi/2)
|
||
|
|
int16_t const SinTab[1572]={
|
||
|
|
0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180, 190, 200, 210, 220, 230, 240, 250, 260, 270,
|
||
|
|
280, 290, 300, 310, 320, 330, 340, 350, 360, 370, 380, 390, 400, 410, 420, 430, 440, 450, 460, 470, 480, 490, 500, 510, 520, 530,
|
||
|
|
540, 550, 560, 570, 580, 590, 600, 610, 620, 630, 640, 650, 660, 669, 679, 689, 699, 709, 719, 729, 739, 749, 759, 769, 779, 789,
|
||
|
|
799, 809, 819, 829, 839, 849, 859, 869, 879, 889, 899, 909, 919, 929, 939, 949, 959, 968, 978, 988, 998, 1008, 1018, 1028, 1038,
|
||
|
|
1048, 1058, 1068, 1078, 1088, 1098, 1108, 1118, 1128, 1138, 1147, 1157, 1167, 1177, 1187, 1197, 1207, 1217, 1227, 1237, 1247, 1257,
|
||
|
|
1267, 1277, 1286, 1296, 1306, 1316, 1326, 1336, 1346, 1356, 1366, 1376, 1386, 1395, 1405, 1415, 1425, 1435, 1445, 1455, 1465, 1475,
|
||
|
|
1484, 1494, 1504, 1514, 1524, 1534, 1544, 1554, 1564, 1573, 1583, 1593, 1603, 1613, 1623, 1633, 1643, 1652, 1662, 1672, 1682, 1692,
|
||
|
|
1702, 1712, 1721, 1731, 1741, 1751, 1761, 1771, 1780, 1790, 1800, 1810, 1820, 1830, 1839, 1849, 1859, 1869, 1879, 1889, 1898, 1908,
|
||
|
|
1918, 1928, 1938, 1947, 1957, 1967, 1977, 1987, 1996, 2006, 2016, 2026, 2036, 2045, 2055, 2065, 2075, 2085, 2094, 2104, 2114, 2124,
|
||
|
|
2133, 2143, 2153, 2163, 2173, 2182, 2192, 2202, 2212, 2221, 2231, 2241, 2251, 2260, 2270, 2280, 2290, 2299, 2309, 2319, 2328, 2338,
|
||
|
|
2348, 2358, 2367, 2377, 2387, 2396, 2406, 2416, 2426, 2435, 2445, 2455, 2464, 2474, 2484, 2493, 2503, 2513, 2522, 2532, 2542, 2551,
|
||
|
|
2561, 2571, 2580, 2590, 2600, 2609, 2619, 2629, 2638, 2648, 2658, 2667, 2677, 2687, 2696, 2706, 2715, 2725, 2735, 2744, 2754, 2764,
|
||
|
|
2773, 2783, 2792, 2802, 2812, 2821, 2831, 2840, 2850, 2860, 2869, 2879, 2888, 2898, 2907, 2917, 2927, 2936, 2946, 2955, 2965, 2974,
|
||
|
|
2984, 2993, 3003, 3012, 3022, 3032, 3041, 3051, 3060, 3070, 3079, 3089, 3098, 3108, 3117, 3127, 3136, 3146, 3155, 3165, 3174, 3184,
|
||
|
|
3193, 3203, 3212, 3222, 3231, 3240, 3250, 3259, 3269, 3278, 3288, 3297, 3307, 3316, 3325, 3335, 3344, 3354, 3363, 3373, 3382, 3391,
|
||
|
|
3401, 3410, 3420, 3429, 3438, 3448, 3457, 3467, 3476, 3485, 3495, 3504, 3513, 3523, 3532, 3541, 3551, 3560, 3569, 3579, 3588, 3598,
|
||
|
|
3607, 3616, 3625, 3635, 3644, 3653, 3663, 3672, 3681, 3691, 3700, 3709, 3718, 3728, 3737, 3746, 3756, 3765, 3774, 3783, 3793, 3802,
|
||
|
|
3811, 3820, 3830, 3839, 3848, 3857, 3867, 3876, 3885, 3894, 3903, 3913, 3922, 3931, 3940, 3949, 3959, 3968, 3977, 3986, 3995, 4004,
|
||
|
|
4014, 4023, 4032, 4041, 4050, 4059, 4068, 4078, 4087, 4096, 4105, 4114, 4123, 4132, 4141, 4151, 4160, 4169, 4178, 4187, 4196, 4205,
|
||
|
|
4214, 4223, 4232, 4241, 4250, 4259, 4268, 4277, 4287, 4296, 4305, 4314, 4323, 4332, 4341, 4350, 4359, 4368, 4377, 4386, 4395, 4404,
|
||
|
|
4413, 4422, 4431, 4439, 4448, 4457, 4466, 4475, 4484, 4493, 4502, 4511, 4520, 4529, 4538, 4547, 4556, 4564, 4573, 4582, 4591, 4600,
|
||
|
|
4609, 4618, 4627, 4636, 4644, 4653, 4662, 4671, 4680, 4689, 4697, 4706, 4715, 4724, 4733, 4742, 4750, 4759, 4768, 4777, 4785, 4794,
|
||
|
|
4803, 4812, 4821, 4829, 4838, 4847, 4856, 4864, 4873, 4882, 4890, 4899, 4908, 4917, 4925, 4934, 4943, 4951, 4960, 4969, 4977, 4986,
|
||
|
|
4995, 5003, 5012, 5021, 5029, 5038, 5047, 5055, 5064, 5073, 5081, 5090, 5098, 5107, 5116, 5124, 5133, 5141, 5150, 5159, 5167, 5176,
|
||
|
|
5184, 5193, 5201, 5210, 5218, 5227, 5235, 5244, 5252, 5261, 5269, 5278, 5286, 5295, 5303, 5312, 5320, 5329, 5337, 5346, 5354, 5363,
|
||
|
|
5371, 5379, 5388, 5396, 5405, 5413, 5422, 5430, 5438, 5447, 5455, 5463, 5472, 5480, 5489, 5497, 5505, 5514, 5522, 5530, 5539, 5547,
|
||
|
|
5555, 5564, 5572, 5580, 5589, 5597, 5605, 5613, 5622, 5630, 5638, 5646, 5655, 5663, 5671, 5679, 5688, 5696, 5704, 5712, 5720, 5729,
|
||
|
|
5737, 5745, 5753, 5761, 5770, 5778, 5786, 5794, 5802, 5810, 5818, 5827, 5835, 5843, 5851, 5859, 5867, 5875, 5883, 5891, 5900, 5908,
|
||
|
|
5916, 5924, 5932, 5940, 5948, 5956, 5964, 5972, 5980, 5988, 5996, 6004, 6012, 6020, 6028, 6036, 6044, 6052, 6060, 6068, 6076, 6084,
|
||
|
|
6092, 6100, 6107, 6115, 6123, 6131, 6139, 6147, 6155, 6163, 6171, 6178, 6186, 6194, 6202, 6210, 6218, 6226, 6233, 6241, 6249, 6257,
|
||
|
|
6265, 6272, 6280, 6288, 6296, 6303, 6311, 6319, 6327, 6334, 6342, 6350, 6358, 6365, 6373, 6381, 6388, 6396, 6404, 6412, 6419, 6427,
|
||
|
|
6435, 6442, 6450, 6457, 6465, 6473, 6480, 6488, 6496, 6503, 6511, 6518, 6526, 6533, 6541, 6549, 6556, 6564, 6571, 6579, 6586, 6594,
|
||
|
|
6601, 6609, 6616, 6624, 6631, 6639, 6646, 6654, 6661, 6669, 6676, 6684, 6691, 6698, 6706, 6713, 6721, 6728, 6735, 6743, 6750, 6758,
|
||
|
|
6765, 6772, 6780, 6787, 6794, 6802, 6809, 6816, 6824, 6831, 6838, 6846, 6853, 6860, 6867, 6875, 6882, 6889, 6896, 6904, 6911, 6918,
|
||
|
|
6925, 6933, 6940, 6947, 6954, 6961, 6969, 6976, 6983, 6990, 6997, 7004, 7011, 7019, 7026, 7033, 7040, 7047, 7054, 7061, 7068, 7075,
|
||
|
|
7082, 7089, 7096, 7104, 7111, 7118, 7125, 7132, 7139, 7146, 7153, 7160, 7167, 7174, 7181, 7187, 7194, 7201, 7208, 7215, 7222, 7229,
|
||
|
|
7236, 7243, 7250, 7257, 7264, 7270, 7277, 7284, 7291, 7298, 7305, 7311, 7318, 7325, 7332, 7339, 7345, 7352, 7359, 7366, 7373, 7379,
|
||
|
|
7386, 7393, 7400, 7406, 7413, 7420, 7426, 7433, 7440, 7446, 7453, 7460, 7466, 7473, 7480, 7486, 7493, 7500, 7506, 7513, 7519, 7526,
|
||
|
|
7533, 7539, 7546, 7552, 7559, 7565, 7572, 7578, 7585, 7591, 7598, 7604, 7611, 7617, 7624, 7630, 7637, 7643, 7650, 7656, 7663, 7669,
|
||
|
|
7675, 7682, 7688, 7695, 7701, 7707, 7714, 7720, 7726, 7733, 7739, 7745, 7752, 7758, 7764, 7771, 7777, 7783, 7790, 7796, 7802, 7808,
|
||
|
|
7815, 7821, 7827, 7833, 7839, 7846, 7852, 7858, 7864, 7870, 7877, 7883, 7889, 7895, 7901, 7907, 7913, 7920, 7926, 7932, 7938, 7944,
|
||
|
|
7950, 7956, 7962, 7968, 7974, 7980, 7986, 7992, 7998, 8004, 8010, 8016, 8022, 8028, 8034, 8040, 8046, 8052, 8058, 8064, 8070, 8076,
|
||
|
|
8081, 8087, 8093, 8099, 8105, 8111, 8117, 8123, 8128, 8134, 8140, 8146, 8152, 8157, 8163, 8169, 8175, 8180, 8186, 8192, 8198, 8203,
|
||
|
|
8209, 8215, 8220, 8226, 8232, 8238, 8243, 8249, 8255, 8260, 8266, 8271, 8277, 8283, 8288, 8294, 8299, 8305, 8311, 8316, 8322, 8327,
|
||
|
|
8333, 8338, 8344, 8349, 8355, 8360, 8366, 8371, 8377, 8382, 8388, 8393, 8398, 8404, 8409, 8415, 8420, 8425, 8431, 8436, 8442, 8447,
|
||
|
|
8452, 8458, 8463, 8468, 8474, 8479, 8484, 8490, 8495, 8500, 8505, 8511, 8516, 8521, 8526, 8532, 8537, 8542, 8547, 8552, 8558, 8563,
|
||
|
|
8568, 8573, 8578, 8583, 8588, 8594, 8599, 8604, 8609, 8614, 8619, 8624, 8629, 8634, 8639, 8644, 8649, 8654, 8659, 8664, 8669, 8674,
|
||
|
|
8679, 8684, 8689, 8694, 8699, 8704, 8709, 8714, 8719, 8724, 8728, 8733, 8738, 8743, 8748, 8753, 8758, 8762, 8767, 8772, 8777, 8782,
|
||
|
|
8786, 8791, 8796, 8801, 8805, 8810, 8815, 8820, 8824, 8829, 8834, 8838, 8843, 8848, 8852, 8857, 8862, 8866, 8871, 8876, 8880, 8885,
|
||
|
|
8889, 8894, 8898, 8903, 8908, 8912, 8917, 8921, 8926, 8930, 8935, 8939, 8944, 8948, 8953, 8957, 8961, 8966, 8970, 8975, 8979, 8984,
|
||
|
|
8988, 8992, 8997, 9001, 9005, 9010, 9014, 9018, 9023, 9027, 9031, 9036, 9040, 9044, 9048, 9053, 9057, 9061, 9065, 9070, 9074, 9078,
|
||
|
|
9082, 9086, 9091, 9095, 9099, 9103, 9107, 9111, 9115, 9119, 9124, 9128, 9132, 9136, 9140, 9144, 9148, 9152, 9156, 9160, 9164, 9168,
|
||
|
|
9172, 9176, 9180, 9184, 9188, 9192, 9196, 9200, 9204, 9208, 9211, 9215, 9219, 9223, 9227, 9231, 9235, 9238, 9242, 9246, 9250, 9254,
|
||
|
|
9257, 9261, 9265, 9269, 9272, 9276, 9280, 9284, 9287, 9291, 9295, 9298, 9302, 9306, 9309, 9313, 9317, 9320, 9324, 9328, 9331, 9335,
|
||
|
|
9338, 9342, 9346, 9349, 9353, 9356, 9360, 9363, 9367, 9370, 9374, 9377, 9381, 9384, 9388, 9391, 9394, 9398, 9401, 9405, 9408, 9411,
|
||
|
|
9415, 9418, 9422, 9425, 9428, 9432, 9435, 9438, 9441, 9445, 9448, 9451, 9455, 9458, 9461, 9464, 9468, 9471, 9474, 9477, 9480, 9484,
|
||
|
|
9487, 9490, 9493, 9496, 9499, 9502, 9505, 9509, 9512, 9515, 9518, 9521, 9524, 9527, 9530, 9533, 9536, 9539, 9542, 9545, 9548, 9551,
|
||
|
|
9554, 9557, 9560, 9563, 9566, 9569, 9572, 9574, 9577, 9580, 9583, 9586, 9589, 9592, 9594, 9597, 9600, 9603, 9606, 9608, 9611, 9614,
|
||
|
|
9617, 9619, 9622, 9625, 9628, 9630, 9633, 9636, 9638, 9641, 9644, 9646, 9649, 9651, 9654, 9657, 9659, 9662, 9664, 9667, 9670, 9672,
|
||
|
|
9675, 9677, 9680, 9682, 9685, 9687, 9690, 9692, 9695, 9697, 9699, 9702, 9704, 9707, 9709, 9711, 9714, 9716, 9719, 9721, 9723, 9726,
|
||
|
|
9728, 9730, 9733, 9735, 9737, 9739, 9742, 9744, 9746, 9748, 9751, 9753, 9755, 9757, 9759, 9762, 9764, 9766, 9768, 9770, 9772, 9774,
|
||
|
|
9777, 9779, 9781, 9783, 9785, 9787, 9789, 9791, 9793, 9795, 9797, 9799, 9801, 9803, 9805, 9807, 9809, 9811, 9813, 9815, 9817, 9819,
|
||
|
|
9820, 9822, 9824, 9826, 9828, 9830, 9832, 9833, 9835, 9837, 9839, 9841, 9842, 9844, 9846, 9848, 9849, 9851, 9853, 9854, 9856, 9858,
|
||
|
|
9860, 9861, 9863, 9865, 9866, 9868, 9869, 9871, 9873, 9874, 9876, 9877, 9879, 9880, 9882, 9883, 9885, 9887, 9888, 9890, 9891, 9892,
|
||
|
|
9894, 9895, 9897, 9898, 9900, 9901, 9902, 9904, 9905, 9907, 9908, 9909, 9911, 9912, 9913, 9915, 9916, 9917, 9918, 9920, 9921, 9922,
|
||
|
|
9923, 9925, 9926, 9927, 9928, 9930, 9931, 9932, 9933, 9934, 9935, 9936, 9938, 9939, 9940, 9941, 9942, 9943, 9944, 9945, 9946, 9947,
|
||
|
|
9948, 9949, 9950, 9951, 9952, 9953, 9954, 9955, 9956, 9957, 9958, 9959, 9960, 9961, 9961, 9962, 9963, 9964, 9965, 9966, 9967, 9967,
|
||
|
|
9968, 9969, 9970, 9971, 9971, 9972, 9973, 9974, 9974, 9975, 9976, 9976, 9977, 9978, 9978, 9979, 9980, 9980, 9981, 9982, 9982, 9983,
|
||
|
|
9983, 9984, 9984, 9985, 9986, 9986, 9987, 9987, 9988, 9988, 9989, 9989, 9990, 9990, 9990, 9991, 9991, 9992, 9992, 9992, 9993, 9993,
|
||
|
|
9994, 9994, 9994, 9995, 9995, 9995, 9996, 9996, 9996, 9996, 9997, 9997, 9997, 9997, 9998, 9998, 9998, 9998, 9998, 9999, 9999, 9999,
|
||
|
|
9999, 9999, 9999, 9999, 10000, 10000, 10000, 10000, 10000, 10000, 10000, 10000, 10000, 10000, 10000
|
||
|
|
};
|
||
|
|
|
||
|
|
//*********fixed-point sin****************
|
||
|
|
// input: -3142 to 3142, angle is in units radians/1000
|
||
|
|
// e.g., 90 degrees (pi/2 radians) is 1571
|
||
|
|
// output: -10000 to +10000, fixed point resolution 1/10000
|
||
|
|
int16_t fixed_sin(int32_t theta){
|
||
|
|
if(theta>3142) return 0;
|
||
|
|
if(theta<-3142) return 0;
|
||
|
|
if(theta<-1571) return -SinTab[3142+theta]; // -180 to -90 deg
|
||
|
|
if(theta<0) return -SinTab[-theta]; // -90 to 0 deg
|
||
|
|
if(theta<1571) return SinTab[theta]; // 0 to 90 deg
|
||
|
|
return SinTab[3142-theta]; // 90 to 180 deg
|
||
|
|
}
|
||
|
|
|
||
|
|
// cos(theta) = sin(pi/2-theta)
|
||
|
|
//*********fixed-point cos****************
|
||
|
|
// input: -3142 to 3142, angle is in units radians/1000
|
||
|
|
// e.g., 90 degrees (pi/2 radians) is 1571
|
||
|
|
// output: -10000 to +10000, fixed point resolution 1/10000
|
||
|
|
int16_t fixed_cos(int32_t theta){
|
||
|
|
if(theta>3142) return -10000;
|
||
|
|
if(theta<-3142) return -10000;
|
||
|
|
if(theta<-1571) return -SinTab[-1571-theta]; // -180 to -90 deg
|
||
|
|
if(theta<0) return SinTab[1571+theta]; // -90 to 0 deg
|
||
|
|
if(theta<1571) return SinTab[1571-theta]; // 0 to 90 deg
|
||
|
|
return -SinTab[-1571+theta]; // 90 to 180 deg
|
||
|
|
}
|
||
|
|
|
||
|
|
uint16_t const SinTab2[4096]={
|
||
|
|
0, 25, 50, 75, 101, 126, 151, 176, 201, 226, 251, 276, 302, 327, 352, 377, 402, 427, 452, 478, 503, 528, 553, 578, 603, 628, 653, 679, 704, 729, 754, 779, 804, 829, 854, 880, 905, 930, 955, 980, 1005, 1030, 1056, 1081, 1106, 1131, 1156, 1181, 1206, 1231, 1257, 1282, 1307, 1332, 1357, 1382, 1407, 1432, 1458, 1483, 1508, 1533, 1558, 1583, 1608, 1633, 1659, 1684, 1709, 1734, 1759, 1784, 1809, 1834, 1860, 1885, 1910, 1935, 1960, 1985, 2010, 2035, 2061, 2086, 2111, 2136, 2161, 2186, 2211, 2236, 2261, 2287, 2312, 2337, 2362, 2387, 2412, 2437, 2462, 2488, 2513, 2538, 2563, 2588, 2613, 2638, 2663, 2688, 2714, 2739, 2764, 2789, 2814, 2839, 2864, 2889, 2914, 2940, 2965, 2990, 3015, 3040, 3065, 3090, 3115, 3140, 3165, 3191, 3216, 3241, 3266, 3291, 3316, 3341, 3366, 3391, 3417, 3442, 3467, 3492, 3517, 3542, 3567, 3592, 3617, 3642, 3667, 3693, 3718, 3743, 3768, 3793, 3818, 3843, 3868, 3893, 3918, 3943, 3969, 3994, 4019, 4044, 4069, 4094, 4119, 4144, 4169, 4194, 4219, 4244, 4270, 4295, 4320, 4345, 4370, 4395, 4420, 4445, 4470, 4495, 4520, 4545, 4570, 4596, 4621, 4646, 4671, 4696, 4721, 4746, 4771, 4796, 4821, 4846, 4871, 4896, 4921, 4946, 4972, 4997, 5022, 5047, 5072, 5097, 5122, 5147, 5172, 5197, 5222, 5247, 5272, 5297, 5322, 5347, 5372, 5397, 5422, 5448, 5473, 5498, 5523, 5548, 5573, 5598, 5623, 5648, 5673, 5698, 5723, 5748, 5773, 5798, 5823, 5848, 5873, 5898, 5923, 5948, 5973, 5998, 6023, 6048, 6073, 6098, 6123, 6148, 6173, 6199, 6224, 6249, 6274, 6299, 6324, 6349, 6374, 6399, 6424, 6449, 6474, 6499, 6524, 6549, 6574, 6599, 6624, 6649, 6674, 6699, 6724, 6749, 6774, 6799, 6824, 6849, 6874, 6899, 6924, 6949, 6974, 6999, 7024, 7049, 7074, 7099, 7124, 7149, 7174, 7199, 7224, 7249, 7273, 7298, 7323, 7348, 7373, 7398, 7423, 7448, 7473, 7498, 7523, 7548, 7573, 7598, 7623, 7648, 7673, 7698, 7723, 7748, 7773, 7798, 7823, 7848, 7873, 7898, 7923, 7947, 7972, 7997, 8022, 8047, 8072, 8097, 8122, 8147, 8172, 8197, 8222, 8247, 8272, 8297, 8322, 8346, 8371, 8396, 8421, 8446, 8471, 8496, 8521, 8546, 8571, 8596, 8621, 8646, 8670, 8695, 8720, 8745, 8770, 8795, 8820, 8845, 8870, 8895, 8919, 8944, 8969, 8994, 9019, 9044, 9069, 9094, 9119, 9144, 9168, 9193, 9218, 9243, 9268, 9293, 9318, 9343, 9367, 9392, 9417, 9442, 9467, 9492, 9517, 9542, 9566, 9591, 9616, 9641, 9666, 9691, 9716, 9740, 9765, 9790, 9815, 9840, 9865, 9890, 9914, 9939, 9964, 9989, 10014, 10039, 10063, 10088, 10113, 10138, 10163, 10188, 10212, 10237, 10262, 10287, 10312, 10336, 10361, 10386, 10411, 10436, 10461, 10485, 10510, 10535, 10560, 10585, 10609, 10634, 10659, 10684, 10709, 10733, 10758, 10783, 10808, 10833, 10857, 10882, 10907, 10932, 10956, 10981, 11006, 11031, 11056, 11080, 11105, 11130, 11155, 11179, 11204, 11229, 11254, 11278, 11303, 11328, 11353, 11377, 11402, 11427, 11452, 11476, 11501, 11526, 11551, 11575, 11600, 11625, 11650, 11674, 11699, 11724, 11749, 11773, 11798, 11823, 11847, 11872, 11897, 11922, 11946, 11971, 11996, 12020, 12045, 12070, 12095, 12119, 12144, 12169, 12193, 12218, 12243, 12267, 12292, 12317, 12341, 12366, 12391, 12415, 12440, 12465, 12490, 12514, 12539, 12564, 12588, 12613, 12638, 12662, 12687, 12711, 12736, 12761, 12785, 12810, 12835, 12859, 12884, 12909, 12933, 12958, 12983, 13007, 13032, 13056, 13081, 13106, 13130, 13155, 13180, 13204, 13229, 13253, 13278, 13303, 13327, 13352, 13376, 13401, 13426, 13450, 13475, 13499, 13524, 13549, 13573, 13598, 13622, 13647, 13672, 13696, 13721, 13745, 13770, 13794, 13819, 13844, 13868, 13893, 13917, 13942, 13966, 13991, 14016, 14040, 14065, 14089, 14114, 14138, 14163, 14187, 14212, 14236, 14261, 14285, 14310, 14334, 14359, 14384, 14408, 14433, 14457, 14482, 14506, 14531, 14555, 14580, 14604, 14629, 14653, 14678, 14702, 14727, 14751, 14776, 14800, 14825, 14849, 14874, 14898, 14922, 14947, 14971, 14996, 15020, 15045, 15069, 15094, 15118, 15143, 15167, 15192, 15216, 15240, 15265, 15289, 15314, 15338, 15363, 15387, 15411, 15436, 15460, 15485, 15509, 15534, 15558, 15582, 15607, 15631, 15656, 15680, 15704, 15729, 15753, 15778, 15802, 15826, 15851, 15875, 15900, 15924, 15948, 15973, 15997, 16021, 16046, 16070, 16095, 16
|
||
|
|
};
|
||
|
|
|
||
|
|
//*********fixed-point sin****************
|
||
|
|
// input: -8192 to 8191, angle is in units 2*pi/16384 radians (-pi to +pi)
|
||
|
|
// e.g., 90 degrees (pi/2 radians) is 4096
|
||
|
|
// output: -65536 to +65536, fixed point resolution 1/65536
|
||
|
|
int32_t fixed_sin2(int32_t theta){
|
||
|
|
if(theta> 8191) return 0;
|
||
|
|
if(theta<-8192) return 0;
|
||
|
|
if(theta<-4106) return -SinTab2[8192+theta]; // -180 to -90 deg
|
||
|
|
if(theta<-4086) return -65536; // -90 deg
|
||
|
|
if(theta< 0) return -SinTab2[-theta]; // -90 to 0 deg
|
||
|
|
if(theta< 4086) return SinTab2[theta]; // 0 to 90 deg
|
||
|
|
if(theta< 4106) return 65536; // 90 deg
|
||
|
|
return SinTab2[8192-theta]; // 90 to 180 deg
|
||
|
|
}
|
||
|
|
|
||
|
|
//*********fixed-point cos****************
|
||
|
|
// input: -8192 to 8191, angle is in units 2*pi/16384 radians (-pi to +pi)
|
||
|
|
// e.g., 90 degrees (pi/2 radians) is 4096
|
||
|
|
// output: -65536 to +65536, fixed point resolution 1/65536
|
||
|
|
int32_t fixed_cos2(int32_t theta){
|
||
|
|
if(theta> 8181) return -65536;
|
||
|
|
if(theta<-8182) return -65536;
|
||
|
|
if(theta<-4096) return -SinTab2[-4096-theta]; // -180 to -90 deg
|
||
|
|
if(theta< -10) return SinTab2[4096+theta]; // -90 to 0 deg
|
||
|
|
if(theta< 10) return 65536; // 0 deg
|
||
|
|
if(theta< 4096) return SinTab2[4096-theta]; // 0 to 90 deg
|
||
|
|
return -SinTab2[-4096+theta]; // 90 to 180 deg
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
int32_t SinTab540[540] = {
|
||
|
|
0, 763, 1525, 2287, 3049, 3811, 4572, 5332, 6092, 6850, 7608, 8365, 9121, 9875, 10628, 11380, 12130, 12879, 13626, 14371, 15114, 15855, 16593, 17330, 18064, 18796, 19525, 20252, 20976, 21697, 22415, 23130, 23842, 24550, 25256, 25957, 26656, 27351, 28042, 28729, 29413, 30092, 30767, 31438, 32105, 32768, 33426, 34080, 34729, 35373, 36013, 36647, 37277, 37902, 38521, 39135, 39744, 40348, 40946, 41539, 42126, 42707, 43283, 43852, 44416, 44974, 45525, 46071, 46610, 47143, 47669, 48189, 48703, 49210, 49710, 50203, 50690, 51170, 51643, 52109, 52568, 53020, 53464, 53902, 54332, 54755, 55170, 55578, 55978, 56371, 56756, 57133, 57503, 57865, 58219, 58565, 58903, 59234, 59556, 59870, 60176, 60474, 60764, 61045, 61319, 61584, 61840, 62089, 62328, 62560, 62783, 62997, 63203, 63401, 63589, 63769, 63941, 64104, 64258, 64404, 64540, 64668, 64788, 64898, 65000, 65093, 65177, 65252, 65319, 65376, 65425, 65465, 65496, 65518, 65532, 65536,
|
||
|
|
|
||
|
|
65532, 65518, 65496, 65465, 65425, 65376, 65319, 65252, 65177, 65093, 65000, 64898, 64788, 64668, 64540, 64404, 64258, 64104, 63941, 63769, 63589, 63401, 63203, 62997, 62783, 62560, 62328, 62089, 61840, 61584, 61319, 61045, 60764, 60474, 60176, 59870, 59556, 59234, 58903, 58565, 58219, 57865, 57503, 57133, 56756, 56371, 55978, 55578, 55170, 54755, 54332, 53902, 53464, 53020, 52568, 52109, 51643, 51170, 50690, 50203, 49710, 49210, 48703, 48189, 47669, 47143, 46610, 46071, 45525, 44974, 44416, 43852, 43283, 42707, 42126, 41539, 40946, 40348, 39744, 39135, 38521, 37902, 37277, 36647, 36013, 35373, 34729, 34080, 33426, 32768, 32105, 31438, 30767, 30092, 29413, 28729, 28042, 27351, 26656, 25957, 25256, 24550, 23842, 23130, 22415, 21697, 20976, 20252, 19525, 18796, 18064, 17330, 16593, 15855, 15114, 14371, 13626, 12879, 12130, 11380, 10628, 9875, 9121, 8365, 7608, 6850, 6092, 5332, 4572, 3811, 3049, 2287, 1525, 763, 0,
|
||
|
|
|
||
|
|
-763, -1525, -2287, -3049, -3811, -4572, -5332, -6092, -6850, -7608, -8365, -9121, -9875, -10628, -11380, -12130, -12879, -13626, -14371, -15114, -15855, -16593, -17330, -18064, -18796, -19525, -20252, -20976, -21697, -22415, -23130, -23842, -24550, -25256, -25957, -26656, -27351, -28042, -28729, -29413, -30092, -30767, -31438, -32105, -32768, -33426, -34080, -34729, -35373, -36013, -36647, -37277, -37902, -38521, -39135, -39744, -40348, -40946, -41539, -42126, -42707, -43283, -43852, -44416, -44974, -45525, -46071, -46610, -47143, -47669, -48189, -48703, -49210, -49710, -50203, -50690, -51170, -51643, -52109, -52568, -53020, -53464, -53902, -54332, -54755, -55170, -55578, -55978, -56371, -56756, -57133, -57503, -57865, -58219, -58565, -58903, -59234, -59556, -59870, -60176, -60474, -60764, -61045, -61319, -61584, -61840, -62089, -62328, -62560, -62783, -62997, -63203, -63401, -63589, -63769, -63941, -64104, -64258, -64404, -64540, -64668, -64788, -64898, -65000, -65093, -65177, -65252, -65319, -65376, -65425, -65465, -65496, -65518, -65532, -65536,
|
||
|
|
|
||
|
|
-65532, -65518, -65496, -65465, -65425, -65376, -65319, -65252, -65177, -65093, -65000, -64898, -64788, -64668, -64540, -64404, -64258, -64104, -63941, -63769, -63589, -63401, -63203, -62997, -62783, -62560, -62328, -62089, -61840, -61584, -61319, -61045, -60764, -60474, -60176, -59870, -59556, -59234, -58903, -58565, -58219, -57865, -57503, -57133, -56756, -56371, -55978, -55578, -55170, -54755, -54332, -53902, -53464, -53020, -52568, -52109, -51643, -51170, -50690, -50203, -49710, -49210, -48703, -48189, -47669, -47143, -46610, -46071, -45525, -44974, -44416, -43852, -43283, -42707, -42126, -41539, -40946, -40348, -39744, -39135, -38521, -37902, -37277, -36647, -36013, -35373, -34729, -34080, -33426, -32768, -32105, -31438, -30767, -30092, -29413, -28729, -28042, -27351, -26656, -25957, -25256, -24550, -23842, -23130, -22415, -21697, -20976, -20252, -19525, -18796, -18064, -17330, -16593, -15855, -15114, -14371, -13626, -12879, -12130, -11380, -10628, -9875, -9121, -8365, -7608, -6850, -6092, -5332, -4572, -3811, -3049, -2287, -1525, -763};
|
||
|
|
//*********sin540****************
|
||
|
|
// input: 0 to 539, angle is in units 2*pi/540 = 0.011635528 radians (0 to 2pi)
|
||
|
|
// e.g., 359.3degrees (2pi radians) theta=539 sin540(539) = -763 (-0.0116)
|
||
|
|
// e.g., 270 degrees (3pi/4 radians) theta=405 sin540(405) = -65536 (-1)
|
||
|
|
// e.g., 180 degrees (pi radians) theta=270 sin540(270) = 0 (0)
|
||
|
|
// e.g., 90 degrees (pi/2 radians) theta=135 sin540(135) = 65536 (+1)
|
||
|
|
// e.g., 45 degrees (pi/4 radians) theta= 67 sin540(67) = 46071 (sqrt(2)/2)
|
||
|
|
// e.g., 0 degrees (0 radians) theta= 0 sin540(0) = 0 (0)
|
||
|
|
// output: -65536 to +65536, fixed point resolution 1/65536
|
||
|
|
int32_t sin540(int32_t theta){
|
||
|
|
if((theta<0)||(theta>539)) return 0;
|
||
|
|
return SinTab540[theta];
|
||
|
|
}
|
||
|
|
|
||
|
|
int32_t cos540(int32_t theta){
|
||
|
|
if((theta<0)||(theta>539)) return 65536;
|
||
|
|
if(theta > 539) return -65536;
|
||
|
|
if(theta < 0) return 65536;
|
||
|
|
if(theta < 135) return SinTab540[135-theta]; // 0 to 90 deg
|
||
|
|
if(theta < 270) return -SinTab540[theta-270]; // 90 to 180 deg
|
||
|
|
if(theta < 405) return -SinTab540[405-theta]; // 180 to 270 deg
|
||
|
|
return SinTab540[theta-405]; // 270 to 539
|
||
|
|
}
|
||
|
|
|