diff --git a/firmware/src/command_parser.rs b/firmware/src/command_parser.rs index 7dfa91c..728bfdb 100644 --- a/firmware/src/command_parser.rs +++ b/firmware/src/command_parser.rs @@ -12,7 +12,7 @@ use btoi::{btoi, ParseIntegerError}; use super::session::ReportMode; -#[derive(Debug)] +#[derive(Clone, Debug)] pub enum Error { Parser(ErrorKind), Incomplete, @@ -42,6 +42,19 @@ impl From for Error { #[derive(Debug, Clone)] pub enum ShowCommand { ReportMode, + Pid, +} + +#[derive(Debug, Clone)] +pub enum PidParameter { + Target(f32), + KP(f32), + KI(f32), + KD(f32), + OutputMin(f32), + OutputMax(f32), + IntegralMin(f32), + IntegralMax(f32), } #[derive(Debug, Clone)] @@ -53,6 +66,7 @@ pub enum Command { width: u32, total: u32, }, + Pid(PidParameter), } fn whitespace(input: &[u8]) -> IResult<&[u8], ()> { @@ -113,17 +127,50 @@ fn pwm(input: &[u8]) -> IResult<&[u8], Result> { Ok((input, Ok(Command::Pwm { width, total }))) } -fn command(input: &[u8]) -> IResult<&[u8], Command> { - alt((value(Command::Quit, tag("quit")), - report +fn pid_parameter(input: &[u8]) -> IResult<&[u8], Result> { + let (input, parameter_f) = + alt((value(PidParameter::KP, tag("kp")), + value(PidParameter::KP, tag("ki")) + ))(input)?; + let (input, _) = whitespace(input)?; + // TODO: parse float + let (input, value) = unsigned(input)?; + let value = value.map(|value| parameter_f(value as f32)) + .map_err(|e| e.into()); + Ok((input, value)) +} + +fn pid(input: &[u8]) -> IResult<&[u8], Result> { + let (input, _) = tag("pid")(input)?; + let (input, _) = whitespace(input)?; + + alt(( + preceded( + whitespace, + |input| pid_parameter(input) + .map(|(input, parameter)| { + (input, parameter.map(Command::Pid)) + }) + ), + |input| Ok((input, Ok(Command::Show(ShowCommand::Pid)))) + ))(input) +} + +fn command(input: &[u8]) -> IResult<&[u8], Result> { + alt((value(Ok(Command::Quit), tag("quit")), + |input| report(input).map(|(input, command)| { + (input, Ok(command)) + }), + pwm, + pid, ))(input) } impl Command { pub fn parse(input: &[u8]) -> Result { match command(input) { - Ok((b"", command)) => - Ok(command), + Ok((b"", result)) => + result, Ok((input_remain, _)) => Err(Error::UnexpectedInput(input_remain[0])), Err(e) => diff --git a/firmware/src/main.rs b/firmware/src/main.rs index 827c3b0..42f49f9 100644 --- a/firmware/src/main.rs +++ b/firmware/src/main.rs @@ -1,4 +1,4 @@ -#![feature(const_fn)] +#![feature(const_fn, proc_macro_hygiene)] #![no_std] #![no_main] @@ -41,6 +41,7 @@ use command_parser::{Command, ShowCommand}; mod session; use self::session::{Session, SessionOutput}; mod ad7172; +mod pid; pub struct UART0; @@ -77,6 +78,15 @@ macro_rules! create_socket { /// In nanoseconds const REPORT_INTERVAL: u64 = 100_000; +const DEFAULT_PID_PARAMETERS: pid::Parameters = pid::Parameters { + kp: 1.0, + ki: 1.0, + kd: 1.0, + output_min: 0.0, + output_max: 0xffff as f32, + integral_min: 0.0, + integral_max: 0xffff as f32, +}; #[entry] fn main() -> ! { @@ -143,6 +153,8 @@ fn main() -> ! { // SENS1_{P,N} adc.setup_channel(1, ad7172::Input::Ain2, ad7172::Input::Ain3).unwrap(); + let mut pid = pid::Controller::new(DEFAULT_PID_PARAMETERS.clone()); + let mut hardware_addr = EthernetAddress(board::get_mac_address()); if hardware_addr.is_multicast() { println!("programmed MAC address is invalid, using default"); @@ -241,6 +253,7 @@ fn main() -> ! { if socket.may_recv() && socket.may_send() { let output = socket.recv(|buf| session.feed(buf)); + // TODO: use "{}" to display pretty errors match output { Ok(SessionOutput::Nothing) => {} Ok(SessionOutput::Command(command)) => match command { @@ -252,10 +265,49 @@ fn main() -> ! { Command::Show(ShowCommand::ReportMode) => { let _ = writeln!(socket, "Report mode: {:?}", session.report_mode()); } + Command::Show(ShowCommand::Pid) => { + let _ = writeln!(socket, "PID settings"); + let _ = writeln!(socket, "target: {:.4}", pid.get_target()); + let p = pid.get_parameters(); + macro_rules! out { + ($p: tt) => { + let _ = writeln!(socket, "{}: {:.4}", stringify!($p), p.$p); + }; + } + out!(kp); + out!(ki); + out!(kd); + out!(output_min); + out!(output_max); + out!(integral_min); + out!(integral_max); + } Command::Pwm { width, total } => { board::set_timer_pwm(width, total); let _ = writeln!(socket, "PWM duty cycle: {}/{}", width, total); } + Command::Pid(p) => { + use command_parser::PidParameter::*; + match p { + Target(value) => + pid.set_target(value), + KP(value) => + pid.update_parameters(|parameters| parameters.kp = value), + KI(value) => + pid.update_parameters(|parameters| parameters.ki = value), + KD(value) => + pid.update_parameters(|parameters| parameters.kd = value), + OutputMin(value) => + pid.update_parameters(|parameters| parameters.output_min = value), + OutputMax(value) => + pid.update_parameters(|parameters| parameters.output_max = value), + IntegralMin(value) => + pid.update_parameters(|parameters| parameters.integral_min = value), + IntegralMax(value) => + pid.update_parameters(|parameters| parameters.integral_max = value), + } + let _ = writeln!(socket, "PID parameter updated"); + } } Ok(SessionOutput::Error(e)) => { let _ = writeln!(socket, "Command error: {:?}", e); diff --git a/firmware/src/pid.rs b/firmware/src/pid.rs index b5309a5..5ea28d6 100644 --- a/firmware/src/pid.rs +++ b/firmware/src/pid.rs @@ -56,10 +56,22 @@ impl Controller { output } + pub fn get_target(&mut self) -> f32 { + self.target + } + pub fn set_target(&mut self, target: f32) { self.target = target; } + pub fn get_parameters(&self) -> &Parameters { + &self.parameters + } + + pub fn update_parameters(&mut self, f: F) { + f(&mut self.parameters); + } + #[allow(dead_code)] pub fn reset(&mut self) { self.integral = 0.0;