motors are swapped on new chassis

restored esc pwm calculator

send length before data

common/Cargo.lock

@@ -0,0 +1,266 @@
+# This file is automatically @generated by Cargo.
+# It is not intended for manual editing.
+version = 4
+
+[[package]]
+name = "approx"
+version = "0.5.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6"
+dependencies = [
+ "num-traits",
+]
+
+[[package]]
+name = "atomic-polyfill"
+version = "1.0.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8cf2bce30dfe09ef0bfaef228b9d414faaf7e563035494d7fe092dba54b300f4"
+dependencies = [
+ "critical-section",
+]
+
+[[package]]
+name = "autocfg"
+version = "1.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26"
+
+[[package]]
+name = "byteorder"
+version = "1.5.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b"
+
+[[package]]
+name = "cobs"
+version = "0.2.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "67ba02a97a2bd10f4b59b25c7973101c79642302776489e030cd13cdab09ed15"
+
+[[package]]
+name = "common"
+version = "0.1.0"
+dependencies = [
+ "nalgebra",
+ "postcard",
+ "serde",
+]
+
+[[package]]
+name = "critical-section"
+version = "1.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b"
+
+[[package]]
+name = "hash32"
+version = "0.2.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b0c35f58762feb77d74ebe43bdbc3210f09be9fe6742234d573bacc26ed92b67"
+dependencies = [
+ "byteorder",
+]
+
+[[package]]
+name = "heapless"
+version = "0.7.17"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cdc6457c0eb62c71aac4bc17216026d8410337c4126773b9c5daba343f17964f"
+dependencies = [
+ "atomic-polyfill",
+ "hash32",
+ "rustc_version",
+ "serde",
+ "spin",
+ "stable_deref_trait",
+]
+
+[[package]]
+name = "lock_api"
+version = "0.4.12"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "07af8b9cdd281b7915f413fa73f29ebd5d55d0d3f0155584dade1ff18cea1b17"
+dependencies = [
+ "autocfg",
+ "scopeguard",
+]
+
+[[package]]
+name = "nalgebra"
+version = "0.31.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "20bd243ab3dbb395b39ee730402d2e5405e448c75133ec49cc977762c4cba3d1"
+dependencies = [
+ "approx",
+ "num-complex",
+ "num-rational",
+ "num-traits",
+ "serde",
+ "simba",
+ "typenum",
+]
+
+[[package]]
+name = "num-complex"
+version = "0.4.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495"
+dependencies = [
+ "num-traits",
+ "serde",
+]
+
+[[package]]
+name = "num-integer"
+version = "0.1.46"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f"
+dependencies = [
+ "num-traits",
+]
+
+[[package]]
+name = "num-rational"
+version = "0.4.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824"
+dependencies = [
+ "num-integer",
+ "num-traits",
+]
+
+[[package]]
+name = "num-traits"
+version = "0.2.19"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841"
+dependencies = [
+ "autocfg",
+]
+
+[[package]]
+name = "paste"
+version = "1.0.15"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a"
+
+[[package]]
+name = "postcard"
+version = "1.1.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "170a2601f67cc9dba8edd8c4870b15f71a6a2dc196daec8c83f72b59dff628a8"
+dependencies = [
+ "cobs",
+ "heapless",
+ "serde",
+]
+
+[[package]]
+name = "proc-macro2"
+version = "1.0.93"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "60946a68e5f9d28b0dc1c21bb8a97ee7d018a8b322fa57838ba31cc878e22d99"
+dependencies = [
+ "unicode-ident",
+]
+
+[[package]]
+name = "quote"
+version = "1.0.38"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0e4dccaaaf89514f546c693ddc140f729f958c247918a13380cccc6078391acc"
+dependencies = [
+ "proc-macro2",
+]
+
+[[package]]
+name = "rustc_version"
+version = "0.4.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cfcb3a22ef46e85b45de6ee7e79d063319ebb6594faafcf1c225ea92ab6e9b92"
+dependencies = [
+ "semver",
+]
+
+[[package]]
+name = "scopeguard"
+version = "1.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49"
+
+[[package]]
+name = "semver"
+version = "1.0.24"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3cb6eb87a131f756572d7fb904f6e7b68633f09cca868c5df1c4b8d1a694bbba"
+
+[[package]]
+name = "serde"
+version = "1.0.217"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "02fc4265df13d6fa1d00ecff087228cc0a2b5f3c0e87e258d8b94a156e984c70"
+dependencies = [
+ "serde_derive",
+]
+
+[[package]]
+name = "serde_derive"
+version = "1.0.217"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5a9bf7cf98d04a2b28aead066b7496853d4779c9cc183c440dbac457641e19a0"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "simba"
+version = "0.7.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2f3fd720c48c53cace224ae62bef1bbff363a70c68c4802a78b5cc6159618176"
+dependencies = [
+ "approx",
+ "num-complex",
+ "num-traits",
+ "paste",
+]
+
+[[package]]
+name = "spin"
+version = "0.9.8"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6980e8d7511241f8acf4aebddbb1ff938df5eebe98691418c4468d0b72a96a67"
+dependencies = [
+ "lock_api",
+]
+
+[[package]]
+name = "stable_deref_trait"
+version = "1.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3"
+
+[[package]]
+name = "syn"
+version = "2.0.96"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d5d0adab1ae378d7f53bdebc67a39f1f151407ef230f0ce2883572f5d8985c80"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "unicode-ident",
+]
+
+[[package]]
+name = "typenum"
+version = "1.17.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "42ff0bf0c66b8238c6f3b578df37d0b7848e55df8577b3f74f92a69acceeb825"
+
+[[package]]
+name = "unicode-ident"
+version = "1.0.14"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "adb9e6ca4f869e1180728b7950e35922a7fc6397f7b641499e8f3ef06e50dc83"

common/Cargo.toml

@@ -0,0 +1,9 @@
+[package]
+name = "common"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
+serde = { version = "1.0.*", default-features = false }
+postcard = "1.0.0"
+nalgebra = { version = "0.31.2", default-features=false, features = ["serde-serialize-no-std"]}

common/src/lib.rs

@@ -0,0 +1,54 @@
+#![no_std]
+
+use nalgebra::Vector3;
+use serde::{Deserialize, Serialize};
+
+/// -1..1
+pub type Speed = f32;
+
+#[derive(Serialize, Deserialize, Debug)]
+pub enum ControlPacket {
+    /// Forward, Clockwise
+    Twist(Speed, Speed),
+    /// Launch the flipper once
+    Fire,
+    /// Run the flipper motor manually
+    FireOverride(Speed),
+    /// Enable/Disable flipper staging
+    Arm(bool),
+    /// Enable/Disable auto light
+    SetAuto(bool),
+    RunAuto,
+    /// Stop all motors
+    Stop,
+    /// Permanently stop all motors
+    EStop,
+}
+
+#[derive(Clone, Debug, Serialize, Deserialize)]
+pub struct SensorData {
+    /// left distance, mm
+    pub tof_l: Option<u16>,
+    /// right distance, mm
+    pub tof_r: Option<u16>,
+    /// left side distance, mm
+    pub tof_s: Option<u16>,
+    /// acceleration, rad/s
+    pub gyro: Option<Vector3<f32>>,
+    /// acceleration, g
+    pub accel: Option<Vector3<f32>>,
+}
+
+#[derive(Serialize, Deserialize, Debug)]
+pub struct TelemetryPacket {
+    pub sensors: SensorData,
+    pub cam_state: CamState,
+}
+
+#[derive(Serialize, Deserialize, Debug)]
+pub enum CamState {
+    Firing,
+    Charged,
+    Charging,
+    Idle,
+}

controller/src/main.rs

@@ -88,7 +88,8 @@ async fn main(spawner: Spawner) {
     let driver = Driver::new(p.USB, Irqs);
     spawner.spawn(logger_task(driver)).unwrap();
 
-    let limit_switch = Input::new(p.PIN_22, Pull::Up);
+    let limit_switch = Input::new(p.PIN_16, Pull::Up);
+    let mut auto_status = Output::new(p.PIN_2, Level::Low);
 
     let mut d: pwm::Config = Default::default();
     d.divider = 40.into();
@@ -98,9 +99,9 @@ async fn main(spawner: Spawner) {
     let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE1, p.PIN_18, p.PIN_19, d.clone());
 
     let mut f: pwm::Config = Default::default();
-    f.divider = 40.into();
+    f.divider = 1.into(); // no reason to slow down, this is going right into a mosfet
-    f.top = 62500; // 20ms
+    f.top = u16::MAX;
-    f.compare_b = 4687; // 1.5ms
+    f.compare_b = 0;
     let flip_pwm = Pwm::new_output_b(p.PWM_SLICE0, p.PIN_17, f.clone());
 
     let sda = p.PIN_20;
@@ -112,12 +113,14 @@ async fn main(spawner: Spawner) {
     let scl = p.PIN_27;
     let config = embassy_rp::i2c::Config::default();
     let bus1 = embassy_rp::i2c::I2c::new_async(p.I2C1, scl, sda, Irqs, config);
-    let tof2enable = Output::new(p.PIN_16, Level::Low);
+    let tof_r_enable = Output::new(p.PIN_28, Level::Low);
+    let tof_l_enable = Output::new(p.PIN_22, Level::Low);
 
     let hardware = SensorHardware {
-        bus_tof: unsafe {transmute(bus)},
+        bus_tof: unsafe {transmute(bus1)},
-        bus_gyro: unsafe {transmute(bus1)},
+        bus_gyro: unsafe {transmute(bus)},
-        tof2enable,
+        tof_l_enable,
+        tof_r_enable,
     };
 
     spawn_core1(
@@ -164,7 +167,7 @@ async fn main(spawner: Spawner) {
 
     // Use a link-local address for communication without DHCP server
     let config = Config::ipv4_static(embassy_net::StaticConfigV4 {
-        address: embassy_net::Ipv4Cidr::new(embassy_net::Ipv4Address::new(169, 254, 1, 1), 16),
+        address: embassy_net::Ipv4Cidr::new(embassy_net::Ipv4Address::new(192, 168, 1, 2), 16),
         dns_servers: heapless::Vec::new(),
         gateway: None,
     });
@@ -247,8 +250,8 @@ async fn main(spawner: Spawner) {
                         info!("left to {}", clamp(forward+right, -1., 1.));
                         info!("right to {}", clamp(forward-right, -1., 1.));
 
-                        c.compare_a = calc_speed(forward - right);
+                        c.compare_a = calc_speed(-forward - right);
-                        c.compare_b = calc_speed(forward + right);
+                        c.compare_b = calc_speed(-forward + right);
                         drive_pwm.set_config(&c);
                     },
                     ControlPacket::Fire => {
@@ -264,6 +267,9 @@ async fn main(spawner: Spawner) {
                     ControlPacket::FireOverride(speed) => {
                         cam_state.replace(CamState::Override(speed));
                     },
+                    ControlPacket::SetAuto(state) => {
+                        auto_status.set_level(Level::from(state));
+                    },
                     ControlPacket::Arm(enable) => {
                         if enable {
                             cam_state.replace(CamState::Charging);
@@ -282,16 +288,33 @@ async fn main(spawner: Spawner) {
                 };
 
                 if let Ok(telem) = postcard::to_slice(&telem, &mut telem_buf) {
+                    if let Err(e) = socket.write_all(&u32::to_be_bytes(telem.len() as u32)).await {
+                        warn!("write error: {:?}", e);
+                        break;
+                    }
                     if let Err(e) = socket.write_all(&telem).await {
                         warn!("write error: {:?}", e);
                         break;
                     }
+                    //if let Err(e) = socket.flush().await {
+                    //    warn!("write error: {:?}", e);
+                    //    break;
+                    //}
                 }
             }
         }
     }
 }
 
+/// 0 to 1
+fn calc_speed_mosfet(speed: f32) -> u16 {
+    let speed = speed.clamp(0., 1.);
+
+    let speed = speed * u16::MAX as f32;
+
+    speed as u16
+}
+
 /// -1 to 1
 fn calc_speed(speed: f32) -> u16 {
     const COUNTS_PER_MS: f32 = 3125.;
@@ -358,8 +381,8 @@ fn handle_cam(cam: &mut Pwm, state: &mut CamState, limit: &Input) {
 
 
     let mut f: pwm::Config = Default::default();
-    f.divider = 40.into();
+    f.divider = 1.into();
-    f.top = 62500; // 20ms
+    f.top = u16::MAX;
-    f.compare_b = calc_speed(-speed); // 1.5ms
+    f.compare_b = calc_speed_mosfet(speed);
     cam.set_config(&f);
 }

controller/src/sensor_manager.rs

@@ -18,10 +18,11 @@ use crate::CHANNEL;
 pub struct SensorHardware {
     pub bus_tof: I2c<'static, I2C1, Async>,
     pub bus_gyro: I2c<'static, I2C0, Async>,
-    pub tof2enable: Output<'static>,
+    pub tof_l_enable: Output<'static>,
+    pub tof_r_enable: Output<'static>,
 }
 
-pub fn sensor_manager(mut hardware: SensorHardware) -> ! {
+pub fn sensor_manager(hardware: SensorHardware) -> ! {
     let mut executor = Executor::new();
     let executor: &'static mut Executor = unsafe{transmute(&mut executor)};
     executor.run(move |spawner| {
@@ -33,8 +34,7 @@ static POLL_STATE: AtomicU8 = AtomicU8::new(PollState::None as u8);
 #[repr(u8)]
 enum PollState {
     None,
-    Tof1,
+    Tof,
-    Tof2,
     Gyro,
 }
 
@@ -43,57 +43,66 @@ async fn init_sensors(mut hardware: SensorHardware) {
     let bus = RefCell::new(hardware.bus_tof);
 
     Timer::after_millis(1).await;
-    let mut tof = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).unwrap();
+    let mut tof_s = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).ok();
-    tof.set_address(0x32).unwrap();
+    if let Some(ref mut tof_s) = tof_s {
-    hardware.tof2enable.set_high();
+        tof_s.set_address(0x32).unwrap();
-    Timer::after_micros(1200).await; // DS11555 3.2
+    }
-    let mut tof2 = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).unwrap();
 
-    let mut gyro = Mpu6050::new_with_addr(hardware.bus_gyro,0x68);
+    hardware.tof_l_enable.set_high();
-    gyro.init(&mut Delay).unwrap();
+    Timer::after_micros(1200).await; // DS11555 3.2
-    gyro.set_gyro_range(mpu6050::device::GyroRange::D2000).unwrap();
+    let mut tof_l = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).ok();
-    gyro.set_accel_range(mpu6050::device::AccelRange::G16).unwrap();
+    if let Some(ref mut tof_l) = tof_l {
+        tof_l.set_address(0x33).unwrap();
+    }
+
+    hardware.tof_r_enable.set_high();
+    Timer::after_micros(1200).await; // DS11555 3.2
+    let mut tof_r = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).ok();
+    if let Some(ref mut tof_r) = tof_r {
+        tof_r.set_address(0x34).unwrap();
+    }
+
+    //let mut gyro = Mpu6050::new_with_addr(hardware.bus_gyro,0x68);
+    //gyro.init(&mut Delay).unwrap();
+    //gyro.set_gyro_range(mpu6050::device::GyroRange::D2000).unwrap();
+    //gyro.set_accel_range(mpu6050::device::AccelRange::G16).unwrap();
 
 
     let mut data = SensorData {
-        tof1: None,
+        tof_l: None,
-        tof2: None,
+        tof_r: None,
+        tof_s: None,
         gyro: None,
         accel: None,
     };
-    let mut updated_ts = Instant::now();
 
     loop {
-        let mut updated = false;
+        POLL_STATE.store(PollState::Tof as u8, Ordering::SeqCst);
-        POLL_STATE.store(PollState::Tof1 as u8, Ordering::SeqCst);
+        for (tof, store) in 
-        if let Ok(dist) = tof.read_range_single_millimeters_blocking() {
+            [(&mut tof_s, &mut data.tof_s), (&mut tof_l, &mut data.tof_l), (&mut tof_r, &mut data.tof_r)] {
-            info!("dist1: {dist}mm");
+            if let Some(ref mut tof) = tof {
-            data.tof1 = Some(dist);
+                if let Ok(dist) = tof.read_range_single_millimeters_blocking() {
-            updated = true;
+                    *store = Some(dist);
-        }
+                } else {
-        Timer::after_millis(3).await;
+                    *store = None;
-        POLL_STATE.store(PollState::Tof2 as u8, Ordering::SeqCst);
+                }
-        if let Ok(dist) = tof2.read_range_single_millimeters_blocking() {
+                Timer::after_millis(3).await;
-            info!("dist2: {dist}mm");
+            }
-            data.tof2 = Some(dist);
-            updated = true;
-        }
-        Timer::after_millis(3).await;
-        POLL_STATE.store(PollState::Gyro as u8, Ordering::SeqCst);
-        if let Ok(gyro) = gyro.get_gyro() {
-            info!("rotation: {gyro}");
-            data.gyro = Some(gyro);
-            updated = true;
-        }
-        if let Ok(accel) = gyro.get_acc() {
-            data.accel = Some(accel);
-            updated = true;
-        }
-        if updated {
-            updated_ts = Instant::now();
         }
+
+        //POLL_STATE.store(PollState::Gyro as u8, Ordering::SeqCst);
+        //if let Ok(gyro) = gyro.get_gyro() {
+        //    info!("rotation: {gyro}");
+        //    data.gyro = Some(gyro);
+        //    updated = true;
+        //}
+        //if let Ok(accel) = gyro.get_acc() {
+        //    data.accel = Some(accel);
+        //    updated = true;
+        //}
         POLL_STATE.store(PollState::None as u8, Ordering::SeqCst);
 
-        CHANNEL.send((data.clone(), updated_ts.clone())).await;
+        info!("sensors: {data:?}");
+        CHANNEL.send((data.clone(), Instant::now())).await;
     }
 }

interface/src/main.rs

@@ -46,7 +46,7 @@ fn main() -> Result<()> {
     //let latest_telem = Arc::new(RwLock::new(None));
 
     let control = executor.block_on(async {
-        let cruisecontrol = TcpStream::connect("169.254.1.1:1234").await?;
+        let cruisecontrol = TcpStream::connect("192.168.1.2:1234").await?;
         println!("connected");
         cruisecontrol.set_nodelay(true)?;
         let (telem, control) = cruisecontrol.into_split();