From a39c95e2768e39442b144ff7e13b21eea6b6d520 Mon Sep 17 00:00:00 2001 From: Sebastien Bourdeauducq Date: Sat, 6 May 2017 17:17:41 +0800 Subject: [PATCH] HV PID control (untested) --- firmware/src/main.rs | 31 ++++++++++++++++--- firmware/src/pid.rs | 72 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 99 insertions(+), 4 deletions(-) create mode 100644 firmware/src/pid.rs diff --git a/firmware/src/main.rs b/firmware/src/main.rs index 8d0edf3..bf53ea2 100644 --- a/firmware/src/main.rs +++ b/firmware/src/main.rs @@ -1,4 +1,4 @@ -#![feature(used, const_fn)] +#![feature(used, const_fn, core_float)] #![no_std] #[macro_use] @@ -6,12 +6,30 @@ extern crate cortex_m; extern crate cortex_m_rt; extern crate tm4c129x; -use core::cell::Cell; +use core::cell::{Cell, RefCell}; use cortex_m::ctxt::Local; use cortex_m::exception::Handlers as ExceptionHandlers; +use cortex_m::interrupt::Mutex; use tm4c129x::interrupt::Interrupt; use tm4c129x::interrupt::Handlers as InterruptHandlers; +mod pid; + + +const HV_PID_PARAMETERS: pid::Parameters = pid::Parameters { + kp: 0.01, + ki: 0.005, + kd: 0.0, + output_min: 0.0, + output_max: 30.0, + integral_min: -5000.0, + integral_max: 5000.0 +}; + +static HV_PID: Mutex> = Mutex::new(RefCell::new( + pid::Controller::new(HV_PID_PARAMETERS))); + + const LED1: u8 = 0x10; // PF1 const LED2: u8 = 0x40; // PF3 @@ -35,6 +53,7 @@ const AI_ERRN: u8 = 0x10; // PL4 const PWM_LOAD: u16 = (/*pwmclk*/16_000_000u32 / /*freq*/100_000) as u16; const ADC_TIMER_LOAD: u32 = /*timerclk*/16_000_000 / /*freq*/100; + fn set_led(nr: u8, state: bool) { cortex_m::interrupt::free(|cs| { let gpio_k = tm4c129x::GPIO_PORTK.borrow(cs); @@ -88,6 +107,7 @@ fn set_emission_range(range: EmissionRange) { }); } + fn main() { cortex_m::interrupt::free(|cs| { let sysctl = tm4c129x::SYSCTL.borrow(cs); @@ -221,7 +241,7 @@ fn main() { timer0.ctl.write(|w| w.taen().bit(true)); set_emission_range(EmissionRange::Med); - set_hv_pwm(PWM_LOAD/64); + HV_PID.borrow(cs).borrow_mut().set_target(200.0); set_fv_pwm(PWM_LOAD/16); set_fbv_pwm(PWM_LOAD/8); }); @@ -278,8 +298,11 @@ extern fn adc0_ss0(_ctxt: ADC0SS0) { let _fbi_sample = adc0.ssfifo0.read().data().bits(); let _fv_sample = adc0.ssfifo0.read().data().bits(); let _fd_sample = adc0.ssfifo0.read().data().bits(); - let _av_sample = adc0.ssfifo0.read().data().bits(); + let av_sample = adc0.ssfifo0.read().data().bits(); let _fbv_sample = adc0.ssfifo0.read().data().bits(); + + let mut hv_pid = HV_PID.borrow(cs).borrow_mut(); + set_hv_pwm(hv_pid.update(av_sample as f32) as u16); }) } diff --git a/firmware/src/pid.rs b/firmware/src/pid.rs new file mode 100644 index 0000000..2396824 --- /dev/null +++ b/firmware/src/pid.rs @@ -0,0 +1,72 @@ +use core::f32; +use core::num::Float; + +#[derive(Clone, Copy)] +pub struct Parameters { + pub kp: f32, + pub ki: f32, + pub kd: f32, + pub output_min: f32, + pub output_max: f32, + pub integral_min: f32, + pub integral_max: f32 +} + +pub struct Controller { + parameters: Parameters, + target: f32, + integral: f32, + last_input: f32 +} + +impl Controller { + pub const fn new(parameters: Parameters) -> Controller { + Controller { + parameters: parameters, + target: 0.0, + last_input: f32::NAN, + integral: 0.0 + } + } + + pub fn update(&mut self, input: f32) -> f32 { + let error = self.target - input; + + let p = self.parameters.kp * error; + + self.integral += error; + if self.integral < self.parameters.integral_min { + self.integral = self.parameters.integral_min; + } + if self.integral > self.parameters.integral_max { + self.integral = self.parameters.integral_max; + } + let i = self.parameters.ki * self.integral; + + let d = if self.last_input.is_nan() { + 0.0 + } else { + self.parameters.kd * (self.last_input - input) + }; + self.last_input = input; + + let mut output = p + i + d; + if output < self.parameters.output_min { + output = self.parameters.output_min; + } + if output > self.parameters.output_max { + output = self.parameters.output_max; + } + output + } + + pub fn set_target(&mut self, target: f32) { + self.target = target + } + + #[allow(dead_code)] + pub fn reset(&mut self) { + self.integral = 0.0; + self.last_input = f32::NAN; + } +}