fafo/maasier
16
0
Fork 0
Code Issues 6 Pull requests Projects Releases Packages Wiki Activity Actions

basic pid controller working???

Browse source
This commit is contained in:
Robert Schauklies 2026-02-24 21:58:24 +01:00
parent 290d157d4c
commit e897d25b09
2 changed files with 24 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

11
spinnyboy_rust/src/bin/dc_driver/dshot.rs
View file

@ -149,8 +149,15 @@ fn decode_bidi_telemetry(pulses: &[PulseCode]) -> Option<u16> {
// Process edges. // Process edges.
// TODO: strip until we find the first negative edge. // TODO: strip until we find the first negative edge.
assert_eq!(edges[0].0, false); // assert_eq!(edges[0].0, false);
let mut idx = 0;
for i in 00..edges.len() {
if edges[i].0 == false{
idx = i;
break
}
}
// Approximate. Should be calculated instead, in practice these pulses are // Approximate. Should be calculated instead, in practice these pulses are
// around 1.34us. // around 1.34us.
let period = 104u32; let period = 104u32;
@ -158,7 +165,7 @@ fn decode_bidi_telemetry(pulses: &[PulseCode]) -> Option<u16> {
// Grad middle of each period, find latest applying period. Shitty // Grad middle of each period, find latest applying period. Shitty
// algorithm. // algorithm.
let mut res = [false; 22]; let mut res = [false; 22];
for i in 0..res.len() { for i in idx..res.len() {
let pos = (i as u32)*period + period/2; let pos = (i as u32)*period + period/2;
let mut level = edges[0].0; let mut level = edges[0].0;
for edge in edges[..nedges].iter() { for edge in edges[..nedges].iter() {

27
spinnyboy_rust/src/bin/main.rs
View file

@ -28,7 +28,7 @@ use discrete_pid::time::InstantLike;
use core::cell::RefCell; use core::cell::RefCell;
use core::mem::forget; use core::mem::forget;
use critical_section::Mutex; use critical_section::Mutex;
use esp_backtrace as _; use esp_backtrace::{self as _, Backtrace};
use rtt_target::rprintln; use rtt_target::rprintln;
mod dc_driver; mod dc_driver;
@ -44,16 +44,19 @@ use crate::peripherals::ErrCommand;
use esp_hal::rmt::RxChannelConfig; use esp_hal::rmt::RxChannelConfig;
use esp_hal::rmt::RxChannelCreator; use esp_hal::rmt::RxChannelCreator;
#[panic_handler] #[panic_handler]
fn panic(_: &core::panic::PanicInfo) -> ! { fn panic(panic: &core::panic::PanicInfo) -> ! {
rprintln!("PANIC!"); rprintln!("{}",panic.message());
loop {} let trace = Backtrace::capture();
loop {
// rprintln!("{}",panic.location().unwrap())
}
} }
use alloc::format; use alloc::format;
// //
static EMERGENCY_BUTTON: Mutex<RefCell<Option<Input>>> = Mutex::new(RefCell::new(None)); static EMERGENCY_BUTTON: Mutex<RefCell<Option<Input>>> = Mutex::new(RefCell::new(None));
extern crate alloc; extern crate alloc;
//target RPM //target RPM
const DEFAULT_TARGET_RPM: u32 = 2000; const DEFAULT_TARGET_RPM: u32 = 500;
//in seconds //in seconds
const DEFAULT_SPIN_TIME: u32 = 10; const DEFAULT_SPIN_TIME: u32 = 10;
// This creates a default app-descriptor required by the esp-idf bootloader. // This creates a default app-descriptor required by the esp-idf bootloader.
@ -121,10 +124,10 @@ fn main() -> ! {
//dshot_esc.arm(&delay).unwrap(); //dshot_esc.arm(&delay).unwrap();
//PID-Controller //PID-Controller
let loop_time = Duration::from_micros(125); let loop_time = Duration::from_micros(10);
let cfg = pid::PidConfigBuilder::default() let cfg = pid::PidConfigBuilder::default()
.kp(2.0) .kp(2.0)
.ki(1.5) .ki(1.5).output_limits(100.00, 2040.00)
.sample_time(loop_time) .sample_time(loop_time)
.filter_tc(0.1) .filter_tc(0.1)
.build() .build()
@ -134,13 +137,13 @@ fn main() -> ! {
let set_point = DEFAULT_TARGET_RPM; let set_point = DEFAULT_TARGET_RPM;
dshot_esc.arm(); dshot_esc.arm();
let mut rpm = 0; let mut rpm = 0;
let mut control: f32 = 0.0; let mut control: f32 = 200.0;
// let timestamp = Instant::now(); // let timestamp = Instant::now();
loop { loop {
dshot_esc.process(); dshot_esc.process();
//let control_val = dc_driver:: //let control_val = dc_driver::
//this bad boy needs floats, this will be fun :) //this bad boy needs floats, this will be fun :)
dshot_esc.set_throttle(400); dshot_esc.set_throttle(control as _);
rpm = match dshot_esc.get_rpm() { rpm = match dshot_esc.get_rpm() {
Some(x) => { Some(x) => {
// rprintln!("GOT RPM {}", x); // rprintln!("GOT RPM {}", x);
@ -154,10 +157,10 @@ fn main() -> ! {
// rprintln!("RPM:{}",rpm); // rprintln!("RPM:{}",rpm);
let last_time = esp_hal::time::Instant::now(); let last_time = esp_hal::time::Instant::now();
let timestamp = last_time + Duration::from_millis(Instant::now().duration_since_epoch().as_millis() as _); let timestamp = esp_hal::time::Instant::now().duration_since_epoch().as_millis();
(control, ctx) = controller.compute(ctx, rpm as _, set_point as _, timestamp, None); (control, ctx) = controller.compute(ctx, rpm as _, set_point as _, Millis(timestamp), None);
let dumped_context = ctx.last_time().expect("LAST TIME!"); let dumped_context = ctx.last_time().expect("LAST TIME!");
rprintln!("control:{},rpm:{},timestamp:{:?}", control,rpm,dumped_context); rprintln!("control:{},rpm:{}", control,rpm);
} }
// rprintln!("RMT SENT!"); // rprintln!("RMT SENT!");