Outline ![]()
Files ![]()
loading...
SourceVu![]()
Raspberry Pi Pico SDK and Examples![]()
quadrature_encoder_substep sample![]()
quadrature_encoder_substep.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
/**
* Copyright (c) 2023 Raspberry Pi (Trading) Ltd.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
#include <stdio.h>
#include <string.h>
#include "pico/stdlib.h"
#include "hardware/pio.h"
#include "hardware/timer.h"
#include "hardware/pwm.h"
#include <pico/divider.h>
#include "quadrature_encoder_substep.pio.h"
//
// ---- quadrature encoder interface with sub-step accuracy
//
// At low speeds, or high sample rates, a quadrature encoder can produce a small
// number of steps per sample, which makes any speed estimate made from those
// counts to be very noisy.
//
// This code uses a PIO program to count steps like the standard quadrature
// encoder example, but also mark the timestamp of the last step transition,
// which then allows the sampling code to measure the actual speed by computing
// the time passed between transitions and the "distance" traveled. See
// README.md for more details
// this section is an optional DC motor control code to help test an encoder
// attached to a DC motor (and calibrate phase sizes)
const int dir_pin = 5;
const int pwm_pin = 7;
static void set_pwm(float value)
{
int ivalue = value * 6250;
if (ivalue < 0) {
gpio_put(dir_pin, true);
pwm_set_gpio_level(pwm_pin, 6250+ivalue);
} else {
gpio_put(dir_pin, false);
pwm_set_gpio_level(pwm_pin, ivalue);
}
}
static void init_pwm(void)
{
gpio_init(dir_pin);
gpio_set_dir(dir_pin, true);
gpio_init(pwm_pin);
gpio_set_dir(pwm_pin, true);
pwm_config cfg = pwm_get_default_config();
pwm_config_set_clkdiv_int(&cfg, 1);
pwm_config_set_wrap(&cfg, 6250);
pwm_init(pwm_gpio_to_slice_num(pwm_pin), &cfg, true);
gpio_set_function(pwm_pin, GPIO_FUNC_PWM);
set_pwm(0);
}
// the quadrature encoder code starts here
typedef struct substep_state_t {
// configuration data:
uint calibration_data[4]; // relative phase sizes
uint clocks_per_us; // save the clk_sys frequency in clocks per us
uint idle_stop_samples; // after these samples without transitions, assume the encoder is stopped
PIO pio;
uint sm;
// internal fields to keep track of the previous state:
uint prev_trans_pos, prev_trans_us;
uint prev_step_us;
uint prev_low, prev_high;
uint idle_stop_sample_count;
int speed_2_20;
int stopped;
// output of the encoder update function:
int speed; // estimated speed in substeps per second
uint position; // estimated position in substeps
uint raw_step; // raw step count
} substep_state_t;
// internal helper functions (not to be used by user code)
static void read_pio_data(substep_state_t *state, uint *step, uint *step_us, uint *transition_us, int *forward)
{
int cycles;
// get the raw data from the PIO state machine
quadrature_encoder_substep_get_counts(state->pio, state->sm, step, &cycles, step_us);
// when the PIO program detects a transition, it sets cycles to either zero
// (when step is incrementing) or 2^31 (when step is decrementing) and keeps
// decrementing it on each 13 clock loop. We can use this information to get
// the time and direction of the last transition
if (cycles < 0) {
cycles = -cycles;
*forward = 1;
} else {
cycles = 0x80000000 - cycles;
*forward = 0;
}
*transition_us = *step_us - ((cycles * 13) / state->clocks_per_us);
}
// get the sub-step position of the start of a step
static uint get_step_start_transition_pos(substep_state_t *state, uint step)
{
return ((step << 6) & 0xFFFFFF00) | state->calibration_data[step & 3];
}
// compute speed in "sub-steps per 2^20 us" from a delta substep position and
// delta time in microseconds. This unit is cheaper to compute and use, so we
// only convert to "sub-steps per second" once per update, at most
static int substep_calc_speed(int delta_substep, int delta_us)
{
return ((int64_t) delta_substep << 20) / delta_us;
}
// main functions to be used by user code
// initialize the substep state structure and start PIO code
static void substep_init_state(PIO pio, int sm, int pin_a, substep_state_t *state)
{
int forward;
// set all fields to zero by default
memset(state, 0, sizeof(substep_state_t));
// initialize the PIO program (and save the PIO reference)
state->pio = pio;
state->sm = sm;
quadrature_encoder_substep_program_init(pio, sm, pin_a);
// start with equal phase size calibration
state->calibration_data[0] = 0;
state->calibration_data[1] = 64;
state->calibration_data[2] = 128;
state->calibration_data[3] = 192;
state->idle_stop_samples = 3;
// start "stopped" so that we don't use stale data to compute speeds
state->stopped = 1;
// cache the PIO cycles per us
state->clocks_per_us = (clock_get_hz(clk_sys) + 500000) / 1000000;
// initialize the "previous state"
read_pio_data(state, &state->raw_step, &state->prev_step_us, &state->prev_trans_us, &forward);
state->position = get_step_start_transition_pos(state, state->raw_step) + 32;
}
// read the PIO data and update the speed / position estimate
static void substep_update(substep_state_t *state)
{
uint step, step_us, transition_us, transition_pos, low, high;
int forward, speed_high, speed_low;
// read the current encoder state from the PIO
read_pio_data(state, &step, &step_us, &transition_us, &forward);
// from the current step we can get the low and high boundaries in substeps
// of the current position
low = get_step_start_transition_pos(state, step);
high = get_step_start_transition_pos(state, step + 1);
// if we were not stopped, but the last transition was more than
// "idle_stop_samples" ago, we are stopped now
if (step == state->raw_step)
state->idle_stop_sample_count++;
else
state->idle_stop_sample_count = 0;
if (!state->stopped && state->idle_stop_sample_count >= state->idle_stop_samples) {
state->speed = 0;
state->speed_2_20 = 0;
state->stopped = 1;
}
// when we are at a different step now, there is certainly a transition
if (state->raw_step != step) {
// the transition position depends on the direction of the move
transition_pos = forward ? low : high;
// if we are not stopped, that means there is valid previous transition
// we can use to estimate the current speed
if (!state->stopped)
state->speed_2_20 = substep_calc_speed(transition_pos - state->prev_trans_pos, transition_us - state->prev_trans_us);
// if we have a transition, we are not stopped now
state->stopped = 0;
// save the timestamp and position of this transition to use later to
// estimate speed
state->prev_trans_pos = transition_pos;
state->prev_trans_us = transition_us;
}
// if we are stopped, speed is zero and the position estimate remains
// constant. If we are not stopped, we have to update the position and speed
if (!state->stopped) {
// although the current step doesn't give us a precise position, it does
// give boundaries to the position, which together with the last
// transition gives us boundaries for the speed value. This can be very
// useful especially in two situations:
// - we have been stopped for a while and start moving quickly: although
// we only have one transition initially, the number of steps we moved
// can already give a non-zero speed estimate
// - we were moving but then stop: without any extra logic we would just
// keep the last speed for a while, but we know from the step
// boundaries that the speed must be decreasing
// if there is a transition between the last sample and now, and that
// transition is closer to now than the previous sample time, we should
// use the slopes from the last sample to the transition as these will
// have less numerical issues and produce a tighter boundary
if (state->prev_trans_us > state->prev_step_us &&
(int)(state->prev_trans_us - state->prev_step_us) > (int)(step_us - state->prev_trans_us)) {
speed_high = substep_calc_speed(state->prev_trans_pos - state->prev_low, state->prev_trans_us - state->prev_step_us);
speed_low = substep_calc_speed(state->prev_trans_pos - state->prev_high, state->prev_trans_us - state->prev_step_us);
} else {
// otherwise use the slopes from the last transition to now
speed_high = substep_calc_speed(high - state->prev_trans_pos, step_us - state->prev_trans_us);
speed_low = substep_calc_speed(low - state->prev_trans_pos, step_us - state->prev_trans_us);
}
// make sure the current speed estimate is between the maximum and
// minimum values obtained from the step slopes
if (state->speed_2_20 > speed_high)
state->speed_2_20 = speed_high;
if (state->speed_2_20 < speed_low)
state->speed_2_20 = speed_low;
// convert the speed units from "sub-steps per 2^20 us" to "sub-steps
// per second"
state->speed = (state->speed_2_20 * 62500LL) >> 16;
// estimate the current position by applying the speed estimate to the
// most recent transition
state->position = state->prev_trans_pos + (((int64_t)state->speed_2_20 * (step_us - transition_us)) >> 20);
// make sure the position estimate is between "low" and "high", as we
// can be sure the actual current position must be in this range
if ((int)(state->position - high) > 0)
state->position = high;
else if ((int)(state->position - low) < 0)
state->position = low;
}
// save the current values to use on the next sample
state->prev_low = low;
state->prev_high = high;
state->raw_step = step;
state->prev_step_us = step_us;
}
// function to measure the difference between the different steps on the encoder
static void substep_calibrate_phases(PIO pio, uint sm)
{
#define sample_count 1024
//#define SHOW_ALL_SAMPLES
#ifdef SHOW_ALL_SAMPLES
static int result[sample_count];
int i;
#endif
int index, cycles, clocks_per_us, calib[4];
uint cur_us, last_us, step_us, step, last_step;
int64_t sum[4], total;
memset(sum, 0, sizeof(sum));
clocks_per_us = (clock_get_hz(clk_sys) + 500000) / 1000000;
// keep reading the PIO state in a tight loop to get all steps and use the
// transition measures of the PIO code to measure the time of each step
last_step = -10;
index = -10;
while (index < sample_count) {
quadrature_encoder_substep_get_counts(pio, sm, &step, &cycles, &step_us);
// wait until we have a transition
if (step == last_step)
continue;
// synchronize the index with the lower 2 bits of the current step
if (index < 0 && index > -4 && (step & 3) == 1)
index = 0;
// convert the "time since last transition" to an absolute microsecond
// timestamp
if (cycles > 0) {
printf("error: expected forward motion\n");
return;
}
cur_us = step_us + (cycles * 13) / clocks_per_us;
// if the index is already synchronized, use the step size
if (index >= 0) {
#ifdef SHOW_ALL_SAMPLES
result[index] = cur_us - last_us;
#endif
sum[(step - 1) & 3] += cur_us - last_us;
}
index++;
last_step = step;
last_us = cur_us;
}
#ifdef SHOW_ALL_SAMPLES
printf("full sample table:\n");
for (i = 0; i < sample_count; i++) {
printf("%d ", result[i]);
if ((i & 3) == 3)
printf("\n");
}
#endif
// scale the sizes to a total of 256 to be used as sub-steps
total = sum[0] + sum[1] + sum[2] + sum[3];
calib[0] = (sum[0] * 256 + total / 2) / total;
calib[1] = ((sum[0] + sum[1]) * 256 + total / 2) / total;
calib[2] = ((sum[0] + sum[1] + sum[2]) * 256 + total / 2) / total;
// print calibration information
printf("calibration command:\n\n");
printf("\tsubstep_set_calibration_data(&state, %d, %d, %d);\n\n",
calib[0], calib[1], calib[2]);
}
// set the phase size calibration, use the "substep_calibrate_phases" function
// to get the values. Many encoders (especially low cost ones) have phases that
// don't have the same size. To get good substep accuracy, the code should know
// about this. This is specially important at low speeds with encoders that have
// big phase size differences
static void substep_set_calibration_data(substep_state_t *state, int step0, int step1, int step2)
{
state->calibration_data[0] = 0;
state->calibration_data[1] = step0;
state->calibration_data[2] = step1;
state->calibration_data[3] = step2;
}
int main(void)
{
substep_state_t state;
// base pin to connect the A phase of the encoder. the B phase must be
// connected to the next pin
const uint PIN_A = 2;
stdio_init_all();
PIO pio = pio0;
const uint sm = 0;
pio_add_program(pio, &quadrature_encoder_substep_program);
substep_init_state(pio, sm, PIN_A, &state);
// example calibration code, uncomment to calibrate the encoder:
// // - turn on a DC motor at 50% PWM
// init_pwm();
// set_pwm(-0.5);
// // - wait for the motor to reach a reasonably stable speed
// sleep_ms(2000);
// // - run the phase size calibration code
// substep_calibrate_phases(pio, sm);
// // - stop the motor
// set_pwm(0);
// replace this with the output of the calibration function
substep_set_calibration_data(&state, 64, 128, 192);
uint last_position = 0;
int last_speed = 0;
uint last_raw_step = 0;
while (1) {
// read the PIO and update the state data
substep_update(&state);
if (last_position != state.position || last_speed != state.speed || last_raw_step != state.raw_step) {
// print out the result
printf("pos: %-10d speed: %-10d raw_steps: %-10d\n", state.position, state.speed, state.raw_step);
last_position = state.position;
last_speed = state.speed;
last_raw_step = state.raw_step;
}
// run at roughly 100Hz
sleep_ms(10);
}
}
Details ![]()
Show: from Types: Columns: ![]( "Show Project Names")
![]( "Show Locations")
![]( "Show Referring Symbols")
![]( "Show Scope")
This file uses the notable symbols shown below. Click anywhere in the file to view more details.