From a8ba1add0e025a2e7586b9038682a6e8096a14f9 Mon Sep 17 00:00:00 2001 From: Thomas Kolb Date: Sun, 15 Jan 2023 21:42:41 +0100 Subject: [PATCH] Moved switch controller to separate module --- src/main.rs | 68 +++++--------------------------- src/switch_control.rs | 91 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 101 insertions(+), 58 deletions(-) create mode 100644 src/switch_control.rs diff --git a/src/main.rs b/src/main.rs index 6c424f7..2bce6a5 100644 --- a/src/main.rs +++ b/src/main.rs @@ -42,6 +42,7 @@ use critical_section::Mutex; mod ext_adc; mod logger; +mod switch_control; use logger::Logger; @@ -67,11 +68,11 @@ const XTAL_FREQ_HZ: u32 = 12_000_000u32; const SWITCH_PWM_MAX: i32 = 512; const SWITCH_PWM_LIMIT: i32 = 93 * SWITCH_PWM_MAX / 100; -fn convert_adc_measurements(raw: &[u16; 4]) -> (u32, u32, u32) +fn convert_adc_measurements(raw: &[u16; 4]) -> (i32, i32, i32) { - let iout = (raw[0] - raw[1]) as u32 * 3300 * 20 / 4096 / 28; // *20 = division by 50 mΩ shunt - let vout = raw[2] as u32 * 3300 * (47 + 10) / 10 / 4096; - let vin = raw[3] as u32 * 3300 * (10 + 10) / 10 / 4096; + let iout = (raw[0] - raw[1]) as i32 * 3300 * 20 / 4096 / 28; // *20 = division by 50 mΩ shunt + let vout = raw[2] as i32 * 3300 * (47 + 10) / 10 / 4096; + let vin = raw[3] as i32 * 3300 * (10 + 10) / 10 / 4096; (vin, vout, iout) /* units: mV, mV, mA */ } @@ -215,17 +216,7 @@ fn main() -> ! { pac::NVIC::unmask(pac::Interrupt::TIMER_IRQ_0); } - let vtarget: i32 = 8000; - - const GAINSCALE: i32 = 10000; - let pgain: i32 = 50; - let igain: i32 = 100; - let dgain: i32 = 0; - - let mut iaccu: i32 = 0; - let mut elast: i32 = 0; - - const T: i32 = 1; + let mut switchctrl = switch_control::SwitchControl::<1>::new(8000, SWITCH_PWM_LIMIT); let mut loopcnt: u64 = 0; @@ -280,35 +271,8 @@ fn main() -> ! { let (vin, vout, iout) = convert_adc_measurements(&adc_value); - // PID controller - let err: i32 = -(vout as i32 - vtarget); - - iaccu += igain * T * err; - - // limit iaccu - if iaccu > SWITCH_PWM_LIMIT * GAINSCALE { - iaccu = SWITCH_PWM_LIMIT * GAINSCALE; - } else if iaccu < 0 { - iaccu = 0; - } - - let pval = pgain * err / GAINSCALE; - let ival = iaccu / GAINSCALE; - let dval = dgain * (err - elast) / GAINSCALE; - - elast = err; - - let ctrlout = pval + ival + dval; - - // limit the control value - let pwmval; - if ctrlout > SWITCH_PWM_LIMIT { - pwmval = SWITCH_PWM_LIMIT; - } else if ctrlout < 0 { - pwmval = 0; - } else { - pwmval = ctrlout; - } + // Update the PWM value for the switch + let pwmval = switchctrl.update(vin, vout, iout); pwr_switch_ch.set_duty(pwmval as u16); @@ -337,20 +301,8 @@ fn main() -> ! { } } - logger.log(b"]; e=").unwrap(); - data = String::from(err); - logger.log(data.as_bytes()).unwrap(); - logger.log(b" => ").unwrap(); - data = String::from(pval); - logger.log(data.as_bytes()).unwrap(); - logger.log(b"+").unwrap(); - data = String::from(ival); - logger.log(data.as_bytes()).unwrap(); - logger.log(b"+").unwrap(); - data = String::from(dval); - logger.log(data.as_bytes()).unwrap(); - logger.log(b" = ").unwrap(); - data = String::from(ctrlout); + logger.log(b"]; PWM: ").unwrap(); + data = String::from(pwmval); logger.log(data.as_bytes()).unwrap(); logger.log(b"\r\n").unwrap(); diff --git a/src/switch_control.rs b/src/switch_control.rs new file mode 100644 index 0000000..f886881 --- /dev/null +++ b/src/switch_control.rs @@ -0,0 +1,91 @@ + +struct PID +{ + pgain: i32, // proportional gain + igain: i32, // integral gain + dgain: i32, // differential gain + + iaccu: i32, // accumulator for the integral part + elast: i32, // last seen deviation value + + target: i32, + + iaccu_min: i32, // minimum allowed value in iaccu + iaccu_max: i32, // maximum allowed value in iaccu +} + +impl PID +{ + pub fn new(pgain: i32, igain: i32, dgain: i32, target: i32, start_iaccu: i32, iaccu_min: i32, + iaccu_max: i32) -> Self + { + PID:: { + pgain, + igain, + dgain, + iaccu: start_iaccu, + elast: 0, + target, + iaccu_min, + iaccu_max + } + } + + pub fn update(&mut self, meas: i32) -> i32 + { + let err = self.target - meas; + + self.iaccu += self.igain * T0 * err; + + // limit iaccu + if self.iaccu > self.iaccu_max { + self.iaccu = self.iaccu_max; + } else if self.iaccu < self.iaccu_min { + self.iaccu = self.iaccu_min; + } + + let pval = self.pgain * err / GAINSCALE; + let ival = self.iaccu / GAINSCALE; + let dval = self.dgain * (err - self.elast) / GAINSCALE; + + self.elast = err; + + pval + ival + dval + } + + pub fn set_target(&mut self, new_target: i32) + { + self.target = new_target; + } +} + +pub struct SwitchControl +{ + cv_pid: PID<10000, T0>, + pwm_max: i32 +} + +impl SwitchControl +{ + pub fn new(cv_target: i32, pwm_max: i32) -> Self + { + SwitchControl:: { + cv_pid: PID::<10000, T0>::new(100, 50, 0, cv_target, 0, 0, pwm_max * 10000), + pwm_max + } + } + + // returns the new PWM value + pub fn update(&mut self, vin: i32, vout: i32, iout: i32) -> i32 + { + let pwm = self.cv_pid.update(vout); + + if pwm > self.pwm_max { + self.pwm_max + } else if pwm < 0 { + 0 + } else { + pwm + } + } +}