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
|
//! Pio Stepper Driver for 5-wire steppers
use core::mem::{self, MaybeUninit};
use crate::Peri;
use crate::pio::{Common, Config, Direction, Instance, Irq, LoadedProgram, PioPin, StateMachine};
use crate::pio_programs::clock_divider::calculate_pio_clock_divider;
/// This struct represents a Stepper driver program loaded into pio instruction memory.
pub struct PioStepperProgram<'a, PIO: Instance> {
prg: LoadedProgram<'a, PIO>,
}
impl<'a, PIO: Instance> PioStepperProgram<'a, PIO> {
/// Load the program into the given pio
pub fn new(common: &mut Common<'a, PIO>) -> Self {
let prg = pio::pio_asm!(
"pull block",
"mov x, osr",
"pull block",
"mov y, osr",
"jmp !x end",
"loop:",
"jmp !osre step",
"mov osr, y",
"step:",
"out pins, 4 [31]"
"jmp x-- loop",
"end:",
"irq 0 rel"
);
let prg = common.load_program(&prg.program);
Self { prg }
}
}
/// Pio backed Stepper driver
pub struct PioStepper<'d, T: Instance, const SM: usize> {
irq: Irq<'d, T, SM>,
sm: StateMachine<'d, T, SM>,
}
impl<'d, T: Instance, const SM: usize> PioStepper<'d, T, SM> {
/// Configure a state machine to drive a stepper
pub fn new(
pio: &mut Common<'d, T>,
mut sm: StateMachine<'d, T, SM>,
irq: Irq<'d, T, SM>,
pin0: Peri<'d, impl PioPin>,
pin1: Peri<'d, impl PioPin>,
pin2: Peri<'d, impl PioPin>,
pin3: Peri<'d, impl PioPin>,
program: &PioStepperProgram<'d, T>,
) -> Self {
let pin0 = pio.make_pio_pin(pin0);
let pin1 = pio.make_pio_pin(pin1);
let pin2 = pio.make_pio_pin(pin2);
let pin3 = pio.make_pio_pin(pin3);
sm.set_pin_dirs(Direction::Out, &[&pin0, &pin1, &pin2, &pin3]);
let mut cfg = Config::default();
cfg.set_out_pins(&[&pin0, &pin1, &pin2, &pin3]);
cfg.clock_divider = calculate_pio_clock_divider(100 * 136);
cfg.use_program(&program.prg, &[]);
sm.set_config(&cfg);
sm.set_enable(true);
Self { irq, sm }
}
/// Set pulse frequency
pub fn set_frequency(&mut self, freq: u32) {
let clock_divider = calculate_pio_clock_divider(freq * 136);
let divider_f32 = clock_divider.to_num::<f32>();
assert!(divider_f32 <= 65536.0, "clkdiv must be <= 65536");
assert!(divider_f32 >= 1.0, "clkdiv must be >= 1");
self.sm.set_clock_divider(clock_divider);
self.sm.clkdiv_restart();
}
/// Full step, one phase
pub async fn step(&mut self, steps: i32) {
if steps > 0 {
self.run(steps, 0b1000_0100_0010_0001_1000_0100_0010_0001).await
} else {
self.run(-steps, 0b0001_0010_0100_1000_0001_0010_0100_1000).await
}
}
/// Full step, two phase
pub async fn step2(&mut self, steps: i32) {
if steps > 0 {
self.run(steps, 0b1001_1100_0110_0011_1001_1100_0110_0011).await
} else {
self.run(-steps, 0b0011_0110_1100_1001_0011_0110_1100_1001).await
}
}
/// Half step
pub async fn step_half(&mut self, steps: i32) {
if steps > 0 {
self.run(steps, 0b1001_1000_1100_0100_0110_0010_0011_0001).await
} else {
self.run(-steps, 0b0001_0011_0010_0110_0100_1100_1000_1001).await
}
}
async fn run(&mut self, steps: i32, pattern: u32) {
self.sm.tx().wait_push(steps as u32).await;
self.sm.tx().wait_push(pattern).await;
let drop = OnDrop::new(|| {
self.sm.clear_fifos();
unsafe {
self.sm.exec_instr(
pio::InstructionOperands::JMP {
address: 0,
condition: pio::JmpCondition::Always,
}
.encode(),
);
}
});
self.irq.wait().await;
drop.defuse();
}
}
struct OnDrop<F: FnOnce()> {
f: MaybeUninit<F>,
}
impl<F: FnOnce()> OnDrop<F> {
pub fn new(f: F) -> Self {
Self { f: MaybeUninit::new(f) }
}
pub fn defuse(self) {
mem::forget(self)
}
}
impl<F: FnOnce()> Drop for OnDrop<F> {
fn drop(&mut self) {
unsafe { self.f.as_ptr().read()() }
}
}
|
