1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
use crate::fb::StateMachine;
use super::axis_view::AxisHandle;
/// Configuration for the Pressure Control function block.
#[derive(Debug, Clone)]
pub struct PressureControlConfig {
/// Proportional gain (Kp).
pub kp: f64,
/// Integral gain (Ki).
pub ki: f64,
/// Derivative gain (Kd).
pub kd: f64,
/// Feed forward value added directly to the output.
pub feed_forward: f64,
/// Maximum allowed position delta (in user units) per call/tick.
/// Critical for safety to prevent crushing the load cell.
/// E.g., 0.001 inches per ms tick.
pub max_step: f64,
/// Maximum accumulated integral windup.
pub max_integral: f64,
/// Exponential Moving Average filter coefficient for the load cell (0.0 to 1.0).
/// 1.0 = No filtering (raw data).
/// 0.1 = Heavy filtering.
pub filter_alpha: f64,
/// If true, a positive error (need more pressure) results in a *negative* move.
/// Set to true if moving the axis down (negative) increases compression.
pub invert_direction: bool,
/// The acceptable load error window to be considered "in tolerance" (e.g., +/- 2.0 lbs).
pub tolerance: f64,
/// How long the load must remain within `tolerance` before reporting `in_tolerance = true`.
pub settling_time: f64,
}
impl Default for PressureControlConfig {
fn default() -> Self {
Self {
kp: 0.0,
ki: 0.0,
kd: 0.0,
feed_forward: 0.0,
max_step: 0.005, // Conservative 5 thousandths max step
max_integral: 100.0,
filter_alpha: 0.5,
invert_direction: false,
tolerance: 1.0,
settling_time: 0.1,
}
}
}
/// A closed-loop PID pressure/force controller for Profile Position (PP) axes.
///
/// This function block uses an Exponential Moving Average (EMA) filter to smooth
/// incoming load cell data. It calculates a PID output which is clamped to a safe
/// maximum step size and issued as a small, incremental `move_absolute` target
/// to the drive every cycle.
#[derive(Debug, Clone)]
pub struct PressureControl {
/// True when the block is actively executing and controlling the axis.
pub active: bool,
/// True when the current load has been within `config.tolerance` of the target
/// for at least `config.settling_time` seconds.
pub in_tolerance: bool,
/// True if a fault occurred (e.g., axis error). Check `state.error_code`.
pub error: bool,
/// Internal state machine for operation sequencing and error reporting.
pub state: StateMachine,
// Internal PID state
integral: f64,
prev_error: f64,
filtered_load: f64,
// Internal Setpoint state (to prevent read-modify-write jitter on actual position)
commanded_position: f64,
// Tolerance timer
settling_timer: f64,
// First execution flag
is_first_run: bool,
}
impl Default for PressureControl {
fn default() -> Self {
Self {
active: false,
in_tolerance: false,
error: false,
state: StateMachine::new(),
integral: 0.0,
prev_error: 0.0,
filtered_load: 0.0,
commanded_position: 0.0,
settling_timer: 0.0,
is_first_run: true,
}
}
}
impl PressureControl {
/// Creates a new, uninitialized PressureControl block.
pub fn new() -> Self {
Self::default()
}
/// Execute the pressure control loop.
///
/// - `axis`: The handle to the PP axis.
/// - `execute`: Set to true to engage the controller. On a falling edge, the axis halts.
/// - `target_load`: The desired pressure/force (e.g., lbs or Newtons).
/// - `current_load`: The raw, instantaneous reading from the load cell.
/// - `config`: Tuning and safety parameters.
/// - `dt`: The cycle time delta in seconds (e.g., 0.001 for 1ms).
pub fn call(
&mut self,
axis: &mut impl AxisHandle,
execute: bool,
target_load: f64,
current_load: f64,
config: &PressureControlConfig,
dt: f64,
) {
// Handle abort/halt on falling edge of execute
if !execute {
if self.active {
axis.halt();
self.active = false;
self.in_tolerance = false;
self.state.index = 0; // Idle
}
self.is_first_run = true;
self.integral = 0.0;
self.prev_error = 0.0;
self.settling_timer = 0.0;
self.error = false;
return;
}
// Safety: Check for axis faults
if axis.is_error() {
self.error = true;
self.active = false;
self.in_tolerance = false;
self.state.set_error(100, "Axis is in error state");
return;
}
// 1. Initialize on rising edge
if self.is_first_run {
self.filtered_load = current_load; // Seed filter
self.commanded_position = axis.position(); // Seed trajectory from current reality
self.is_first_run = false;
self.active = true;
self.error = false;
self.state.index = 10; // Controlling
}
// 2. Exponential Moving Average (EMA) Filter
let alpha = config.filter_alpha.clamp(0.0, 1.0);
self.filtered_load = (alpha * current_load) + ((1.0 - alpha) * self.filtered_load);
// 3. PID Calculation
let error = target_load - self.filtered_load;
// Anti-windup Integral
self.integral += error * dt;
self.integral = self.integral.clamp(-config.max_integral, config.max_integral);
// Derivative (change in error over time)
let derivative = if dt > 0.0 { (error - self.prev_error) / dt } else { 0.0 };
self.prev_error = error;
// Calculate raw output
let mut output = (config.kp * error) + (config.ki * self.integral) + (config.kd * derivative) + config.feed_forward;
// 4. Direction & Clamping
if config.invert_direction {
output = -output;
}
// Clamp output to the maximum safe step size
let step = output.clamp(-config.max_step, config.max_step);
// 5. Issue PP Move
// We accumulate the internal commanded_position rather than reading axis.position()
// every tick. Reading actual position every tick to calculate the next target
// creates a positive feedback loop of lag that destabilizes the PID.
self.commanded_position += step;
// We use move_absolute to our internal tracking position rather than move_relative
// to prevent compounding loss of precision over thousands of micro-moves.
let vel = axis.config().jog_speed;
let acc = axis.config().jog_accel;
let dec = axis.config().jog_decel;
// Only command motion if we actually need to move a non-zero distance.
if step.abs() > f64::EPSILON {
// Note: If the axis options include dynamic max/min position limits,
// the DriveHandle's internal move_absolute implementation will automatically
// reject this move if it violates the safety envelope.
axis.move_absolute(self.commanded_position, vel, acc, dec);
}
// 6. Tolerance Window Evaluation
if error.abs() <= config.tolerance {
self.settling_timer += dt;
if self.settling_timer >= config.settling_time {
self.in_tolerance = true;
}
} else {
self.settling_timer = 0.0;
self.in_tolerance = false;
}
}
}