Trikarus/firmware_smartstepper_trikarus/stepper_nano_zero/sine.cpp

/*
 * since.cpp
 *
 *  Created on: Dec 24, 2016
 *      Author: tstern
 *
	Copyright (C) 2018  MisfitTech,  All rights reserved.

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <https://www.gnu.org/licenses/>.

    Written by Trampas Stern for MisfitTech.

    Misfit Tech invests time and resources providing this open source code,
    please support MisfitTech and open-source hardware by purchasing
	products from MisfitTech, www.misifittech.net!
 *********************************************************************/

#include "sine.h"

#pragma GCC push_options
#pragma GCC optimize ("-Ofast")

#ifdef NZS_FAST_SINE
static const int16_t sineTable[1280]={
0,201,402,603,804,1005,1206,1407,1608,1808,2009,2210,2410,2611,2811,3011,
3212,3412,3611,3811,4011,4210,4410,4609,4808,5007,5205,5404,5602,5800,5998,6195,
6392,6589,6786,6983,7179,7375,7571,7766,7962,8156,8351,8545,8739,8933,9126,9319,
9512,9704,9896,10087,10278,10469,10659,10849,11039,11228,11417,11605,11793,11980,12167,12353,
12539,12725,12910,13094,13278,13462,13645,13828,14010,14191,14372,14553,14732,14912,15090,15269,
15446,15623,15800,15976,16151,16325,16499,16673,16846,17018,17189,17360,17530,17700,17869,18037,
18204,18371,18537,18703,18868,19032,19195,19357,19519,19680,19841,20000,20159,20317,20475,20631,
20787,20942,21097,21250,21403,21554,21705,21856,22005,22154,22301,22448,22594,22739,22884,23027,
23170,23312,23452,23592,23732,23870,24007,24143,24279,24413,24547,24680,24812,24942,25072,25201,
25329,25456,25583,25708,25832,25955,26077,26199,26319,26438,26556,26674,26790,26905,27019,27133,
27245,27356,27466,27575,27683,27791,27897,28001,28105,28208,28310,28411,28510,28609,28706,28803,
28898,28992,29085,29178,29268,29358,29447,29535,29621,29707,29791,29874,29956,30037,30117,30195,
30273,30349,30425,30499,30572,30643,30714,30783,30852,30919,30985,31050,31113,31176,31237,31297,
31356,31414,31471,31526,31580,31633,31685,31736,31785,31834,31881,31926,31971,32015,32057,32098,
32138,32176,32214,32250,32285,32319,32351,32382,32413,32441,32469,32496,32521,32545,32568,32589,
32609,32629,32646,32663,32678,32693,32706,32717,32728,32737,32745,32752,32757,32762,32765,32767,
32767,32767,32765,32762,32757,32752,32745,32737,32728,32717,32706,32693,32678,32663,32646,32629,
32609,32589,32568,32545,32521,32496,32469,32441,32413,32382,32351,32319,32285,32250,32214,32176,
32138,32098,32057,32015,31971,31926,31881,31834,31785,31736,31685,31633,31580,31526,31471,31414,
31356,31297,31237,31176,31113,31050,30985,30919,30852,30783,30714,30643,30572,30499,30425,30349,
30273,30195,30117,30037,29956,29874,29791,29707,29621,29535,29447,29358,29268,29178,29085,28992,
28898,28803,28706,28609,28510,28411,28310,28208,28105,28001,27897,27791,27683,27575,27466,27356,
27245,27133,27019,26905,26790,26674,26556,26438,26319,26199,26077,25955,25832,25708,25583,25456,
25329,25201,25072,24942,24812,24680,24547,24413,24279,24143,24007,23870,23732,23592,23452,23312,
23170,23027,22884,22739,22594,22448,22301,22154,22005,21856,21705,21554,21403,21250,21097,20942,
20787,20631,20475,20317,20159,20000,19841,19680,19519,19357,19195,19032,18868,18703,18537,18371,
18204,18037,17869,17700,17530,17360,17189,17018,16846,16673,16499,16325,16151,15976,15800,15623,
15446,15269,15090,14912,14732,14553,14372,14191,14010,13828,13645,13462,13278,13094,12910,12725,
12539,12353,12167,11980,11793,11605,11417,11228,11039,10849,10659,10469,10278,10087,9896,9704,
9512,9319,9126,8933,8739,8545,8351,8156,7962,7766,7571,7375,7179,6983,6786,6589,
6392,6195,5998,5800,5602,5404,5205,5007,4808,4609,4410,4210,4011,3811,3611,3412,
3212,3011,2811,2611,2410,2210,2009,1808,1608,1407,1206,1005,804,603,402,201,
0,-201,-402,-603,-804,-1005,-1206,-1407,-1608,-1809,-2010,-2210,-2411,-2611,-2812,-3012,
-3212,-3412,-3612,-3812,-4011,-4211,-4410,-4609,-4808,-5007,-5206,-5404,-5602,-5800,-5998,-6196,
-6393,-6590,-6787,-6983,-7180,-7376,-7571,-7767,-7962,-8157,-8352,-8546,-8740,-8933,-9127,-9320,
-9512,-9704,-9896,-10088,-10279,-10470,-10660,-10850,-11039,-11228,-11417,-11605,-11793,-11980,-12167,-12354,
-12540,-12725,-12910,-13095,-13279,-13463,-13646,-13828,-14010,-14192,-14373,-14553,-14733,-14912,-15091,-15269,
-15447,-15624,-15800,-15976,-16151,-16326,-16500,-16673,-16846,-17018,-17190,-17361,-17531,-17700,-17869,-18037,
-18205,-18372,-18538,-18703,-18868,-19032,-19195,-19358,-19520,-19681,-19841,-20001,-20160,-20318,-20475,-20632,
-20788,-20943,-21097,-21250,-21403,-21555,-21706,-21856,-22006,-22154,-22302,-22449,-22595,-22740,-22884,-23028,
-23170,-23312,-23453,-23593,-23732,-23870,-24007,-24144,-24279,-24414,-24548,-24680,-24812,-24943,-25073,-25202,
-25330,-25457,-25583,-25708,-25832,-25956,-26078,-26199,-26319,-26439,-26557,-26674,-26790,-26906,-27020,-27133,
-27245,-27357,-27467,-27576,-27684,-27791,-27897,-28002,-28106,-28209,-28310,-28411,-28511,-28609,-28707,-28803,
-28899,-28993,-29086,-29178,-29269,-29359,-29448,-29535,-29622,-29707,-29791,-29875,-29957,-30038,-30117,-30196,
-30273,-30350,-30425,-30499,-30572,-30644,-30715,-30784,-30852,-30919,-30985,-31050,-31114,-31176,-31238,-31298,
-31357,-31415,-31471,-31527,-31581,-31634,-31686,-31736,-31786,-31834,-31881,-31927,-31972,-32015,-32057,-32098,
-32138,-32177,-32214,-32250,-32285,-32319,-32352,-32383,-32413,-32442,-32470,-32496,-32521,-32545,-32568,-32590,
-32610,-32629,-32647,-32664,-32679,-32693,-32706,-32718,-32728,-32738,-32746,-32752,-32758,-32762,-32765,-32767,
-32768,-32767,-32765,-32762,-32758,-32752,-32746,-32738,-32728,-32718,-32706,-32693,-32679,-32664,-32647,-32629,
-32610,-32590,-32568,-32545,-32521,-32496,-32470,-32442,-32413,-32383,-32352,-32319,-32285,-32250,-32214,-32177,
-32138,-32098,-32057,-32015,-31972,-31927,-31881,-31834,-31786,-31736,-31686,-31634,-31581,-31527,-31471,-31415,
-31357,-31298,-31238,-31176,-31114,-31050,-30985,-30919,-30852,-30784,-30715,-30644,-30572,-30499,-30425,-30350,
-30273,-30196,-30117,-30038,-29957,-29875,-29791,-29707,-29622,-29535,-29448,-29359,-29269,-29178,-29086,-28993,
-28899,-28803,-28707,-28609,-28511,-28411,-28310,-28209,-28106,-28002,-27897,-27791,-27684,-27576,-27467,-27357,
-27245,-27133,-27020,-26906,-26790,-26674,-26557,-26439,-26319,-26199,-26078,-25956,-25832,-25708,-25583,-25457,
-25330,-25202,-25073,-24943,-24812,-24680,-24548,-24414,-24279,-24144,-24007,-23870,-23732,-23593,-23453,-23312,
-23170,-23028,-22884,-22740,-22595,-22449,-22302,-22154,-22006,-21856,-21706,-21555,-21403,-21250,-21097,-20943,
-20788,-20632,-20475,-20318,-20160,-20001,-19841,-19681,-19520,-19358,-19195,-19032,-18868,-18703,-18538,-18372,
-18205,-18037,-17869,-17700,-17531,-17361,-17190,-17018,-16846,-16673,-16500,-16326,-16151,-15976,-15800,-15624,
-15447,-15269,-15091,-14912,-14733,-14553,-14373,-14192,-14010,-13828,-13646,-13463,-13279,-13095,-12910,-12725,
-12540,-12354,-12167,-11980,-11793,-11605,-11417,-11228,-11039,-10850,-10660,-10470,-10279,-10088,-9896,-9704,
-9512,-9320,-9127,-8933,-8740,-8546,-8352,-8157,-7962,-7767,-7571,-7376,-7180,-6983,-6787,-6590,
-6393,-6196,-5998,-5800,-5602,-5404,-5206,-5007,-4808,-4609,-4410,-4211,-4011,-3812,-3612,-3412,
-3212,-3012,-2812,-2611,-2411,-2210,-2010,-1809,-1608,-1407,-1206,-1005,-804,-603,-402,-201,
0,201,402,603,804,1005,1206,1407,1608,1808,2009,2210,2410,2611,2811,3011,
3212,3412,3611,3811,4011,4210,4410,4609,4808,5007,5205,5404,5602,5800,5998,6195,
6392,6589,6786,6983,7179,7375,7571,7766,7962,8156,8351,8545,8739,8933,9126,9319,
9512,9704,9896,10087,10278,10469,10659,10849,11039,11228,11417,11605,11793,11980,12167,12353,
12539,12725,12910,13094,13278,13462,13645,13828,14010,14191,14372,14553,14732,14912,15090,15269,
15446,15623,15800,15976,16151,16325,16499,16673,16846,17018,17189,17360,17530,17700,17869,18037,
18204,18371,18537,18703,18868,19032,19195,19357,19519,19680,19841,20000,20159,20317,20475,20631,
20787,20942,21097,21250,21403,21554,21705,21856,22005,22154,22301,22448,22594,22739,22884,23027,
23170,23312,23452,23592,23732,23870,24007,24143,24279,24413,24547,24680,24812,24942,25072,25201,
25329,25456,25583,25708,25832,25955,26077,26199,26319,26438,26556,26674,26790,26905,27019,27133,
27245,27356,27466,27575,27683,27791,27897,28001,28105,28208,28310,28411,28510,28609,28706,28803,
28898,28992,29085,29178,29268,29358,29447,29535,29621,29707,29791,29874,29956,30037,30117,30195,
30273,30349,30425,30499,30572,30643,30714,30783,30852,30919,30985,31050,31113,31176,31237,31297,
31356,31414,31471,31526,31580,31633,31685,31736,31785,31834,31881,31926,31971,32015,32057,32098,
32138,32176,32214,32250,32285,32319,32351,32382,32413,32441,32469,32496,32521,32545,32568,32589,
32609,32629,32646,32663,32678,32693,32706,32717,32728,32737,32745,32752,32757,32762,32765,32767
};
#else
static const uint16_t sineTable[257]={
		0,402,804,1206,1608,2010,2412,2814,3216,3617,4019,4420,4821,5222,5623,6023,
		6424,6824,7223,7623,8022,8421,8820,9218,9616,10014,10411,10808,11204,11600,11996,12391,
		12785,13179,13573,13966,14359,14751,15142,15533,15924,16313,16703,17091,17479,17866,18253,18639,
		19024,19408,19792,20175,20557,20939,21319,21699,22078,22456,22834,23210,23586,23960,24334,24707,
		25079,25450,25820,26189,26557,26925,27291,27656,28020,28383,28745,29106,29465,29824,30181,30538,
		30893,31247,31600,31952,32302,32651,32999,33346,33692,34036,34379,34721,35061,35400,35738,36074,
		36409,36743,37075,37406,37736,38064,38390,38715,39039,39361,39682,40001,40319,40635,40950,41263,
		41575,41885,42194,42500,42806,43109,43411,43712,44011,44308,44603,44897,45189,45479,45768,46055,
		46340,46624,46905,47185,47464,47740,48014,48287,48558,48827,49095,49360,49624,49885,50145,50403,
		50659,50913,51166,51416,51664,51911,52155,52398,52638,52877,53113,53348,53580,53811,54039,54266,
		54490,54713,54933,55151,55367,55582,55794,56003,56211,56417,56620,56822,57021,57218,57413,57606,
		57797,57985,58171,58356,58537,58717,58895,59070,59243,59414,59582,59749,59913,60075,60234,60391,
		60546,60699,60850,60998,61144,61287,61429,61567,61704,61838,61970,62100,62227,62352,62475,62595,
		62713,62829,62942,63053,63161,63267,63371,63472,63571,63668,63762,63853,63943,64030,64114,64196,
		64276,64353,64428,64500,64570,64638,64703,64765,64826,64883,64939,64992,65042,65090,65136,65179,
		65219,65258,65293,65327,65357,65386,65412,65435,65456,65475,65491,65504,65515,65524,65530,65534,
		65535,
};
#endif




int16_t sine(uint16_t angle)
{
#ifdef NZS_FAST_SINE
	return sineTable[angle];
#else
	int sign=1;
	int16_t ret;
	//our sine table has 1024 points per rotation so convert angle to closest step

	if (angle>=(SINE_STEPS/2))
	{
		sign=-1;
	}

	angle=angle % (SINE_STEPS/2); //limit to 0-180 as sign takes care of 180-360

	if (angle>(SINE_STEPS/4-1))  //if we are greater than 90 we need to look up table backwards
	{
		angle=(SINE_STEPS/2)-angle;
	}

	ret=(int16_t)(sineTable[angle]/2)*sign;
	return ret;
#endif
}

int16_t cosine(uint16_t angle)
{
#ifdef NZS_FAST_SINE
	angle=angle+(SINE_STEPS/4);
	return sineTable[angle];
#else

	int sign=1;
	int16_t ret;
	//our sine table has 1024 points per rotation so convert angle to closest step

	if (angle>=(SINE_STEPS/4) and angle<(3*(SINE_STEPS/4)))
	{
		sign=-1;
	}

	angle=angle % (SINE_STEPS/2); //limit to 0-180 as sign takes care of 180-360

	if (angle>(SINE_STEPS/4-1))  //if we are greater than 90 we need to look up table backwards
	{
		angle=(SINE_STEPS/2)-angle;
	}

	//for cosine we need 90 degree phase shift
	angle=(SINE_STEPS/4)-angle;

	ret=(int16_t)(sineTable[angle]/2)*sign;
	return ret;
#endif
}

#pragma GCC pop_options