pamoja_actuators/pca9685/
driver.rs

1//! The PCA9685 driven over I2C: set the frequency, wake the oscillator, load channels.
2
3use embedded_hal::delay::DelayNs;
4use embedded_hal::i2c::I2c;
5use pamoja_core::Actuator;
6
7use super::{
8    channel_register, frequency_for_prescale, mode1, mode2, prescale_for_frequency, register, Pwm,
9    CHANNELS, INTERNAL_OSC_HZ, OSCILLATOR_STARTUP_MICROS, SOFTWARE_RESET_ADDRESS,
10    SOFTWARE_RESET_BYTE,
11};
12use crate::error::DriverError;
13
14/// One channel's setting, the command a [`Pca9685`] takes as an [`Actuator`].
15#[derive(Clone, Copy, Debug, PartialEq, Eq)]
16pub struct Output {
17    /// The channel, 0 to 15.
18    pub channel: u8,
19    /// The on and off counts to load.
20    pub pwm: Pwm,
21}
22
23/// How the sixteen outputs are wired, the MODE2 register.
24#[derive(Clone, Copy, Debug, PartialEq, Eq)]
25pub struct Outputs {
26    /// Totem-pole (push-pull) outputs, the reset default, or open-drain when
27    /// `false`; the datasheet asks for open-drain when driving LEDs with built-in
28    /// protection.
29    pub totem_pole: bool,
30    /// Invert the output logic, for boards with no external driver.
31    pub inverted: bool,
32    /// Change the outputs on the acknowledge of each register write rather than on
33    /// the stop condition; the default synchronizes a whole update.
34    pub change_on_ack: bool,
35}
36
37impl Default for Outputs {
38    fn default() -> Self {
39        Outputs {
40            totem_pole: true,
41            inverted: false,
42            change_on_ack: false,
43        }
44    }
45}
46
47impl Outputs {
48    /// Packs the wiring into the MODE2 register byte.
49    ///
50    /// # Returns
51    ///
52    /// The byte to write to [`register::MODE2`].
53    pub fn bits(self) -> u8 {
54        let mut bits = 0;
55        if self.totem_pole {
56            bits |= mode2::OUTDRV;
57        }
58        if self.inverted {
59            bits |= mode2::INVRT;
60        }
61        if self.change_on_ack {
62            bits |= mode2::OCH;
63        }
64        bits
65    }
66}
67
68/// A PCA9685 on an I2C bus, driving up to sixteen PWM outputs.
69///
70/// [`init`](Pca9685::init) follows the datasheet's prescale procedure: the
71/// oscillator is put to sleep, because the prescale register only takes a write
72/// while it is, the prescale for the requested frequency is written, the output
73/// wiring is set, the oscillator is woken, the 500 us it needs to stabilize pass,
74/// and the restart bit is written so any channel loaded before the sleep resumes.
75/// Register auto-increment stays on so a channel's four bytes load in one transfer.
76/// As an [`Actuator`] the command is an [`Output`]; a servo is
77/// [`Pwm::servo`] on its channel, and [`Actuator::map_command`] turns an angle or
78/// a `bool` into one.
79///
80/// # Examples
81///
82/// A pan-tilt camera mount's servo, driven through a PCA9685 that is not plugged in yet: the
83/// board is set up for the 50 Hz a hobby servo wants, and channel 0 is sent to the center of
84/// its travel, a 1.5 ms pulse. The simulated part keeps what it was told, so the program can
85/// read it back.
86///
87/// ```
88/// use pamoja_actuators::pca9685::{
89///     channel_register, frequency_for_prescale, register, sim, Output, Pca9685, Pwm,
90///     DEFAULT_I2C_ADDRESS, INTERNAL_OSC_HZ,
91/// };
92/// use pamoja_core::Actuator;
93/// use pamoja_hal::i2c::I2c;
94/// use pamoja_hal::script::{block_on, DelayLog};
95///
96/// const PART: u8 = DEFAULT_I2C_ADDRESS;
97/// let mut board = Pca9685::new(sim::part(PART), PART, DelayLog::new()).with_frequency(50);
98/// let center = Pwm::servo(1_500, 50);
99/// block_on(board.apply(Output { channel: 0, pwm: center }))?;
100///
101/// // The part runs at 50 Hz, to the nearest step its prescale allows, and channel 0 holds
102/// // the pulse.
103/// let (mut part, delay) = board.release();
104/// let prescale = part.register(register::PRE_SCALE);
105/// assert!((frequency_for_prescale(prescale, INTERNAL_OSC_HZ) - 50.0).abs() < 0.5);
106/// let mut held = [0u8; 4];
107/// part.write_read(PART, &[channel_register(0)], &mut held).expect("the part answers");
108/// assert_eq!(Pwm::from_bytes(&held), center);
109/// assert_eq!(delay.total_micros(), 500, "the oscillator's start-up time");
110/// # Ok::<(), pamoja_core::Error>(())
111/// ```
112#[derive(Debug)]
113pub struct Pca9685<I2C, D> {
114    bus: I2C,
115    delay: D,
116    address: u8,
117    frequency_hz: u32,
118    oscillator_hz: u32,
119    outputs: Outputs,
120    initialized: bool,
121}
122
123impl<I2C, D> Pca9685<I2C, D> {
124    /// Wraps an I2C bus and the part's address.
125    ///
126    /// # Arguments
127    ///
128    /// * `bus` - the I2C bus the part is on.
129    /// * `address` - the address the A5 to A0 pins select;
130    ///   [`DEFAULT_I2C_ADDRESS`](super::DEFAULT_I2C_ADDRESS) with all six low.
131    /// * `delay` - the timer that paces the oscillator start-up.
132    ///
133    /// # Returns
134    ///
135    /// The driver, at the part's default 200 Hz on its internal oscillator with
136    /// totem-pole outputs.
137    pub fn new(bus: I2C, address: u8, delay: D) -> Self {
138        Pca9685 {
139            bus,
140            delay,
141            address,
142            frequency_hz: 200,
143            oscillator_hz: INTERNAL_OSC_HZ,
144            outputs: Outputs::default(),
145            initialized: false,
146        }
147    }
148
149    /// Sets the PWM frequency every channel shares, 24 to 1526 Hz.
150    ///
151    /// The prescaler can only approximate a frequency; [`frequency`](Pca9685::frequency)
152    /// reports the one the part will run at.
153    ///
154    /// # Arguments
155    ///
156    /// * `hz` - the update rate; 50 Hz for hobby servos.
157    ///
158    /// # Returns
159    ///
160    /// The driver.
161    pub fn with_frequency(mut self, hz: u32) -> Self {
162        self.frequency_hz = hz;
163        self.initialized = false;
164        self
165    }
166
167    /// Sets the clock the prescaler divides, for a board that drives EXTCLK.
168    ///
169    /// # Arguments
170    ///
171    /// * `hz` - the oscillator frequency; the internal one is
172    ///   [`INTERNAL_OSC_HZ`](super::INTERNAL_OSC_HZ).
173    ///
174    /// # Returns
175    ///
176    /// The driver.
177    pub fn with_oscillator(mut self, hz: u32) -> Self {
178        self.oscillator_hz = hz;
179        self.initialized = false;
180        self
181    }
182
183    /// Sets how the outputs are wired.
184    ///
185    /// # Arguments
186    ///
187    /// * `outputs` - the MODE2 settings.
188    ///
189    /// # Returns
190    ///
191    /// The driver.
192    pub fn with_outputs(mut self, outputs: Outputs) -> Self {
193        self.outputs = outputs;
194        self.initialized = false;
195        self
196    }
197
198    /// Returns the prescale value the driver writes for its frequency.
199    pub fn prescale(&self) -> u8 {
200        prescale_for_frequency(self.frequency_hz, self.oscillator_hz)
201    }
202
203    /// Returns the frequency the part will run at, after the prescaler rounds.
204    pub fn frequency(&self) -> f32 {
205        frequency_for_prescale(self.prescale(), self.oscillator_hz)
206    }
207
208    /// Gives back the bus and the delay.
209    ///
210    /// # Returns
211    ///
212    /// The bus and delay the driver was built from.
213    pub fn release(self) -> (I2C, D) {
214        (self.bus, self.delay)
215    }
216}
217
218impl<I2C: I2c, D: DelayNs> Pca9685<I2C, D> {
219    /// Sets the prescale and output wiring and starts the oscillator.
220    ///
221    /// # Errors
222    ///
223    /// Returns [`DriverError::Bus`] if the bus fails.
224    pub fn init(&mut self) -> Result<(), DriverError<I2C::Error>> {
225        let awake = mode1::AUTO_INCREMENT;
226        self.write(register::MODE1, awake | mode1::SLEEP)?;
227        self.write(register::PRE_SCALE, self.prescale())?;
228        self.write(register::MODE2, self.outputs.bits())?;
229        self.write(register::MODE1, awake)?;
230        self.delay.delay_us(OSCILLATOR_STARTUP_MICROS);
231        self.write(register::MODE1, awake | mode1::RESTART)?;
232        self.initialized = true;
233        Ok(())
234    }
235
236    /// Loads one channel's on and off counts.
237    ///
238    /// Initializes the part first if [`init`](Pca9685::init) has not run.
239    ///
240    /// # Arguments
241    ///
242    /// * `channel` - the output, 0 to 15.
243    /// * `pwm` - the counts to load.
244    ///
245    /// # Errors
246    ///
247    /// Returns [`DriverError::Command`] for a channel the part does not have, and
248    /// [`DriverError::Bus`] if the bus fails.
249    pub fn set_channel(&mut self, channel: u8, pwm: Pwm) -> Result<(), DriverError<I2C::Error>> {
250        if channel >= CHANNELS {
251            return Err(DriverError::Command(
252                "the PCA9685 has sixteen channels, 0 to 15",
253            ));
254        }
255        if !self.initialized {
256            self.init()?;
257        }
258        let [on_l, on_h, off_l, off_h] = pwm.bytes();
259        self.bus
260            .write(
261                self.address,
262                &[channel_register(channel), on_l, on_h, off_l, off_h],
263            )
264            .map_err(DriverError::Bus)
265    }
266
267    /// Reads one channel's on and off counts back from the part.
268    ///
269    /// The four registers are read one at a time, so the read works whether or not
270    /// MODE1's auto-increment bit is set, and it changes nothing on the part: a
271    /// program that restarts can ask a running part what it holds.
272    ///
273    /// # Arguments
274    ///
275    /// * `channel` - the output, 0 to 15.
276    ///
277    /// # Returns
278    ///
279    /// The setting the channel holds, full-on and full-off flags included.
280    ///
281    /// # Errors
282    ///
283    /// Returns [`DriverError::Command`] for a channel the part does not have, and
284    /// [`DriverError::Bus`] if the bus fails.
285    pub fn channel(&mut self, channel: u8) -> Result<Pwm, DriverError<I2C::Error>> {
286        if channel >= CHANNELS {
287            return Err(DriverError::Command(
288                "the PCA9685 has sixteen channels, 0 to 15",
289            ));
290        }
291        let first = channel_register(channel);
292        let mut bytes = [0; 4];
293        for (register, byte) in (first..).zip(bytes.iter_mut()) {
294            *byte = self.read(register)?;
295        }
296        Ok(Pwm::from_bytes(&bytes))
297    }
298
299    /// Loads every channel with the same on and off counts in one transfer.
300    ///
301    /// # Arguments
302    ///
303    /// * `pwm` - the counts to load.
304    ///
305    /// # Errors
306    ///
307    /// Returns [`DriverError::Bus`] if the bus fails.
308    pub fn set_all(&mut self, pwm: Pwm) -> Result<(), DriverError<I2C::Error>> {
309        if !self.initialized {
310            self.init()?;
311        }
312        let [on_l, on_h, off_l, off_h] = pwm.bytes();
313        self.bus
314            .write(
315                self.address,
316                &[register::ALL_LED_ON_L, on_l, on_h, off_l, off_h],
317            )
318            .map_err(DriverError::Bus)
319    }
320
321    /// Stops the oscillator; the channel registers keep their values.
322    ///
323    /// # Errors
324    ///
325    /// Returns [`DriverError::Bus`] if the bus fails.
326    pub fn sleep(&mut self) -> Result<(), DriverError<I2C::Error>> {
327        self.write(register::MODE1, mode1::AUTO_INCREMENT | mode1::SLEEP)
328    }
329
330    /// Restarts the oscillator and every channel that was running before sleep.
331    ///
332    /// # Errors
333    ///
334    /// Returns [`DriverError::Bus`] if the bus fails.
335    pub fn wake(&mut self) -> Result<(), DriverError<I2C::Error>> {
336        self.write(register::MODE1, mode1::AUTO_INCREMENT)?;
337        self.delay.delay_us(OSCILLATOR_STARTUP_MICROS);
338        self.write(register::MODE1, mode1::AUTO_INCREMENT | mode1::RESTART)
339    }
340
341    /// Resets every PCA9685 on the bus to its power-up state.
342    ///
343    /// This is the general-call software reset the datasheet defines; it reaches
344    /// every part on the bus that honors it, not only this one.
345    ///
346    /// # Errors
347    ///
348    /// Returns [`DriverError::Bus`] if the bus fails.
349    pub fn software_reset(&mut self) -> Result<(), DriverError<I2C::Error>> {
350        self.initialized = false;
351        self.bus
352            .write(SOFTWARE_RESET_ADDRESS, &[SOFTWARE_RESET_BYTE])
353            .map_err(DriverError::Bus)
354    }
355
356    fn write(&mut self, register: u8, value: u8) -> Result<(), DriverError<I2C::Error>> {
357        self.bus
358            .write(self.address, &[register, value])
359            .map_err(DriverError::Bus)
360    }
361
362    fn read(&mut self, register: u8) -> Result<u8, DriverError<I2C::Error>> {
363        let mut value = [0];
364        self.bus
365            .write_read(self.address, &[register], &mut value)
366            .map_err(DriverError::Bus)?;
367        Ok(value[0])
368    }
369}
370
371impl<I2C, D> Actuator for Pca9685<I2C, D>
372where
373    I2C: I2c + Send,
374    I2C::Error: core::fmt::Debug,
375    D: DelayNs + Send,
376{
377    type Command = Output;
378
379    async fn apply(&mut self, command: Output) -> pamoja_core::Result<()> {
380        self.set_channel(command.channel, command.pwm)
381            .map_err(pamoja_core::Error::from)
382    }
383}
384
385#[cfg(test)]
386mod tests {
387    use super::*;
388    use crate::pca9685::DEFAULT_I2C_ADDRESS;
389    use alloc::vec;
390    use alloc::vec::Vec;
391    use embedded_hal::i2c::{ErrorKind, NoAcknowledgeSource};
392    use pamoja_hal::script::{block_on, DelayLog, I2cScript, I2cStep};
393
394    fn init_steps(address: u8, prescale: u8, mode2: u8) -> Vec<I2cStep> {
395        vec![
396            I2cStep::write(address, [register::MODE1, 0x30]),
397            I2cStep::write(address, [register::PRE_SCALE, prescale]),
398            I2cStep::write(address, [register::MODE2, mode2]),
399            I2cStep::write(address, [register::MODE1, 0x20]),
400            I2cStep::write(address, [register::MODE1, 0xA0]),
401        ]
402    }
403
404    #[test]
405    fn init_sleeps_to_set_the_prescale_then_wakes_and_restarts() {
406        let mut board = Pca9685::new(
407            I2cScript::new(init_steps(DEFAULT_I2C_ADDRESS, 0x79, 0x04)),
408            DEFAULT_I2C_ADDRESS,
409            DelayLog::new(),
410        )
411        .with_frequency(50);
412        assert_eq!(board.prescale(), 0x79, "the datasheet formula at 50 Hz");
413        assert!((board.frequency() - 50.03).abs() < 0.01);
414        board.init().unwrap();
415        let (bus, delay) = board.release();
416        assert!(bus.done());
417        assert_eq!(delay.waits_ns(), [OSCILLATOR_STARTUP_MICROS * 1_000]);
418    }
419
420    #[test]
421    fn the_default_frequency_is_the_datasheet_default_prescale() {
422        let board = Pca9685::new(I2cScript::new([]), 0x41, DelayLog::new());
423        assert_eq!(board.prescale(), super::super::PRE_SCALE_RESET);
424    }
425
426    #[test]
427    fn channels_load_four_bytes_through_auto_increment() {
428        let pulse = Pwm::servo(1_500, 50);
429        let [on_l, on_h, off_l, off_h] = pulse.bytes();
430        let mut steps = init_steps(0x41, 0x79, 0x14);
431        steps.push(I2cStep::write(
432            0x41,
433            [0x06 + 4 * 15, on_l, on_h, off_l, off_h],
434        ));
435        steps.push(I2cStep::write(
436            0x41,
437            [register::ALL_LED_ON_L, 0x00, 0x10, 0x00, 0x00],
438        ));
439        let mut board = Pca9685::new(I2cScript::new(steps), 0x41, DelayLog::new())
440            .with_frequency(50)
441            .with_outputs(Outputs {
442                inverted: true,
443                ..Outputs::default()
444            });
445        board.set_channel(15, pulse).unwrap();
446        board.set_all(Pwm::full_on()).unwrap();
447        assert_eq!(
448            board.set_channel(16, pulse),
449            Err(DriverError::Command(
450                "the PCA9685 has sixteen channels, 0 to 15"
451            ))
452        );
453        assert!(board.release().0.done());
454    }
455
456    #[test]
457    fn a_channel_reads_back_one_register_a_transfer_without_initializing() {
458        let pulse = Pwm::servo(1_500, 50);
459        let [on_l, on_h, off_l, off_h] = pulse.bytes();
460        let first = channel_register(2);
461        let steps = [
462            I2cStep::write_read(0x40, [first], [on_l]),
463            I2cStep::write_read(0x40, [first + 1], [on_h]),
464            I2cStep::write_read(0x40, [first + 2], [off_l]),
465            I2cStep::write_read(0x40, [first + 3], [off_h]),
466        ];
467        let mut board = Pca9685::new(I2cScript::new(steps), 0x40, DelayLog::new());
468        assert_eq!(board.channel(2).unwrap(), pulse);
469        assert_eq!(
470            board.channel(16),
471            Err(DriverError::Command(
472                "the PCA9685 has sixteen channels, 0 to 15"
473            ))
474        );
475        assert!(board.release().0.done());
476    }
477
478    #[test]
479    fn sleep_wake_and_software_reset_write_what_the_datasheet_names() {
480        let steps = [
481            I2cStep::write(0x40, [register::MODE1, 0x30]),
482            I2cStep::write(0x40, [register::MODE1, 0x20]),
483            I2cStep::write(0x40, [register::MODE1, 0xA0]),
484            I2cStep::write(SOFTWARE_RESET_ADDRESS, [SOFTWARE_RESET_BYTE]),
485        ];
486        let mut board = Pca9685::new(I2cScript::new(steps), 0x40, DelayLog::new());
487        board.sleep().unwrap();
488        board.wake().unwrap();
489        board.software_reset().unwrap();
490        let (bus, delay) = board.release();
491        assert!(bus.done());
492        assert_eq!(delay.total_micros(), 500);
493    }
494
495    #[test]
496    fn a_bus_fault_surfaces_as_a_bus_error_and_maps_to_the_core() {
497        let kind = ErrorKind::NoAcknowledge(NoAcknowledgeSource::Address);
498        let mut board = Pca9685::new(
499            I2cScript::new([I2cStep::fault(0x40, kind)]),
500            0x40,
501            DelayLog::new(),
502        );
503        let error = board.init().unwrap_err();
504        assert!(matches!(error, DriverError::Bus(_)));
505        let core: pamoja_core::Error = error.into();
506        assert!(matches!(core, pamoja_core::Error::Io(_)));
507        let refused: pamoja_core::Error = DriverError::<ErrorKind>::Command("channel").into();
508        assert!(matches!(refused, pamoja_core::Error::Codec(_)));
509    }
510
511    #[test]
512    fn as_an_actuator_a_mapped_command_drives_a_servo_from_a_bool() {
513        let open = Pwm::servo(2_000, 50);
514        let closed = Pwm::servo(1_000, 50);
515        let mut steps = init_steps(0x40, 0x79, 0x04);
516        let [a, b, c, d] = open.bytes();
517        steps.push(I2cStep::write(0x40, [0x06, a, b, c, d]));
518        let [a, b, c, d] = closed.bytes();
519        steps.push(I2cStep::write(0x40, [0x06, a, b, c, d]));
520        let board = Pca9685::new(I2cScript::new(steps), 0x40, DelayLog::new()).with_frequency(50);
521        let mut valve = board.map_command(|open_now: bool| Output {
522            channel: 0,
523            pwm: if open_now { open } else { closed },
524        });
525        block_on(valve.apply(true)).unwrap();
526        block_on(valve.apply(false)).unwrap();
527        assert!(valve.into_inner().release().0.done());
528    }
529
530    #[test]
531    fn init_at_fifty_hertz_and_one_servo_make_the_transfers_the_datasheet_gives() {
532        const PART: u8 = DEFAULT_I2C_ADDRESS;
533        let center = Pwm::servo(1_500, 50);
534        let [on_l, on_h, off_l, off_h] = center.bytes();
535        let bus = I2cScript::new([
536            I2cStep::write(PART, [0x00, 0x30]),
537            I2cStep::write(PART, [0xFE, 0x79]),
538            I2cStep::write(PART, [0x01, 0x04]),
539            I2cStep::write(PART, [0x00, 0x20]),
540            I2cStep::write(PART, [0x00, 0xA0]),
541            I2cStep::write(PART, [0x06, on_l, on_h, off_l, off_h]),
542        ]);
543
544        let mut board = Pca9685::new(bus, PART, DelayLog::new()).with_frequency(50);
545        block_on(board.apply(Output {
546            channel: 0,
547            pwm: center,
548        }))
549        .expect("the scripted part answers");
550        let (bus, delay) = board.release();
551        assert!(bus.done());
552        assert_eq!(delay.total_micros(), 500);
553    }
554}
pamoja_actuators/pca9685/driver.rs

pamoja_actuators/pca9685/
driver.rs
