enable PWM and have degrees2duty

src/main.rs

@@ -1,23 +1,42 @@
 #![no_std]
 #![no_main]
 
+use cortex_m::prelude::_embedded_hal_Pwm;
 use defmt::unwrap;
 use embassy_executor::Spawner;
 use embassy_stm32::{
-    Config, Peri,
     adc::{Adc, SampleTime},
     gpio::{AnyPin, Level, Output, Speed},
     peripherals::{ADC1, PA0},
+    time::Hertz,
+    timer::{
+        simple_pwm::{PwmPin, SimplePwm},
+        Channel,
+    },
+    Config, Peri,
 };
-use embassy_time::Timer;
+use embassy_time::{Duration, Timer};
 use {defmt_rtt as _, panic_probe as _};
 
+static SERVO_HZ: Hertz = Hertz::hz(60);
+static SERVO_MIN_USECS: f32 = 550.0;
+static SERVO_MAX_USECS: f32 = 1900.0;
+static SERVO_MIN_DEG: f32 = -180.0;
+static SERVO_MAX_DEG: f32 = 180.0;
+static SERVO_SLOPE: f32 = (SERVO_MAX_USECS - SERVO_MIN_USECS) / (SERVO_MAX_DEG - SERVO_MIN_DEG);
+
+fn degrees2duty(degrees: f32, max_dur: Duration, max_duty: u32) -> u32 {
+    let degrees = degrees.clamp(SERVO_MIN_DEG, SERVO_MAX_DEG);
+    let micros = 0.5 + SERVO_MIN_USECS + SERVO_SLOPE * (degrees - SERVO_MIN_DEG);
+    ((micros / max_dur.as_micros() as f32) * max_duty as f32) as u32
+}
+
 #[embassy_executor::main]
 async fn main(spawner: Spawner) {
     let mut config = Config::default();
 
     {
-        use embassy_stm32::rcc::{Pll, PllMul, PllPreDiv, PllRDiv, PllSource, Sysclk, mux};
+        use embassy_stm32::rcc::{mux, Pll, PllMul, PllPreDiv, PllRDiv, PllSource, Sysclk};
         config.rcc.pll = Some(Pll {
             source: PllSource::HSI,
             prediv: PllPreDiv::DIV4,
@@ -33,18 +52,27 @@ async fn main(spawner: Spawner) {
 
     let p = embassy_stm32::init(config);
 
+    let pwm_pin = PwmPin::new(p.PC7, embassy_stm32::gpio::OutputType::PushPull);
+    let mut pwm = SimplePwm::new(
+        p.TIM3,
+        None,
+        Some(pwm_pin),
+        None,
+        None,
+        SERVO_HZ,
+        embassy_stm32::timer::low_level::CountingMode::EdgeAlignedUp,
+    );
+
+    pwm.enable(Channel::Ch2);
+
+    let pwm_max_dur = Duration::from_hz(SERVO_HZ.0 as u64);
+    let pwm_max_duty = pwm.get_max_duty();
+
     let a = Adc::new(p.ADC1, Default::default());
 
-    // let mut temp = a.enable_temperature();
-
-    // let t = a.blocking_read(&mut temp, SampleTime::CYCLES247_5);
-    // defmt::info!("got temp: {}", t);
-
     let channel = p.PA0.into();
     spawner.spawn(unwrap!(read_adc1_pa0(a, channel)));
 
-    // spawner.spawn(unwrap!(blinky(p.PA5.into())));
-
     Timer::after_secs(60).await;
     treatbot_5k::exit()
 }