diff --git a/Firmware/include/MyEncoder.h b/Firmware/include/MyEncoder.h index 3fcf7fd..a6065e0 100755 --- a/Firmware/include/MyEncoder.h +++ b/Firmware/include/MyEncoder.h @@ -2,7 +2,7 @@ #define MYENCODER #include "Stm32F4_Encoder.h" #include -#define RINGBUFFERLEN 101 +#define RINGBUFFERLEN 11 class MyEncoder { @@ -30,6 +30,7 @@ private: CircularBuffer Pos; CircularBuffer TDelta; double curPos; + double oldFrequency; TIM_TypeDef *tim_base; }; diff --git a/Firmware/include/Stm32F4_Encoder.h b/Firmware/include/Stm32F4_Encoder.h index b0a8978..6ffc5c1 100755 --- a/Firmware/include/Stm32F4_Encoder.h +++ b/Firmware/include/Stm32F4_Encoder.h @@ -641,7 +641,8 @@ public: uint16_t GetCount(); }; -void rcc_config(); +void encoder_config(); +void encoder2_config(); // Experimental void GpioConfigPortA(GPIO_TypeDef *GPIOx); void GpioConfigPortC(GPIO_TypeDef *GPIOx); void GpioConfigPortD(GPIO_TypeDef *GPIOx); diff --git a/Firmware/src/MyEncoder.cpp b/Firmware/src/MyEncoder.cpp index 2bde1e7..b2439c2 100755 --- a/Firmware/src/MyEncoder.cpp +++ b/Firmware/src/MyEncoder.cpp @@ -61,14 +61,19 @@ double MyEncoder::frequency(uint64_t time) double diffT = 0; double diffPos = 0; + double frequency; TDelta.push(time); // Running average over the length of the circular buffer Pos.push(curPos); - if (Pos.size() >= 2) + if (Pos.size() == RINGBUFFERLEN) { - diffT = 1.0e-9 * (TDelta.last() - TDelta.first()); // Time is in nanoseconds + diffT = 1.0e-6 * (TDelta.last() - TDelta.first()); // Time is in microseconds diffPos = fabs(Pos.last() - Pos.first()); + frequency = diffPos / diffT; + oldFrequency = frequency; + return frequency; // Revolutions per second } - return diffT != 0 ? diffPos / diffT : 0.0; // Revolutions per second + else + return oldFrequency; } uint8_t MyEncoder::getIndexState() { diff --git a/Firmware/src/Stm32F4_Encoder.cpp b/Firmware/src/Stm32F4_Encoder.cpp index e16473c..7b9bf08 100755 --- a/Firmware/src/Stm32F4_Encoder.cpp +++ b/Firmware/src/Stm32F4_Encoder.cpp @@ -263,7 +263,7 @@ void GPIO_PinAF(GPIO_TypeDef *GPIOx, uint16_t GPIO_PinSource, uint8_t GPIO_AF) GPIOx->AFR[GPIO_PinSource >> 0x03] = temp_2; } -void rcc_config() +void encoder_config() { RCC->AHB1ENR |= 0x1; // GPIOA RCC->AHB1ENR |= 0x4; // GPIOC @@ -271,13 +271,13 @@ void rcc_config() RCC->AHB1ENR |= 0x10; // GPIOE RCC->APB1ENR |= 0x20000000; // ENABLE DAC - // RCC->APB2ENR |= 0x00000002; // APB2 TIM8 + // RCC->APB2ENR |= 0x00000002; // APB2 TIM8 RCC->APB1ENR |= 0x00000004; // APB1 TIM4 RCC->APB1ENR |= 0x00000001; // APB1 TIM2 - // RCC->APB1ENR |= 0x00000002; // APB1 TIM3 + // RCC->APB1ENR |= 0x00000002; // APB1 TIM3 GpioConfigPortA(GPIOA); - // GpioConfigPortC(GPIOC); + // GpioConfigPortC(GPIOC); GpioConfigPortD(GPIOD); #if 0 // Skipping since TIM8 is step generator and TIM3, chan4 is smae as TIM8, chan4 GPIO_PinAF(GPIOA, GPIO_PinSource6, GPIO_AF_TIM3); @@ -326,4 +326,66 @@ void rcc_config() TIM3->CNT = 0; #endif +} + +void encoder2_config() +{ +#if 0 +#include "mbed.h" +#include "stm32f4xx.h" +#include "stm32f4xx_hal_tim_ex.h" + + TIM_HandleTypeDef timer; + TIM_Encoder_InitTypeDef encoder; + + // direction to PA_9 -- step pulse to PA_8 + + int main() + { + + GPIO_InitTypeDef GPIO_InitStruct; + __TIM1_CLK_ENABLE(); + __GPIOA_CLK_ENABLE(); + GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_PULLDOWN; + GPIO_InitStruct.Speed = GPIO_SPEED_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF1_TIM1; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + timer.Instance = TIM1; + timer.Init.Period = 0xffff; + timer.Init.Prescaler = 0; + timer.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + timer.Init.CounterMode = TIM_COUNTERMODE_UP; + + encoder.EncoderMode = TIM_ENCODERMODE_TI12; + encoder.IC1Filter = 0x0f; + encoder.IC1Polarity = TIM_INPUTCHANNELPOLARITY_RISING; + encoder.IC1Prescaler = TIM_ICPSC_DIV4; + encoder.IC1Selection = TIM_ICSELECTION_DIRECTTI; + + encoder.IC2Filter = 0x0f; + encoder.IC2Polarity = TIM_INPUTCHANNELPOLARITY_FALLING; + encoder.IC2Prescaler = TIM_ICPSC_DIV4; + encoder.IC2Selection = TIM_ICSELECTION_DIRECTTI; + + HAL_TIM_Encoder_Init(&timer, &encoder); + HAL_TIM_Encoder_Start(&timer,TIM_CHANNEL_1); + + + TIM1->EGR = 1; // Generate an update event + TIM1->CR1 = 1; // Enable the counter + + + while (1) { + int16_t count1; + count1=TIM1->CNT; + + printf("%d\r\n", count1); + wait(1.0); + + }; +} +#endif } \ No newline at end of file diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index f3e8a53..c963375 100755 --- a/Firmware/src/main.cpp +++ b/Firmware/src/main.cpp @@ -15,10 +15,12 @@ HardwareTimer *syncTimer; // The timer that syncs "with linuxcnc cycle" uint16_t sync0CycleTime; // usecs #include "MyEncoder.h" +volatile uint16_t encCnt = 0; void indexPulseEncoderCB1(void); MyEncoder Encoder1(TIM2, PA2, indexPulseEncoderCB1); void indexPulseEncoderCB1(void) { + encCnt++; Encoder1.indexPulse(); } #include @@ -120,13 +122,13 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation { Obj.IndexStatus = Encoder1.indexHappened(); Obj.EncPos = Encoder1.currentPos(); - Obj.EncFrequency = Encoder1.frequency(ESCvar.Time); + Obj.EncFrequency = Encoder1.frequency(longTime.extendTime(micros())); Obj.IndexByte = Encoder1.getIndexState(); Obj.DiffT = nLoops; Obj.D1 = 1000 * Obj.CommandedPosition2; // abs(1000 * (ap2 - Obj.CommandedPosition2)); // Step2.actPos(); Obj.D2 = 1000 * Obj.ActualPosition2; // Step->stepgen_array[1].pos_fb; // Step->stepgen_array[1].rawcount % INT16_MAX; // Step->stepgen_array[1].freq; - Obj.D3 = irqCnt % 256; // Step->stepgen_array[1].freq; + Obj.D3 = encCnt % 256; // Step->stepgen_array[1].freq; Obj.D4 = 1000 * (Obj.CommandedPosition2 - oldCommandedPosition); // deltaMakePulsesCnt; // Step->stepgen_array[1].rawcount % UINT16_MAX; oldCommandedPosition = Obj.CommandedPosition2; } @@ -166,7 +168,9 @@ void setup(void) #if 0 measureCrystalFrequency(); // Calibrate crystal frequency #endif - rcc_config(); // Needed by encoder, probably breaks some timers. + Step = new StepGen3; + + encoder_config(); // Needed by encoder, probably breaks some timers. ecat_slv_init(&config); pinMode(PA11, OUTPUT); // Step X @@ -174,7 +178,7 @@ void setup(void) pinMode(PC9, OUTPUT); // Step Z pinMode(PC10, OUTPUT); // Dir Z - Step = new StepGen3; + baseTimer = new HardwareTimer(TIM11); // The base period timer setFrequencyAdjustedMicrosSeconds(baseTimer, BASE_PERIOD / 1000); diff --git a/linuxcnc/metalmusings_encoder.comp b/linuxcnc/metalmusings_encoder.comp index 80c146e..878f38f 100644 --- a/linuxcnc/metalmusings_encoder.comp +++ b/linuxcnc/metalmusings_encoder.comp @@ -10,5 +10,12 @@ license "GPL"; //main function FUNCTION(_) { index_latch_enable = index_c_enable; - index_c_enable = index_status; + if (index_latch_enable) { + if (index_status) { + index_c_enable = 0; + } + // else wait for index-status + } else { + index_c_enable = index_status; + } }