Im not having any luck even removing the LUT functionality. the acos function is misbehaving now. Ive included my initial code below
#define FIXMATH_NO_CACHE
#define FIXMATH_FAST_SIN
#define FIXMATH_NO_ROUNDING
#include <fixmath.h>
#include <math.h>
float pi=3.141592654;
fix16_t coxa=fix16_from_float(4.4);
fix16_t tibia=fix16_from_float(10.0);
fix16_t femur=fix16_from_float(6.0);
fix16_t tibia2=fix16_mul(tibia,tibia);
fix16_t femur2=fix16_mul(tibia,tibia);
fix16_t t2mf2=fix16_sub(tibia2,femur2);
fix16_t t2pf2=fix16_add(tibia2,femur2);
fix16_t m2f=fix16_mul(fix16_from_float(-2.0),femur);
fix16_t m2tf=fix16_mul(m2f,tibia);
fix16_t x;
fix16_t y;
fix16_t x2;
fix16_t y2;
fix16_t legxh[] = {fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6)};
fix16_t legyh[] = {fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6)};
fix16_t leggammah[] = {fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6)};
fix16_t legxc[] = {fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6)};
fix16_t legyc[] = {fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6)};
fix16_t legx[] = {fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6)};
fix16_t legy[] = {fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6)};
fix16_t legz[] = {fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6)};
fix16_t legL1[] = {fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6)};
fix16_t legL[] = {fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6)};
fix16_t leggamma[] = {fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6)};
fix16_t legalpha[] = {fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6)};
fix16_t legbeta[] = {fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6),fix16_from_float(6)};
void setup() {
// put your setup code here, to run once:
for (int i=0; i <=5; i++){
leggammah=fix16_from_float(((2*i+1)/12)*pi);
legxh=fix16_mul(fix16_from_float(6),fix16_sin(leggammah));
legyh=fix16_mul(fix16_from_float(6),fix16_cos(leggammah));
legxc=fix16_mul(fix16_from_float(11),fix16_sin(leggammah));
legyc=fix16_mul(fix16_from_float(11),fix16_cos(leggammah));
legx=legxc;
legy=legyc;
legz=fix16_from_float(4.0);
}
}
void loop() {
// put your main code here, to run repeatedly:
for (int i=0; i <=5; i++){
x=fix16_sub(legx,legxh);
y=fix16_sub(legy,legyh);
x2=fix16_mul(legx,legx);
y2=fix16_mul(legy,legy);
legL1=fix16_sqrt(fix16_add(x2,y2));
leggamma=fix16_sub(fix16_atan2(x,y),leggammah);
fix16_t L2= fix16_sub(legL1,coxa);
fix16_t Lsq= fix16_add(fix16_mul(legz,legz),fix16_mul(L2,L2));
legL=fix16_sqrt(Lsq);
legalpha= fix16_add(fix16_acos(fix16_div(legz,legL)),fix16_acos(fix16_div(fix16_sub(t2mf2,Lsq),fix16_mul(m2f,legL))));
legbeta= fix16_acos(fix16_div(fix16_sub(Lsq,t2pf2),m2tf));