diff --git a/spinnyboy_rust/src/bin/dc_driver/dshot.rs b/spinnyboy_rust/src/bin/dc_driver/dshot.rs index 558d6bf..5f27fbf 100644 --- a/spinnyboy_rust/src/bin/dc_driver/dshot.rs +++ b/spinnyboy_rust/src/bin/dc_driver/dshot.rs @@ -304,16 +304,28 @@ impl<'a> DShot<'a> { // Decode to a 16 bit value. if let Some(frame) = decode_bidi_telemetry(&buf[..len]) { // Check CRC. - let data = frame >> 4; + let mut data = frame >> 4; let crc = calculate_crc(data); if data != 0xfff && crc == (frame&0b1111) { // Fucking got it. - let exp = data >> 9; - let mantissa = data & 0b111111111; - let mut rpm: u32 = (mantissa << exp).into(); - rpm = (rpm*1000); + // ESC_telemetry[4] = (SerialBuf[7] << 8) | SerialBuf[8]; // eRpM *100 + // rprintln!("SETTING RPM"); + // rprintln!("DATA:{}",data.to_le()); + let exp = data >> 9; + + let mantissa = data & 0b111111111; + let period_ms: u32 = (mantissa << exp).into(); + let erpm = 60*100_000/period_ms*2; + // value sent by ESC is a period between each pole changes [us]. + // to achive eRPM we need to find out how many of these changes are in one minute. + // eRPM = (60*1000 000)/T_us next RPM can be achived -> RPM = eRPM/(poles/2): + + let rpm = (((erpm))/((XING_EPRO_22_COILS))); rprintln!("RPM: {}", rpm); + rprintln!("ERPM: {}", erpm); + rprintln!("period_ms: {}", period_ms); + self.rpm.set(Some(rpm)) } } } @@ -330,7 +342,7 @@ impl<'a> DShot<'a> { let tx_chann = rc.take().unwrap().wait().unwrap(); let mut pulses = self.pulses.borrow_mut(); let throttle = self.requested_throttle.borrow(); - *pulses = self.create_pulses(*throttle, false); + *pulses = self.create_pulses(*throttle, true); let pulses = pulses.as_ref(); // SAFETY: eat shit, rust let pulses = unsafe { diff --git a/spinnyboy_rust/src/bin/dc_driver/mod.rs b/spinnyboy_rust/src/bin/dc_driver/mod.rs index 564bce8..de31ea7 100644 --- a/spinnyboy_rust/src/bin/dc_driver/mod.rs +++ b/spinnyboy_rust/src/bin/dc_driver/mod.rs @@ -1,2 +1,7 @@ //pub mod afroesc; pub mod dshot; +pub fn arduino_map(x:i32, in_min:i32, in_max:i32, out_min:i32, out_max:i32) -> i32 { + + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; + +} \ No newline at end of file diff --git a/spinnyboy_rust/src/bin/main.rs b/spinnyboy_rust/src/bin/main.rs index b2fa9d9..6b5bb36 100644 --- a/spinnyboy_rust/src/bin/main.rs +++ b/spinnyboy_rust/src/bin/main.rs @@ -9,14 +9,20 @@ use alloc::boxed::Box; use alloc::string::ToString; use esp_hal::clock::CpuClock; use esp_hal::delay::Delay; -use esp_hal::{gpio, main}; use esp_hal::uart::{Config, Uart}; +use esp_hal::{gpio, main}; use esp_hal::gpio::{Event, OutputConfig}; use esp_hal::gpio::{Input, InputConfig}; use esp_hal::handler; use esp_hal::rmt::{Rmt, TxChannelConfig, TxChannelCreator}; -use esp_hal::time::Rate; +use esp_hal::time::{ Instant, Rate}; +//pid-controller +use core::time::Duration; +use discrete_pid::pid::FuncPidController; +use discrete_pid::pid::PidContext; +use discrete_pid::time::Millis; +use discrete_pid::{pid, time}; use core::cell::RefCell; use core::mem::forget; @@ -70,8 +76,13 @@ fn main() -> ! { let freq = Rate::from_mhz(80); let rmt = Rmt::new(peripherals.RMT, freq).expect("CAN NOT SET FREQUENCY"); - let rx_config = RxChannelConfig::default().with_clk_divider(1).with_idle_threshold(1000); - let tx_config = TxChannelConfig::default().with_clk_divider(1).with_idle_output(true).with_idle_output_level(esp_hal::gpio::Level::High); + let rx_config = RxChannelConfig::default() + .with_clk_divider(1) + .with_idle_threshold(1000); + let tx_config = TxChannelConfig::default() + .with_clk_divider(1) + .with_idle_output(true) + .with_idle_output_level(esp_hal::gpio::Level::High); let oc = OutputConfig::default().with_drive_mode(esp_hal::gpio::DriveMode::OpenDrain); let ic = InputConfig::default(); let rx_pin = gpio::Input::new(unsafe { peripherals.GPIO23.clone_unchecked() }, ic); @@ -80,10 +91,7 @@ fn main() -> ! { .channel0 .configure_tx(tx_pin, tx_config) .expect("creation of TX_CHANNEL FAILED!"); - let mut rx_channel = rmt - .channel3 - .configure_rx(rx_pin, rx_config) - .unwrap(); + let mut rx_channel = rmt.channel3.configure_rx(rx_pin, rx_config).unwrap(); let dshot_esc = dshot::DShot::new( &mut rx_channel, &mut tx_channel, @@ -95,15 +103,46 @@ fn main() -> ! { let dshot_esc = Box::leak(dshot_esc); //dshot_esc.arm(&delay).unwrap(); + //PID-Controller + let loop_time = core::time::Duration::from_millis(500); + let cfg = pid::PidConfigBuilder::default() + .kp(1.0) + .ki(1.5) + .sample_time(loop_time) + // .filter_tc(0.1) + .output_limits(50.00, 1000.00) + .build() + .expect("Failed to build a PID configuration"); + let pid = FuncPidController::new(cfg); + + let mut ctx = PidContext::new(Millis(0), 0.0, 0.0); + let set_point = DEFAULT_TARGET_RPM; dshot_esc.arm(); + let mut rpm = 0; + let mut control: f32 = 0.0; loop { + let mut begin = Instant::now(); dshot_esc.process(); - dshot_esc.set_throttle(150); - match dshot_esc.get_rpm() { - Some(x) => {rprintln!("GOT RPM {}",x)}, - None => {rprintln!("NO RPM!")} - } + //let control_val = dc_driver:: + //this bad boy needs floats, this will be fun :) + dshot_esc.set_throttle(200); + rpm = match dshot_esc.get_rpm() { + Some(x) => { + // rprintln!("GOT RPM {}", x); + x + } + None => { + rprintln!("NO RPM!"); + rpm + } + }; + // rprintln!("RPM:{}",rpm); + let last_time = ctx.last_time().unwrap_or(Millis(0)); + // let timestamp = last_time + Duration::from_millis(begin.elapsed().as_millis() as _); + // (control, ctx) = pid.compute(ctx, rpm as _, set_point as _, timestamp, None); + + // rprintln!("control:{},rpm:{}", control,rpm); } // rprintln!("RMT SENT!");