stmbl

#ifdef FB_TX
#error at the moment you can only have one component using the tx channel of the feeback port(res or encm)
#else
#define FB_TX
#endif

COMP(res);

HAL_PIN(pos) = 0.0;
HAL_PIN(amp) = 0.0;

/*HAL_PIN(sin0) = 0.0;
HAL_PIN(cos0) = 0.0;
HAL_PIN(sin1) = 0.0;
HAL_PIN(cos1) = 0.0;
HAL_PIN(sin2) = 0.0;
HAL_PIN(cos2) = 0.0;*/
HAL_PIN(sin) = 0.0;
HAL_PIN(cos) = 0.0;

HAL_PIN(enable) = 0.0;
HAL_PIN(error) = 0.0;

RT(
	GPIO_ResetBits(GPIOB,GPIO_Pin_3);//messpin

	float s = 0.0;
	float c = 0.0;
	float a = 0.0;

	s = PIN(sin);
  c = PIN(cos);
	a = s * s + c * c;

	if(a < 0.3 * 0.3){
		PIN(error) = 1.0;
	}
	else{
		PIN(error) = 0.0;
		PIN(pos) = atan2f(s, c);
	}
	PIN(amp) = a;
);


ENDCOMP;