I2C?

Cargo.lock

@@ -222,6 +222,7 @@ dependencies = [
  "embassy-rp",
  "embassy-sync",
  "embassy-time",
+ "embedded-hal-async",
  "nanorand",
  "panic-probe",
  "static_cell",

Cargo.toml

@@ -19,6 +19,7 @@ embassy-sync = { version = "0.2.0", git = "https://github.com/embassy-rs/embassy
 static_cell = { version = "1.1", features = ["nightly"]}
 nanorand = { version = "0.7.0", default-features = false, features = ["wyrand"] }
 tinyvec = "1.6.0"
+embedded-hal-async = "0.2.0-alpha.2"
 
 [profile.release]
 lto = "fat"

src/main.rs

@@ -7,13 +7,20 @@ use embassy_executor::{Executor, Spawner};
 use embassy_rp::adc::{Adc, Config, InterruptHandler, Pin};
 use embassy_rp::bind_interrupts;
 use embassy_rp::gpio::Pull;
+use embassy_rp::i2c::{self, Config as I2Config, InterruptHandler as I2InterruptHandler};
 use embassy_rp::multicore::{spawn_core1, Stack};
+use embassy_rp::peripherals::I2C1;
 use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
 use embassy_sync::{channel::Channel, signal::Signal};
 use embassy_time::Instant;
+use embedded_hal_async::i2c::I2c;
 use static_cell::StaticCell;
 use {defmt_rtt as _, panic_probe as _};
 
+bind_interrupts!(struct I2Crqs {
+    I2C1_IRQ => I2InterruptHandler<I2C1>;
+});
+
 static mut CORE1_STACK: Stack<4096> = Stack::new();
 static EXECUTOR0: StaticCell<Executor> = StaticCell::new();
 static EXECUTOR1: StaticCell<Executor> = StaticCell::new();
@@ -113,25 +120,40 @@ async fn process_data(spawner: Spawner) {
 
 #[embassy_executor::task]
 async fn servo_control() {
+    let p = embassy_rp::init(Default::default());
+
+    let sda = p.PIN_14;
+    let scl = p.PIN_15;
+
+    // What the fuck
+    // I hate this
+    // Fuck you I2C
+    info!("set up i2c");
+    let mut i2c = i2c::I2c::new_async(p.I2C1, scl, sda, I2Crqs, I2Config::default());
+    let addr: u8 = 0x05;
+    let val: u16 = 2047;
+    // Will this work? Who knows!!
+    i2c.write(addr, &[0, val as u8, (val >> 8) as u8])
+        .await
+        .unwrap();
+
+    use nanorand::{Rng, WyRand};
+    let mut rng = WyRand::new();
+
     loop {
         let state = SERVO_SIG.wait().await;
         // Use a map of positions for each state, and move the servos towards it
         // Use lerp and randomness
         // I2C control board manages servos
         match state {
-            Some(s) => set_servo_position(s.servo_positions()).await,
+            Some(s) => {
+                info!("moving servos");
+            }
             None => warn!("No state given"),
         }
     }
 }
 
-async fn set_servo_position(pos: ServoPosition) {
-    use nanorand::{Rng, WyRand};
-    let mut rng = WyRand::new();
-
-    info!("Setting position to {}", pos)
-}
-
 #[inline]
 fn lerp(v0: f32, v1: f32, t: f32) -> f32 {
     return (1. - t) * v0 + t * v1;