HV PID control (untested)

This commit is contained in:
Sebastien Bourdeauducq 2017-05-06 17:17:41 +08:00
parent 8abeff05a1
commit a39c95e276
2 changed files with 99 additions and 4 deletions

View File

@ -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<RefCell<pid::Controller>> = 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);
})
}

72
firmware/src/pid.rs Normal file
View File

@ -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;
}
}