stmbl

HAL_COMP(freq_fb);

HAL_PIN(vel) = 0.0;
HAL_PIN(a) = 0.0;
HAL_PIN(res) = 6.0;
HAL_PIN(t) = 0.025;
HAL_PIN(lp) = 0.5;

MEM(uint32_t lastpos) = 0;
MEM(float time) = 0.0;
MEM(float vel_lp) = 0.0;

RT_INIT(
  GPIO_InitTypeDef GPIO_InitStructure;
 
  // enable clocks
  RCC_APB1PeriphClockCmd(ENC1_TIM_RCC, ENABLE);

  // pin mode: af
  GPIO_InitStructure.GPIO_Pin = ENC1_A_PIN;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
  GPIO_Init(ENC1_A_PORT, &GPIO_InitStructure);

  // pin af -> tim
  GPIO_PinAFConfig(ENC1_A_PORT, ENC1_A_PIN_SOURCE, ENC1_A_AF_SOURCE);
  // GPIO_PinAFConfig(ENC1_B_PORT, ENC1_B_PIN_SOURCE, ENC1_B_AF_SOURCE);

  // enc res / turn
  TIM_SetAutoreload(ENC1_TIM, 5000-1);

  // quad
  TIM_Cmd(ENC1_TIM, DISABLE);
  GPIO_InitStructure.GPIO_Pin = ENC1_A_PIN;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
  GPIO_Init(ENC1_A_PORT, &GPIO_InitStructure);
  TIM_EncoderInterfaceConfig(ENC1_TIM, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
  
  TIM_ETRClockMode1Config(ENC1_TIM,TIM_ExtTRGPSC_OFF,TIM_ExtTRGPolarity_NonInverted,0x00);
  
  TIM_TIxExternalClockConfig(ENC1_TIM,TIM_TIxExternalCLK1Source_TI1,TIM_ICPolarity_Rising,0x00);
  
  TIM_Cmd(ENC1_TIM, ENABLE);
);

RT_DEINIT(
   TIM_Cmd(ENC1_TIM, DISABLE);
   TIM_DeInit(ENC1_TIM);
   RCC_APB2PeriphClockCmd(ENC1_TIM_RCC, DISABLE);
   GPIO_InitStructure.GPIO_Pin = ENC1_A_PIN | ENC1_B_PIN;
   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
   GPIO_Init(ENC1_A_PORT, &GPIO_InitStructure);
);

RT(
   float lp = PIN(lp);
   uint32_t tim = TIM_GetCounter(ENC1_TIM);
   if(time > PIN(t)){
      float vel = ((tim - lastpos + 5000)%5000)/PIN(res)/time;
      vel_lp = vel * lp + vel_lp * (1.0 - lp);
      PIN(vel) = vel_lp;
      time = 0;
      lastpos = tim;
   }
   time += period;

  if(GPIO_ReadInputDataBit(ENC1_A_PORT,ENC1_A_PIN)){//TODO: setup pin
     PIN(a) = 0.0;
  }else{
     PIN(a) = 1.0;
  }
  
);

ENDCOMP;