Compare commits
5 Commits
69a4f5a5d2
...
cee0a1fcab
Author | SHA1 | Date |
---|---|---|
Astro | cee0a1fcab | |
Astro | 0f4442b124 | |
Astro | 1ad821299b | |
Astro | 0575b52bc1 | |
Astro | ac728a2aff |
|
@ -1,4 +1,5 @@
|
||||||
use core::fmt;
|
use core::fmt;
|
||||||
|
use lexical_core::Float;
|
||||||
use stm32f4xx_hal::{
|
use stm32f4xx_hal::{
|
||||||
time::{MegaHertz, U32Ext},
|
time::{MegaHertz, U32Ext},
|
||||||
spi,
|
spi,
|
||||||
|
@ -146,18 +147,9 @@ impl PostFilter {
|
||||||
];
|
];
|
||||||
|
|
||||||
pub fn closest(rate: f32) -> Option<Self> {
|
pub fn closest(rate: f32) -> Option<Self> {
|
||||||
/// (x - y).abs()
|
|
||||||
fn d(x: f32, y: f32) -> f32 {
|
|
||||||
if x >= y {
|
|
||||||
x - y
|
|
||||||
} else {
|
|
||||||
y - x
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
let mut best: Option<(f32, Self)> = None;
|
let mut best: Option<(f32, Self)> = None;
|
||||||
for value in Self::VALID_VALUES {
|
for value in Self::VALID_VALUES {
|
||||||
let error = d(rate, value.output_rate().unwrap());
|
let error = (rate - value.output_rate().unwrap()).abs();
|
||||||
let better = best
|
let better = best
|
||||||
.map(|(best_error, _)| error < best_error)
|
.map(|(best_error, _)| error < best_error)
|
||||||
.unwrap_or(true);
|
.unwrap_or(true);
|
||||||
|
|
|
@ -123,12 +123,12 @@ pub enum Command {
|
||||||
Pid {
|
Pid {
|
||||||
channel: usize,
|
channel: usize,
|
||||||
parameter: PidParameter,
|
parameter: PidParameter,
|
||||||
value: f32,
|
value: f64,
|
||||||
},
|
},
|
||||||
SteinhartHart {
|
SteinhartHart {
|
||||||
channel: usize,
|
channel: usize,
|
||||||
parameter: ShParameter,
|
parameter: ShParameter,
|
||||||
value: f32,
|
value: f64,
|
||||||
},
|
},
|
||||||
PostFilter {
|
PostFilter {
|
||||||
channel: usize,
|
channel: usize,
|
||||||
|
@ -158,12 +158,12 @@ fn unsigned(input: &[u8]) -> IResult<&[u8], Result<u16, Error>> {
|
||||||
})
|
})
|
||||||
}
|
}
|
||||||
|
|
||||||
fn float(input: &[u8]) -> IResult<&[u8], Result<f32, Error>> {
|
fn float(input: &[u8]) -> IResult<&[u8], Result<f64, Error>> {
|
||||||
let (input, sign) = opt(is_a("-"))(input)?;
|
let (input, sign) = opt(is_a("-"))(input)?;
|
||||||
let negative = sign.is_some();
|
let negative = sign.is_some();
|
||||||
let (input, digits) = take_while1(|c| is_digit(c) || c == '.' as u8)(input)?;
|
let (input, digits) = take_while1(|c| is_digit(c) || c == '.' as u8)(input)?;
|
||||||
let result = lexical::parse(digits)
|
let result = lexical::parse(digits)
|
||||||
.map(|result: f32| if negative { -result } else { result })
|
.map(|result: f64| if negative { -result } else { result })
|
||||||
.map_err(|e| e.into());
|
.map_err(|e| e.into());
|
||||||
Ok((input, result))
|
Ok((input, result))
|
||||||
}
|
}
|
||||||
|
@ -357,7 +357,8 @@ fn postfilter(input: &[u8]) -> IResult<&[u8], Result<Command, Error>> {
|
||||||
let (input, rate) = float(input)?;
|
let (input, rate) = float(input)?;
|
||||||
let result = rate
|
let result = rate
|
||||||
.map(|rate| Command::PostFilter {
|
.map(|rate| Command::PostFilter {
|
||||||
channel, rate,
|
channel,
|
||||||
|
rate: rate as f32,
|
||||||
});
|
});
|
||||||
Ok((input, result))
|
Ok((input, result))
|
||||||
}
|
}
|
||||||
|
|
|
@ -38,6 +38,8 @@ use session::{CHANNELS, Session, SessionOutput};
|
||||||
mod command_parser;
|
mod command_parser;
|
||||||
use command_parser::{Command, ShowCommand, PwmSetup, PwmMode};
|
use command_parser::{Command, ShowCommand, PwmSetup, PwmMode};
|
||||||
mod timer;
|
mod timer;
|
||||||
|
mod pid;
|
||||||
|
mod steinhart_hart;
|
||||||
|
|
||||||
|
|
||||||
#[derive(Clone, Copy, Debug)]
|
#[derive(Clone, Copy, Debug)]
|
||||||
|
|
|
@ -0,0 +1,122 @@
|
||||||
|
#[derive(Clone, Copy)]
|
||||||
|
pub struct Parameters {
|
||||||
|
pub kp: f64,
|
||||||
|
pub ki: f64,
|
||||||
|
pub kd: f64,
|
||||||
|
pub output_min: f64,
|
||||||
|
pub output_max: f64,
|
||||||
|
pub integral_min: f64,
|
||||||
|
pub integral_max: f64
|
||||||
|
}
|
||||||
|
|
||||||
|
#[derive(Clone)]
|
||||||
|
pub struct Controller {
|
||||||
|
parameters: Parameters,
|
||||||
|
target: f64,
|
||||||
|
integral: f64,
|
||||||
|
last_input: Option<f64>
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Controller {
|
||||||
|
pub const fn new(parameters: Parameters) -> Controller {
|
||||||
|
Controller {
|
||||||
|
parameters: parameters,
|
||||||
|
target: 0.0,
|
||||||
|
last_input: None,
|
||||||
|
integral: 0.0
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn update(&mut self, input: f64) -> f64 {
|
||||||
|
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 = match self.last_input {
|
||||||
|
None => 0.0,
|
||||||
|
Some(last_input) => self.parameters.kd * (last_input - input)
|
||||||
|
};
|
||||||
|
self.last_input = Some(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 get_target(&self) -> f64 {
|
||||||
|
self.target
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn set_target(&mut self, target: f64) {
|
||||||
|
self.target = target;
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn get_parameters(&self) -> &Parameters {
|
||||||
|
&self.parameters
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn update_parameters<F: FnOnce(&mut Parameters)>(&mut self, f: F) {
|
||||||
|
f(&mut self.parameters);
|
||||||
|
}
|
||||||
|
|
||||||
|
#[allow(dead_code)]
|
||||||
|
pub fn reset(&mut self) {
|
||||||
|
self.integral = 0.0;
|
||||||
|
self.last_input = None;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(test)]
|
||||||
|
mod test {
|
||||||
|
use super::*;
|
||||||
|
|
||||||
|
const PARAMETERS: Parameters = Parameters {
|
||||||
|
kp: 0.055,
|
||||||
|
ki: 0.005,
|
||||||
|
kd: 0.04,
|
||||||
|
output_min: -10.0,
|
||||||
|
output_max: 10.0,
|
||||||
|
integral_min: -100.0,
|
||||||
|
integral_max: 100.0,
|
||||||
|
};
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn test_controller() {
|
||||||
|
const DEFAULT: f64 = 0.0;
|
||||||
|
const TARGET: f64 = 1234.56;
|
||||||
|
const ERROR: f64 = 0.01;
|
||||||
|
const DELAY: usize = 10;
|
||||||
|
|
||||||
|
let mut pid = Controller::new(PARAMETERS.clone());
|
||||||
|
pid.set_target(TARGET);
|
||||||
|
|
||||||
|
let mut values = [DEFAULT; DELAY];
|
||||||
|
let mut t = 0;
|
||||||
|
let mut total_t = 0;
|
||||||
|
let target = (TARGET - ERROR)..=(TARGET + ERROR);
|
||||||
|
while !values.iter().all(|value| target.contains(value)) {
|
||||||
|
let next_t = (t + 1) % DELAY;
|
||||||
|
// Feed the oldest temperature
|
||||||
|
let output = pid.update(values[next_t]);
|
||||||
|
// Overwrite oldest with previous temperature + output
|
||||||
|
values[next_t] = values[t] + output;
|
||||||
|
t = next_t;
|
||||||
|
total_t += 1;
|
||||||
|
}
|
||||||
|
dbg!(values[t], total_t);
|
||||||
|
}
|
||||||
|
}
|
|
@ -15,7 +15,7 @@ pub struct SocketState<S> {
|
||||||
/// Number of server sockets and therefore concurrent client
|
/// Number of server sockets and therefore concurrent client
|
||||||
/// sessions. Many data structures in `Server::run()` correspond to
|
/// sessions. Many data structures in `Server::run()` correspond to
|
||||||
/// this const.
|
/// this const.
|
||||||
const SOCKET_COUNT: usize = 8;
|
const SOCKET_COUNT: usize = 4;
|
||||||
|
|
||||||
const TCP_RX_BUFFER_SIZE: usize = 2048;
|
const TCP_RX_BUFFER_SIZE: usize = 2048;
|
||||||
const TCP_TX_BUFFER_SIZE: usize = 2048;
|
const TCP_TX_BUFFER_SIZE: usize = 2048;
|
||||||
|
@ -52,10 +52,6 @@ impl<'a, 'b, S: Default> Server<'a, 'b, S> {
|
||||||
create_socket!(sockets, tcp_rx_storage1, tcp_tx_storage1, states[1].handle);
|
create_socket!(sockets, tcp_rx_storage1, tcp_tx_storage1, states[1].handle);
|
||||||
create_socket!(sockets, tcp_rx_storage2, tcp_tx_storage2, states[2].handle);
|
create_socket!(sockets, tcp_rx_storage2, tcp_tx_storage2, states[2].handle);
|
||||||
create_socket!(sockets, tcp_rx_storage3, tcp_tx_storage3, states[3].handle);
|
create_socket!(sockets, tcp_rx_storage3, tcp_tx_storage3, states[3].handle);
|
||||||
create_socket!(sockets, tcp_rx_storage4, tcp_tx_storage4, states[4].handle);
|
|
||||||
create_socket!(sockets, tcp_rx_storage5, tcp_tx_storage5, states[5].handle);
|
|
||||||
create_socket!(sockets, tcp_rx_storage6, tcp_tx_storage6, states[6].handle);
|
|
||||||
create_socket!(sockets, tcp_rx_storage7, tcp_tx_storage7, states[7].handle);
|
|
||||||
|
|
||||||
for state in &mut states {
|
for state in &mut states {
|
||||||
state.state = S::default();
|
state.state = S::default();
|
||||||
|
|
|
@ -16,16 +16,13 @@ impl LineReader {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn feed(&mut self, c: u8) -> Option<LineResult> {
|
pub fn feed(&mut self, c: u8) -> Option<&[u8]> {
|
||||||
if c == 13 || c == 10 {
|
if c == 13 || c == 10 {
|
||||||
// Enter
|
// Enter
|
||||||
if self.pos > 0 {
|
if self.pos > 0 {
|
||||||
let len = self.pos;
|
let len = self.pos;
|
||||||
self.pos = 0;
|
self.pos = 0;
|
||||||
Some(LineResult {
|
Some(&self.buf[..len])
|
||||||
buf: self.buf.clone(),
|
|
||||||
len,
|
|
||||||
})
|
|
||||||
} else {
|
} else {
|
||||||
None
|
None
|
||||||
}
|
}
|
||||||
|
@ -41,18 +38,6 @@ impl LineReader {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
pub struct LineResult {
|
|
||||||
buf: [u8; MAX_LINE_LEN],
|
|
||||||
len: usize,
|
|
||||||
}
|
|
||||||
|
|
||||||
impl Deref for LineResult {
|
|
||||||
type Target = [u8];
|
|
||||||
fn deref(&self) -> &Self::Target {
|
|
||||||
&self.buf[..self.len]
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
pub enum SessionOutput {
|
pub enum SessionOutput {
|
||||||
Nothing,
|
Nothing,
|
||||||
Command(Command),
|
Command(Command),
|
||||||
|
|
|
@ -0,0 +1,41 @@
|
||||||
|
use lexical_core::Float;
|
||||||
|
|
||||||
|
/// Steinhart-Hart equation parameters
|
||||||
|
#[derive(Clone, Debug)]
|
||||||
|
pub struct Parameters {
|
||||||
|
pub t0: f64,
|
||||||
|
pub r: f64,
|
||||||
|
pub r0: f64,
|
||||||
|
|
||||||
|
r_fact: f64,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Parameters {
|
||||||
|
/// Update the cached r_fact
|
||||||
|
pub fn update(&mut self) {
|
||||||
|
self.r_fact = (self.r / self.r0).ln();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Perform the voltage to temperature conversion.
|
||||||
|
///
|
||||||
|
/// Result unit: Kelvin
|
||||||
|
///
|
||||||
|
/// TODO: verify
|
||||||
|
pub fn get_temperature(&self, b: f64) -> f64 {
|
||||||
|
let inv_temp = 1.0 / self.t0 + self.r_fact / b;
|
||||||
|
1.0 / inv_temp
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Default for Parameters {
|
||||||
|
fn default() -> Self {
|
||||||
|
let mut p = Parameters {
|
||||||
|
t0: 0.001_4,
|
||||||
|
r: 0.000_000_099,
|
||||||
|
r0: 5_110.0,
|
||||||
|
r_fact: 0.0,
|
||||||
|
};
|
||||||
|
p.update();
|
||||||
|
p
|
||||||
|
}
|
||||||
|
}
|
Loading…
Reference in New Issue