From 09b7462f0107f9b61e89dabbfbaad28f7e72bbcd Mon Sep 17 00:00:00 2001 From: "93094049+kraemr@users.noreply.github.com" <93094049+kraemr@users.noreply.github.com> Date: Fri, 9 May 2025 19:18:49 +0200 Subject: [PATCH] added temperature_sensor and pid control --- ethercat-hal/src/devices/mod.rs | 1 - server/src/machines/extruder1/act.rs | 18 ++++++-- server/src/machines/extruder1/mod.rs | 9 +++- server/src/machines/extruder1/new.rs | 12 +++-- .../machines/extruder1/pressure_controller.rs | 0 .../extruder1/temperature_controller.rs | 46 +++++++++++++++++++ 6 files changed, 77 insertions(+), 9 deletions(-) create mode 100644 server/src/machines/extruder1/pressure_controller.rs create mode 100644 server/src/machines/extruder1/temperature_controller.rs diff --git a/ethercat-hal/src/devices/mod.rs b/ethercat-hal/src/devices/mod.rs index 207f1760..f6bab50d 100644 --- a/ethercat-hal/src/devices/mod.rs +++ b/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) -> 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", diff --git a/server/src/machines/extruder1/act.rs b/server/src/machines/extruder1/act.rs index fc1f1ecb..f8c074d1 100644 --- a/server/src/machines/extruder1/act.rs +++ b/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); diff --git a/server/src/machines/extruder1/mod.rs b/server/src/machines/extruder1/mod.rs index b2fa2c21..5af7190f 100644 --- a/server/src/machines/extruder1/mod.rs +++ b/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 { diff --git a/server/src/machines/extruder1/new.rs b/server/src/machines/extruder1/new.rs index 71df5071..bf542830 100644 --- a/server/src/machines/extruder1/new.rs +++ b/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) }) diff --git a/server/src/machines/extruder1/pressure_controller.rs b/server/src/machines/extruder1/pressure_controller.rs new file mode 100644 index 00000000..e69de29b diff --git a/server/src/machines/extruder1/temperature_controller.rs b/server/src/machines/extruder1/temperature_controller.rs new file mode 100644 index 00000000..64b54fe0 --- /dev/null +++ b/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; + } +}