Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Slight tweaks to encoder PIO #46

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Slight tweaks to encoder PIO #46

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
52 changes: 27 additions & 25 deletions include/encoder2.pio.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
};

Expand All @@ -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);
Expand All @@ -79,4 +82,3 @@ static inline void encoder2_program_init(PIO pio, uint sm, uint offset, uint bas
}

#endif

2 changes: 1 addition & 1 deletion platformio.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand Down
20 changes: 10 additions & 10 deletions src/encoder.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,15 +40,15 @@ static PIOProgram encoderProgram(&encoder2_program);
* Initialize PIO program that measures encoder period.
*
* Returns true on sucess.
*
*
*****************************************************************/

bool Encoder::init(const int pin) {
last_sample_time = millis();

this->pin = pin;

if (!encoderProgram.prepare(&PioInstance, &StateMachineIdx, &offset)) {
if (!encoderProgram.prepare(&PioInstance, &StateMachineIdx, &offset, pin, 2)) {
return false;
}

Expand All @@ -60,7 +60,7 @@ bool Encoder::init(const int pin) {
* Encoder::enable()
* Enable the encoder.
*
*
*
*****************************************************************/

void Encoder::enable() {
Expand All @@ -75,7 +75,7 @@ void Encoder::enable() {
* Encoder::disable()
* Disable the encoder.
*
*
*
*****************************************************************/

void Encoder::disable() {
Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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).
Expand Down Expand Up @@ -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.

Expand Down
10 changes: 7 additions & 3 deletions src/encoder2.pio
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand Down
Loading