Reimplement control logic

Move logic into its own module and implement it using nicer programming
patterns

src/display.rs

@@ -45,6 +45,21 @@ impl Display {
             self.inner.signal.notify_one();
         }
     }
+
+    pub fn update_all(&mut self, texts: &[String; 4]) {
+        let mut shared = self.inner.shared.lock().unwrap();
+        for (i, text) in texts.iter().enumerate() {
+            let line = &mut shared.lines[i];
+            if line != text {
+                line.clear();
+                line.push_str(text);
+                shared.pending = true;
+            }
+        }
+        if shared.pending {
+            self.inner.signal.notify_one();
+        }
+    }
 }
 
 fn display_thread(display_data: Arc<DisplayInner>) {

src/fieldbus.rs

@@ -66,6 +66,15 @@ impl Fieldbus {
         pii.copy_from_slice(&data.pii);
         data.piq.copy_from_slice(piq);
     }
+
+    pub fn with_process_images<F, R>(&mut self, f: F) -> R
+    where
+        F: FnOnce(&[u8; PII_SIZE], &mut [u8; PIQ_SIZE]) -> R,
+    {
+        let mut data = self.inner.lock().unwrap();
+        let data = &mut *data;
+        f(&data.pii, &mut data.piq)
+    }
 }
 
 struct PeripheralInfo {

src/logic.rs

@@ -0,0 +1,114 @@
+use crate::util::*;
+
+#[derive(Debug, Default)]
+pub struct LogicInputs {
+    pub left_limit: bool,
+    pub right_limit: bool,
+    pub pressure_switch: bool,
+
+    pub switch_run_continuous: bool,
+    pub switch_run_once: bool,
+
+    pub rio_online: bool,
+}
+
+#[derive(Debug, Default)]
+pub struct LogicOutputs {
+    pub cylinder_left: bool,
+    pub cylinder_right: bool,
+
+    pub signal_operate: bool,
+    pub signal_ready: bool,
+    pub signal_fault: bool,
+    pub signal_alarm: bool,
+
+    pub display: [String; 4],
+}
+
+#[derive(Debug, Default, PartialEq, Eq, Clone, Copy)]
+enum MachineState {
+    #[default]
+    Stopped,
+    Idle,
+    ExecuteContinuous,
+    ExecuteOnce,
+    Aborted,
+}
+
+#[derive(Debug, Default)]
+enum CylinderState {
+    #[default]
+    Unknown,
+    Left,
+    Right,
+}
+
+#[derive(Debug, Default)]
+pub struct Logic {
+    inp: LogicInputs,
+    out: LogicOutputs,
+
+    counter: usize,
+    running: bool,
+    faulted: bool,
+    pressure_ok: bool,
+    cylinder_state: CylinderState,
+    state: MachineState,
+
+    t_alarm: PulseTimer,
+}
+
+impl Logic {
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    pub fn inputs_mut(&mut self) -> &mut LogicInputs {
+        &mut self.inp
+    }
+
+    pub fn outputs(&self) -> &LogicOutputs {
+        &self.out
+    }
+}
+
+impl Logic {
+    pub fn run(&mut self, now: profirust::time::Instant) {
+        self.out.signal_fault = matches!(self.state, MachineState::Stopped | MachineState::Aborted);
+        self.out.signal_ready = matches!(self.state, MachineState::Idle);
+        self.out.signal_operate = matches!(
+            self.state,
+            MachineState::ExecuteContinuous | MachineState::ExecuteOnce
+        );
+        self.out.signal_alarm = self
+            .t_alarm
+            .run(
+                now,
+                matches!(self.state, MachineState::Aborted),
+                500.millis(),
+            )
+            .done;
+
+        self.state = match self.state {
+            MachineState::Stopped if !self.inp.pressure_switch => MachineState::Stopped,
+            _ if !self.inp.pressure_switch => MachineState::Aborted,
+
+            // Reset aborted state when no run mode is requested
+            MachineState::Aborted
+                if !self.inp.switch_run_once && !self.inp.switch_run_continuous =>
+            {
+                MachineState::Stopped
+            }
+
+            // Go to idle state once pressure is okay
+            MachineState::Stopped if self.inp.pressure_switch => MachineState::Idle,
+
+            // Start from idle state when one of the modes is requested
+            MachineState::Idle if self.inp.switch_run_continuous => MachineState::ExecuteContinuous,
+            MachineState::Idle if self.inp.switch_run_once => MachineState::ExecuteOnce,
+
+            // Stay in current state if when no transition is active
+            state => state,
+        };
+    }
+}

src/main.rs

@@ -2,6 +2,7 @@ use process_image as pi;
 
 mod display;
 mod fieldbus;
+mod logic;
 mod util;
 
 // Parameters
@@ -103,21 +104,35 @@ fn main() {
     display.update_line(0, &format!("{:^20}", "torque tester"));
 
     let mut fieldbus = fieldbus::Fieldbus::new();
-
-    let mut pii = [0u8; fieldbus::PII_SIZE];
-    let mut piq = [0u8; fieldbus::PIQ_SIZE];
+    let mut logic = logic::Logic::new();
 
     fieldbus.enter_state(fieldbus::OperatingState::Operate);
 
     loop {
-        fieldbus.update_process_images(&mut pii, &piq);
-        let pii = Pii::from(&pii);
-        let piq = PiqMut::from(&mut piq);
+        fieldbus.with_process_images(|pii, piq| {
+            use process_image::{tag, tag_mut};
 
-        display.update_line(2, &format!("Switch: {}", pii.want_run()));
-        display.update_line(3, &format!("Button: {}", pii.want_run_once()));
+            *tag_mut!(piq, X, 0, 0) = logic.outputs().cylinder_left; // -MBB1
+            *tag_mut!(piq, X, 0, 1) = logic.outputs().cylinder_right; // -MBB2
+                                                                      //
+            *tag_mut!(piq, X, 0, 2) = logic.outputs().signal_operate; // -PZA1:WHITE
+            *tag_mut!(piq, X, 0, 3) = logic.outputs().signal_ready; // -PZA1:BLUE
+            *tag_mut!(piq, X, 0, 4) = logic.outputs().signal_fault; // -PZA1:ORANGE
+            *tag_mut!(piq, X, 0, 5) = logic.outputs().signal_alarm; // -PZA1:BUZZER (VIOLET)
 
-        program(pii, piq, profirust::time::Instant::now());
+            logic.inputs_mut().left_limit = tag!(pii, X, 0, 0); // -BGB2
+            logic.inputs_mut().right_limit = tag!(pii, X, 0, 1); // -BGB2
+            logic.inputs_mut().pressure_switch = tag!(pii, X, 0, 2); // -BPB1
+
+            logic.inputs_mut().switch_run_continuous = tag!(pii, X, 0, 4);
+            logic.inputs_mut().switch_run_once = tag!(pii, X, 0, 5);
+
+            logic.inputs_mut().rio_online = tag!(pii, X, 64, 0);
+        });
+
+        logic.run(profirust::time::Instant::now());
+
+        display.update_all(&logic.outputs().display);
 
         std::thread::sleep(std::time::Duration::from_millis(20));
     }