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
extern crate i2cdev;
use i2cdev::core::I2CDevice;
use std::{thread, time};
const MODE1: u8 = 0x00;
const MODE2: u8 = 0x01;
const PRESCALE: u8 = 0xFE;
const LED0_ON_L: u8 = 0x06;
const LED0_ON_H: u8 = 0x07;
const LED0_OFF_L: u8 = 0x08;
const LED0_OFF_H: u8 = 0x09;
const ALL_LED_ON_L: u8 = 0xFA;
const ALL_LED_ON_H: u8 = 0xFB;
const ALL_LED_OFF_L: u8 = 0xFC;
const ALL_LED_OFF_H: u8 = 0xFD;
const SLEEP: u8 = 0x10;
const ALLCALL: u8 = 0x01;
const OUTDRV: u8 = 0x04;
fn sleep_5ms() {
let five_millis = time::Duration::from_millis(50);
thread::sleep(five_millis);
}
pub struct PCA9685<T: I2CDevice + Sized> {
i2cdev: T,
}
impl<T> PCA9685<T>
where T: I2CDevice + Sized
{
#[allow(unused_must_use)]
pub fn new(mut i2cdev: T) -> Result<PCA9685<T>, T::Error> {
i2cdev.smbus_write_byte_data(MODE2, OUTDRV)?;
i2cdev.smbus_write_byte_data(MODE1, ALLCALL)?;
sleep_5ms();
let mut mode1 = i2cdev.smbus_read_byte_data(MODE1)?;
mode1 = mode1 & SLEEP;
i2cdev.smbus_write_byte_data(MODE1, mode1)?;
sleep_5ms();
Ok(PCA9685 { i2cdev: i2cdev })
}
#[allow(unused_must_use)]
pub fn set_pwm_freq(&mut self, freq: f32) -> Result<(), T::Error> {
let mut prescaleval = 25000000.0;
prescaleval /= 4096.0;
prescaleval /= freq;
prescaleval -= 1.0;
let prescale: u8 = (prescaleval + 0.5).floor() as u8;
let oldmode = self.i2cdev.smbus_read_byte_data(MODE1)?;
let newmode = (oldmode & 0x7F) | SLEEP;
self.i2cdev.smbus_write_byte_data(MODE1, newmode)?;
self.i2cdev.smbus_write_byte_data(PRESCALE, prescale)?;
self.i2cdev.smbus_write_byte_data(MODE1, oldmode)?;
sleep_5ms();
self.i2cdev.smbus_write_byte_data(MODE1, oldmode | 0xa1)?;
Ok(())
}
#[allow(unused_must_use)]
pub fn set_pwm(&mut self, channel: u8, on: u8, off: u8) -> Result<(), T::Error> {
self.i2cdev.smbus_write_byte_data(LED0_ON_L + 4 * channel, on & 0xFF)?;
self.i2cdev.smbus_write_byte_data(LED0_ON_H + 4 * channel, on >> 7)?;
self.i2cdev.smbus_write_byte_data(LED0_OFF_L + 4 * channel, off & 0xFF)?;
self.i2cdev.smbus_write_byte_data(LED0_OFF_H + 4 * channel, off >> 7)?;
Ok(())
}
#[allow(unused_must_use)]
pub fn set_all_pwm(&mut self, on: u8, off: u8) -> Result<(), T::Error> {
self.i2cdev.smbus_write_byte_data(ALL_LED_ON_L, on & 0xFF)?;
self.i2cdev.smbus_write_byte_data(ALL_LED_ON_H, on >> 7)?;
self.i2cdev.smbus_write_byte_data(ALL_LED_OFF_L, off & 0xFF)?;
self.i2cdev.smbus_write_byte_data(ALL_LED_OFF_H, off >> 7)?;
Ok(())
}
#[allow(unused_must_use)]
pub fn reset_all_servos(&mut self) -> Result<(), T::Error> {
self.i2cdev.smbus_write_byte(0x00)?;
Ok(())
}
}
#[cfg(test)]
mod tests {
#[test]
fn it_works() {}
}