stmbl

COMP(enc);

HAL_PIN(res0) = 2048.0;
HAL_PIN(pos0) = 0.0;
HAL_PIN(reverse0) = 0.0;


HAL_PIN(res1) = 2048.0;
HAL_PIN(pos1) = 0.0;
HAL_PIN(reverse1) = 0.0;
HAL_PIN(ireverse1) = 0.0;
HAL_PIN(ipos1) = 0.0;
HAL_PIN(iposd1) = 0.0;
HAL_PIN(s) = 0.0;
HAL_PIN(c) = 0.0;



MEM(int e_res0) = 0.0;
MEM(int e_res1) = 0.0;

MEM(float s_) = 0.0;
MEM(float c_) = 0.0;

MEM(float smin) = 16384.0;
MEM(float smax) = 0.0;
MEM(float cmin) = 16384.0;
MEM(float cmax) = 0.0;


INIT(
  e_res0 = (int)PIN(res0);

  // enable pin clock
  RCC_AHB1PeriphClockCmd(ENC0_A_IO_RCC, ENABLE);

  // enable clock
  RCC_APB2PeriphClockCmd(ENC0_TIM_RCC, ENABLE);

  // pin mode: af
  GPIO_InitTypeDef GPIO_InitStructure;
  GPIO_InitStructure.GPIO_Pin = ENC0_A_PIN | ENC0_B_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(ENC0_A_PORT, &GPIO_InitStructure);

  // pin af -> tim
  GPIO_PinAFConfig(ENC0_A_PORT, ENC0_A_PIN_SOURCE, ENC0_A_AF_SOURCE);
  GPIO_PinAFConfig(ENC0_B_PORT, ENC0_B_PIN_SOURCE, ENC0_B_AF_SOURCE);

  // tim mode: count on A and B on rising edge
  TIM_EncoderInterfaceConfig(ENC0_TIM, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);

  // enc res / turn
  TIM_SetAutoreload(ENC0_TIM, e_res0 - 1);

  // enable tim
  TIM_Cmd(ENC0_TIM, ENABLE);

  e_res1 = (int)PIN(res1);

  // enable pin clock
  RCC_AHB1PeriphClockCmd(ENC1_A_IO_RCC, ENABLE);

  // enable clock
  RCC_APB1PeriphClockCmd(ENC1_TIM_RCC, ENABLE);

  // pin mode: af
  GPIO_InitStructure.GPIO_Pin = ENC1_A_PIN | ENC1_B_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);

  // tim mode: count on A and B on rising edge
  TIM_EncoderInterfaceConfig(ENC1_TIM, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);

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

  // enable tim
  TIM_Cmd(ENC1_TIM, ENABLE);
);

FRT(
  float si[ADC_ANZ];
  float co[ADC_ANZ];
  uint32_t sc[ADC_ANZ];

  for(int i = 0; i < PID_WAVES; i++){
    sc[i] = 0.0;

    for(int j = 0; j < ADC_ANZ; j++){
      //si[i] += (uint16_t)(ADC_DMA_Buffer[i * ADC_ANZ + j] & 0x0000ffff);
      //co[i] += (uint16_t)(ADC_DMA_Buffer[i * ADC_ANZ + j] >> 16);
      sc[i] += ADC_DMA_Buffer[i * ADC_ANZ + j];
    }
  }

  for(int i = 0; i < PID_WAVES; i++){
    si[i] = 0.0;
    co[i] = 0.0;

    si[i] = sc[i] & 0x0000ffff;
    co[i] = sc[i] >> 16;
  }

  //PIN(iamp) = si[PID_WAVES - 1] * si[PID_WAVES - 1] + co[PID_WAVES - 1] * co[PID_WAVES - 1]; // nur letzte wave

  if(PIN(ireverse1) > 0.0){
    s_ = co[PID_WAVES - 1] - 18000;
    c_ = si[PID_WAVES - 1] - 18000;
  }
  else{
    s_ = si[PID_WAVES - 1] - 18000;
    c_ = co[PID_WAVES - 1] - 18000;
  }

  PIN(s) = s_;
  PIN(c) = c_;

);

RT_IN(
  float p = 0.0;
  int i = (int)PIN(res0);

  p = mod(TIM_GetCounter(ENC0_TIM) * 2.0f * M_PI / (float)e_res0);
  if(PIN(reverse0) > 0.0){
    p = minus(0, p);
  }
  PIN(pos0) = p;
  if(e_res0 != i){
    e_res0 = i;
    TIM_SetAutoreload(ENC0_TIM, e_res0 - 1);
  }

  i = (int)PIN(res1);
  p = mod(TIM_GetCounter(ENC1_TIM) * 2.0f * M_PI / (float)e_res1);
  if(PIN(reverse1) > 0.0){
    p = minus(0, p);
  }
  PIN(pos1) = p;
  PIN(ipos1) = mod(p + mod(atan2f(s_, c_) * 4.0 + M_PI) / (float)e_res1);
  PIN(iposd1) = mod(atan2f(s_, c_) * 4.0) / 4.0;

  if(e_res1 != i){
    e_res1 = i;
    TIM_SetAutoreload(ENC1_TIM, e_res1 - 1);
  }

);

ENDCOMP;