From caed4c426a53a1d503ac17ecfb3596976f656293 Mon Sep 17 00:00:00 2001 From: Zhiquan Yeo Date: Mon, 30 Dec 2024 16:52:05 -0500 Subject: [PATCH] Slight tweaks to encoder PIO --- include/encoder2.pio.h | 52 ++++++++++++++++++++++-------------------- platformio.ini | 2 +- src/encoder.cpp | 20 ++++++++-------- src/encoder2.pio | 10 +++++--- 4 files changed, 45 insertions(+), 39 deletions(-) diff --git a/include/encoder2.pio.h b/include/encoder2.pio.h index 9c7cf26..c4af2a3 100644 --- a/include/encoder2.pio.h +++ b/include/encoder2.pio.h @@ -20,38 +20,38 @@ static const uint16_t encoder2_program_instructions[] = { 0x0b10, // 0: jmp 16 [11] - 0x0315, // 1: jmp 21 [3] - 0x0318, // 2: jmp 24 [3] + 0x0315, // 1: jmp 21 [3] + 0x0318, // 2: jmp 24 [3] 0x0b11, // 3: jmp 17 [11] - 0x0318, // 4: jmp 24 [3] + 0x0318, // 4: jmp 24 [3] 0x0b10, // 5: jmp 16 [11] 0x0b11, // 6: jmp 17 [11] - 0x0315, // 7: jmp 21 [3] - 0x0315, // 8: jmp 21 [3] + 0x0315, // 7: jmp 21 [3] + 0x0315, // 8: jmp 21 [3] 0x0b11, // 9: jmp 17 [11] 0x0b10, // 10: jmp 16 [11] - 0x0318, // 11: jmp 24 [3] + 0x0318, // 11: jmp 24 [3] 0x0b11, // 12: jmp 17 [11] - 0x0318, // 13: jmp 24 [3] - 0x0315, // 14: jmp 21 [3] + 0x0318, // 13: jmp 24 [3] + 0x0315, // 14: jmp 21 [3] 0x0b10, // 15: jmp 16 [11] // .wrap_target - 0x0052, // 16: jmp x--, 18 - 0xe020, // 17: set x, 0 - 0x4002, // 18: in pins, 2 - 0xa0e6, // 19: mov osr, isr - 0x60a4, // 20: out pc, 4 - 0x0079, // 21: jmp !y, 25 - 0xa04a, // 22: mov y, !y - 0x0019, // 23: jmp 25 - 0x0076, // 24: jmp !y, 22 - 0xa0e6, // 25: mov osr, isr - 0xa0c9, // 26: mov isr, !x - 0x4041, // 27: in y, 1 - 0x8000, // 28: push noblock - 0x60c2, // 29: out isr, 2 - 0xa0eb, // 30: mov osr, !null - 0x603f, // 31: out x, 31 + 0x0052, // 16: jmp x--, 18 + 0xe020, // 17: set x, 0 + 0x4002, // 18: in pins, 2 + 0xa0e6, // 19: mov osr, isr + 0x60a4, // 20: out pc, 4 + 0x0079, // 21: jmp !y, 25 + 0xa04a, // 22: mov y, !y + 0x0019, // 23: jmp 25 + 0x0076, // 24: jmp !y, 22 + 0xa0e6, // 25: mov osr, isr + 0xa0c9, // 26: mov isr, !x + 0x4041, // 27: in y, 1 + 0x8000, // 28: push noblock + 0x60c2, // 29: out isr, 2 + 0xa0eb, // 30: mov osr, !null + 0x603f, // 31: out x, 31 // .wrap }; @@ -69,6 +69,9 @@ static inline pio_sm_config encoder2_program_get_default_config(uint offset) { } static inline void encoder2_program_init(PIO pio, uint sm, uint offset, uint base_pin) { + pio_gpio_init(pio, base_pin); + pio_gpio_init(pio, base_pin+1); + pio_sm_set_consecutive_pindirs(pio, sm, base_pin, 2, false); pio_sm_config c = encoder2_program_get_default_config(offset); sm_config_set_in_pins(&c, base_pin); sm_config_set_in_shift(&c, false, false, 32); @@ -79,4 +82,3 @@ static inline void encoder2_program_init(PIO pio, uint sm, uint offset, uint bas } #endif - diff --git a/platformio.ini b/platformio.ini index d87b356..be357e9 100644 --- a/platformio.ini +++ b/platformio.ini @@ -1,5 +1,5 @@ [env] -platform = https://github.com/maxgerhardt/platform-raspberrypi.git#088a074ba98e091f9abc48db91b99bedf908332b +platform = https://github.com/maxgerhardt/platform-raspberrypi.git#39b90392af50585e429941bd2561a91949a2ba46 framework = arduino board_build.core = earlephilhower board_build.filesystem_size = 0.5m diff --git a/src/encoder.cpp b/src/encoder.cpp index 04fd4a9..8778886 100644 --- a/src/encoder.cpp +++ b/src/encoder.cpp @@ -40,7 +40,7 @@ static PIOProgram encoderProgram(&encoder2_program); * Initialize PIO program that measures encoder period. * * Returns true on sucess. -* +* *****************************************************************/ bool Encoder::init(const int pin) { @@ -48,7 +48,7 @@ bool Encoder::init(const int pin) { this->pin = pin; - if (!encoderProgram.prepare(&PioInstance, &StateMachineIdx, &offset)) { + if (!encoderProgram.prepare(&PioInstance, &StateMachineIdx, &offset, pin, 2)) { return false; } @@ -60,7 +60,7 @@ bool Encoder::init(const int pin) { * Encoder::enable() * Enable the encoder. * -* +* *****************************************************************/ void Encoder::enable() { @@ -75,7 +75,7 @@ void Encoder::enable() { * Encoder::disable() * Disable the encoder. * -* +* *****************************************************************/ void Encoder::disable() { @@ -90,14 +90,14 @@ void Encoder::disable() { * Encoder::update() * Get the latest encoder period(s) from the PIO if available * store the latest, and updated the tick count. -* +* * Returns number of samples that were retrieved. -* +* *****************************************************************/ uint Encoder::getFraction(const uint count) const { return count & (samples_to_average() - 1); -} +} uint Encoder::getWholeNumber(const uint count) const { return count >> samples_to_average_shift; @@ -147,7 +147,7 @@ int Encoder::update() { ++period; period_fraction = getFraction(period_fraction); } - period_queue[sample_index++] = raw_rx_fifo; + period_queue[sample_index++] = raw_rx_fifo; ++sample_count; if(sample_index == samples_to_average()) sample_index = 0; @@ -191,7 +191,7 @@ void Encoder::setSamplesToAverage(const int n) { * Return the period calculated by the PIO in the following format: * 31 1 0 * | Period in 16-cycle ticks | dir | -* +* * This is the same format return by the PIO. * (Note: PIO calculated value may be overidden if a relatively * large amount of time has passed since the last sample from the PIO). @@ -228,7 +228,7 @@ uint Encoder::getPeriod() { //If PIO value is correct, the amount of time that has passed since the last sample //should be less than or equal to period calculated by the PIO. //Otherwise, motor may have slowed way down; use the time passed for the current period length. - + //If time since last sample is significantly longer than the last period returned by the PIO, //use the time passed instead. diff --git a/src/encoder2.pio b/src/encoder2.pio index 38d1c6b..2d9d220 100644 --- a/src/encoder2.pio +++ b/src/encoder2.pio @@ -29,14 +29,14 @@ .program encoder2 .origin 0 -.define PUBLIC VERSION 102 +.define PUBLIC VERSION 102 .define PUBLIC CYCLES_PER_COUNT 16 ; This PIO program monitors the encoder inputs to measure the period ; between encoder pulses. At the start of a period, a 31-bit counter is ; initialized to its maximum value (0x7FFFFFFF) and is decremented every 16 cycles ; until the start of the next period. The period start/end is determined -; by the transition of the encoder signals from one state to another (e.g., 01 -> 11 or 01 -> 00) +; by the transition of the encoder signals from one state to another (e.g., 01 -> 11 or 01 -> 00) ; Every period, the total count is passed to the robot code through the RX FIFO. ; The upper 31 bits is the count. It represents the number of 16-cycle intervals @@ -117,11 +117,15 @@ send_period: push noblock ;send period/direction to robot code out isr,2 ;restore last state to isr mov osr,~null ;set all bits in osr - out x, 31 ;set 31 bits of x register + out x, 31 ;set 31 bits of x register .wrap % c-sdk { static inline void encoder2_program_init(PIO pio, uint sm, uint offset, uint base_pin) { + pio_gpio_init(pio, base_pin); + pio_gpio_init(pio, base_pin+1); + pio_sm_set_consecutive_pindirs(pio, sm, base_pin, 2, false); + pio_sm_config c = encoder2_program_get_default_config(offset); sm_config_set_in_pins(&c, base_pin);