control the pid

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<ParseIntegerError> 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<Command, Error>> {
     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<PidParameter, Error>> {
+    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<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)
 }
 
 impl Command {
     pub fn parse(input: &[u8]) -> Result<Self, Error> {
         match command(input) {
-            Ok((b"", command)) =>
-                Ok(command),
+            Ok((b"", result)) =>
+                result,
             Ok((input_remain, _)) =>
                 Err(Error::UnexpectedInput(input_remain[0])),
             Err(e) =>

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);

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<F: FnOnce(&mut Parameters)>(&mut self, f: F) {
+        f(&mut self.parameters);
+    }
+
     #[allow(dead_code)]
     pub fn reset(&mut self) {
         self.integral = 0.0;