Encoder Mode - konwersja danych

[ps19]

Witam,

Mam mały problem zdaje się algorytmiczny i nie mam pomysłu jak go rozwiązać. Mam enkoder 10 bitowy podpięty do licznika 16-bitowego (tak wiem są 32 bitowe ale przy robieniu PCB o tym nie pomyślałem ) i o ile zlicza translację -+> przejechaną odległość poprawnie przy zliczaniu w górę oraz w dół dopóki nie osiągnie zera i wtedy (translacja też się zeruje) i nie mam pomysłu jak to ominąć

Kod: Zaznacz cały

static void encoderTimerInit(TIM_TypeDef *tim)
{
	__IO uint32_t arrVal =  0xFFFF;
	__IO uint32_t ccmr1Val = TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0 |
			TIM_CCMR1_IC1F_3 | TIM_CCMR1_IC1F_2 | TIM_CCMR1_IC1F_1 | TIM_CCMR1_IC1F_0 |
			TIM_CCMR1_IC2F_3 | TIM_CCMR1_IC2F_2 | TIM_CCMR1_IC2F_1 | TIM_CCMR1_IC2F_0;
	__IO uint32_t smcrVal = TIM_SMCR_SMS_1 | TIM_SMCR_SMS_0;
	__IO uint32_t dierVal = TIM_DIER_UIE;
	__IO uint32_t cr1Val = TIM_CR1_CEN;

	//ENC2
	tim->ARR = arrVal;
	tim->CCMR1 =  ccmr1Val;
	encoderTimerChangePolarization(tim, FORWARD);
	tim->SMCR = smcrVal;
	//tim->DIER = dierVal;
	tim->CR1 |= cr1Val;

	tim->CNT = 0;
}

void encodersInit(void)
{
	RCC->APB1ENR |= RCC_APB1ENR_TIM3EN | RCC_APB1ENR_TIM4EN;

	gpio_pin_cfg(ENC1_A_PORT, ENC1_A_PIN, GPIO_AF2_PP_100MHz);
	gpio_pin_cfg(ENC1_B_PORT, ENC1_B_PIN, GPIO_AF2_PP_100MHz);

	gpio_pin_cfg(ENC2_A_PORT, ENC2_A_PIN, GPIO_AF2_PP_100MHz);
	gpio_pin_cfg(ENC2_B_PORT, ENC2_B_PIN, GPIO_AF2_PP_100MHz);

	encoderTimerInit(TIM_ENC_LEFT);
	encoderTimerInit(TIM_ENC_RIGHT);
	NVIC_EnableIRQ(TIM_ENC_LEFT_IRQn);
	NVIC_EnableIRQ(TIM_ENC_RIGHT_IRQn);
}

void ProcessEncoders(void)
{
	encoderMilimeterSpeedRead(&encoderLeft, TIM_ENC_LEFT);
	encoderMilimeterSpeedRead(&encoderRight, TIM_ENC_RIGHT);
}
void encoderMilimeterSpeedRead(encoderTypedef *enc, TIM_TypeDef *tim)
{
	enc->timerImpulsesPrev = enc->timerImpulsesPresent;
	enc->distanceMetersPrev = enc->distanceMeters;
	enc->timerImpulsesPresent = tim->CNT;
	enc->timerImpulsesDiff = (enc->timerImpulsesPresent - enc->timerImpulsesPrev);

	enc->timerImpulsesTotal += enc->timerImpulsesDiff;
	enc->distanceMeters = convertTicksToMeters(enc->timerImpulsesTotal);
	enc->distanceMetersDiff = enc->distanceMeters - enc->distanceMetersPrev;
}

float32_t convertTicksToMeters(float32_t distTicks)
{
	return -(((ENCODER_MAX - distTicks)/ENCODER_TICKS)*(M_PI*WHEEL_DIAMETER));
}

float32_t convertMetersToTicks(float32_t distMeters)
{
	return ((distMeters*ENCODER_TICKS))/(M_PI*WHEEL_DIAMETER) -ENCODER_MAX_TICKS;
}

Kod: Zaznacz cały

#define ENCODER_MAX (UINT16_MAX/2)
#define ENCODER_MAX_TICKS (-(((ENCODER_MAX - ENCODER_MAX)/ENCODER_TICKS)*(M_PI*WHEEL_DIAMETER)));
typedef struct {
	volatile uint32_t timerImpulsesPresent_u32, timerImpulsesMul_u32, timerImpulsesOverflowsMul_u32;
	volatile int32_t timerImpulsesDiff_s32, timerImpulsesTemp_s32;
	volatile int32_t timerImpulsesTotal_s32, timerImpulsesPrev_s32, timerImpulsesTotalTemp_s32;
	volatile float32_t distanceMeters_f32, distanceMetersPrev_f32, distanceMetersDiff_f32;
} encoderTypedef;

encoderTypedef encoderLeft;
encoderTypedef encoderRight;

[SunRiver]

Przyznam się ze nie analizowałem kodu dokładnie , pobieżnie wygląda całkiem ok
może zaproponuję rozwiązanie mało eleganckie , ale moze pomocne
a gdyby tak zapamiętywać wartość translacji w dodatkowej zmiennej i potem dodawał następne translacje to tej ,
i tak przy każdym przejściu przez zero ??

[ps19]

Zrobiłem jescze inaczej i zdaje się, że działa. Przerwanie wyłączyłem bo przepełnienie obchodzą if`em służącym za filtr dolnoprzepustowy - nie wskakuje wartość 65535 co powodowało obliczenie błędnej translacji.

Kod: Zaznacz cały

void encoderMilimeterSpeedRead(encoderTypedef *enc, TIM_TypeDef *tim)
{
	enc->timerImpulsesPresent_u32 = tim->CNT;
	enc->timerImpulsesDiff_s32 = (enc->timerImpulsesPresent_u32 - enc->timerImpulsesPrev_s32);

	if(abs(enc->timerImpulsesDiff_s32) < 1024) //1024 imp/10ms --> ~15m/s (max theoretical robot speed 7.26m/s)
	{
		enc->timerImpulsesTotal_s32 += enc->timerImpulsesDiff_s32;
		enc->distanceMeters_f32 = convertTicksToMeters(enc->timerImpulsesTotal_s32);
		enc->distanceMetersDiff_f32 = enc->distanceMeters_f32 - enc->distanceMetersPrev_f32;
	}

	enc->timerImpulsesPrev_s32 = enc->timerImpulsesPresent_u32;
	enc->distanceMetersPrev_f32 = enc->distanceMeters_f32;
}