control the pid

This commit is contained in:
Astro 2019-09-14 21:11:26 +02:00
parent 426be0d5f1
commit b6af43feda
3 changed files with 118 additions and 7 deletions

View File

@ -12,7 +12,7 @@ use btoi::{btoi, ParseIntegerError};
use super::session::ReportMode; use super::session::ReportMode;
#[derive(Debug)] #[derive(Clone, Debug)]
pub enum Error { pub enum Error {
Parser(ErrorKind), Parser(ErrorKind),
Incomplete, Incomplete,
@ -42,6 +42,19 @@ impl From<ParseIntegerError> for Error {
#[derive(Debug, Clone)] #[derive(Debug, Clone)]
pub enum ShowCommand { pub enum ShowCommand {
ReportMode, 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)] #[derive(Debug, Clone)]
@ -53,6 +66,7 @@ pub enum Command {
width: u32, width: u32,
total: u32, total: u32,
}, },
Pid(PidParameter),
} }
fn whitespace(input: &[u8]) -> IResult<&[u8], ()> { fn whitespace(input: &[u8]) -> IResult<&[u8], ()> {
@ -113,17 +127,50 @@ fn pwm(input: &[u8]) -> IResult<&[u8], Result<Command, Error>> {
Ok((input, Ok(Command::Pwm { width, total }))) Ok((input, Ok(Command::Pwm { width, total })))
} }
fn command(input: &[u8]) -> IResult<&[u8], Command> { fn pid_parameter(input: &[u8]) -> IResult<&[u8], Result<PidParameter, Error>> {
alt((value(Command::Quit, tag("quit")), let (input, parameter_f) =
report 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<Command, Error>> {
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<Command, Error>> {
alt((value(Ok(Command::Quit), tag("quit")),
|input| report(input).map(|(input, command)| {
(input, Ok(command))
}),
pwm,
pid,
))(input) ))(input)
} }
impl Command { impl Command {
pub fn parse(input: &[u8]) -> Result<Self, Error> { pub fn parse(input: &[u8]) -> Result<Self, Error> {
match command(input) { match command(input) {
Ok((b"", command)) => Ok((b"", result)) =>
Ok(command), result,
Ok((input_remain, _)) => Ok((input_remain, _)) =>
Err(Error::UnexpectedInput(input_remain[0])), Err(Error::UnexpectedInput(input_remain[0])),
Err(e) => Err(e) =>

View File

@ -1,4 +1,4 @@
#![feature(const_fn)] #![feature(const_fn, proc_macro_hygiene)]
#![no_std] #![no_std]
#![no_main] #![no_main]
@ -41,6 +41,7 @@ use command_parser::{Command, ShowCommand};
mod session; mod session;
use self::session::{Session, SessionOutput}; use self::session::{Session, SessionOutput};
mod ad7172; mod ad7172;
mod pid;
pub struct UART0; pub struct UART0;
@ -77,6 +78,15 @@ macro_rules! create_socket {
/// In nanoseconds /// In nanoseconds
const REPORT_INTERVAL: u64 = 100_000; 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] #[entry]
fn main() -> ! { fn main() -> ! {
@ -143,6 +153,8 @@ 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 mut hardware_addr = EthernetAddress(board::get_mac_address()); let mut hardware_addr = EthernetAddress(board::get_mac_address());
if hardware_addr.is_multicast() { if hardware_addr.is_multicast() {
println!("programmed MAC address is invalid, using default"); println!("programmed MAC address is invalid, using default");
@ -241,6 +253,7 @@ fn main() -> ! {
if socket.may_recv() && socket.may_send() { if socket.may_recv() && socket.may_send() {
let output = socket.recv(|buf| session.feed(buf)); let output = socket.recv(|buf| session.feed(buf));
// TODO: use "{}" to display pretty errors
match output { match output {
Ok(SessionOutput::Nothing) => {} Ok(SessionOutput::Nothing) => {}
Ok(SessionOutput::Command(command)) => match command { Ok(SessionOutput::Command(command)) => match command {
@ -252,10 +265,49 @@ fn main() -> ! {
Command::Show(ShowCommand::ReportMode) => { Command::Show(ShowCommand::ReportMode) => {
let _ = writeln!(socket, "Report mode: {:?}", session.report_mode()); 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 } => { Command::Pwm { width, total } => {
board::set_timer_pwm(width, total); board::set_timer_pwm(width, total);
let _ = writeln!(socket, "PWM duty cycle: {}/{}", 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)) => { Ok(SessionOutput::Error(e)) => {
let _ = writeln!(socket, "Command error: {:?}", e); let _ = writeln!(socket, "Command error: {:?}", e);

View File

@ -56,10 +56,22 @@ impl Controller {
output output
} }
pub fn get_target(&mut self) -> f32 {
self.target
}
pub fn set_target(&mut self, target: f32) { pub fn set_target(&mut self, target: f32) {
self.target = target; 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)] #[allow(dead_code)]
pub fn reset(&mut self) { pub fn reset(&mut self) {
self.integral = 0.0; self.integral = 0.0;