diff --git a/ethercat-hal/src/devices/el3001.rs b/ethercat-hal/src/devices/el3001.rs index dc5b9c02..5fc230a3 100644 --- a/ethercat-hal/src/devices/el3001.rs +++ b/ethercat-hal/src/devices/el3001.rs @@ -82,6 +82,8 @@ impl AnalogInputDevice for EL3001 { AnalogInputRange::Potential { min: ElectricPotential::new::(-10.0), max: ElectricPotential::new::(10.0), + min_raw: i16::MIN, + max_raw: i16::MAX, } } } diff --git a/ethercat-hal/src/devices/el3021.rs b/ethercat-hal/src/devices/el3021.rs index 46fbc3c7..9ff1450b 100644 --- a/ethercat-hal/src/devices/el3021.rs +++ b/ethercat-hal/src/devices/el3021.rs @@ -107,6 +107,8 @@ impl AnalogInputDevice for EL3021 { AnalogInputRange::Current { min: ElectricCurrent::new::(4.0), max: ElectricCurrent::new::(20.0), + min_raw: 0, + max_raw: i16::MAX, } } } diff --git a/ethercat-hal/src/devices/el3024.rs b/ethercat-hal/src/devices/el3024.rs index 108263b8..880899ab 100644 --- a/ethercat-hal/src/devices/el3024.rs +++ b/ethercat-hal/src/devices/el3024.rs @@ -152,6 +152,8 @@ impl AnalogInputDevice for EL3024 { AnalogInputRange::Current { min: ElectricCurrent::new::(4.0), max: ElectricCurrent::new::(20.0), + min_raw: 0, + max_raw: 32767, } } } diff --git a/ethercat-hal/src/devices/el7031_0030/mod.rs b/ethercat-hal/src/devices/el7031_0030/mod.rs index 10fa2be3..63be93af 100644 --- a/ethercat-hal/src/devices/el7031_0030/mod.rs +++ b/ethercat-hal/src/devices/el7031_0030/mod.rs @@ -304,8 +304,10 @@ impl AnalogInputDevice for EL7031_0030 { fn analog_input_range(&self) -> AnalogInputRange { AnalogInputRange::Potential { - min: ElectricPotential::new::(-10.0), + min: ElectricPotential::new::(0.0), max: ElectricPotential::new::(10.0), + min_raw: 0, + max_raw: i16::MAX, } } } diff --git a/ethercat-hal/src/io/analog_input/physical.rs b/ethercat-hal/src/io/analog_input/physical.rs index 46ace793..7bfb4ec8 100644 --- a/ethercat-hal/src/io/analog_input/physical.rs +++ b/ethercat-hal/src/io/analog_input/physical.rs @@ -11,33 +11,66 @@ pub enum AnalogInputRange { Potential { min: ElectricPotential, max: ElectricPotential, + min_raw: i16, + max_raw: i16, }, Current { min: ElectricCurrent, max: ElectricCurrent, + min_raw: i16, + max_raw: i16, }, } impl AnalogInputRange { - /// Convert a normalized value (-1.0 to 1.0) to a physical value - pub fn normalized_to_physical(&self, normalized: f32) -> AnalogInputValue { - // map -1/1 to 0/1 - let clipped = normalized as f64 / 2.0 + 0.5; + pub fn get_min_raw(&self) -> i16 { + return match self { + AnalogInputRange::Potential { min_raw, .. } => *min_raw, + AnalogInputRange::Current { min_raw, .. } => *min_raw, + }; + } + pub fn get_max_raw(&self) -> i16 { + return match self { + AnalogInputRange::Potential { max_raw, .. } => *max_raw, + AnalogInputRange::Current { max_raw, .. } => *max_raw, + }; + } + + pub fn raw_to_normalized(&self, raw_value: i16) -> f64 { + let range = (self.get_max_raw() as i32 - self.get_min_raw() as i32) as f64; + return (raw_value as i32 - self.get_min_raw() as i32) as f64 / range; + } + + pub fn raw_to_physical(&self, raw_value: i16) -> AnalogInputValue { + let normalized = self.raw_to_normalized(raw_value); match self { - AnalogInputRange::Potential { min, max } => { - let value = *min + (*max - *min).abs() * clipped; + AnalogInputRange::Potential { min, max, .. } => { + let value = *min + (*max - *min).abs() * normalized as f64; AnalogInputValue::Potential(value) } - AnalogInputRange::Current { min, max } => { - let value = *min + (*max - *min).abs() * clipped; + AnalogInputRange::Current { min, max, .. } => { + let value = *min + (*max - *min).abs() * normalized as f64; + AnalogInputValue::Current(value) + } + } + } + + /// Convert a normalized value (0 to 1.0) to a physical value + pub fn normalized_to_physical(&self, normalized: f32) -> AnalogInputValue { + match self { + AnalogInputRange::Potential { min, max, .. } => { + let value = *min + (*max - *min).abs() * normalized as f64; + AnalogInputValue::Potential(value) + } + AnalogInputRange::Current { min, max, .. } => { + let value = *min + (*max - *min).abs() * normalized as f64; AnalogInputValue::Current(value) } } } } -// test using AnalogInputRange not AnalogInputGetter #[cfg(test)] mod tests { use core::f64; @@ -53,10 +86,12 @@ mod tests { let analog_input_getter = AnalogInputRange::Potential { min: ElectricPotential::new::(-10.0), max: ElectricPotential::new::(10.0), + min_raw: i16::MIN, + max_raw: i16::MAX, }; - // Check that normalized -1.0 is -10V - let value = analog_input_getter.normalized_to_physical(-1.0); + // -32768 raw = -10V + let value = analog_input_getter.raw_to_physical(i16::MIN); match value { AnalogInputValue::Potential(v) => { assert_relative_eq!(v.get::(), -10.0, epsilon = f64::EPSILON); @@ -64,26 +99,17 @@ mod tests { _ => panic!("Expected a potential value"), } - // Check that normalized 0.0 is 0V - let value = analog_input_getter.normalized_to_physical(0.0); + // 0 raw ~ 0V + let value = analog_input_getter.raw_to_physical(0); match value { AnalogInputValue::Potential(v) => { - assert_relative_eq!(v.get::(), 0.0, epsilon = f64::EPSILON); + assert_relative_eq!(v.get::(), 0.0, epsilon = 0.01); } _ => panic!("Expected a potential value"), } - // Check that normalized 0.5 is 5V - let value = analog_input_getter.normalized_to_physical(0.5); - match value { - AnalogInputValue::Potential(v) => { - assert_relative_eq!(v.get::(), 5.0, epsilon = f64::EPSILON); - } - _ => panic!("Expected a potential value"), - } - - // Check that normalized 1.0 is 10V - let value = analog_input_getter.normalized_to_physical(1.0); + // 32767 raw = 10V + let value = analog_input_getter.raw_to_physical(i16::MAX); match value { AnalogInputValue::Potential(v) => { assert_relative_eq!(v.get::(), 10.0, epsilon = f64::EPSILON); @@ -93,15 +119,16 @@ mod tests { } #[test] - // 4mA to 20mA fn test_analog_input_getter_current() { let analog_input_getter = AnalogInputRange::Current { min: ElectricCurrent::new::(4.0), max: ElectricCurrent::new::(20.0), + min_raw: 0, + max_raw: i16::MAX, }; - // Check that normalized -1.0 is 4mA - let value = analog_input_getter.normalized_to_physical(-1.0); + // 0 raw = 4mA + let value = analog_input_getter.raw_to_physical(0); match value { AnalogInputValue::Current(v) => { assert_relative_eq!(v.get::(), 4.0, epsilon = f64::EPSILON); @@ -109,26 +136,17 @@ mod tests { _ => panic!("Expected a current value"), } - // Check that normalized 0.0 is 12mA - let value = analog_input_getter.normalized_to_physical(0.0); + // 16383 raw ~ 12mA + let value = analog_input_getter.raw_to_physical(i16::MAX / 2); match value { AnalogInputValue::Current(v) => { - assert_relative_eq!(v.get::(), 12.0, epsilon = f64::EPSILON); + assert_relative_eq!(v.get::(), 12.0, epsilon = 0.01); } _ => panic!("Expected a current value"), } - // Check that normalized 0.5 is 16mA - let value = analog_input_getter.normalized_to_physical(0.5); - match value { - AnalogInputValue::Current(v) => { - assert_relative_eq!(v.get::(), 16.0, epsilon = f64::EPSILON); - } - _ => panic!("Expected a current value"), - } - - // Check that normalized 1.0 is 20mA - let value = analog_input_getter.normalized_to_physical(1.0); + // 32767 raw = 20mA + let value = analog_input_getter.raw_to_physical(i16::MAX); match value { AnalogInputValue::Current(v) => { assert_relative_eq!(v.get::(), 20.0, epsilon = f64::EPSILON); diff --git a/ethercat-hal/src/io/analog_input_dummy.rs b/ethercat-hal/src/io/analog_input_dummy.rs index 16116918..9c5cd5c5 100644 --- a/ethercat-hal/src/io/analog_input_dummy.rs +++ b/ethercat-hal/src/io/analog_input_dummy.rs @@ -65,6 +65,8 @@ mod tests { let mut dummy = AnalogInputDummy::new(AnalogInputRange::Potential { min: ElectricPotential::new::(0.0), max: ElectricPotential::new::(10.0), + min_raw: 0, + max_raw: i16::MAX, }); let state = AnalogInputState { input: AnalogInputInput { normalized: 0.5 }, diff --git a/server/src/machines/extruder1/screw_speed_controller.rs b/server/src/machines/extruder1/screw_speed_controller.rs index f88baede..716d2447 100644 --- a/server/src/machines/extruder1/screw_speed_controller.rs +++ b/server/src/machines/extruder1/screw_speed_controller.rs @@ -1,4 +1,4 @@ -use std::time::Instant; +use std::{any, time::Instant}; use control_core::{ actors::{ @@ -8,10 +8,13 @@ use control_core::{ }, controllers::pid::PidController, converters::transmission_converter::TransmissionConverter, + helpers::interpolation::normalize, }; use uom::si::{ angular_velocity::revolution_per_minute, - f64::{AngularVelocity, Frequency, Pressure}, + electric_current::milliampere, + electric_potential::millivolt, + f64::{AngularVelocity, ElectricCurrent, Frequency, Pressure}, frequency::{cycle_per_minute, hertz}, pressure::bar, }; @@ -156,15 +159,34 @@ impl ScrewSpeedController { self.target_pressure } + pub fn get_sensor_current(&self) -> Result { + let phys: ethercat_hal::io::analog_input::physical::AnalogInputValue = self + .pressure_sensor + .get_physical() + .ok_or_else(|| anyhow::anyhow!("no value"))?; + + match phys { + ethercat_hal::io::analog_input::physical::AnalogInputValue::Potential(_) => { + Err(anyhow::anyhow!("Potential is not expected")) + } + ethercat_hal::io::analog_input::physical::AnalogInputValue::Current(quantity) => { + Ok(quantity) + } + } + } + pub fn get_pressure(&mut self) -> Pressure { - let normalized = self.pressure_sensor.get_normalized(); - let normalized = match normalized { - Some(normalized) => normalized, - None => 0.0, + let current_result = self.get_sensor_current(); + let current = match current_result { + Ok(current) => current.get::(), + Err(_) => todo!(), }; - // assuming full scale pressure of 10 bar - let pressure: f64 = normalized as f64 * 10.0; - return Pressure::new::(pressure); + let normalized = normalize(current, 4.0, 20.0); + // Our pressure sensor has a range of Up to 350 Bar + + let actual_pressure = (normalized) * 350.0; + + return Pressure::new::(actual_pressure); } pub async fn update(&mut self, now: Instant) { @@ -179,9 +201,10 @@ impl ScrewSpeedController { self.turn_motor_off(); return; } - if !self.uses_rpm { + let measured_pressure = self.get_pressure(); let error = self.target_pressure - measured_pressure; + let freq = self .pid .update(error.get::(), now) diff --git a/server/src/machines/winder2/tension_arm.rs b/server/src/machines/winder2/tension_arm.rs index 0c85d08a..db51207a 100644 --- a/server/src/machines/winder2/tension_arm.rs +++ b/server/src/machines/winder2/tension_arm.rs @@ -84,14 +84,16 @@ mod tests { analog_input::{AnalogInputInput, physical::AnalogInputRange}, analog_input_dummy::AnalogInputDummy, }; - use std::time::Instant; + use std::{i16, time::Instant}; #[test] fn volts_to_angle() { let tension_arm = TensionArm::new(AnalogInputGetter::new( AnalogInputDummy::new(AnalogInputRange::Potential { - min: ElectricPotential::new::(-10.0), + min: ElectricPotential::new::(0.0), max: ElectricPotential::new::(10.0), + min_raw: 0, + max_raw: i16::MAX, }) .analog_input(), )); @@ -121,8 +123,10 @@ mod tests { #[test] fn get_volts() { let mut analog_input_dummy = AnalogInputDummy::new(AnalogInputRange::Potential { - min: ElectricPotential::new::(-10.0), + min: ElectricPotential::new::(0.0), max: ElectricPotential::new::(10.0), + min_raw: 0, + max_raw: i16::MAX, }); let analog_input_getter = AnalogInputGetter::new(analog_input_dummy.analog_input()); let mut tension_arm = TensionArm::new(analog_input_getter); @@ -150,8 +154,10 @@ mod tests { #[test] fn test_tension_arm() { let mut analog_input_dummy = AnalogInputDummy::new(AnalogInputRange::Potential { - min: ElectricPotential::new::(-10.0), + min: ElectricPotential::new::(0.0), max: ElectricPotential::new::(10.0), + min_raw: 0, + max_raw: i16::MAX, }); let analog_input = analog_input_dummy.analog_input(); let analog_input_getter = AnalogInputGetter::new(analog_input);