diff --git a/common/pimoroni_common.hpp b/common/pimoroni_common.hpp index 6fc0a903..990bc99f 100644 --- a/common/pimoroni_common.hpp +++ b/common/pimoroni_common.hpp @@ -74,4 +74,20 @@ namespace pimoroni { 162, 163, 165, 167, 169, 170, 172, 174, 176, 178, 179, 181, 183, 185, 187, 189, 191, 193, 194, 196, 198, 200, 202, 204, 206, 208, 210, 212, 214, 216, 218, 220, 222, 224, 227, 229, 231, 233, 235, 237, 239, 241, 244, 246, 248, 250, 252, 255}; + + struct pin_pair { + union { + uint8_t first; + uint8_t a; + uint8_t positive; + }; + union { + uint8_t second; + uint8_t b; + uint8_t negative; + }; + + pin_pair() : first(0), second(0) {} + pin_pair(uint8_t first, uint8_t second) : first(first), second(second) {} + }; } \ No newline at end of file diff --git a/drivers/encoder-pio/capture.cpp b/drivers/encoder-pio/capture.cpp index e0208f08..9e1b428d 100644 --- a/drivers/encoder-pio/capture.cpp +++ b/drivers/encoder-pio/capture.cpp @@ -8,8 +8,8 @@ namespace pimoroni { // CONSTRUCTORS //////////////////////////////////////////////////////////////////////////////////////////////////// Capture::Capture(int32_t captured_count, int32_t count_change, float average_frequency, float counts_per_revolution) : - captured_count(captured_count), count_change(count_change), average_frequency(average_frequency), - counts_per_revolution(std::max(counts_per_revolution, FLT_MIN)) { //Clamp counts_per_rev to avoid potential NaN + captured_count(captured_count), capture_count_change(count_change), average_frequency(average_frequency), + counts_per_revolution(MAX(counts_per_revolution, FLT_MIN)) { //Clamp counts_per_rev to avoid potential NaN } @@ -17,53 +17,53 @@ namespace pimoroni { //////////////////////////////////////////////////////////////////////////////////////////////////// // METHODS //////////////////////////////////////////////////////////////////////////////////////////////////// - int32_t Capture::get_count() const { + int32_t Capture::count() const { return captured_count; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_revolutions() const { - return (float)get_count() / counts_per_revolution; + float Capture::revolutions() const { + return (float)count() / counts_per_revolution; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_angle_degrees() const { - return get_revolutions() * 360.0f; + float Capture::angle_degrees() const { + return revolutions() * 360.0f; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_angle_radians() const { - return get_revolutions() * M_TWOPI; + float Capture::angle_radians() const { + return revolutions() * M_TWOPI; } //////////////////////////////////////////////////////////////////////////////////////////////////// - int32_t Capture::get_count_change() const { - return count_change; + int32_t Capture::count_change() const { + return capture_count_change; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_frequency() const { + float Capture::frequency() const { return average_frequency; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_revolutions_per_second() const { - return get_frequency() / counts_per_revolution; + float Capture::revolutions_per_second() const { + return frequency() / counts_per_revolution; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_revolutions_per_minute() const { - return get_revolutions_per_second() * 60.0f; + float Capture::revolutions_per_minute() const { + return revolutions_per_second() * 60.0f; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_degrees_per_second() const { - return get_revolutions_per_second() * 360.0f; + float Capture::degrees_per_second() const { + return revolutions_per_second() * 360.0f; } //////////////////////////////////////////////////////////////////////////////////////////////////// - float Capture::get_radians_per_second() const { - return get_revolutions_per_second() * M_TWOPI; + float Capture::radians_per_second() const { + return revolutions_per_second() * M_TWOPI; } //////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/drivers/encoder-pio/capture.hpp b/drivers/encoder-pio/capture.hpp index 458daebe..d5e46168 100644 --- a/drivers/encoder-pio/capture.hpp +++ b/drivers/encoder-pio/capture.hpp @@ -10,7 +10,7 @@ namespace pimoroni { //-------------------------------------------------- private: const int32_t captured_count = 0; - const int32_t count_change = 0; + const int32_t capture_count_change = 0; const float average_frequency = 0.0f; const float counts_per_revolution = 1; @@ -20,25 +20,26 @@ namespace pimoroni { //-------------------------------------------------- public: Capture() {} - Capture(int32_t captured_count, int32_t count_change, float average_frequency, float counts_per_revolution); + Capture(int32_t captured_count, int32_t count_change, + float average_frequency, float counts_per_revolution); //-------------------------------------------------- // Methods //-------------------------------------------------- public: - int32_t get_count() const; - float get_revolutions() const; - float get_angle_degrees() const; - float get_angle_radians() const; + int32_t count() const; + float revolutions() const; + float angle_degrees() const; + float angle_radians() const; - int32_t get_count_change() const; + int32_t count_change() const; - float get_frequency() const; - float get_revolutions_per_second() const; - float get_revolutions_per_minute() const; - float get_degrees_per_second() const; - float get_radians_per_second() const; + float frequency() const; + float revolutions_per_second() const; + float revolutions_per_minute() const; + float degrees_per_second() const; + float radians_per_second() const; }; } \ No newline at end of file diff --git a/drivers/encoder-pio/encoder.cpp b/drivers/encoder-pio/encoder.cpp index 908472d7..873d7673 100644 --- a/drivers/encoder-pio/encoder.cpp +++ b/drivers/encoder-pio/encoder.cpp @@ -9,325 +9,347 @@ namespace pimoroni { - //////////////////////////////////////////////////////////////////////////////////////////////////// - // STATICS - //////////////////////////////////////////////////////////////////////////////////////////////////// - Encoder* Encoder::pio_encoders[][NUM_PIO_STATE_MACHINES] = { { nullptr, nullptr, nullptr, nullptr }, { nullptr, nullptr, nullptr, nullptr } }; - uint8_t Encoder::pio_claimed_sms[] = { 0x0, 0x0 }; +//////////////////////////////////////////////////////////////////////////////////////////////////// +// STATICS +//////////////////////////////////////////////////////////////////////////////////////////////////// +Encoder* Encoder::encoders[][NUM_PIO_STATE_MACHINES] = { { nullptr, nullptr, nullptr, nullptr }, { nullptr, nullptr, nullptr, nullptr } }; +uint8_t Encoder::claimed_sms[] = { 0x0, 0x0 }; +uint Encoder::pio_program_offset[] = { 0, 0 }; - //////////////////////////////////////////////////////////////////////////////////////////////////// - void Encoder::pio0_interrupt_callback() { - // Go through each of encoders on this PIO to see which triggered this interrupt - for(uint8_t sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) { - if(pio_encoders[0][sm] != nullptr) { - pio_encoders[0][sm]->check_for_transition(); + +Encoder::Encoder(PIO pio, uint sm, const pin_pair &pins, uint pin_c, + float counts_per_revolution, bool count_microsteps, + uint16_t freq_divider) +: pio(pio) +, sm(sm) +, enc_pins(pins) +, pin_c(pin_c) +, counts_per_revolution(counts_per_revolution) +, count_microsteps(count_microsteps) +, freq_divider(freq_divider) +, clocks_per_time((float)(clock_get_hz(clk_sys) / (ENC_LOOP_CYCLES * freq_divider))) { +} + +Encoder::~Encoder() { + if(initialised) { + pio_sm_set_enabled(pio, sm, false); + pio_sm_unclaim(pio, sm); + + uint pio_idx = pio_get_index(pio); + encoders[pio_idx][sm] = nullptr; + claimed_sms[pio_idx] &= ~(1u << sm); + + hw_clear_bits(&pio->inte1, PIO_IRQ1_INTE_SM0_RXNEMPTY_BITS << sm); + + //If there are no more SMs using the encoder program, then we can remove it from the PIO + if(claimed_sms[pio_idx] == 0) { + pio_remove_program(pio, &encoder_program, pio_program_offset[pio_idx]); + + if(pio_idx == 0) { + irq_remove_handler(PIO0_IRQ_1, pio0_interrupt_handler); + } + else { + irq_remove_handler(PIO1_IRQ_1, pio1_interrupt_handler); } } - } - //////////////////////////////////////////////////////////////////////////////////////////////////// - void Encoder::pio1_interrupt_callback() { - // Go through each of encoders on this PIO to see which triggered this interrupt - for(uint8_t sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) { - if(pio_encoders[1][sm] != nullptr) { - pio_encoders[1][sm]->check_for_transition(); - } + // Reset all the pins this PWM will control back to an unused state + gpio_set_function(enc_pins.a, GPIO_FUNC_NULL); + gpio_set_function(enc_pins.b, GPIO_FUNC_NULL); + + if(pin_c != PIN_UNUSED) { + gpio_set_function(pin_c, GPIO_FUNC_NULL); } } +} - - - //////////////////////////////////////////////////////////////////////////////////////////////////// - // CONSTRUCTORS / DESTRUCTOR - //////////////////////////////////////////////////////////////////////////////////////////////////// - Encoder::Encoder(PIO pio, uint8_t pinA, uint8_t pinB, uint8_t pinC, - float counts_per_revolution, bool count_microsteps, - uint16_t freq_divider) : - enc_pio(pio), pinA(pinA), pinB(pinB), pinC(pinC), - counts_per_revolution(counts_per_revolution), count_microsteps(count_microsteps), - freq_divider(freq_divider), clocks_per_time((float)(clock_get_hz(clk_sys) / (ENC_LOOP_CYCLES * freq_divider))) { - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - Encoder::~Encoder() { - // Clean up our use of the SM associated with this encoder - encoder_program_release(enc_pio, enc_sm); - uint index = pio_get_index(enc_pio); - pio_encoders[index][enc_sm] = nullptr; - pio_claimed_sms[index] &= ~(1u << enc_sm); - - // If there are no more SMs using the encoder program, then we can remove it from the PIO - if(pio_claimed_sms[index] == 0) { - pio_remove_program(enc_pio, &encoder_program, enc_offset); +void Encoder::pio_interrupt_handler(uint pio_idx) { + // Go through each SM on the PIO to see which triggered this interrupt, + // and if there's an associated encoder, have it update its state + for(uint8_t sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) { + if(encoders[pio_idx][sm] != nullptr) { + encoders[pio_idx][sm]->check_for_transition(); } } +} +void Encoder::pio0_interrupt_handler() { + pio_interrupt_handler(0); +} +void Encoder::pio1_interrupt_handler() { + pio_interrupt_handler(1); +} - //////////////////////////////////////////////////////////////////////////////////////////////////// - // METHODS - //////////////////////////////////////////////////////////////////////////////////////////////////// - bool Encoder::init() { - bool initialised = false; - +bool Encoder::init() { + if(!initialised && !pio_sm_is_claimed(pio, sm)) { // Are the pins we want to use actually valid? - if((pinA < NUM_BANK0_GPIOS) && (pinB < NUM_BANK0_GPIOS)) { + if((enc_pins.a < NUM_BANK0_GPIOS) && (enc_pins.b < NUM_BANK0_GPIOS)) { // If a Pin C was defined, and valid, set it as a GND to pull the other two pins down - if((pinC != PIN_UNUSED) && (pinC < NUM_BANK0_GPIOS)) { - gpio_init(pinC); - gpio_set_dir(pinC, GPIO_OUT); - gpio_put(pinC, false); + if((pin_c != PIN_UNUSED) && (pin_c < NUM_BANK0_GPIOS)) { + gpio_init(pin_c); + gpio_set_dir(pin_c, GPIO_OUT); + gpio_put(pin_c, false); } - enc_sm = pio_claim_unused_sm(enc_pio, true); - uint pio_idx = pio_get_index(enc_pio); + pio_sm_claim(pio, sm); + uint pio_idx = pio_get_index(pio); - // Is this the first time using an encoder on this PIO? - if(pio_claimed_sms[pio_idx] == 0) { - // Add the program to the PIO memory and enable the appropriate interrupt - enc_offset = pio_add_program(enc_pio, &encoder_program); - encoder_program_init(enc_pio, enc_sm, enc_offset, pinA, pinB, freq_divider); - hw_set_bits(&enc_pio->inte0, PIO_IRQ0_INTE_SM0_RXNEMPTY_BITS << enc_sm); + // If this is the first time using an encoder on this PIO, add the program to the PIO memory + if(claimed_sms[pio_idx] == 0) { + pio_program_offset[pio_idx] = pio_add_program(pio, &encoder_program); + } + + // Initialise the A and B pins of this encoder + pio_gpio_init(pio, enc_pins.a); + pio_gpio_init(pio, enc_pins.b); + gpio_pull_up(enc_pins.a); + gpio_pull_up(enc_pins.b); + + // Set their default direction + pio_sm_set_consecutive_pindirs(pio, sm, enc_pins.a, 1, false); + pio_sm_set_consecutive_pindirs(pio, sm, enc_pins.b, 1, false); + + pio_sm_config c = encoder_program_get_default_config(pio_program_offset[pio_idx]); + sm_config_set_jmp_pin(&c, enc_pins.a); + sm_config_set_in_pins(&c, enc_pins.b); + + sm_config_set_in_shift(&c, false, false, 1); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); + + sm_config_set_clkdiv_int_frac(&c, freq_divider, 0); + //float div = clock_get_hz(clk_sys) / 500000; + //sm_config_set_clkdiv(&c, div); + + pio_sm_init(pio, sm, pio_program_offset[pio_idx], &c); + + hw_set_bits(&pio->inte1, PIO_IRQ1_INTE_SM0_RXNEMPTY_BITS << sm); + if(claimed_sms[pio_idx] == 0) { + // Configure the processor to run pio_handler() when PIO IRQ 0 is asserted if(pio_idx == 0) { - irq_set_exclusive_handler(PIO0_IRQ_0, pio0_interrupt_callback); - irq_set_enabled(PIO0_IRQ_0, true); + irq_add_shared_handler(PIO0_IRQ_1, pio0_interrupt_handler, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY); + irq_set_enabled(PIO0_IRQ_1, true); } else { - irq_set_exclusive_handler(PIO1_IRQ_0, pio1_interrupt_callback); - irq_set_enabled(PIO1_IRQ_0, true); + irq_add_shared_handler(PIO1_IRQ_1, pio1_interrupt_handler, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY); + irq_set_enabled(PIO1_IRQ_1, true); } } - // Keep a record of this encoder for the interrupt callback - pio_encoders[pio_idx][enc_sm] = this; - pio_claimed_sms[pio_idx] |= 1u << enc_sm; + //Keep a record of this encoder for the interrupt callback + encoders[pio_idx][sm] = this; + claimed_sms[pio_idx] |= 1u << sm; - // Read the current state of the encoder pins and start the PIO program on the SM - stateA = gpio_get(pinA); - stateB = gpio_get(pinB); - encoder_program_start(enc_pio, enc_sm, stateA, stateB); + enc_state_a = gpio_get(enc_pins.a); + enc_state_b = gpio_get(enc_pins.b); + pio_sm_exec(pio, sm, pio_encode_set(pio_x, (uint)enc_state_a << 1 | (uint)enc_state_b)); + pio_sm_set_enabled(pio, sm, true); initialised = true; } - return initialised; } - //////////////////////////////////////////////////////////////////////////////////////////////////// - bool Encoder::get_state_a() const { - return stateA; + return initialised; +} + +pin_pair Encoder::pins() const { + return enc_pins; +} + +bool_pair Encoder::state() const { + return bool_pair(enc_state_a, enc_state_b); +} + +int32_t Encoder::count() const { + return enc_count - count_offset; +} + +float Encoder::revolutions() const { + return (float)count() / counts_per_revolution; +} + +float Encoder::angle_degrees() const { + return revolutions() * 360.0f; +} + +float Encoder::angle_radians() const { + return revolutions() * M_TWOPI; +} + +float Encoder::frequency() const { + return clocks_per_time / (float)time_since; +} + +float Encoder::revolutions_per_second() const { + return frequency() / counts_per_revolution; +} + +float Encoder::revolutions_per_minute() const { + return revolutions_per_second() * 60.0f; +} + +float Encoder::degrees_per_second() const { + return revolutions_per_second() * 360.0f; +} + +float Encoder::radians_per_second() const { + return revolutions_per_second() * M_TWOPI; +} + +void Encoder::zero_count() { + count_offset = enc_count; +} + +Capture Encoder::perform_capture() { + // Capture the current values + int32_t captured_count = enc_count; + int32_t captured_cumulative_time = cumulative_time; + cumulative_time = 0; + + // Determine the change in counts since the last capture was performed + int32_t count_change = captured_count - last_captured_count; + last_captured_count = captured_count; + + // Calculate the average frequency of state transitions + float average_frequency = 0.0f; + if(count_change != 0 && captured_cumulative_time != INT_MAX) { + average_frequency = (clocks_per_time * (float)count_change) / (float)captured_cumulative_time; } - //////////////////////////////////////////////////////////////////////////////////////////////////// - bool Encoder::get_state_b() const { - return stateB; - } + return Capture(captured_count, count_change, average_frequency, counts_per_revolution); +} - //////////////////////////////////////////////////////////////////////////////////////////////////// - int32_t Encoder::get_count() const { - return count - count_offset; - } +void Encoder::microstep_up(int32_t time) { + enc_count++; + time_since = time; + microstep_time = 0; - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_revolutions() const { - return (float)get_count() / counts_per_revolution; - } + if(time + cumulative_time < time) //Check to avoid integer overflow + cumulative_time = INT_MAX; + else + cumulative_time += time; +} - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_angle_degrees() const { - return get_revolutions() * 360.0f; - } +void Encoder::microstep_down(int32_t time) { + enc_count--; + time_since = 0 - time; + microstep_time = 0; - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_angle_radians() const { - return get_revolutions() * M_TWOPI; - } + if(time + cumulative_time < time) //Check to avoid integer overflow + cumulative_time = INT_MAX; + else + cumulative_time += time; +} - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_frequency() const { - return clocks_per_time / (float)time_since; - } +void Encoder::check_for_transition() { + while(pio->ints1 & (PIO_IRQ1_INTS_SM0_RXNEMPTY_BITS << sm)) { + uint32_t received = pio_sm_get(pio, sm); - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_revolutions_per_second() const { - return get_frequency() / counts_per_revolution; - } + // Extract the current and last encoder states from the received value + enc_state_a = (bool)(received & STATE_A_MASK); + enc_state_b = (bool)(received & STATE_B_MASK); + uint8_t states = (received & STATES_MASK) >> 28; - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_revolutions_per_minute() const { - return get_revolutions_per_second() * 60.0f; - } + // Extract the time (in cycles) it has been since the last received + int32_t time_received = (received & TIME_MASK) + ENC_DEBOUNCE_TIME; - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_degrees_per_second() const { - return get_revolutions_per_second() * 360.0f; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - float Encoder::get_radians_per_second() const { - return get_revolutions_per_second() * M_TWOPI; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - void Encoder::zero_count() { - count_offset = count; - } - - //////////////////////////////////////////////////////////////////////////////////////////////////// - Capture Encoder::perform_capture() { - // Capture the current values - int32_t captured_count = count; - int32_t captured_cumulative_time = cumulative_time; - cumulative_time = 0; - - // Determine the change in counts since the last capture was performed - int32_t count_change = captured_count - last_captured_count; - last_captured_count = captured_count; - - // Calculate the average frequency of state transitions - float average_frequency = 0.0f; - if(count_change != 0 && captured_cumulative_time != INT_MAX) { - average_frequency = (clocks_per_time * (float)count_change) / (float)captured_cumulative_time; + // For rotary encoders, only every fourth transition is cared about, causing an inaccurate time value + // To address this we accumulate the times received and zero it when a transition is counted + if(!count_microsteps) { + if(time_received + microstep_time < time_received) // Check to avoid integer overflow + time_received = INT32_MAX; + else + time_received += microstep_time; + microstep_time = time_received; } - return Capture(captured_count, count_change, average_frequency, counts_per_revolution); - } + // Determine what transition occurred + switch(LAST_STATE(states)) { + //-------------------------------------------------- + case MICROSTEP_0: + switch(CURR_STATE(states)) { + // A ____|‾‾‾‾ + // B _________ + case MICROSTEP_1: + if(count_microsteps) + microstep_up(time_received); + break; - //////////////////////////////////////////////////////////////////////////////////////////////////// - void Encoder::microstep_up(int32_t time) { - count++; - time_since = time; - microstep_time = 0; + // A _________ + // B ____|‾‾‾‾ + case MICROSTEP_3: + if(count_microsteps) + microstep_down(time_received); + break; + } + break; - if(time + cumulative_time < time) //Check to avoid integer overflow - cumulative_time = INT_MAX; - else - cumulative_time += time; - } + //-------------------------------------------------- + case MICROSTEP_1: + switch(CURR_STATE(states)) { + // A ‾‾‾‾‾‾‾‾‾ + // B ____|‾‾‾‾ + case MICROSTEP_2: + if(count_microsteps || last_travel_dir == CLOCKWISE) + microstep_up(time_received); - //////////////////////////////////////////////////////////////////////////////////////////////////// - void Encoder::microstep_down(int32_t time) { - count--; - time_since = 0 - time; - microstep_time = 0; + last_travel_dir = NO_DIR; // Finished turning clockwise + break; - if(time + cumulative_time < time) //Check to avoid integer overflow - cumulative_time = INT_MAX; - else - cumulative_time += time; - } + // A ‾‾‾‾|____ + // B _________ + case MICROSTEP_0: + if(count_microsteps) + microstep_down(time_received); + break; + } + break; + + //-------------------------------------------------- + case MICROSTEP_2: + switch(CURR_STATE(states)) { + // A ‾‾‾‾|____ + // B ‾‾‾‾‾‾‾‾‾ + case MICROSTEP_3: + if(count_microsteps) + microstep_up(time_received); + + last_travel_dir = CLOCKWISE; // Started turning clockwise + break; - //////////////////////////////////////////////////////////////////////////////////////////////////// - void Encoder::check_for_transition() { - while(enc_pio->ints0 & (PIO_IRQ0_INTS_SM0_RXNEMPTY_BITS << enc_sm)) { - uint32_t received = pio_sm_get(enc_pio, enc_sm); + // A ‾‾‾‾‾‾‾‾‾ + // B ‾‾‾‾|____ + case MICROSTEP_1: + if(count_microsteps) + microstep_down(time_received); + + last_travel_dir = COUNTERCLOCK; // Started turning counter-clockwise + break; + } + break; - // Extract the current and last encoder states from the received value - stateA = (bool)(received & STATE_A_MASK); - stateB = (bool)(received & STATE_B_MASK); - uint8_t states = (received & STATES_MASK) >> 28; + //-------------------------------------------------- + case MICROSTEP_3: + switch(CURR_STATE(states)) { + // A _________ + // B ‾‾‾‾|____ + case MICROSTEP_0: + if(count_microsteps) + microstep_up(time_received); + break; - // Extract the time (in cycles) it has been since the last received - int32_t time_received = (received & TIME_MASK) + ENC_DEBOUNCE_TIME; + // A ____|‾‾‾‾ + // B ‾‾‾‾‾‾‾‾‾ + case MICROSTEP_2: + if(count_microsteps || last_travel_dir == COUNTERCLOCK) + microstep_down(time_received); - // For rotary encoders, only every fourth transition is cared about, causing an inaccurate time value - // To address this we accumulate the times received and zero it when a transition is counted - if(!count_microsteps) { - if(time_received + microstep_time < time_received) // Check to avoid integer overflow - time_received = INT32_MAX; - else - time_received += microstep_time; - microstep_time = time_received; - } - - // Determine what transition occurred - switch(LAST_STATE(states)) { - //-------------------------------------------------- - case MICROSTEP_0: - switch(CURR_STATE(states)) { - // A ____|‾‾‾‾ - // B _________ - case MICROSTEP_1: - if(count_microsteps) - microstep_up(time_received); - break; - - // A _________ - // B ____|‾‾‾‾ - case MICROSTEP_3: - if(count_microsteps) - microstep_down(time_received); - break; - } - break; - - //-------------------------------------------------- - case MICROSTEP_1: - switch(CURR_STATE(states)) { - // A ‾‾‾‾‾‾‾‾‾ - // B ____|‾‾‾‾ - case MICROSTEP_2: - if(count_microsteps || last_travel_dir == CLOCKWISE) - microstep_up(time_received); - - last_travel_dir = NO_DIR; // Finished turning clockwise - break; - - // A ‾‾‾‾|____ - // B _________ - case MICROSTEP_0: - if(count_microsteps) - microstep_down(time_received); - break; - } - break; - - //-------------------------------------------------- - case MICROSTEP_2: - switch(CURR_STATE(states)) { - // A ‾‾‾‾|____ - // B ‾‾‾‾‾‾‾‾‾ - case MICROSTEP_3: - if(count_microsteps) - microstep_up(time_received); - - last_travel_dir = CLOCKWISE; // Started turning clockwise - break; - - // A ‾‾‾‾‾‾‾‾‾ - // B ‾‾‾‾|____ - case MICROSTEP_1: - if(count_microsteps) - microstep_down(time_received); - - last_travel_dir = COUNTERCLOCK; // Started turning counter-clockwise - break; - } - break; - - //-------------------------------------------------- - case MICROSTEP_3: - switch(CURR_STATE(states)) { - // A _________ - // B ‾‾‾‾|____ - case MICROSTEP_0: - if(count_microsteps) - microstep_up(time_received); - break; - - // A ____|‾‾‾‾ - // B ‾‾‾‾‾‾‾‾‾ - case MICROSTEP_2: - if(count_microsteps || last_travel_dir == COUNTERCLOCK) - microstep_down(time_received); - - last_travel_dir = NO_DIR; // Finished turning counter-clockwise - break; - } - break; - } + last_travel_dir = NO_DIR; // Finished turning counter-clockwise + break; + } + break; } } - //////////////////////////////////////////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////////////// +} } \ No newline at end of file diff --git a/drivers/encoder-pio/encoder.hpp b/drivers/encoder-pio/encoder.hpp index 535db71c..ec353a62 100644 --- a/drivers/encoder-pio/encoder.hpp +++ b/drivers/encoder-pio/encoder.hpp @@ -1,10 +1,25 @@ #pragma once #include "hardware/pio.h" +#include "common/pimoroni_common.hpp" #include "capture.hpp" namespace pimoroni { + struct bool_pair { + union { + bool first; + bool a; + }; + union { + bool second; + bool b; + }; + + bool_pair() : first(false), second(false) {} + bool_pair(bool first, bool second) : first(first), second(second) {} + }; + class Encoder { //-------------------------------------------------- // Constants @@ -13,7 +28,6 @@ namespace pimoroni { static constexpr float DEFAULT_COUNTS_PER_REV = 24; static const uint16_t DEFAULT_COUNT_MICROSTEPS = false; static const uint16_t DEFAULT_FREQ_DIVIDER = 1; - static const uint8_t PIN_UNUSED = UINT8_MAX; private: static const uint32_t STATE_A_MASK = 0x80000000; @@ -41,16 +55,16 @@ namespace pimoroni { CLOCKWISE = 1, COUNTERCLOCK = -1, }; - - + + //-------------------------------------------------- // Variables //-------------------------------------------------- private: - const PIO enc_pio = pio0; - const uint8_t pinA = PIN_UNUSED; - const uint8_t pinB = PIN_UNUSED; - const uint8_t pinC = PIN_UNUSED; + PIO pio; + uint sm; + pin_pair enc_pins; + uint pin_c; const float counts_per_revolution = DEFAULT_COUNTS_PER_REV; const bool count_microsteps = DEFAULT_COUNT_MICROSTEPS; @@ -59,12 +73,9 @@ namespace pimoroni { //-------------------------------------------------- - uint enc_sm = 0; - uint enc_offset = 0; - - volatile bool stateA = false; - volatile bool stateB = false; - volatile int32_t count = 0; + volatile bool enc_state_a = false; + volatile bool enc_state_b = false; + volatile int32_t enc_count = 0; volatile int32_t time_since = 0; volatile Direction last_travel_dir = NO_DIR; volatile int32_t microstep_time = 0; @@ -73,15 +84,18 @@ namespace pimoroni { int32_t count_offset = 0; int32_t last_captured_count = 0; + bool initialised = false; + //-------------------------------------------------- // Statics //-------------------------------------------------- - public: - static Encoder* pio_encoders[NUM_PIOS][NUM_PIO_STATE_MACHINES]; - static uint8_t pio_claimed_sms[NUM_PIOS]; - static void pio0_interrupt_callback(); - static void pio1_interrupt_callback(); + static Encoder* encoders[NUM_PIOS][NUM_PIO_STATE_MACHINES]; + static uint8_t claimed_sms[NUM_PIOS]; + static uint pio_program_offset[NUM_PIOS]; + static void pio_interrupt_handler(uint pio_idx); + static void pio0_interrupt_handler(); + static void pio1_interrupt_handler(); //-------------------------------------------------- @@ -89,7 +103,7 @@ namespace pimoroni { //-------------------------------------------------- public: Encoder() {} - Encoder(PIO pio, uint8_t pinA, uint8_t pinB, uint8_t pinC = PIN_UNUSED, + Encoder(PIO pio, uint sm, const pin_pair &pins, uint pin_c = PIN_UNUSED, float counts_per_revolution = DEFAULT_COUNTS_PER_REV, bool count_microsteps = DEFAULT_COUNT_MICROSTEPS, uint16_t freq_divider = DEFAULT_FREQ_DIVIDER); ~Encoder(); @@ -98,24 +112,26 @@ namespace pimoroni { //-------------------------------------------------- // Methods //-------------------------------------------------- - public: + public: bool init(); - bool get_state_a() const; - bool get_state_b() const; - int32_t get_count() const; - float get_revolutions() const; - float get_angle_degrees() const; - float get_angle_radians() const; + // For print access in micropython + pin_pair pins() const; - float get_frequency() const; - float get_revolutions_per_second() const; - float get_revolutions_per_minute() const; - float get_degrees_per_second() const; - float get_radians_per_second() const; + bool_pair state() const; + int32_t count() const; + float revolutions() const; + float angle_degrees() const; + float angle_radians() const; + + float frequency() const; + float revolutions_per_second() const; + float revolutions_per_minute() const; + float degrees_per_second() const; + float radians_per_second() const; void zero_count(); - Capture perform_capture(); + Capture perform_capture(); //TODO rename capture to snapshot private: void microstep_up(int32_t time_since); diff --git a/drivers/encoder-pio/encoder.pio b/drivers/encoder-pio/encoder.pio index 26138986..0c124afb 100644 --- a/drivers/encoder-pio/encoder.pio +++ b/drivers/encoder-pio/encoder.pio @@ -3,30 +3,35 @@ ; by Christopher (@ZodiusInfuser) Parrott ; -------------------------------------------------- ; -; Watches any two pins (i.e. do not need to be consecutive) for -; when their state changes, and pushes that new state along with -; the old state, and time since the last change. +; Watches any two pins (i.e. do not need to be +; consecutive) for when their state changes, and +; pushes that new state along with the old state, +; and time since the last change. ; ; - X is used for storing the last state -; - Y is used as a general scratch register and for storing the current state +; - Y is used as a general scratch register +; and for storing the current state ; - OSR is used for storing the state-change timer ; -; After data is pushed into the system, a long delay takes place -; as a form of switch debounce to deal with rotary encoder dials. -; This is currently set to 500 cycles, but can be changed using the -; debounce constants below, as well as adjusting the frequency the PIO -; state machine runs at. E.g. a freq_divider of 250 gives a 1ms debounce. +; After data is pushed into the system, a long delay +; takes place as a form of switch debounce to deal +; with rotary encoder dials. This is currently set +; to 500 cycles, but can be changed using the debounce +; constants below, as well as adjusting the frequency +; the PIO state machine runs at. E.g. a freq_divider +; of 250 gives a 1ms debounce. ; Debounce Constants ; -------------------------------------------------- .define SET_CYCLES 10 .define ITERATIONS 30 -.define JMP_CYCLES 16 +.define JMP_CYCLES 16 .define public ENC_DEBOUNCE_CYCLES (SET_CYCLES + (JMP_CYCLES * ITERATIONS)) -; Ensure that ENC_DEBOUNCE_CYCLES is a multiple of the number of cycles the -; wrap takes, which is currently 10 cycles, otherwise timing may be inaccurate +; Ensure that ENC_DEBOUNCE_CYCLES is a multiple of the +; number of cycles the wrap takes, which is currently +; 10 cycles, otherwise timing may be inaccurate ; Encoder Program @@ -35,14 +40,16 @@ .wrap_target loop: - ; Copy the state-change timer from OSR, decrement it, and save it back + ; Copy the state-change timer from OSR, + ; decrement it, and save it back mov y, osr jmp y-- osr_dec osr_dec: mov osr, y ; takes 3 cycles - ; Read the state of both encoder pins and check if they are different from the last state + ; Read the state of both encoder pins and check + ; if they are different from the last state jmp pin encA_was_high mov isr, null jmp read_encB @@ -57,10 +64,13 @@ read_encB: .wrap state_changed: - ; Put the last state and the timer value into ISR alongside the current state, - ; and push that state to the system. Then override the last state with the current state + ; Put the last state and the timer value into + ; ISR alongside the current state, and push that + ; state to the system. Then override the last + ; state with the current state in x, 2 - mov x, ~osr ; invert the timer value to give a sensible value to the system + mov x, ~osr ; invert the timer value to give + ; a sensible value to the system in x, 28 push noblock ; this also clears isr mov x, y @@ -70,7 +80,8 @@ state_changed: debounce_loop: jmp y-- debounce_loop [JMP_CYCLES - 1] - ; Initialise the timer, as an inverse, and decrement it to account for the time this setup takes + ; Initialise the timer, as an inverse, and decrement + ; it to account for the time this setup takes mov y, ~null jmp y-- y_dec y_dec: @@ -86,34 +97,6 @@ y_dec: static const uint8_t ENC_LOOP_CYCLES = encoder_wrap - encoder_wrap_target; -//The time that the debounce takes, as the number of wrap loops that the debounce is equivalent to +// The time that the debounce takes, as the number of wrap loops that the debounce is equivalent to static const uint8_t ENC_DEBOUNCE_TIME = ENC_DEBOUNCE_CYCLES / ENC_LOOP_CYCLES; - - -static inline void encoder_program_init(PIO pio, uint sm, uint offset, uint pinA, uint pinB, uint16_t divider) { - pio_sm_config c = encoder_program_get_default_config(offset); - - sm_config_set_jmp_pin(&c, pinA); - sm_config_set_in_pins(&c, pinB); - sm_config_set_in_shift(&c, false, false, 1); - sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); - pio_gpio_init(pio, pinA); - pio_gpio_init(pio, pinB); - gpio_pull_up(pinA); - gpio_pull_up(pinB); - pio_sm_set_consecutive_pindirs(pio, sm, pinA, 1, 0); - pio_sm_set_consecutive_pindirs(pio, sm, pinB, 1, 0); - sm_config_set_clkdiv_int_frac(&c, divider, 0); - pio_sm_init(pio, sm, offset, &c); -} - -static inline void encoder_program_start(PIO pio, uint sm, bool stateA, bool stateB) { - pio_sm_exec(pio, sm, pio_encode_set(pio_x, (uint)stateA << 1 | (uint)stateB)); - pio_sm_set_enabled(pio, sm, true); -} - -static inline void encoder_program_release(PIO pio, uint sm) { - pio_sm_set_enabled(pio, sm, false); - pio_sm_unclaim(pio, sm); -} %} \ No newline at end of file diff --git a/examples/pico_explorer_encoder/demo.cpp b/examples/pico_explorer_encoder/demo.cpp index fbf17dff..e51e8393 100644 --- a/examples/pico_explorer_encoder/demo.cpp +++ b/examples/pico_explorer_encoder/demo.cpp @@ -11,10 +11,9 @@ using namespace pimoroni; //-------------------------------------------------- // Constants //-------------------------------------------------- -static const uint8_t ENCODER_PIN_A = 1; -static const uint8_t ENCODER_PIN_B = 0; -static const uint8_t ENCODER_PIN_C = Encoder::PIN_UNUSED; -static const uint8_t ENCODER_SWITCH_PIN = 4; +static const pin_pair ENCODER_PINS = {1, 0}; +static const uint ENCODER_PIN_C = PIN_UNUSED; +static const uint ENCODER_SWITCH_PIN = 4; static constexpr float COUNTS_PER_REVOLUTION = 24; // 24 is for rotary encoders. For motor magnetic encoders uses // 12 times the gear ratio (e.g. 12 * 20 with a 20:1 ratio motor @@ -55,7 +54,7 @@ enum DrawState { uint16_t buffer[PicoExplorer::WIDTH * PicoExplorer::HEIGHT]; PicoExplorer pico_explorer(buffer); -Encoder encoder(pio0, ENCODER_PIN_A, ENCODER_PIN_B, ENCODER_PIN_C, COUNTS_PER_REVOLUTION, COUNT_MICROSTEPS, FREQ_DIVIDER); +Encoder encoder(pio0, 0, ENCODER_PINS, ENCODER_PIN_C, COUNTS_PER_REVOLUTION, COUNT_MICROSTEPS, FREQ_DIVIDER); volatile bool encA_readings[READINGS_SIZE]; volatile bool encB_readings[READINGS_SIZE]; @@ -146,13 +145,13 @@ uint32_t draw_plot(Point p1, Point p2, volatile bool (&readings)[READINGS_SIZE], //////////////////////////////////////////////////////////////////////////////////////////////////// bool repeating_timer_callback(struct repeating_timer *t) { if(drawing_to_screen && next_scratch_index < SCRATCH_SIZE) { - encA_scratch[next_scratch_index] = encoder.get_state_a(); - encB_scratch[next_scratch_index] = encoder.get_state_b(); + encA_scratch[next_scratch_index] = encoder.state().a; + encB_scratch[next_scratch_index] = encoder.state().b; next_scratch_index++; } else { - encA_readings[next_reading_index] = encoder.get_state_a(); - encB_readings[next_reading_index] = encoder.get_state_b(); + encA_readings[next_reading_index] = encoder.state().a; + encB_readings[next_reading_index] = encoder.state().b; next_reading_index++; if(next_reading_index >= READINGS_SIZE) @@ -169,7 +168,7 @@ void setup() { gpio_init(PICO_DEFAULT_LED_PIN); gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); - if(ENCODER_SWITCH_PIN != Encoder::PIN_UNUSED) { + if(ENCODER_SWITCH_PIN != PIN_UNUSED) { gpio_init(ENCODER_SWITCH_PIN); gpio_set_dir(ENCODER_SWITCH_PIN, GPIO_IN); gpio_pull_down(ENCODER_SWITCH_PIN); @@ -182,8 +181,8 @@ void setup() { encoder.init(); - bool encA = encoder.get_state_a(); - bool encB = encoder.get_state_b(); + bool encA = encoder.state().a; + bool encB = encoder.state().b; for(uint i = 0; i < READINGS_SIZE; i++) { encA_readings[i] = encA; encB_readings[i] = encB; @@ -226,7 +225,7 @@ int main() { gpio_put(PICO_DEFAULT_LED_PIN, true); // Show the screen refresh has stated // If the user has wired up their encoder switch, and it is pressed, set the encoder count to zero - if(ENCODER_SWITCH_PIN != Encoder::PIN_UNUSED && gpio_get(ENCODER_SWITCH_PIN)) { + if(ENCODER_SWITCH_PIN != PIN_UNUSED && gpio_get(ENCODER_SWITCH_PIN)) { encoder.zero_count(); } @@ -317,21 +316,21 @@ int main() { { std::stringstream sstream; - sstream << capture.get_count(); + sstream << capture.count(); pico_explorer.set_pen(255, 255, 255); pico_explorer.text("Count:", Point(10, 150), 200, 3); pico_explorer.set_pen(255, 128, 255); pico_explorer.text(sstream.str(), Point(110, 150), 200, 3); } { std::stringstream sstream; - sstream << std::fixed << std::setprecision(1) << capture.get_frequency() << "hz"; + sstream << std::fixed << std::setprecision(1) << capture.frequency() << "hz"; pico_explorer.set_pen(255, 255, 255); pico_explorer.text("Freq: ", Point(10, 180), 220, 3); pico_explorer.set_pen(128, 255, 255); pico_explorer.text(sstream.str(), Point(90, 180), 220, 3); } { std::stringstream sstream; - sstream << std::fixed << std::setprecision(1) << capture.get_revolutions_per_minute(); + sstream << std::fixed << std::setprecision(1) << capture.revolutions_per_minute(); pico_explorer.set_pen(255, 255, 255); pico_explorer.text("RPM: ", Point(10, 210), 220, 3); pico_explorer.set_pen(255, 255, 128); pico_explorer.text(sstream.str(), Point(80, 210), 220, 3); }