├── LICENSE
├── README.md
├── examples
└── SpeedMeasure
│ └── SpeedMeasure.ino
├── images
├── calib.svg
├── chart.svg
└── steps.svg
├── library.properties
└── src
├── PicoEncoder.cpp
├── PicoEncoder.h
├── pico_encoder.pio
└── pico_encoder.pio.h
/LICENSE:
--------------------------------------------------------------------------------
1 | BSD 2-Clause License
2 |
3 | Copyright (c) 2024, pmarques-dev
4 |
5 | Redistribution and use in source and binary forms, with or without
6 | modification, are permitted provided that the following conditions are met:
7 |
8 | 1. Redistributions of source code must retain the above copyright notice, this
9 | list of conditions and the following disclaimer.
10 |
11 | 2. Redistributions in binary form must reproduce the above copyright notice,
12 | this list of conditions and the following disclaimer in the documentation
13 | and/or other materials provided with the distribution.
14 |
15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
19 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
21 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
22 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
23 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
24 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Sub-Step Quadrature Encoder
2 |
3 | ## The problem
4 |
5 | Hardware peripherals to read quadrature encoders count steps by looking at the sequence of phase states read from a quadrature encoder.
6 |
7 | To estimate the encoder speed, the user can check how many steps were counted over a period of time. For instance, if the count was 120 at some point and was 134 10ms later, we can estimate that the speed is 14 steps in 10ms or 1400 steps per second.
8 |
9 | This works relatively well for high counts, but at low speeds (or high sample rates), the quantization effect of looking at integer counts can introduce a high level of quantization noise in the speed measurement.
10 |
11 | A different approach that would work better at low speeds, would be to measure the time it takes to advance one step and use its inverse to compute the speed. For instance, if one step takes 540us, then the speed is 1/0.000540 = ~1852 steps per second.
12 |
13 | Unfortunately, as speed increases the resolution of this method decreases rapidly. At 100.000 steps per second, a step takes 10us. Measuring 9, 10 or 11 us per step will give speeds of 111k, 100k or 90k steps per second, respectively.
14 |
15 | Moreover, especially in low cost encoders, the phases are not all the same size which introduces another source of measurement noise if not compensated properly.
16 |
17 |
18 | ## The Solution
19 |
20 | The "sub-step" quadrature encoder code solves this problem by using an hybrid approach: the PIO code keeps track of the integer step count, but also how long ago was the last transition and the direction of that transition. The high level code can then use that information to compute a speed estimate that doesn't suffer from the integer quantization problem of just counting steps and can support high step rates as well.
21 |
22 | The high level code also takes into account and compensates the phase size differences of the encoder.
23 |
24 | The chart below shows the speed estimate using just integer step counts in red and the sub-step speed estimate in blue for an encoder connected to a DC motor doing some voltage step changes.
25 |
26 | 
27 |
28 | Note that unlike what we would get by doing a low-pass filter on the integer count signal, the sub-step version has no delay and does not limit the frequency response of the encoder.
29 |
30 | ## Usage
31 |
32 | Start by declaring a PicoEncoder instance:
33 |
34 | `PicoEncoder encoder;`
35 |
36 | Call the begin method to initialize the encoder instance and start counting steps:
37 |
38 | `encoder.begin(phaseAPin);`
39 |
40 | The two phases of the encoder must be connected to consecutive pins and the parameter is the lowest numbered pin using arduino naming. Note that the pins must be consecutive in the RP2040 and may not correspond to consecutive numbers in the arduino mapping.
41 |
42 | For instance, on the Arduino Nano RP2040 board, pins D2 and D3 are GPIO25 and GPIO15, so they can not be used for this purpose. However, pin D4 is GPIO16, which means pins D3 and D4 could be used to connect the encoder by passing pin D3 to `begin` (actually all pins from D3 to D9 are consecutive on that board).
43 |
44 | The method also sets the pins as inputs and will turn on the pull-ups on the pins if the parameter "pullUp" is true (default value). Many encoders have open-collector outputs and require pull-ups. If unsure, leave the default value of true.
45 |
46 | Returns 0 on success, -1 if there is no PIO available, -2 if there are too many encoder instances
47 |
48 | The begin method allocates the necessary PIO resources. The PIO code uses all 32 instructions on the PIO, so it can not share the PIO with other PIO programs. However it can run up to 4 encoder instances in that PIO (and up to 8 in total using both PIO units on the RP2040).
49 |
50 | To make a measurement call the `update` method. This will update the `position`, `speed` and `step` fields on the encoder instance:
51 |
52 | - `position` is given in "sub-steps". You can think of a sub-step as 1/64th of a step. For example, an encoder that advertises 100 steps per revolution, will actually have 100 full cycles per revolution, so 400 steps in this context and the position field will go up by 400 * 64 = 25600 in a single revolution. The position field is useful in situations where the encoder is measuring something that doesn't accumulate indefinitely, like the position of a 3D printer head. There is a `resetPosition` method that can be used to set the zero position according to an absolute external reference, like a limit switch. When `resetPosition` is called, the current position will become the new zero position
53 |
54 | - `speed` is given in "sub-steps per second". This is the most up to date speed estimate. Don't use differences in the position field to estimate speed as that will produce a worse estimate than to just read the speed field directly
55 |
56 | - `step` is the current position in steps, like what would be returned by a classical quadrature encoder hardware
57 |
58 | For more details, see the SpeedMeasure example distributed with the library.
59 |
60 |
61 | ## Phase calibration
62 |
63 | To increase the precision of the speed estimate, the user can run a calibration function to measure the relative sizes of the phases of the encoder. The sub-step code will then use this information to compensate the phase size differences while computing speed estimates. This is not strictly necessary, but if not done, the code will assume equal size phases, which will introduce noise, especially at low speeds.
64 |
65 | Note that for an encoder to have perfectly balanced 90 degree phases, each individual phase would require a perfect 50% duty cycle and the phases would need to be exactly 90 degrees apart. That is almost never the case in low cost encoders.
66 |
67 | The simplest way to do phase calibration is to just call `autoCalibratePhases` during the idle time of the main control loop (see the SpeedMeasure example). This method will look for encoder transitions, measure relative phase sizes and adjust the phase size calibration, all automatically.
68 |
69 | The chart below shows the difference between an uncalibrated speed measure and a calibrated one (and the speed estimate we would get from raw steps only). Note that both versions are the same until around sample 53 when the auto calibration code has enough information to start compensating phase sizes.
70 |
71 | 
72 |
73 | The only downside of this method is that there will be a short period at system startup where the PicoEncoder won't have phase calibration information and will operate as if the phases have the same size. This is usually not a problem, but it can be avoided with a more complex sequence:
74 | - call `autoCalibrationDone` to check if the auto calibration process has already completed the phase size measurement. Continue running `autoCalibratePhases` until it does
75 | - when the auto calibration is done, call `getPhases` to get an integer representing the phase sizes. Save this number
76 | - on the initialization code, call `setPhases` passing the value saved previously. This will immediately set the phase calibration at init time and avoid the learning period. After calling `setPhases` there is no need to call `autoCalibratePhases` any more
77 |
78 | If you are curious about how unbalanced your encoder is, the value returned by `getPhases` has one phase size per byte, so a value of 0x404040 would represent a perfect encoder (the fourth size is implied by being 256 minus the sum of the other three).
79 |
80 |
81 | ## Sub-step vs simple quadrature encoder
82 |
83 | Compared with a simple step counting solution, this library has the advantage that it uses the information from the transition times to extract a much better speed and position estimate.
84 |
85 | Like with an hardware encoder, it doesn't use CPU time while counting steps and it's only the update function that uses CPU time. Note that this function is called at the control frequency of your system, say 100Hz, and so the CPU time used is independent of the step rate of the encoder. To use as little CPU as possible, this function only uses integer arithmetic.
86 |
87 | Note: the `PicoEncoder` class has a `step` field that contains the same step count that would have been returned by a simple quadrature encoder. This can be useful if, for instance, we have a project with a couple of DC motors with encoders (that would benefit from using the sub-step version), but also a rotary encoder for the user interface (for which we only need integer steps).
88 |
89 | ## Implementation details
90 |
91 | The chart below illustrates how the sub-step works. The horizontal scale is time in micro-seconds, the vertical step is in sub-steps. 4 hardware steps (one cycle) are always 256 sub-steps. To simplify, lets assume the phases are balanced and each step takes exactly 64 sub-steps.
92 |
93 | 
94 |
95 | The red line is the upper bound of the encoder position, whereas the blue line is the lower bound. The yellow line is the speed estimate.
96 |
97 | At the start, the encoder is stopped at hardware step 3, so the sub-step position estimate is between 192 and 256.
98 |
99 | At 30ms, we read the data from the PIO and there was a transition at ~21ms and the current integer step is 4. At the time of the transition the lower and upper bounds for the position touch, because we know exactly where the encoder is in that instant.
100 |
101 | At this point we don't know much about the speed. The sub-step position of the encoder could have been close to 256 (close to the upper bound) and it could still be there now (close to the lower bound).
102 |
103 | At 40ms, we read the PIO again. The encoder has made another step so we know the sub-step position is between 320 and 384. We also know there was a transition at ~34ms, so we can compute a speed estimate as the encoder did 64 sub-steps in 34 - 21 = 13ms (note: the actual code uses micro-seconds).
104 |
105 | At 50ms, the encoder actually did 2 steps, the sub-step position is now between 448 and 512 and the last transition was at ~49ms. Note that there were two transitions in this period, but we only get the latest transition from the PIO code. This still allows us to compute a new speed estimate, as we made 128 sub-steps in 49 - 34 = 15 ms.
106 |
107 | On top of using actual transitions to compute the speed, the current step position is also used to give a upper and lower bound on the speed so that the code can react faster to large variations in speed.
108 |
109 | To compensate for unbalanced phase sizes, the sub-step position assigned to each hardware step transition is not a multiple of 64 but instead depends on the phase size calibration. The lower bound of phase 0 is always a multiple of 256, though.
110 |
111 | ## References
112 |
113 | The base algorithm for PicoEncoder (using variable acquisition window and quadrature phase recovery) comes from this paper: [Improving Incremental Encoder Measurement: Variable Acquisition Window and Quadrature Phase Compensation to Minimize Acquisition Errors](https://bibliotecadigital.ipb.pt/bitstream/10198/15052/4/Improving...Acquisition_Errors.pdf)
114 |
115 | The actual implementation deviates considerably from the paper, as it tries to make the best possible use of the PIO hardware and minimize CPU intervention.
116 |
--------------------------------------------------------------------------------
/examples/SpeedMeasure/SpeedMeasure.ino:
--------------------------------------------------------------------------------
1 | #include "Arduino.h"
2 | #include "PicoEncoder.h"
3 |
4 | // declare two encoders that will actually be reading the same pins:
5 | // one will measure the phase sizes and compensate for differences, while
6 | // the other will work as if the phase sizes were perfectly uniform
7 | PicoEncoder encoder;
8 | PicoEncoder non_calibrated_encoder;
9 |
10 |
11 | // this section is an optional DC motor control code to help test an encoder
12 | // attached to a DC motor (and calibrate phase sizes)
13 |
14 | const int dir_pin = p10;
15 | const int pwm_pin = p11;
16 |
17 | // must be consecutive pins on the rp2040
18 | const int encoder_pinA = p5;
19 | const int encoder_pinB = p6;
20 |
21 | // keep track of current time
22 | uint period_start_us;
23 |
24 | static void set_pwm(float value)
25 | {
26 | if (value < 0) {
27 | digitalWrite(dir_pin, 1);
28 | analogWrite(pwm_pin, 255 * value + 255);
29 | } else {
30 | digitalWrite(dir_pin, 0);
31 | analogWrite(pwm_pin, 255 * value);
32 | }
33 | }
34 |
35 |
36 | void setup()
37 | {
38 | Serial.begin(115200);
39 |
40 | pinMode(dir_pin, OUTPUT);
41 | pinMode(pwm_pin, OUTPUT);
42 | set_pwm(0);
43 |
44 | // use the same pins for both the calibrated and non-calibrated encoders
45 | encoder.begin(encoder_pinA);
46 | non_calibrated_encoder.begin(encoder_pinA);
47 |
48 | delay(2000);
49 |
50 | period_start_us = time_us_32();
51 | }
52 |
53 | void loop()
54 | {
55 | static int count;
56 |
57 | count++;
58 | if ((count & 63) == 0) {
59 | if (count > 1000)
60 | set_pwm(0.0);
61 | else
62 | set_pwm(random(-100, 100) * 0.01);
63 | }
64 |
65 | // wait for the next sample period, while calling "autoCalibratePhases"
66 | // to measure the phase sizes dynamically. If we had two encoders (to
67 | // read two motors, for instance), we could call both autoCalibratePhases
68 | // methods here, to calibrate both encoders simultaneously
69 | while ((int)(time_us_32() - period_start_us) < 10000)
70 | encoder.autoCalibratePhases();
71 | period_start_us += 10000;
72 |
73 | encoder.update();
74 | non_calibrated_encoder.update();
75 |
76 | if (count < 1050) {
77 | Serial.print("speed: ");
78 | Serial.print(encoder.speed);
79 | Serial.print(", position: ");
80 | Serial.print(encoder.position);
81 | Serial.print(", step: ");
82 | Serial.print(encoder.step);
83 | if (encoder.autoCalibrationDone()) {
84 | Serial.print(", phases: 0x");
85 | Serial.print(encoder.getPhases(), HEX);
86 | }
87 | Serial.print(", non calibrated encoder speed: ");
88 | Serial.println(non_calibrated_encoder.speed);
89 | }
90 | }
91 |
--------------------------------------------------------------------------------
/images/chart.svg:
--------------------------------------------------------------------------------
1 |
2 |
694 |
--------------------------------------------------------------------------------
/images/steps.svg:
--------------------------------------------------------------------------------
1 |
2 |
3 |
176 |
--------------------------------------------------------------------------------
/library.properties:
--------------------------------------------------------------------------------
1 | name=PicoEncoder
2 | version=1.1.1
3 | author=Paulo Marques
4 | maintainer=Paulo Marques
5 | sentence=High resolution quadrature encoder using the PIO on the RP2040
6 | paragraph=Uses both the step count and transition timings to compute a high resolution speed estimate
7 | category=Sensors
8 | url=https://github.com/pmarques-dev/PicoEncoder
9 | architectures=mbed_rp2040,mbed_nano,rp2040
10 | includes=PicoEncoder.h
11 |
--------------------------------------------------------------------------------
/src/PicoEncoder.cpp:
--------------------------------------------------------------------------------
1 | /*
2 | PicoEncoder - High resolution quadrature encoder using the PIO on the RP2040
3 | Created by Paulo Marques, Pedro Pereira, Paulo Costa, 2022
4 | Distributed under the BSD 2-clause license. For full terms, please refer to the LICENSE file.
5 | */
6 |
7 | #include "PicoEncoder.h"
8 |
9 | #ifdef ARDUINO_ARCH_RP2040
10 |
11 | #include
12 | #include
13 | #include
14 | #include
15 |
16 |
17 | // global configuration: after this number of samples with no step change,
18 | // consider the encoder stopped
19 | static const int idle_stop_samples = 3;
20 |
21 | // global structures to control which PIO and state machines to use
22 | static int encoder_count;
23 | static PIO pio_used[2];
24 |
25 |
26 | // low level PIO interface
27 |
28 | // initialize the PIO state and the substep_state_t structure that keeps track
29 | // of the encoder state
30 | static inline void pico_encoder_program_init(PIO pio, uint sm, uint pin_A)
31 | {
32 | uint pin_state, position, ints;
33 |
34 | pio_gpio_init(pio, pin_A);
35 | pio_gpio_init(pio, pin_A + 1);
36 |
37 | pio_sm_set_consecutive_pindirs(pio, sm, pin_A, 2, false);
38 |
39 | pio_sm_config c = pico_encoder_program_get_default_config(0);
40 | sm_config_set_in_pins(&c, pin_A); // for WAIT, IN
41 | // shift to left, auto-push at 32 bits
42 | sm_config_set_in_shift(&c, false, true, 32);
43 | sm_config_set_out_shift(&c, true, false, 32);
44 | // don't join FIFO's
45 | sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE);
46 |
47 | // always run at sysclk, to have the maximum possible time resolution
48 | sm_config_set_clkdiv(&c, 1.0);
49 |
50 | pio_sm_init(pio, sm, 0, &c);
51 |
52 | // set up status to be rx_fifo < 1
53 | pio->sm[sm].execctrl = ((pio->sm[sm].execctrl & 0xFFFFFF80) | 0x12);
54 |
55 | // init the state machine according to the current phase. Since we are
56 | // setting the state running PIO instructions from C state, the encoder may
57 | // step during this initialization. This should not be a problem though,
58 | // because as long as it is just one step, the state machine will update
59 | // correctly when it starts. We disable interrupts anyway, to be safe
60 | ints = save_and_disable_interrupts();
61 |
62 | pin_state = (gpio_get_all() >> pin_A) & 3;
63 |
64 | // to setup the state machine, we need to set the lower 2 bits of OSR to be
65 | // the negated pin state
66 | pio_sm_exec(pio, sm, pio_encode_set(pio_y, ~pin_state));
67 | pio_sm_exec(pio, sm, pio_encode_mov(pio_osr, pio_y));
68 |
69 | // also set the Y (current step) so that the lower 2 bits of Y have a 1:1
70 | // mapping to the current phase (input pin state). That simplifies the code
71 | // to compensate for differences in encoder phase sizes:
72 | switch (pin_state) {
73 | case 0: position = 0; break;
74 | case 1: position = 3; break;
75 | case 2: position = 1; break;
76 | case 3: position = 2; break;
77 | }
78 | pio_sm_exec(pio, sm, pio_encode_set(pio_y, position));
79 |
80 | pio_sm_set_enabled(pio, sm, true);
81 |
82 | restore_interrupts(ints);
83 | }
84 |
85 | static inline void pico_encoder_get_counts(PIO pio, uint sm, uint *step, int *cycles, uint *us)
86 | {
87 | int i, pairs;
88 | uint ints;
89 |
90 | pairs = pio_sm_get_rx_fifo_level(pio, sm) >> 1;
91 |
92 | // read all data with interrupts disabled, so that there can not be a
93 | // big time gap between reading the PIO data and the current us
94 | ints = save_and_disable_interrupts();
95 | for (i = 0; i < pairs + 1; i++) {
96 | *cycles = pio_sm_get_blocking(pio, sm);
97 | *step = pio_sm_get_blocking(pio, sm);
98 | }
99 | *us = time_us_32();
100 | restore_interrupts(ints);
101 | }
102 |
103 |
104 |
105 | // PicoEncoder class definition
106 |
107 | PicoEncoder::PicoEncoder()
108 | {
109 | // we set the default phase sizes here, so that if the user sets the phase
110 | // sizes before calling begin, those will override the default and it will
111 | // just work
112 | setPhases(DEFAULT_PHASES);
113 |
114 | // just set the position/speed fields to zero
115 | position = 0;
116 | speed = 0;
117 | step = 0;
118 |
119 | // reset incremental calibration data
120 | resetAutoCalibration();
121 | }
122 |
123 | void PicoEncoder::setPhases(int phases)
124 | {
125 | calibration_data[0] = 0;
126 | calibration_data[1] = (phases & 0xFF);
127 | calibration_data[2] = calibration_data[1] + ((phases >> 8) & 0xFF);
128 | calibration_data[3] = calibration_data[2] + ((phases >> 16) & 0xFF);
129 | }
130 |
131 | int PicoEncoder::getPhases(void)
132 | {
133 | return calibration_data[1] |
134 | ((calibration_data[2] - calibration_data[1]) << 8) |
135 | ((calibration_data[3] - calibration_data[2]) << 16);
136 | }
137 |
138 |
139 | // internal helper functions
140 |
141 | void PicoEncoder::read_pio_data(uint *step, uint *step_us, uint *transition_us, int *forward)
142 | {
143 | int cycles;
144 |
145 | // get the raw data from the PIO state machine
146 | pico_encoder_get_counts(pio, sm, step, &cycles, step_us);
147 |
148 | // when the PIO program detects a transition, it sets cycles to either zero
149 | // (when step is incrementing) or 2^31 (when step is decrementing) and keeps
150 | // decrementing it on each 13 clock loop. We can use this information to get
151 | // the time and direction of the last transition
152 | if (cycles < 0) {
153 | cycles = -cycles;
154 | *forward = 1;
155 | } else {
156 | cycles = 0x80000000 - cycles;
157 | *forward = 0;
158 | }
159 | *transition_us = *step_us - ((cycles * 13) / clocks_per_us);
160 | }
161 |
162 | // get the sub-step position of the start of a step
163 | uint PicoEncoder::get_step_start_transition_pos(uint step)
164 | {
165 | return ((step << 6) & 0xFFFFFF00) | calibration_data[step & 3];
166 | }
167 |
168 |
169 | // incrementally update the phase measure, so that the substep estimation takes
170 | // phase sizes into account. The function is not allowed to block for more than
171 | // "period_us" microseconds
172 | void PicoEncoder::autoCalibratePhases(void)
173 | {
174 | uint cur_us, step_us, step, delta;
175 | int forward, steps, need_rescale, i, total;
176 |
177 | // read raw encoder data. Reading encoder data is an idempotent operation,
178 | // so we can still continue calling update and everything should just work
179 | read_pio_data(&step, &step_us, &cur_us, &forward);
180 |
181 | // if we are still on the same step as before, there is nothing to see
182 | if (step == calib_last_step)
183 | return;
184 |
185 | // if calib_last_us is zero, that means we haven't started yet, so don't try
186 | // to use a delta to nothing
187 | if (calib_last_us == 0)
188 | delta = 0;
189 | else
190 | delta = cur_us - calib_last_us;
191 | steps = calib_last_step - step;
192 |
193 | calib_last_step = step;
194 | calib_last_us = cur_us;
195 |
196 | // if we've skipped a step, we can not use this information (and we need to
197 | // reset the data). Also, do the same if we didn't skip a step but the last
198 | // step was just too slow to be usable (> 20ms)
199 | if (abs(steps) > 1 || delta > 20000 || delta == 0) {
200 | memset(calib_data, 0, sizeof(calib_data));
201 | return;
202 | }
203 |
204 | // save the step period in the correct data slot
205 | if (forward)
206 | calib_data[(step - 1) & 3] = delta;
207 | else
208 | calib_data[(step + 1) & 3] = delta;
209 |
210 | // if we don't have a measure of all the steps yet, just continue
211 | if (calib_data[0] == 0 || calib_data[1] == 0 || calib_data[2] == 0 || calib_data[3] == 0)
212 | return;
213 |
214 | // otherwise, use the measurement. Sum the just acquired 4 step sizes to the
215 | // step size total accumulator. Check if the values in the accumulator are
216 | // getting too big and halve them in that case, to keep them manageable
217 | need_rescale = 0;
218 | for (i = 0; i < 4; i++) {
219 | calib_sum[i] += calib_data[i];
220 | calib_data[i] = 0;
221 | if (calib_sum[i] > 2500000)
222 | need_rescale = 1;
223 | }
224 |
225 | total = 0;
226 | for (i = 0; i < 4; i++) {
227 | if (need_rescale)
228 | calib_sum[i] >>= 1;
229 | total += calib_sum[i];
230 | }
231 | calib_count++;
232 |
233 | // if we don't have at least 32 full measurements, don't use them yet, as
234 | // we may still have a big bias (this is just an heuristic)
235 | if (calib_count < 32)
236 | return;
237 |
238 | // scale the sizes to a total of 256 to be used as sub-steps
239 | calibration_data[0] = 0;
240 | calibration_data[1] = (calib_sum[0] * 256 + total / 2) / total;
241 | calibration_data[2] = ((calib_sum[0] + calib_sum[1]) * 256 + total / 2) / total;
242 | calibration_data[3] = ((calib_sum[0] + calib_sum[1] + calib_sum[2]) * 256 + total / 2) / total;
243 | }
244 |
245 | void PicoEncoder::resetAutoCalibration(void)
246 | {
247 | memset(calib_sum, 0, sizeof(calib_sum));
248 | calib_count = 0;
249 | calib_last_us = 0;
250 | }
251 |
252 |
253 | // some Arduino mbed Pico boards have non trivial pin mappings and require a
254 | // function to translate
255 | #if defined(ARDUINO_ARCH_MBED)
256 | #include
257 | static int translate_pin(int pin) { return digitalPinToPinName(pin); }
258 | #else
259 | static int translate_pin(int pin) { return pin; }
260 | #endif
261 |
262 | // try to claim all SM's and load the program to PIO 'pio'. Return true on
263 | // success, false on failure
264 | static bool pico_encoder_claim_pio(PIO pio)
265 | {
266 | // check that we can load the program on this PIO
267 | if (!pio_can_add_program(pio, &pico_encoder_program))
268 | return false;
269 |
270 | // check that all SM's are free. Some libraries claim an SM and later load the
271 | // PIO code and that would not interact well with code that uses the entire
272 | // PIO code space. So just make sure we can claim all the SM's to prevent this
273 | for (int i = 0; i < 4; i++)
274 | if (pio_sm_is_claimed(pio, i))
275 | return false;
276 |
277 | // claim all SM's
278 | for (int i = 0; i < 4; i++)
279 | pio_sm_claim(pio, i);
280 |
281 | // load the code into the PIO
282 | pio_add_program(pio, &pico_encoder_program);
283 |
284 | return true;
285 | }
286 |
287 | int PicoEncoder::begin(int firstPin, bool pullUp)
288 | {
289 | int forward, gpio_pin;
290 |
291 | // the first encoder needs to load a PIO with the PIO code
292 | if (encoder_count == 0) {
293 | // it can either use pio0
294 | if (pico_encoder_claim_pio(pio0))
295 | pio_used[0] = pio0;
296 | else if (pico_encoder_claim_pio(pio1))
297 | pio_used[0] = pio1; // or pio1
298 | else
299 | return -1; // or give up
300 |
301 | } else if (encoder_count == 4) {
302 | // the 5th encoder needs to try to use the other PIO
303 | pio_used[1] = (pio_used[0] == pio0) ? pio1 : pio0;
304 | if (!pico_encoder_claim_pio(pio_used[1]))
305 | return -1;
306 |
307 | } else if (encoder_count >= 8) {
308 | // we don't support more than 8 encoders
309 | return -2;
310 | }
311 |
312 | // assign the PIO and state machine and update the encoder count
313 | pio = pio_used[encoder_count / 4];
314 | sm = encoder_count % 4;
315 | encoder_count++;
316 |
317 | // set all fields to zero by default
318 | prev_trans_pos = 0;
319 | prev_low = 0;
320 | prev_high = 0;
321 | idle_stop_sample_count = 0;
322 | speed_2_20 = 0;
323 | speed = 0;
324 |
325 | // save the pin used
326 | gpio_pin = translate_pin(firstPin);
327 |
328 | // the PIO init code sets the pins as inputs. Optionally turn on pull ups
329 | // here if the user asked for it
330 | if (pullUp) {
331 | gpio_set_pulls(gpio_pin, true, false);
332 | gpio_set_pulls(gpio_pin + 1, true, false);
333 | }
334 |
335 | // initialize the PIO program (and save the PIO reference)
336 | pico_encoder_program_init(pio, sm, gpio_pin);
337 |
338 | // start "stopped" so that we don't use stale data to compute speeds
339 | stopped = 1;
340 |
341 | // cache the PIO cycles per us
342 | clocks_per_us = (clock_get_hz(clk_sys) + 500000) / 1000000;
343 |
344 | // initialize the "previous state"
345 | read_pio_data(&step, &prev_step_us, &prev_trans_us, &forward);
346 |
347 | position = get_step_start_transition_pos(step) + 32;
348 |
349 | return 0;
350 | }
351 |
352 |
353 | // compute speed in "sub-steps per 2^20 us" from a delta substep position and
354 | // delta time in microseconds
355 | static int substep_calc_speed(int delta_substep, int delta_us)
356 | {
357 | return ((int64_t) delta_substep << 20) / delta_us;
358 | }
359 |
360 | // read the PIO data and update the speed / position estimate
361 | void PicoEncoder::update(void)
362 | {
363 | uint new_step, step_us, transition_us, transition_pos, low, high;
364 | int forward, speed_high, speed_low;
365 |
366 | // read the current encoder state from the PIO
367 | read_pio_data(&new_step, &step_us, &transition_us, &forward);
368 |
369 | // from the current step we can get the low and high boundaries in
370 | // substeps of the current position
371 | low = get_step_start_transition_pos(new_step);
372 | high = get_step_start_transition_pos(new_step + 1);
373 |
374 | // if we were not stopped, but the last transition was more than
375 | // "idle_stop_samples" ago, we are stopped now
376 | if (new_step == step)
377 | idle_stop_sample_count++;
378 | else
379 | idle_stop_sample_count = 0;
380 |
381 | if (!stopped && idle_stop_sample_count >= idle_stop_samples) {
382 | speed = 0;
383 | speed_2_20 = 0;
384 | stopped = 1;
385 | }
386 |
387 | // when we are at a different step now, there is certainly a transition
388 | if (step != new_step) {
389 | // the transition position depends on the direction of the move
390 | transition_pos = forward ? low : high;
391 |
392 | // if we are not stopped, that means there is valid previous transition
393 | // we can use to estimate the current speed
394 | if (!stopped)
395 | speed_2_20 = substep_calc_speed(transition_pos - prev_trans_pos, transition_us - prev_trans_us);
396 |
397 | // if we have a transition, we are not stopped now
398 | stopped = 0;
399 | // save the timestamp and position of this transition to use later to
400 | // estimate speed
401 | prev_trans_pos = transition_pos;
402 | prev_trans_us = transition_us;
403 | }
404 |
405 | // if we are stopped, speed is zero and the position estimate remains
406 | // constant. If we are not stopped, we have to update the position and speed
407 | if (!stopped) {
408 | // although the current step doesn't give us a precise position, it does
409 | // give boundaries to the position, which together with the last
410 | // transition gives us boundaries for the speed value. This can be very
411 | // useful especially in two situations:
412 | // - we have been stopped for a while and start moving quickly: although
413 | // we only have one transition initially, the number of steps we moved
414 | // can already give a non-zero speed estimate
415 | // - we were moving but then stop: without any extra logic we would just
416 | // keep the last speed for a while, but we know from the step
417 | // boundaries that the speed must be decreasing
418 |
419 | // if there is a transition between the last sample and now, and that
420 | // transition is closer to now than the previous sample time, we should
421 | // use the slopes from the last sample to the transition as these will
422 | // have less numerical issues
423 | if (prev_trans_us > prev_step_us &&
424 | (int)(prev_trans_us - prev_step_us) > (int)(step_us - prev_trans_us)) {
425 | speed_high = substep_calc_speed(prev_trans_pos - prev_low, prev_trans_us - prev_step_us);
426 | speed_low = substep_calc_speed(prev_trans_pos - prev_high, prev_trans_us - prev_step_us);
427 | } else {
428 | // otherwise use the slopes from the last transition to now
429 | speed_high = substep_calc_speed(high - prev_trans_pos, step_us - prev_trans_us);
430 | speed_low = substep_calc_speed(low - prev_trans_pos, step_us - prev_trans_us);
431 | }
432 | // make sure the current speed estimate is between the maximum and
433 | // minimum values obtained from the step slopes
434 | if (speed_2_20 > speed_high)
435 | speed_2_20 = speed_high;
436 | if (speed_2_20 < speed_low)
437 | speed_2_20 = speed_low;
438 |
439 | // convert the speed units from "sub-steps per 2^20 us" to "sub-steps
440 | // per second"
441 | speed = (speed_2_20 * 62500LL) >> 16;
442 |
443 | // estimate the current position by applying the speed estimate to the
444 | // most recent transition
445 | internal_position = prev_trans_pos + (((int64_t)speed_2_20 * (step_us - transition_us)) >> 20);
446 |
447 | // make sure the position estimate is between "low" and "high", as we
448 | // can be sure the actual current position must be in this range
449 | if ((int)(internal_position - high) > 0)
450 | internal_position = high;
451 | else if ((int)(internal_position - low) < 0)
452 | internal_position = low;
453 | }
454 |
455 | // compute the user position, as the difference to the reset position
456 | position = internal_position - position_reset;
457 |
458 | // save the current values to use on the next sample
459 | prev_low = low;
460 | prev_high = high;
461 | step = new_step;
462 | prev_step_us = step_us;
463 | }
464 |
465 | void PicoEncoder::resetPosition(void)
466 | {
467 | position_reset = internal_position;
468 | position = 0;
469 | }
470 |
471 |
472 | #else // ARCH
473 | #error PicoEncoder library requires a PIO peripheral and only works on the RP2040 architecture
474 | #endif
475 |
--------------------------------------------------------------------------------
/src/PicoEncoder.h:
--------------------------------------------------------------------------------
1 | /*
2 | PicoEncoder - High resolution quadrature encoder using the PIO on the RP2040
3 | Created by Paulo Marques, Pedro Pereira, Paulo Costa, 2022
4 | Distributed under the BSD 2-clause license. For full terms, please refer to the LICENSE file.
5 | */
6 | #ifndef PicoEncoder_h
7 | #define PicoEncoder_h
8 |
9 | #include "Arduino.h"
10 |
11 | #ifdef ARDUINO_ARCH_RP2040
12 |
13 | #include "hardware/pio.h"
14 |
15 | // constant used to represent 4 phases with exactly the same size, which is the
16 | // default used before any phase calibration is applied
17 | #define DEFAULT_PHASES 0x404040
18 |
19 | class PicoEncoder
20 | {
21 | public:
22 | // call update() to update these fields:
23 |
24 | // current encoder speed in "substeps per second"
25 | int speed;
26 |
27 | // current encoder position in substeps. Note: to get a good speed estimate
28 | // it's always better to use the "speed" field than to try and compute
29 | // differences in position between samples
30 | int position;
31 |
32 | // current position in raw quadrature steps. This is useful to handle steps
33 | // in rotary encoders, for instance
34 | uint step;
35 |
36 |
37 | // the constructor just initializes some internal fields
38 | PicoEncoder();
39 |
40 | // initialize and start the PIO code to track the encoder. The two phases of
41 | // the encoder must be connected to consecutive pins and "firstPin" is the
42 | // lowest numbered pin using arduino naming. Note that the pins must be
43 | // consecutive in the RP2040 and may not correspond to consecutive numbers
44 | // in the arduino mapping. For instance, on the Arduino Nano RP2040 board,
45 | // pins D2 and D3 are GPIO25 and GPIO15, so they can not be used for this
46 | // purpose. However, pin D4 is GPIO16, which means pins D3 and D4 could be
47 | // used to connect the encoder by passing p3 as "firstPin" (and actually all
48 | // pins up to D9 are all consecutive). The method also sets the pins as
49 | // inputs and will turn on the pull-ups on the pins if "pullUp" is true.
50 | // Many encoders have open-collector outputs and require pull-ups. If
51 | // unsure, leave the default value of true.
52 | //
53 | // Returns 0 on success, -1 if there is no PIO available, -2 if there are
54 | // too many encoder instances
55 | int begin(int firstPin, bool pullUp = true);
56 |
57 | // read the encoder state and update "speed", "position" and "step". As a
58 | // rule of thumb, for best efficiency, call it once in your control loop
59 | // just before checking the values of "speed", "position" and/or "step".
60 | void update(void);
61 |
62 |
63 | // incrementally update the phase measurements, so that the substep
64 | // estimation takes phase sizes into account. This method should be called
65 | // at high frequency to be able to measure step sizes (see the SpeedMeasure
66 | // example code for a usage scenario)
67 | void autoCalibratePhases(void);
68 |
69 | // return true if the phase auto calibration is ready
70 | int autoCalibrationDone(void) {
71 | return (calib_count >= 128);
72 | }
73 |
74 | // get the current phase sizes. The user may save this value and pass it to
75 | // setPhases to immediately start using calibrated values without having to
76 | // call "autoCalibratePhases"
77 | int getPhases(void);
78 |
79 | // set the phase sizes using the result from a previous calibration
80 | void setPhases(int phases);
81 |
82 |
83 | // in the unlikely event that we want to restart the auto calibration
84 | // process (if there was a change in the encoder geometry, for some reason),
85 | // the user can call this method to reset the calibration data
86 | void resetAutoCalibration(void);
87 |
88 |
89 | // reset the position reference. After calling this method the current
90 | // position will be zero. This is useful to reset the position reference
91 | // using some absolute reference, like a limit switch, for instance
92 | void resetPosition(void);
93 |
94 | private:
95 | // configuration data:
96 | uint calibration_data[4]; // relative phase sizes
97 | uint clocks_per_us; // save the clk_sys frequency in clocks per us
98 |
99 | // PIO resources being used by this encoder
100 | PIO pio;
101 | uint sm;
102 |
103 | // internal fields to keep track of the previous state:
104 | uint prev_trans_pos, prev_trans_us;
105 | uint prev_step_us;
106 | uint prev_low, prev_high;
107 | uint idle_stop_sample_count;
108 | int speed_2_20;
109 | int stopped;
110 |
111 | // variables used to keep track of incremental calibration
112 | uint calib_last_us, calib_last_step, calib_count;
113 | uint calib_sum[4], calib_data[4];
114 |
115 | // position reset value
116 | uint internal_position, position_reset;
117 |
118 | // internal helper methods
119 | void read_pio_data(uint *step, uint *step_us, uint *transition_us, int *forward);
120 | uint get_step_start_transition_pos(uint step);
121 | };
122 |
123 | #else // ARCH
124 | #error PicoEncoder library requires a PIO peripheral and only works on the RP2040 architecture
125 | #endif
126 |
127 | #endif
128 |
--------------------------------------------------------------------------------
/src/pico_encoder.pio:
--------------------------------------------------------------------------------
1 | ;
2 | ; Copyright (c) 2023 Raspberry Pi (Trading) Ltd.
3 | ;
4 | ; SPDX-License-Identifier: BSD-3-Clause
5 | ;
6 | ; pico_encoder: reads a quadrature encoder with no CPU
7 | ; intervention and provides the current position on request.
8 | ;
9 | ; the "substep" version uses not only the step counts, but also the timing of
10 | ; the steps to compute the current speed. This is especially useful at low
11 | ; counts per sample, where the jitter of the counts is very noisy.
12 | ; For instance, an encoder doing 230 steps per second sampled at 100Hz would
13 | ; give something like 2, 3, 2, 2, 3, ... counts. The substep version will keep
14 | ; returning a speed around 230*64 substeps per second at every sample (each
15 | ; encoder step has 64 "substeps").
16 |
17 |
18 | .program pico_encoder
19 |
20 | .origin 0
21 |
22 | ; the PIO code counts steps like a standard quadrature encoder, but also
23 | ; keeps track of the time passed since the last transition. This allows the C
24 | ; code to build a good estimate of a fractional step position based on the
25 | ; lastest speed and time passed
26 | ;
27 | ; since it needs to push two values, it only pushes new data when the FIFO has
28 | ; enough space to hold both values. Otherwise it could either stall or go out
29 | ; of sync
30 | ;
31 | ; since we need to count the time passed, all loops must take the same number of
32 | ; cycles and there are delays added to the fastest branches to make sure it
33 | ; always takes 13 cycles per loop (e.g., sysclk 133MHz, max step rate = ~10.2
34 | ; Msteps/sec)
35 |
36 | ; push the step count and transition clock count to the RX FIFO (using
37 | ; auto push). This is reached by the "MOV PC, ~STATUS" instruction when
38 | ; status is all 1 (meaning fifo has space for this push). It also may
39 | ; execute once at program start, but that has basically no effect
40 | IN X, 32
41 | IN Y, 32
42 |
43 | update_state:
44 | ; build the state by using 2 bits from the negated previous state of the
45 | ; pins and the new 2 bit state of the pins
46 | OUT ISR, 2
47 | IN PINS, 2
48 | MOV OSR, ~ISR
49 | ; use the jump table to update the step count accordingly
50 | MOV PC, OSR
51 |
52 | decrement:
53 | ; decrement the step count
54 | JMP Y--, decrement_cont
55 | decrement_cont:
56 | ; when decrementing, X is set to 2^31, when incrementing it is set to
57 | ; zero. That way the C code can infer in which direction the last
58 | ; transition was taken and how long ago
59 | SET X, 1
60 | MOV X, ::X
61 | ; after incrementing or decrementing, continue to "check_fifo"
62 | check_fifo:
63 | .wrap_target
64 | ; on each iteration we decrement X to count the number of loops since
65 | ; the last transition
66 | JMP X--, check_fifo_cont
67 | check_fifo_cont:
68 | ; push data or continue, depending on the state of the fifo
69 | MOV PC, ~STATUS
70 |
71 | increment:
72 | ; the PIO does not have a increment instruction, so to do that we do a
73 | ; negate, decrement, negate sequence
74 | MOV Y, ~Y
75 | JMP Y--, increment_cont
76 | increment_cont:
77 | MOV Y, ~Y
78 | ; reset X to zero when incrementing
79 | SET X, 0
80 | ; wrap above to check the fifo state
81 | .wrap
82 |
83 | invalid:
84 | ; this is just a placeholder to document what the code does on invalid
85 | ; transitions, where the two phases change at the same time. We don't do
86 | ; anything special, but the encoder should not generate these invalid
87 | ; transitions anyway
88 | JMP update_state
89 |
90 | ; this jump table starts at address 16 and is accessed by the
91 | ; "MOV PC, OSR" instruction above, that loads the PC with the state on
92 | ; the lower 4 bits and the 5th bit on. The delays here extend the faster
93 | ; branches to take the same time as the slower branches
94 | JMP invalid
95 | JMP increment [0]
96 | JMP decrement [1]
97 | JMP check_fifo [4]
98 |
99 | JMP decrement [1]
100 | JMP invalid
101 | JMP check_fifo [4]
102 | JMP increment [0]
103 |
104 | JMP increment [0]
105 | JMP check_fifo [4]
106 | JMP invalid
107 | JMP decrement [1]
108 |
109 | JMP check_fifo [4]
110 | JMP decrement [1]
111 | JMP increment [0]
112 | ; this instruction should be usually reached by the "MOV PC, ~STATUS"
113 | ; instruction above when the status is zero, which means that the fifo
114 | ; has data and we don't want to push more data. This can also be reached
115 | ; on an invalid state transition, which should not happen. Even if it
116 | ; happens, it should be a transient state and the only side effect is
117 | ; that we'll call update_state twice in a row
118 | JMP update_state [1]
119 |
120 |
121 |
122 |
123 |
124 | % c-sdk {
125 |
126 | %}
127 |
--------------------------------------------------------------------------------
/src/pico_encoder.pio.h:
--------------------------------------------------------------------------------
1 | // -------------------------------------------------- //
2 | // This file is autogenerated by pioasm; do not edit! //
3 | // -------------------------------------------------- //
4 |
5 | #pragma once
6 |
7 | #if !PICO_NO_HARDWARE
8 | #include "hardware/pio.h"
9 | #endif
10 |
11 | // ------------ //
12 | // pico_encoder //
13 | // ------------ //
14 |
15 | #define pico_encoder_wrap_target 9
16 | #define pico_encoder_wrap 14
17 |
18 | static const uint16_t pico_encoder_program_instructions[] = {
19 | 0x4020, // 0: in x, 32
20 | 0x4040, // 1: in y, 32
21 | 0x60c2, // 2: out isr, 2
22 | 0x4002, // 3: in pins, 2
23 | 0xa0ee, // 4: mov osr, !isr
24 | 0xa0a7, // 5: mov pc, osr
25 | 0x0087, // 6: jmp y--, 7
26 | 0xe021, // 7: set x, 1
27 | 0xa031, // 8: mov x, ::x
28 | // .wrap_target
29 | 0x004a, // 9: jmp x--, 10
30 | 0xa0ad, // 10: mov pc, !status
31 | 0xa04a, // 11: mov y, !y
32 | 0x008d, // 12: jmp y--, 13
33 | 0xa04a, // 13: mov y, !y
34 | 0xe020, // 14: set x, 0
35 | // .wrap
36 | 0x0002, // 15: jmp 2
37 | 0x000f, // 16: jmp 15
38 | 0x000b, // 17: jmp 11
39 | 0x0106, // 18: jmp 6 [1]
40 | 0x0409, // 19: jmp 9 [4]
41 | 0x0106, // 20: jmp 6 [1]
42 | 0x000f, // 21: jmp 15
43 | 0x0409, // 22: jmp 9 [4]
44 | 0x000b, // 23: jmp 11
45 | 0x000b, // 24: jmp 11
46 | 0x0409, // 25: jmp 9 [4]
47 | 0x000f, // 26: jmp 15
48 | 0x0106, // 27: jmp 6 [1]
49 | 0x0409, // 28: jmp 9 [4]
50 | 0x0106, // 29: jmp 6 [1]
51 | 0x000b, // 30: jmp 11
52 | 0x0102, // 31: jmp 2 [1]
53 | };
54 |
55 | #if !PICO_NO_HARDWARE
56 | static const struct pio_program pico_encoder_program = {
57 | .instructions = pico_encoder_program_instructions,
58 | .length = 32,
59 | .origin = 0,
60 | };
61 |
62 | static inline pio_sm_config pico_encoder_program_get_default_config(uint offset) {
63 | pio_sm_config c = pio_get_default_sm_config();
64 | sm_config_set_wrap(&c, offset + pico_encoder_wrap_target, offset + pico_encoder_wrap);
65 | return c;
66 | }
67 |
68 | #endif
69 |
--------------------------------------------------------------------------------