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
//! PWM Module
use anyhow::{Context, Result};
use rppal::i2c::I2c;
use crate::{pin::PinType, utils::init_i2c};
const REG_PW: u8 = 0x20; // REG_CHN
const REG_PSC: u8 = 0x40; // REG_PSC
const REG_PER: u8 = 0x44; // REG_ARR
/// A robot-hat PWM
pub struct PWM {
channel: u8,
period: Vec<u16>,
bus: I2c,
}
impl PWM {
/// Create a new robot-hat pwm pin with [`PinType`] *(P0-P13)*
pub fn new(pwm_pin: PinType) -> Result<Self> {
let channel = pwm_pin.pwm_channel();
let period = vec![0, 0, 0, 0];
let bus = init_i2c().context("I2C INIT FAILED")?;
let mut pwm = Self {
channel,
period,
bus,
};
pwm.freq(50).context("PWM FREQ INIT FAILED")?;
Ok(pwm)
}
/// Set the frequency of the pwm channel
///
/// Range --> (0 - 65535)Hz
pub fn freq(&mut self, freq: u16) -> Result<()> {
/* Buggy code: For now, we hardcode the values
let mut result_psc = Vec::with_capacity(12); // Create a vector for prescaler
let mut result_per = Vec::with_capacity(12); // Create a vector for period
let mut result_acy = Vec::with_capacity(12); // Create a vector for accuracy
let st = ((CLOCK as f32 / freq as f32).sqrt() as u16) - 5;
for psc in st..st + 10 {
let per = (CLOCK / (freq * psc) as u32) as u16;
result_psc.push(psc);
result_per.push(per);
result_acy.push(f32::abs(freq as f32 - CLOCK as f32 / (psc * per) as f32));
}
let i = result_acy
.iter()
.position(|&x| x == result_acy.iter().cloned().fold(f32::INFINITY, f32::min))
.unwrap();
let psc = result_psc[i];
let per = result_per[i];
*/
let psc: u16 = freq * 24; // 1200
let per: u16 = freq * 24; // 1200
self.prescaler(psc).context("PWM PRESCALER INIT FAILED")?;
self.period(per).context("PWM PERIOD INIT FAILED")?;
Ok(())
}
/// Set the prescaler for the pwm channel
///
/// Range --> (0 - 65535)
pub fn prescaler(&mut self, prescaler: u16) -> Result<()> {
let timer = self.channel / 4_u8;
let reg = REG_PSC + timer;
self.bus
.smbus_write_word(reg, (prescaler - 1).swap_bytes())
.context("PWM PRESCALER SEND FAILED")?;
Ok(())
}
/// Set the period for the pwm channel
///
/// Range --> (0 - 65535)
pub fn period(&mut self, per: u16) -> Result<()> {
let timer = self.channel / 4_u8;
let reg = REG_PER + timer;
self.period[timer as usize] = per - 1;
self.bus
.smbus_write_word(reg, self.period[timer as usize].swap_bytes())
.context("PWM PERIOD SEND FAILED")?;
Ok(())
}
/// Set the pulse width for the pwm channel
///
/// Range --> (0 - 65535)
pub fn pulse_width(&mut self, pw: u16) -> Result<()> {
let reg = REG_PW + self.channel;
self.bus
.smbus_write_word(reg, pw.swap_bytes())
.context("PWM PULSE WIDTH SEND FAILED")?;
Ok(())
}
/// Set the pulse width percentage for the pwm channel
///
/// Range --> (0 - 100)%
pub fn pulse_width_percent(&mut self, pulse_width_percent: u8) -> Result<()> {
// Buggy code ? !!
let pulse_width_percent = pulse_width_percent.clamp(0, 100);
let timer = self.channel / 4_u8;
let pulse_width = (self.period[timer as usize] * pulse_width_percent as u16) / 100;
self.pulse_width(pulse_width)?;
Ok(())
}
}