added temperature_sensor and pid control

ethercat-hal/src/devices/mod.rs

@@ -56,7 +56,6 @@ pub trait EthercatDevice: NewEthercatDevice + Any + Send + Sync + Debug {
     fn input_checked(&mut self, input: &BitSlice<u8, Lsb0>) -> Result<(), anyhow::Error> {
         // validate input has correct length
         let input_len = self.input_len();
-        println!("self: {:?} {:?}", self, input.clone());
         if input.len() != input_len {
             return Err(anyhow::anyhow!(
                 "[{}::Device::input_checked] Input length is {} and must be {} bits",

server/src/machines/extruder1/act.rs

@@ -1,6 +1,5 @@
 use super::ExtruderV2;
 use control_core::actors::Actor;
-use ethercat_hal::io::temperature_input::TemperatureInputInput;
 use std::time::{Duration, Instant};
 
 impl Actor for ExtruderV2 {
@@ -14,17 +13,30 @@ impl Actor for ExtruderV2 {
             self.temp_sensor_1.act(now_ts).await;
             self.temp_sensor_2.act(now_ts).await;
             self.temp_sensor_3.act(now_ts).await;
-            self.test.act(now_ts).await;
+            self.heating_relay_1.act(now_ts).await;
 
             let now = Instant::now();
             if now.duration_since(self.last_measurement_emit) > Duration::from_millis(16) {
                 // channel 1
-                self.heating_front.temperature = self.temp_sensor_1.temperature;
+                self.heating_front.temperature = self.temp_sensor_1.temperature; // set the temperature read from the sensor
                 // channel 2
                 self.heating_middle.temperature = self.temp_sensor_2.temperature;
                 // channel 3
                 self.heating_back.temperature = self.temp_sensor_3.temperature;
 
+                println!(
+                    "temp {} target {}",
+                    self.heating_front.temperature, self.heating_front.target_temperature
+                );
+                let on = self
+                    .temperature_controller
+                    .update(self.heating_front.temperature as f64, now_ts); // check if we need to set our relais to enabled to reach target temp
+
+                self.temperature_controller.target_temp =
+                    self.heating_front.target_temperature as f64; // set target temperature
+
+                self.heating_relay_1.set(on); // set relay to on or off
+
                 self.emit_heating(self.heating_back.clone(), super::HeatingType::Back);
                 self.emit_heating(self.heating_front.clone(), super::HeatingType::Front);
                 self.emit_heating(self.heating_middle.clone(), super::HeatingType::Middle);

server/src/machines/extruder1/mod.rs

@@ -3,6 +3,7 @@ use control_core::{
     actors::{
         analog_input_getter::AnalogInputGetter,
         digital_output_blinker::DigitalOutputBlinker,
+        digital_output_setter::DigitalOutputSetter,
         mitsubishi_inverter_rs485::{MitsubishiControlRequests, MitsubishiInverterRS485Actor},
         temperature_input_getter::TemperatureInputGetter,
     },
@@ -10,12 +11,14 @@ use control_core::{
     modbus::ModbusRequest,
     socketio::namespace::NamespaceCacheingLogic,
 };
-use ethercat_hal::io::temperature_input::TemperatureInput;
+
 use serde::{Deserialize, Serialize};
 use std::time::Instant;
+use temperature_controller::TemperatureController;
 pub mod act;
 pub mod api;
 pub mod new;
+pub mod temperature_controller;
 
 #[derive(Deserialize, Serialize, Debug, Clone, PartialEq)]
 pub enum ExtruderV2Mode {
@@ -57,7 +60,9 @@ pub struct ExtruderV2 {
     temp_sensor_3: TemperatureInputGetter,
     temp_sensor_2: TemperatureInputGetter,
 
-    test: DigitalOutputBlinker,
+    temperature_controller: TemperatureController,
+
+    heating_relay_1: DigitalOutputSetter,
 }
 
 impl std::fmt::Display for ExtruderV2 {

server/src/machines/extruder1/new.rs

@@ -1,10 +1,13 @@
 use std::time::{Duration, Instant};
 
+use crate::machines::extruder1::temperature_controller::{self, TemperatureController};
+
 use super::{ExtruderV2, ExtruderV2Mode, Heating, api::ExtruderV2Namespace};
 use anyhow::Error;
 use control_core::{
     actors::{
         analog_input_getter::AnalogInputGetter, digital_output_blinker::DigitalOutputBlinker,
+        digital_output_setter::DigitalOutputSetter,
         mitsubishi_inverter_rs485::MitsubishiInverterRS485Actor,
         temperature_input_getter::TemperatureInputGetter,
     },
@@ -210,7 +213,7 @@ impl MachineNewTrait for ExtruderV2 {
             let pressure_sensor = AnalogInputGetter::new(AnalogInput::new(el3021, EL3021Port::AI1));
 
             let digital_out_1 = DigitalOutput::new(el2004, EL2004Port::DO1);
-
+            let temperature_controller = TemperatureController::new(0.4, 0.001, 0.001, 0.0);
             let extruder: ExtruderV2 = Self {
                 inverter: MitsubishiInverterRS485Actor::new(SerialInterface::new(
                     el6021,
@@ -223,7 +226,7 @@ impl MachineNewTrait for ExtruderV2 {
                 heating_front: Heating {
                     temperature: 150.0,
                     heating: false,
-                    target_temperature: 150.0,
+                    target_temperature: 60.0,
                 },
                 heating_back: Heating {
                     temperature: 150.0,
@@ -243,7 +246,10 @@ impl MachineNewTrait for ExtruderV2 {
                 temp_sensor_1: t1_getter,
                 temp_sensor_2: t2_getter,
                 temp_sensor_3: t3_getter,
-                test: DigitalOutputBlinker::new(digital_out_1, Duration::from_secs(1)),
+
+                heating_relay_1: DigitalOutputSetter::new(digital_out_1),
+
+                temperature_controller,
             };
             Ok(extruder)
         })

server/src/machines/extruder1/temperature_controller.rs

@@ -0,0 +1,46 @@
+use std::time::{Duration, Instant};
+
+use control_core::controllers::pid::PidController;
+
+const PWM_PERIOD: Duration = Duration::from_millis(200);
+
+#[derive(Debug)]
+
+pub struct TemperatureController {
+    pid: PidController,
+    pub target_temp: f64,
+    window_start: Instant,
+}
+
+impl TemperatureController {
+    pub fn new(kp: f64, ki: f64, kd: f64, target_temp: f64) -> Self {
+        Self {
+            pid: PidController::new(kp, ki, kd),
+            target_temp,
+            window_start: Instant::now(),
+        }
+    }
+
+    pub fn update(&mut self, measured_temp: f64, now: Instant) -> bool {
+        let error = self.target_temp - measured_temp;
+        let control = self.pid.update(error, now); // PID output
+        // println!("control {} error {}", control, error);
+        // Clamp PID output to 0.0 – 1.0 (as duty cycle)
+        let duty = control.clamp(0.0, 1.0);
+
+        // Time since window started
+        let elapsed = now.duration_since(self.window_start);
+
+        // Restart window if needed
+        if elapsed >= PWM_PERIOD {
+            self.window_start = now;
+        }
+
+        // Compare duty cycle to elapsed time
+        let on_time = PWM_PERIOD.mul_f64(duty);
+
+        println!("update Relais: {} duty: {}", elapsed < on_time, duty);
+        // Relay is ON if within duty cycle window
+        return elapsed < on_time;
+    }
+}