command_parser: parameterize `pid`, `pwm` with channel `<0/1>`

This commit is contained in:
Astro 2019-09-19 02:17:54 +02:00
parent 1514131fa3
commit e126bc0fe1
4 changed files with 104 additions and 58 deletions

View File

@ -54,14 +54,15 @@ The scope of this setting is per TCP session.
| --- | --- | | --- | --- |
| `report` | Show current input | | `report` | Show current input |
| `report mode` | Show current report mode | | `report mode` | Show current report mode |
| `report mode <off or on>` | Set report mode | | `report mode <off/on>` | Set report mode |
| `pwm <width> <total>` | Set PWM duty cycle to manual *width / total* | | `pwm <0/1> <width> <total>` | Set PWM duty cycle to manual *width / total* |
| `pwm pid` | Set PWM to be controlled by PID | | `pwm <0/1> pid` | Set PWM to be controlled by PID |
| `pid target <value>` | | | `pid` | Show PID configuration |
| `pid kp <value>` | | | `pid <0/1> target <value>` | |
| `pid ki <value>` | | | `pid <0/1> kp <value>` | |
| `pid kd <value>` | | | `pid <0/1> ki <value>` | |
| `pid output_min <value>` | | | `pid <0/1> kd <value>` | |
| `pid output_max <value>` | | | `pid <0/1> output_min <value>` | |
| `pid integral_min <value>` | | | `pid <0/1> output_max <value>` | |
| `pid integral_max <value>` | | | `pid <0/1> integral_min <value>` | |
| `pid <0/1> integral_max <value>` | |

View File

@ -94,8 +94,12 @@ pub enum Command {
Quit, Quit,
Show(ShowCommand), Show(ShowCommand),
Reporting(bool), Reporting(bool),
Pwm(PwmMode), Pwm {
channel: usize,
mode: PwmMode,
},
Pid { Pid {
channel: usize,
parameter: PidParameter, parameter: PidParameter,
value: f32, value: f32,
}, },
@ -134,6 +138,10 @@ fn off_on(input: &[u8]) -> IResult<&[u8], bool> {
))(input) ))(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> { fn report(input: &[u8]) -> IResult<&[u8], Command> {
preceded( preceded(
tag("report"), tag("report"),
@ -159,8 +167,10 @@ fn report(input: &[u8]) -> IResult<&[u8], Command> {
)(input) )(input)
} }
/// `pwm <width> <total>` - Set pwm duty cycle /// `pwm <0-1> <width> <total>` - Set pwm duty cycle
fn pwm_manual(input: &[u8]) -> IResult<&[u8], Result<Command, Error>> { fn pwm_manual(input: &[u8]) -> IResult<&[u8], Result<Command, Error>> {
let (input, channel) = channel(input)?;
let (input, _) = whitespace(input)?;
let (input, width) = unsigned(input)?; let (input, width) = unsigned(input)?;
let width = match width { let width = match width {
Ok(width) => width, Ok(width) => width,
@ -172,12 +182,20 @@ fn pwm_manual(input: &[u8]) -> IResult<&[u8], Result<Command, Error>> {
Ok(total) => total, Ok(total) => total,
Err(e) => return Ok((input, Err(e.into()))), 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<Command, Error>> { fn pwm_pid(input: &[u8]) -> IResult<&[u8], Result<Command, Error>> {
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<Command, Error>> { fn pwm(input: &[u8]) -> IResult<&[u8], Result<Command, Error>> {
@ -193,8 +211,10 @@ fn pwm(input: &[u8]) -> IResult<&[u8], Result<Command, Error>> {
))(input) ))(input)
} }
/// `pid <parameter> <value>` /// `pid <0-1> <parameter> <value>`
fn pid_parameter(input: &[u8]) -> IResult<&[u8], Result<Command, Error>> { fn pid_parameter(input: &[u8]) -> IResult<&[u8], Result<Command, Error>> {
let (input, channel) = channel(input)?;
let (input, _) = whitespace(input)?;
let (input, parameter) = let (input, parameter) =
alt((value(PidParameter::Target, tag("target")), alt((value(PidParameter::Target, tag("target")),
value(PidParameter::KP, tag("kp")), value(PidParameter::KP, tag("kp")),
@ -208,7 +228,7 @@ fn pid_parameter(input: &[u8]) -> IResult<&[u8], Result<Command, Error>> {
let (input, _) = whitespace(input)?; let (input, _) = whitespace(input)?;
let (input, value) = float(input)?; let (input, value) = float(input)?;
let result = value let result = value
.map(|value| Command::Pid { parameter, value }) .map(|value| Command::Pid { channel, parameter, value })
.map_err(|e| e.into()); .map_err(|e| e.into());
Ok((input, result)) Ok((input, result))
} }

View File

@ -88,6 +88,15 @@ const DEFAULT_PID_PARAMETERS: pid::Parameters = pid::Parameters {
pub const CHANNELS: usize = 2; 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] #[entry]
fn main() -> ! { fn main() -> ! {
let mut stdout = hio::hstdout().unwrap(); let mut stdout = hio::hstdout().unwrap();
@ -155,9 +164,13 @@ fn main() -> ! {
// SENS1_{P,N} // SENS1_{P,N}
adc.setup_channel(1, ad7172::Input::Ain2, ad7172::Input::Ain3).unwrap(); adc.setup_channel(1, ad7172::Input::Ain2, ad7172::Input::Ain3).unwrap();
let mut pid = pid::Controller::new(DEFAULT_PID_PARAMETERS.clone()); let init_state = ControlState {
report: None,
// Start with disengaged PID to let user setup parameters first // Start with disengaged PID to let user setup parameters first
let mut pid_enabled = false; 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()); let mut hardware_addr = EthernetAddress(board::get_mac_address());
writeln!(stdout, "MAC address: {}", hardware_addr).unwrap(); writeln!(stdout, "MAC address: {}", hardware_addr).unwrap();
@ -208,7 +221,6 @@ fn main() -> ! {
(Session::new(), tcp_handle7), (Session::new(), tcp_handle7),
]; ];
let mut report = [None; CHANNELS];
loop { loop {
// ADC input // ADC input
adc.data_ready() adc.data_ready()
@ -218,13 +230,16 @@ fn main() -> ! {
}).map(|channel| { }).map(|channel| {
let now = get_time(); let now = get_time();
let data = adc.read_data().unwrap(); let data = adc.read_data().unwrap();
let state = &mut states[usize::from(channel)];
if channel == 0 && pid_enabled { if state.pid_enabled {
let width = pid.update(data as f32) as u32; let width = state.pid.update(data as f32) as u32;
if channel == 0 { // TODO
board::set_timer_pwm(width as u32, 0xffff); 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() { for (session, _) in sessions_handles.iter_mut() {
session.set_report_pending(channel.into()); session.set_report_pending(channel.into());
} }
@ -256,19 +271,21 @@ fn main() -> ! {
let _ = writeln!(socket, "Report mode: {}", if session.reporting() { "on" } else { "off" }); let _ = writeln!(socket, "Report mode: {}", if session.reporting() { "on" } else { "off" });
} }
Command::Show(ShowCommand::Input) => { Command::Show(ShowCommand::Input) => {
for (channel, report) in report.iter().enumerate() { for (channel, state) in states.iter().enumerate() {
report.map(|(time, data)| { state.report.map(|(time, data)| {
let _ = writeln!(socket, "t={}, sens{}={}", time, channel, data); let _ = writeln!(socket, "t={}, sens{}={}", time, channel, data);
}); });
} }
} }
Command::Show(ShowCommand::Pid) => { Command::Show(ShowCommand::Pid) => {
let _ = writeln!(socket, "PID settings"); for (channel, state) in states.iter().enumerate() {
let _ = writeln!(socket, "target: {:.4}", pid.get_target()); 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(); let p = pid.get_parameters();
macro_rules! out { macro_rules! out {
($p: tt) => { ($p: tt) => {
let _ = writeln!(socket, "{}: {:.4}", stringify!($p), p.$p); let _ = writeln!(socket, "* {}={:.4}", stringify!($p), p.$p);
}; };
} }
out!(kp); out!(kp);
@ -279,21 +296,28 @@ fn main() -> ! {
out!(integral_min); out!(integral_min);
out!(integral_max); out!(integral_max);
} }
}
Command::Show(ShowCommand::Pwm) => { Command::Show(ShowCommand::Pwm) => {
let _ = writeln!(socket, "PWM: PID {}", for (channel, state) in states.iter().enumerate() {
if pid_enabled { "engaged" } else { "disengaged" } 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); 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::Pid { parameter, value } => { Command::Pwm { channel, mode: PwmMode::Pid } => {
states[channel].pid_enabled = true;
let _ = writeln!(socket, "channel {}: PID enabled to control PWM", channel);
}
Command::Pid { channel, parameter, value } => {
let pid = &mut states[channel].pid;
use command_parser::PidParameter::*; use command_parser::PidParameter::*;
match parameter { match parameter {
Target => Target =>
@ -324,7 +348,7 @@ fn main() -> ! {
} }
if socket.may_send() { if socket.may_send() {
if let Some(channel) = session.is_report_pending() { 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); let _ = writeln!(socket, "t={} sens{}={:06X}", time, channel, data);
}); });
session.mark_report_sent(channel); session.mark_report_sent(channel);

View File

@ -9,6 +9,7 @@ pub struct Parameters {
pub integral_max: f32 pub integral_max: f32
} }
#[derive(Clone)]
pub struct Controller { pub struct Controller {
parameters: Parameters, parameters: Parameters,
target: f32, target: f32,
@ -56,7 +57,7 @@ impl Controller {
output output
} }
pub fn get_target(&mut self) -> f32 { pub fn get_target(&self) -> f32 {
self.target self.target
} }