From 42a9b89db1b4831c1dd358f0db14f9b4dd51123e Mon Sep 17 00:00:00 2001 From: Astro Date: Tue, 17 Sep 2019 19:09:47 +0200 Subject: [PATCH] let pid control pwm --- firmware/README.md | 29 +++++++++++++------------ firmware/src/command_parser.rs | 39 +++++++++++++++++++++++++++------- firmware/src/main.rs | 25 ++++++++++++++++++---- 3 files changed, 67 insertions(+), 26 deletions(-) diff --git a/firmware/README.md b/firmware/README.md index fc060d2..290ab5e 100644 --- a/firmware/README.md +++ b/firmware/README.md @@ -18,17 +18,18 @@ 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 width / total | -| `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 ` | 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 ` | | diff --git a/firmware/src/command_parser.rs b/firmware/src/command_parser.rs index 26d43b7..15e6166 100644 --- a/firmware/src/command_parser.rs +++ b/firmware/src/command_parser.rs @@ -64,6 +64,7 @@ impl fmt::Display for Error { pub enum ShowCommand { Input, Reporting, + Pwm, Pid, } @@ -79,15 +80,21 @@ pub enum PidParameter { IntegralMax, } +#[derive(Debug, Clone)] +pub enum PwmMode { + Manual { + width: u32, + total: u32, + }, + Pid, +} + #[derive(Debug, Clone)] pub enum Command { Quit, Show(ShowCommand), Reporting(bool), - Pwm { - width: u32, - total: u32, - }, + Pwm(PwmMode), Pid { parameter: PidParameter, value: f32, @@ -147,9 +154,7 @@ fn report(input: &[u8]) -> IResult<&[u8], Command> { } /// `pwm ` - Set pwm duty cycle -fn pwm(input: &[u8]) -> IResult<&[u8], Result> { - let (input, _) = tag("pwm")(input)?; - let (input, _) = whitespace(input)?; +fn pwm_manual(input: &[u8]) -> IResult<&[u8], Result> { let (input, width) = unsigned(input)?; let width = match width { Ok(width) => width, @@ -161,7 +166,25 @@ fn pwm(input: &[u8]) -> IResult<&[u8], Result> { Ok(total) => total, Err(e) => return Ok((input, Err(e.into()))), }; - Ok((input, Ok(Command::Pwm { width, total }))) + Ok((input, Ok(Command::Pwm(PwmMode::Manual { width, total })))) +} + +/// `pwm 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) +} + +fn pwm(input: &[u8]) -> IResult<&[u8], Result> { + let (input, _) = tag("pwm")(input)?; + alt(( + preceded( + whitespace, + alt(( + pwm_pid, + pwm_manual, + ))), + |input| Ok((input, Ok(Command::Show(ShowCommand::Pwm)))) + ))(input) } fn pid_parameter(input: &[u8]) -> IResult<&[u8], Result> { diff --git a/firmware/src/main.rs b/firmware/src/main.rs index 7fa4fa0..0e5d8d5 100644 --- a/firmware/src/main.rs +++ b/firmware/src/main.rs @@ -37,7 +37,7 @@ mod board; use self::board::{gpio::Gpio, systick::get_time}; mod ethmac; mod command_parser; -use command_parser::{Command, ShowCommand}; +use command_parser::{Command, ShowCommand, PwmMode}; mod session; use self::session::{Session, SessionOutput}; mod ad7172; @@ -155,6 +155,8 @@ fn main() -> ! { 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 mut hardware_addr = EthernetAddress(board::get_mac_address()); if hardware_addr.is_multicast() { @@ -214,8 +216,13 @@ fn main() -> ! { }).map(|channel| { let now = get_time(); let data = adc.read_data().unwrap(); - report[usize::from(channel)] = Some((now, data)); + if channel == 0 && pid_enabled { + let width = pid.update(data as f32) as u32; + board::set_timer_pwm(width as u32, 0xffff); + } + + report[usize::from(channel)] = Some((now, data)); for (session, _) in sessions_handles.iter_mut() { session.set_report_pending(channel.into()); } @@ -270,9 +277,19 @@ fn main() -> ! { out!(integral_min); out!(integral_max); } - Command::Pwm { width, total } => { + Command::Show(ShowCommand::Pwm) => { + let _ = writeln!(socket, "PWM: PID {}", + if pid_enabled { "engaged" } else { "disengaged" } + ); + } + Command::Pwm(PwmMode::Manual { width, total }) => { + pid_enabled = false; board::set_timer_pwm(width, total); - let _ = writeln!(socket, "PWM duty cycle: {}/{}", width, total); + let _ = writeln!(socket, "PWM duty cycle manually set to {}/{}", width, total); + } + Command::Pwm(PwmMode::Pid) => { + pid_enabled = true; + let _ = writeln!(socket, "PID enabled to control PWM"); } Command::Pid { parameter, value } => { use command_parser::PidParameter::*;