Add firmware code to repo (#8)
* Added primary stepper motor driver code to repo * Added odometery encoder code to repo * Added the ability to update encoder wheel ratio via ros service Co-authored-by: Pimpest <82343504+Pimpest@users.noreply.github.com> Co-committed-by: Pimpest <82343504+Pimpest@users.noreply.github.com>
This commit is contained in:
31
firmware/base/pio/blink.pio
Normal file
31
firmware/base/pio/blink.pio
Normal file
@ -0,0 +1,31 @@
|
||||
.program oscillate
|
||||
pause:
|
||||
pull block
|
||||
out x,32
|
||||
.wrap_target
|
||||
pull noblock
|
||||
out x, 32
|
||||
mov y, x
|
||||
set pins, 1 ; Pin ON
|
||||
jmp y-- lp1 ; Delay for (x + 1) cycles, x is a 32 bit number
|
||||
jmp pause
|
||||
lp1:
|
||||
jmp y-- lp1 ; Delay for (x + 1) cycles, x is a 32 bit number
|
||||
mov y, x
|
||||
set pins, 0 [2] ; Pin OFF
|
||||
lp2:
|
||||
jmp y-- lp2 ; Delay for the same number of cycles again
|
||||
.wrap ; Blink forever!
|
||||
|
||||
|
||||
% c-sdk {
|
||||
// this is a raw helper function for use by the user which sets up the GPIO output, and configures the SM to output on a particular pin
|
||||
|
||||
void oscillate_program_init(PIO pio, uint sm, uint offset, uint pin) {
|
||||
pio_gpio_init(pio, pin);
|
||||
pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true);
|
||||
pio_sm_config c = oscillate_program_get_default_config(offset);
|
||||
sm_config_set_set_pins(&c, pin, 1);
|
||||
pio_sm_init(pio, sm, offset, &c);
|
||||
}
|
||||
%}
|
||||
208
firmware/base/pio/quadrature.pio
Normal file
208
firmware/base/pio/quadrature.pio
Normal file
@ -0,0 +1,208 @@
|
||||
;
|
||||
; Copyright (c) 2023 Raspberry Pi (Trading) Ltd.
|
||||
;
|
||||
; SPDX-License-Identifier: BSD-3-Clause
|
||||
;
|
||||
; quadrature_encoder_substep: reads a quadrature encoder with no CPU
|
||||
; intervention and provides the current position on request.
|
||||
;
|
||||
; the "substep" version uses not only the step counts, but also the timing of
|
||||
; the steps to compute the current speed. See README.md for details
|
||||
|
||||
|
||||
.program quadrature_encoder_substep
|
||||
|
||||
.origin 0
|
||||
|
||||
; the PIO code counts steps like the standard quadrature encoder code, but also
|
||||
; keeps track of the time passed since the last transition. That allows the C
|
||||
; code to build a good estimate of a fractional step position based on the
|
||||
; latest speed and time passed
|
||||
;
|
||||
; since it needs to push two values, it only pushes new data when the FIFO has
|
||||
; enough space to hold both values. Otherwise it could either stall or go out
|
||||
; of sync
|
||||
;
|
||||
; because we need to count the time passed, all loops must take the same number
|
||||
; of cycles and there are delays added to the fastest branches to make sure it
|
||||
; always takes 13 cycles per loop (e.g., sysclk 133MHz, max step rate = ~10.2
|
||||
; Msteps/sec)
|
||||
|
||||
; push the step count and transition clock count to the RX FIFO (using
|
||||
; auto push). This is reached by the "MOV PC, ~STATUS" instruction when
|
||||
; status is all 1 (meaning fifo has space for this push). It also may
|
||||
; execute once at program start, but that has little effect
|
||||
IN X, 32
|
||||
IN Y, 32
|
||||
|
||||
update_state:
|
||||
; build the state by using 2 bits from the negated previous state of the
|
||||
; pins and the new 2 bit state of the pins
|
||||
OUT ISR, 2
|
||||
IN PINS, 2
|
||||
MOV OSR, ~ISR
|
||||
; use the jump table to update the step count accordingly
|
||||
MOV PC, OSR
|
||||
|
||||
decrement:
|
||||
; decrement the step count
|
||||
JMP Y--, decrement_cont
|
||||
decrement_cont:
|
||||
; when decrementing, X is set to 2^31, when incrementing it is set to
|
||||
; zero. That way the C code can infer in which direction the last
|
||||
; transition was taken and how long ago
|
||||
SET X, 1
|
||||
MOV X, ::X
|
||||
; after incrementing or decrementing, continue to "check_fifo"
|
||||
check_fifo:
|
||||
.wrap_target
|
||||
; on each iteration we decrement X to count the number of loops since
|
||||
; the last transition
|
||||
JMP X--, check_fifo_cont
|
||||
check_fifo_cont:
|
||||
; push data or continue, depending on the state of the fifo
|
||||
MOV PC, ~STATUS
|
||||
|
||||
increment:
|
||||
; the PIO does not have a increment instruction, so to do that we do a
|
||||
; negate, decrement, negate sequence
|
||||
MOV Y, ~Y
|
||||
JMP Y--, increment_cont
|
||||
increment_cont:
|
||||
MOV Y, ~Y
|
||||
; reset X to zero when incrementing
|
||||
SET X, 0
|
||||
; wrap above to check the fifo state
|
||||
.wrap
|
||||
|
||||
invalid:
|
||||
; this is just a placeholder to document what the code does on invalid
|
||||
; transitions, where the two phases change at the same time. We don't do
|
||||
; anything special, but the encoder should note generate these invalid
|
||||
; transitions anyway
|
||||
JMP update_state
|
||||
|
||||
; this jump table starts at address 16 and is accessed by the
|
||||
; "MOV PC, OSR" instruction above, that loads the PC with the state on
|
||||
; the lower 4 bits and the 5th bit on. The delays here extend the faster
|
||||
; branches to take the same time as the slower branches
|
||||
JMP invalid
|
||||
JMP increment [0]
|
||||
JMP decrement [1]
|
||||
JMP check_fifo [4]
|
||||
|
||||
JMP decrement [1]
|
||||
JMP invalid
|
||||
JMP check_fifo [4]
|
||||
JMP increment [0]
|
||||
|
||||
JMP increment [0]
|
||||
JMP check_fifo [4]
|
||||
JMP invalid
|
||||
JMP decrement [1]
|
||||
|
||||
JMP check_fifo [4]
|
||||
JMP decrement [1]
|
||||
JMP increment [0]
|
||||
; this instruction should be usually reached by the "MOV PC, ~STATUS"
|
||||
; instruction above when the status is zero, which means that the fifo
|
||||
; has data and we don't want to push more data. This can also be reached
|
||||
; on an invalid state transition, which should not happen. Even if it
|
||||
; happens, it should be a transient state and the only side effect is
|
||||
; that we'll call update_state twice in a row
|
||||
JMP update_state [1]
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
% c-sdk {
|
||||
|
||||
#include "hardware/clocks.h"
|
||||
#include "hardware/timer.h"
|
||||
#include "hardware/gpio.h"
|
||||
#include "hardware/sync.h"
|
||||
|
||||
|
||||
// "substep" version low-level interface
|
||||
//
|
||||
// note: user code should use the high level functions in quadrature_encoder.c
|
||||
// and not call these directly
|
||||
|
||||
// initialize the PIO state and the substep_state_t structure that keeps track
|
||||
// of the encoder state
|
||||
static inline void quadrature_encoder_substep_program_init(PIO pio, uint sm, uint pin_A)
|
||||
{
|
||||
uint pin_state, position, ints;
|
||||
pio_gpio_init(pio, pin_A);
|
||||
pio_gpio_init(pio, pin_A + 1);
|
||||
|
||||
pio_sm_set_consecutive_pindirs(pio, sm, pin_A, 2, false);
|
||||
gpio_pull_up(pin_A);
|
||||
gpio_pull_up(pin_A + 1);
|
||||
|
||||
pio_sm_config c = quadrature_encoder_substep_program_get_default_config(0);
|
||||
sm_config_set_in_pins(&c, pin_A); // for WAIT, IN
|
||||
// shift to left, auto-push at 32 bits
|
||||
sm_config_set_in_shift(&c, false, true, 32);
|
||||
sm_config_set_out_shift(&c, true, false, 32);
|
||||
// don't join FIFO's
|
||||
sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE);
|
||||
|
||||
// always run at sysclk, to have the maximum possible time resolution
|
||||
sm_config_set_clkdiv(&c, 1.0);
|
||||
|
||||
pio_sm_init(pio, sm, 0, &c);
|
||||
|
||||
// set up status to be rx_fifo < 1
|
||||
pio->sm[sm].execctrl = ((pio->sm[sm].execctrl & 0xFFFFFF80) | 0x12);
|
||||
|
||||
// init the state machine according to the current phase. Since we are
|
||||
// setting the state running PIO instructions from C state, the encoder may
|
||||
// step during this initialization. This should not be a problem though,
|
||||
// because as long as it is just one step, the state machine will update
|
||||
// correctly when it starts. We disable interrupts anyway, to be safe
|
||||
ints = save_and_disable_interrupts();
|
||||
|
||||
pin_state = (gpio_get_all() >> pin_A) & 3;
|
||||
|
||||
// to setup the state machine, we need to set the lower 2 bits of OSR to be
|
||||
// the negated pin state
|
||||
pio_sm_exec(pio, sm, pio_encode_set(pio_y, ~pin_state));
|
||||
pio_sm_exec(pio, sm, pio_encode_mov(pio_osr, pio_y));
|
||||
|
||||
// also set the Y (current step) so that the lower 2 bits of Y have a 1:1
|
||||
// mapping to the current phase (input pin state). That simplifies the code
|
||||
// to compensate for differences in encoder phase sizes:
|
||||
switch (pin_state) {
|
||||
case 0: position = 0; break;
|
||||
case 1: position = 3; break;
|
||||
case 2: position = 1; break;
|
||||
case 3: position = 2; break;
|
||||
}
|
||||
pio_sm_exec(pio, sm, pio_encode_set(pio_y, position));
|
||||
|
||||
pio_sm_set_enabled(pio, sm, true);
|
||||
|
||||
restore_interrupts(ints);
|
||||
}
|
||||
|
||||
static inline void quadrature_encoder_substep_get_counts(PIO pio, uint sm, uint *step, int *cycles, uint *us)
|
||||
{
|
||||
int i, pairs;
|
||||
uint ints;
|
||||
|
||||
pairs = pio_sm_get_rx_fifo_level(pio, sm) >> 1;
|
||||
|
||||
// read all data with interrupts disabled, so that there can not be a
|
||||
// big time gap between reading the PIO data and the current us
|
||||
ints = save_and_disable_interrupts();
|
||||
for (i = 0; i < pairs + 1; i++) {
|
||||
*cycles = pio_sm_get_blocking(pio, sm);
|
||||
*step = pio_sm_get_blocking(pio, sm);
|
||||
}
|
||||
*us = time_us_32();
|
||||
restore_interrupts(ints);
|
||||
}
|
||||
|
||||
%}
|
||||
Reference in New Issue
Block a user