Pwm upgrades

Cargo.toml

@@ -10,7 +10,9 @@ ads1x1x = "0.2.2"
 ak09915_rs={git = "https://github.com/bluerobotics/AK09915-rs"}
 bmp280 = "0.4.0"
 embedded-hal = "0.2.7"
+env_logger = "0.10.0"
 icm20689 = "0.1.1"
 linux-embedded-hal = "0.3.2"
+log = "0.4.19"
 nb = "1.1.0"
 pwm-pca9685 = "0.3.1"

src/lib.rs

@@ -11,6 +11,7 @@ use linux_embedded_hal::spidev::{self, SpidevOptions};
 use linux_embedded_hal::sysfs_gpio::Direction;
 use linux_embedded_hal::I2cdev;
 use linux_embedded_hal::{Delay, Pin, Spidev};
+use log::{info, warn};
 use nb::block;
 use pwm_pca9685::{Address as pwm_Address, Pca9685};
 use std::ops::{Deref, DerefMut};
@@ -21,6 +22,9 @@ pub use ads1x1x::ChannelSelection as adc_Channel;
 
 use std::fmt::Debug;
 
+/// Navigator's default crystal clock for PWM, with a value of 24.5760 MHz
+const NAVIGATOR_PWM_XTAL_CLOCK_FREQ: f32 = 24_576_000.0;
+
 #[derive(Debug)]
 pub struct AxisData {
     pub x: f32,
@@ -125,6 +129,8 @@ impl Default for Navigator {
 
 impl Navigator {
     pub fn new() -> Navigator {
+        env_logger::init();
+
         let dev = I2cdev::new("/dev/i2c-4").unwrap();
         let address = pwm_Address::default();
         let pwm = Pca9685::new(dev, address).unwrap();
@@ -231,7 +237,30 @@ impl Navigator {
         self.pwm.oe_pin.set_direction(Direction::High).unwrap();
     }
 
-    pub fn set_pwm_channel_value(&mut self, channel: pwm_Channel, value: u16) {
+    /// Sets the Duty Cycle (high value time) of selected channel.
+    ///
+    /// On PCA9685, this function sets the `OFF` counter and uses ON value as 0.
+    ///
+    /// # Further info
+    /// Check **[7.3.3 LED output and PWM control](https://www.nxp.com/docs/en/data-sheet/PCA9685.pdf#page=16)**
+    ///
+    /// # Examples
+    ///
+    /// ```no_run
+    /// use navigator_rs::{pwm_Channel, Navigator};
+    ///
+    /// let mut nav = Navigator::new();
+    /// nav.init();
+    /// nav.pwm_enable();
+    /// nav.set_pwm_freq_prescale(100); // sets the pwm frequency to 60 Hz
+    /// nav.set_pwm_channel_value(pwm_Channel::C0, 2048); // sets the duty cycle to 50%
+    /// ```
+    pub fn set_pwm_channel_value(&mut self, channel: pwm_Channel, mut value: u16) {
+        let max_value = 4095;
+        if value > max_value {
+            warn!("Invalid value. Value must be less than or equal {max_value}.");
+            value = max_value;
+        }
         self.pwm.set_channel_on(channel, 0).unwrap();
         self.pwm.set_channel_off(channel, value).unwrap();
     }
@@ -256,8 +285,92 @@ impl Navigator {
         }
     }
 
-    pub fn set_pwm_freq(&mut self) {
-        todo!()
+    /// Sets the PWM frequency of [`Navigator`].
+    ///
+    /// It changes the PRE_SCALE value on PCA9685.
+    ///
+    /// The prescaler value can be calculated for an update rate using the formula:
+    ///
+    /// `prescale_value = round(clock_freq / (4096 * desired_freq)) - 1`.
+    ///
+    /// The minimum prescaler value is 3, which corresponds to 1526 Hz.
+    /// The maximum prescaler value is 255, which corresponds to 24 Hz.
+    ///
+    /// If you want to control a servo, set a prescaler value of 100. This will
+    /// correspond to a frequency of about 60 Hz, which is the frequency at
+    /// which servos work.
+    ///
+    /// Internally, this function stops the oscillator and restarts it after
+    /// setting the prescaler value if it was running.
+    ///
+    /// Re-run the set_pwm_channel_value() is required.
+    ///
+    /// # Further info
+    /// Check **[7.3.5 - PWM frequency PRE_SCALE](https://www.nxp.com/docs/en/data-sheet/PCA9685.pdf#page=25)**
+    ///
+    /// # Examples
+    ///
+    /// ```no_run
+    /// use navigator_rs::{pwm_Channel, Navigator};
+    ///
+    /// let mut nav = Navigator::new();
+    /// nav.init();
+    /// nav.pwm_enable();
+    /// nav.set_pwm_freq_prescale(100); // sets the pwm frequency to 60 Hz
+    /// nav.set_pwm_channel_value(pwm_Channel::C0, 2048); // sets the duty cycle to 50%
+    /// ```
+    pub fn set_pwm_freq_prescale(&mut self, mut value: u8) {
+        let min_prescale = 3;
+        if value < min_prescale {
+            warn!("Invalid value. Value must be greater than {min_prescale}.");
+            value = min_prescale;
+        }
+        self.pwm.set_prescale(value).unwrap();
+
+        let clamped_freq = NAVIGATOR_PWM_XTAL_CLOCK_FREQ / (4_096.0 * (value as f32 + 1.0));
+        info!("PWM frequency set to {clamped_freq:.2} Hz. Prescaler value: {value}");
+    }
+
+    /// Sets the pwm frequency in Hertz of [`Navigator`].
+    ///
+    /// The navigator module uses a crystal with a 24.5760 MHz clock. You can set a value for a frequency between 24 and 1526 Hz.
+    ///
+    /// # Examples
+    ///
+    /// ```no_run
+    /// use navigator_rs::{pwm_Channel, Navigator, SensorData};
+    /// use std::thread::sleep;
+    /// use std::time::Duration;
+    ///
+    /// let mut nav = Navigator::new();
+    /// nav.init();
+    /// nav.pwm_enable();
+    /// let mut i: f32 = 10.0;
+    ///
+    /// loop {
+    ///     nav.set_pwm_freq_hz(i); // sets the PWM frequency to 60 Hz
+    ///     nav.set_pwm_channel_value(pwm_Channel::C0, 2048); // sets the duty cycle to 50%
+    ///     i = i + 10.0;
+    ///     sleep(Duration::from_millis(1000));
+    /// }
+    /// ```
+    pub fn set_pwm_freq_hz(&mut self, mut freq: f32) {
+        let min_freq = 24.0;
+        if freq < min_freq {
+            warn!("Invalid value. Value must be greater than or equal to {min_freq}.");
+            freq = min_freq;
+        }
+
+        let max_freq = 1526.0;
+        if freq > max_freq {
+            warn!("Invalid value. Value must be less than or equal to {max_freq}.");
+            freq = max_freq;
+        }
+
+        let prescale_clamped_value =
+            (NAVIGATOR_PWM_XTAL_CLOCK_FREQ / (4_096.0 * freq)).round() as u8 - 1;
+
+        self.set_pwm_freq_prescale(prescale_clamped_value);
     }
 
     pub fn set_pwm_off(&mut self) {