From e126bc0fe157ee50d6ecaac66339682e18576511 Mon Sep 17 00:00:00 2001 From: Astro Date: Thu, 19 Sep 2019 02:17:54 +0200 Subject: [PATCH] command_parser: parameterize `pid`, `pwm` with channel `<0/1>` --- firmware/README.md | 31 +++++------ firmware/src/command_parser.rs | 34 +++++++++--- firmware/src/main.rs | 94 +++++++++++++++++++++------------- firmware/src/pid.rs | 3 +- 4 files changed, 104 insertions(+), 58 deletions(-) diff --git a/firmware/README.md b/firmware/README.md index b8312b7..c204219 100644 --- a/firmware/README.md +++ b/firmware/README.md @@ -50,18 +50,19 @@ The scope of this setting is per TCP session. ### Commands -| Syntax | Function | -| --- | --- | -| `report` | Show current input | -| `report mode` | Show current report mode | -| `report mode ` | Set report mode | -| `pwm ` | Set PWM duty cycle to manual *width / total* | -| `pwm pid` | Set PWM to be controlled by PID | -| `pid target ` | | -| `pid kp ` | | -| `pid ki ` | | -| `pid kd ` | | -| `pid output_min ` | | -| `pid output_max ` | | -| `pid integral_min ` | | -| `pid integral_max ` | | +| Syntax | Function | +| --- | --- | +| `report` | Show current input | +| `report mode` | Show current report mode | +| `report mode ` | Set report mode | +| `pwm <0/1> ` | Set PWM duty cycle to manual *width / total* | +| `pwm <0/1> pid` | Set PWM to be controlled by PID | +| `pid` | Show PID configuration | +| `pid <0/1> target ` | | +| `pid <0/1> kp ` | | +| `pid <0/1> ki ` | | +| `pid <0/1> kd ` | | +| `pid <0/1> output_min ` | | +| `pid <0/1> output_max ` | | +| `pid <0/1> integral_min ` | | +| `pid <0/1> integral_max ` | | diff --git a/firmware/src/command_parser.rs b/firmware/src/command_parser.rs index c5e202b..1a2269e 100644 --- a/firmware/src/command_parser.rs +++ b/firmware/src/command_parser.rs @@ -94,8 +94,12 @@ pub enum Command { Quit, Show(ShowCommand), Reporting(bool), - Pwm(PwmMode), + Pwm { + channel: usize, + mode: PwmMode, + }, Pid { + channel: usize, parameter: PidParameter, value: f32, }, @@ -134,6 +138,10 @@ fn off_on(input: &[u8]) -> IResult<&[u8], bool> { ))(input) } +fn channel(input: &[u8]) -> IResult<&[u8], usize> { + map(one_of("01"), |c| (c as usize) - ('0' as usize))(input) +} + fn report(input: &[u8]) -> IResult<&[u8], Command> { preceded( tag("report"), @@ -159,8 +167,10 @@ fn report(input: &[u8]) -> IResult<&[u8], Command> { )(input) } -/// `pwm ` - Set pwm duty cycle +/// `pwm <0-1> ` - Set pwm duty cycle fn pwm_manual(input: &[u8]) -> IResult<&[u8], Result> { + let (input, channel) = channel(input)?; + let (input, _) = whitespace(input)?; let (input, width) = unsigned(input)?; let width = match width { Ok(width) => width, @@ -172,12 +182,20 @@ fn pwm_manual(input: &[u8]) -> IResult<&[u8], Result> { Ok(total) => total, Err(e) => return Ok((input, Err(e.into()))), }; - Ok((input, Ok(Command::Pwm(PwmMode::Manual { width, total })))) + Ok((input, Ok(Command::Pwm { + channel, + mode: PwmMode::Manual { width, total }, + }))) } -/// `pwm pid` - Set PWM to be controlled by PID +/// `pwm <0-1> pid` - Set PWM to be controlled by PID fn pwm_pid(input: &[u8]) -> IResult<&[u8], Result> { - value(Ok(Command::Pwm(PwmMode::Pid)), tag("pid"))(input) + let (input, channel) = channel(input)?; + let (input, _) = whitespace(input)?; + value(Ok(Command::Pwm { + channel, + mode: PwmMode::Pid, + }), tag("pid"))(input) } fn pwm(input: &[u8]) -> IResult<&[u8], Result> { @@ -193,8 +211,10 @@ fn pwm(input: &[u8]) -> IResult<&[u8], Result> { ))(input) } -/// `pid ` +/// `pid <0-1> ` fn pid_parameter(input: &[u8]) -> IResult<&[u8], Result> { + let (input, channel) = channel(input)?; + let (input, _) = whitespace(input)?; let (input, parameter) = alt((value(PidParameter::Target, tag("target")), value(PidParameter::KP, tag("kp")), @@ -208,7 +228,7 @@ fn pid_parameter(input: &[u8]) -> IResult<&[u8], Result> { let (input, _) = whitespace(input)?; let (input, value) = float(input)?; let result = value - .map(|value| Command::Pid { parameter, value }) + .map(|value| Command::Pid { channel, parameter, value }) .map_err(|e| e.into()); Ok((input, result)) } diff --git a/firmware/src/main.rs b/firmware/src/main.rs index efe164e..3d76c18 100644 --- a/firmware/src/main.rs +++ b/firmware/src/main.rs @@ -88,6 +88,15 @@ const DEFAULT_PID_PARAMETERS: pid::Parameters = pid::Parameters { pub const CHANNELS: usize = 2; +/// State per channel +#[derive(Clone)] +struct ControlState { + /// Report data (time, data) + report: Option<(u64, u32)>, + pid_enabled: bool, + pid: pid::Controller, +} + #[entry] fn main() -> ! { let mut stdout = hio::hstdout().unwrap(); @@ -155,9 +164,13 @@ 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()); - // Start with disengaged PID to let user setup parameters first - let mut pid_enabled = false; + let init_state = ControlState { + report: None, + // Start with disengaged PID to let user setup parameters first + pid_enabled: false, + pid: pid::Controller::new(DEFAULT_PID_PARAMETERS.clone()), + }; + let mut states = [init_state.clone(), init_state.clone()]; let mut hardware_addr = EthernetAddress(board::get_mac_address()); writeln!(stdout, "MAC address: {}", hardware_addr).unwrap(); @@ -208,7 +221,6 @@ fn main() -> ! { (Session::new(), tcp_handle7), ]; - let mut report = [None; CHANNELS]; loop { // ADC input adc.data_ready() @@ -218,13 +230,16 @@ fn main() -> ! { }).map(|channel| { let now = get_time(); let data = adc.read_data().unwrap(); + let state = &mut states[usize::from(channel)]; - if channel == 0 && pid_enabled { - let width = pid.update(data as f32) as u32; - board::set_timer_pwm(width as u32, 0xffff); + if state.pid_enabled { + let width = state.pid.update(data as f32) as u32; + if channel == 0 { // TODO + board::set_timer_pwm(width as u32, 0xffff); + } } - report[usize::from(channel)] = Some((now, data)); + state.report = Some((now, data)); for (session, _) in sessions_handles.iter_mut() { session.set_report_pending(channel.into()); } @@ -256,44 +271,53 @@ fn main() -> ! { let _ = writeln!(socket, "Report mode: {}", if session.reporting() { "on" } else { "off" }); } Command::Show(ShowCommand::Input) => { - for (channel, report) in report.iter().enumerate() { - report.map(|(time, data)| { + for (channel, state) in states.iter().enumerate() { + state.report.map(|(time, data)| { let _ = writeln!(socket, "t={}, sens{}={}", time, channel, data); }); } } 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); - }; + for (channel, state) in states.iter().enumerate() { + let _ = writeln!(socket, "PID settings for channel {}", channel); + let pid = &states[channel].pid; + 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); } - out!(kp); - out!(ki); - out!(kd); - out!(output_min); - out!(output_max); - out!(integral_min); - out!(integral_max); } Command::Show(ShowCommand::Pwm) => { - let _ = writeln!(socket, "PWM: PID {}", - if pid_enabled { "engaged" } else { "disengaged" } - ); + for (channel, state) in states.iter().enumerate() { + let _ = writeln!(socket, "PWM {}: PID {}", + channel, + if state.pid_enabled { "engaged" } else { "disengaged" } + ); + } } - Command::Pwm(PwmMode::Manual { width, total }) => { - pid_enabled = false; + Command::Pwm { channel, mode: PwmMode::Manual { width, total }} => { + states[channel].pid_enabled = false; board::set_timer_pwm(width, total); - let _ = writeln!(socket, "PWM duty cycle manually set to {}/{}", width, total); + if channel == 0 { // TODO + let _ = writeln!(socket, "channel {}: PWM duty cycle manually set to {}/{}", channel, width, total); + } } - Command::Pwm(PwmMode::Pid) => { - pid_enabled = true; - let _ = writeln!(socket, "PID enabled to control PWM"); + Command::Pwm { channel, mode: PwmMode::Pid } => { + states[channel].pid_enabled = true; + let _ = writeln!(socket, "channel {}: PID enabled to control PWM", channel); } - Command::Pid { parameter, value } => { + Command::Pid { channel, parameter, value } => { + let pid = &mut states[channel].pid; use command_parser::PidParameter::*; match parameter { Target => @@ -324,7 +348,7 @@ fn main() -> ! { } if socket.may_send() { if let Some(channel) = session.is_report_pending() { - report[channel].map(|(time, data)| { + states[channel].report.map(|(time, data)| { let _ = writeln!(socket, "t={} sens{}={:06X}", time, channel, data); }); session.mark_report_sent(channel); diff --git a/firmware/src/pid.rs b/firmware/src/pid.rs index 5ea28d6..8818945 100644 --- a/firmware/src/pid.rs +++ b/firmware/src/pid.rs @@ -9,6 +9,7 @@ pub struct Parameters { pub integral_max: f32 } +#[derive(Clone)] pub struct Controller { parameters: Parameters, target: f32, @@ -56,7 +57,7 @@ impl Controller { output } - pub fn get_target(&mut self) -> f32 { + pub fn get_target(&self) -> f32 { self.target }