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
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
// Private imports
use alloc::sync::Arc;
use syunit::*;
use crate::ActuatorError;
// ####################
// # SUBMODULES #
// ####################
/// Everything concerning servo-motors
#[deprecated]
pub mod servo;
// pub use servo::MiniServo;
//
// ######################
// # SyncActuator #
// ######################
/// The state of a `SyncActuator` is used to control the component while it is moving and to get data about the current movement
pub trait SyncActuatorState<U : UnitSet = Rotary> {
/// Returns the current absolute position of the actuator
fn pos(&self) -> U::Position;
/// Returns whether the actuator is currently moving or not
fn moving(&self) -> bool;
// Actions
/// Halt the actuator
fn halt(&self);
/// Interrupt the movement of the actuator
fn interrupt(&self);
//
}
/// Trait for defining controls and components of synchronous actuators
///
/// # Parent components
///
/// Components can have multiple layers, for example take a stepper motor with a geaerbox attached to it. The stepper motor and both combined will be a component, the later having
/// the stepper motor component defined as it's parent component. (See [Gear])
pub trait SyncActuator<U : UnitSet = Rotary> {
// Position & U::Velocity
/// Returns the **absolute** position of the component in the components [Position unit](UnitSet::Position).
///
/// ```rust
/// use syact::prelude::*;
///
/// // Position of components
/// const POS : PositionMM = PositionMM(10.0);
///
/// // Create a new linear_axis (implements SyncActuator)
/// let mut linear_axis = LinearAxis::new_belt_axis(
/// // Some demo actuator (implements SyncActuator)
/// DemoActuator::new(),
/// Millimeters(0.5) // The radius is set to 0.5, which means for each radian the motor moves, the linear_axis moves for 0.5 mm
/// );
///
/// linear_axis.overwrite_abs_pos(POS);
///
/// assert!((linear_axis.pos() - POS).abs() < Millimeters(0.001)); // Check with small tolerance requred for stepper motors
/// ```
fn pos(&self) -> U::Position;
/// Overwrite the current **absolute** position of the component without triggering actual movements.
///
/// ### Stepper tolerance
///
/// Be aware that only full steps can be written in distance, meaning that for position comparision a
/// small tolerance has to be considered, as the value written won't be the exact position value given.
///
/// ```rust
/// use syact::prelude::*;
///
/// // Position of components
/// const POS : PositionMM = PositionMM(10.0);
///
/// // Create a new linear_axis (implements SyncActuator)
/// let mut linear_axis = LinearAxis::new_belt_axis(
/// // Some demo actuator (implements SyncActuator)
/// DemoActuator::new(),
/// Millimeters(0.5) // The radius is set to 0.5, which means for each radian the motor moves, the linear_axis moves for 0.5 mm
/// );
///
/// linear_axis.overwrite_abs_pos(POS);
///
/// assert!((linear_axis.pos() - POS).abs() < Millimeters(0.05)); // Check with small tolerance requred for stepper motors
/// ```
fn overwrite_abs_pos(&mut self, pos : U::Position);
//
// U::Velocity max
/// Maximum velocity allowed by the user if specified, otherwise will return `None`
///
/// ```rust
/// use syact::prelude::*;
///
/// // Create a new demo actuator (only available when testing)
/// let mut stepper = DemoActuator::new();
///
/// assert_eq!(stepper.velocity_max(), None);
/// ```
fn velocity_max(&self) -> Option<U::Velocity>;
/// Set the maximum allowed [U::Velocity]
///
/// ## Option
///
/// Set to `None` if no limit is wished
fn set_velocity_max(&mut self, velocity_opt : Option<U::Velocity>) -> Result<(), ActuatorError<U>>;
//
// Acceleration
/// Maximum acceleration that will be allowed, if specified by the user with `set_max_acceleration`
fn acceleration_max(&self) -> Option<U::Acceleration>;
/// Set the maximum allowed [Acceleration]
///
/// ## Option
///
/// Set to `None` if no limit is wished
fn set_acceleration_max(&mut self, acceleration_opt : Option<U::Acceleration>) -> Result<(), ActuatorError<U>>;
//
// Jolt
/// The maximum jolt, if specified by the user
fn jolt_max(&self) -> Option<U::Jolt>;
/// Set the maximum allowed `Jolt`
///
/// ## Option
///
/// Set to `None` if no limit is wished
fn set_jolt_max(&mut self, jolt_opt : Option<U::Jolt>) -> Result<(), ActuatorError<U>>;
//
// Position limits
/// The minimum position limit of the actuator, if set
fn limit_min(&self) -> Option<U::Position>;
/// The maximum position limit of the actuator, if set
fn limit_max(&self) -> Option<U::Position>;
/// Returns if any limit positions have been reached. The value returned can either be radians or millimeters,
/// depending on the type of component.
///
/// # Limits
///
/// If the return value
/// - greater than 0, the maximum has been reached by the returned amount
/// - is smaller than 0, the minimum has been reached by the returned amount
/// - equal to 0, no limit has been reached
/// - NaN, no limit has been set yet
///
/// # Example
///
/// ```rust
/// use syact::prelude::*;
///
/// // Limits
/// const LIM_MAX : PositionRad = PositionRad(1.0);
/// const LIM_MIN : PositionRad = PositionRad(-2.0);
///
/// const LIM_MIN_LOWER : PositionRad = PositionRad(-3.0);
///
/// // Create a new gear bearing (implements SyncActuator)
/// let mut gear = Gear::new(
/// // A demo actuator (implements SyncActuator)
/// DemoActuator::new(),
/// 0.5); // Ratio is set to 0.5, which means for each radian the motor moves, the bearing moves for half a radian
///
/// gear.set_pos_limits(Some(LIM_MIN), Some(LIM_MAX));
///
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(1.5)), Radians(0.5)); // Over the maximum
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(0.5)), Radians::ZERO); // In range
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(-4.0)), Radians(-2.0)); // Under the minimum
///
/// gear.set_pos_limits(Some(LIM_MIN_LOWER), None); // Overwriting only `min` limit
///
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(1.5)), Radians(0.5)); // Over the maximum
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(0.5)), Radians::ZERO); // In range
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(-4.0)), Radians(-1.0)); // Under the minimum, but less
///
/// gear.overwrite_pos_limits(Some(LIM_MIN_LOWER), None); // Overwriting only both limits with [overwrite_pos_limits()]
///
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(1.5)), Radians::ZERO); // In range, as the `max` limit has been deleted
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(0.5)), Radians::ZERO); // In range
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(-4.0)), Radians(-1.0)); // Under the minimum, but less
/// ```
fn resolve_pos_limits_for_abs_pos(&self, pos : U::Position) -> U::Distance {
if let Some(ang) = self.limit_min() {
if pos < ang {
pos - ang
} else {
if let Some(ang) = self.limit_max() {
if pos > ang {
pos - ang
} else {
U::Distance::ZERO
}
} else {
U::Distance::ZERO
}
}
} else {
if let Some(ang) = self.limit_max() {
if pos > ang {
pos - ang
} else {
U::Distance::ZERO
}
} else {
U::Distance::NAN
}
}
}
/// Sets an endpoint in the current direction by modifying the components limits. For example, when the component is moving
/// in the positive direction and the endpoint is set, this function will overwrite the current maximum limit with the current
/// pos value. The component is then not allowed to move in the current direction anymore.
fn set_endpos(&mut self, overwrite_abs_pos : U::Position);
/// Set the limits for the minimum and maximum angles that the component can reach, note that the limit will
/// be converted and transfered to the parent component if defined.
///
/// Unlike [SyncActuator::overwrite_pos_limits()], this function does not overwrite the current `min` or `max` limits if they
/// are set to `None`.
///
/// ```rust
/// use syact::prelude::*;
///
/// // Limits
/// const LIM_MAX : PositionRad = PositionRad(1.0);
/// const LIM_MIN : PositionRad = PositionRad(-2.0);
///
/// const LIM_MIN_LOWER : PositionRad = PositionRad(-3.0);
///
/// // Create a new gear bearing (implements SyncActuator)
/// let mut gear = Gear::new(
/// // A demo actuator (implements SyncActuator)
/// DemoActuator::new(),
/// 0.5); // Ratio is set to 0.5, which means for each radian the motor moves, the bearing moves for half a radian
///
/// gear.set_pos_limits(Some(LIM_MIN), Some(LIM_MAX));
///
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(1.5)), Radians(0.5)); // Over the maximum
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(0.5)), Radians::ZERO); // In range
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(-4.0)), Radians(-2.0)); // Under the minimum
///
/// gear.set_pos_limits(Some(LIM_MIN_LOWER), None); // Overwriting only `min` limit
///
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(1.5)), Radians(0.5)); // Over the maximum
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(0.5)), Radians::ZERO); // In range
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(-4.0)), Radians(-1.0)); // Under the minimum, but less
///
/// gear.overwrite_pos_limits(Some(LIM_MIN_LOWER), None); // Overwriting only both limits with [overwrite_pos_limits()]
///
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(1.5)), Radians::ZERO); // In range, as the `max` limit has been deleted
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(0.5)), Radians::ZERO); // In range
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(-4.0)), Radians(-1.0)); // Under the minimum, but less
/// ```
fn set_pos_limits(&mut self, min : Option<U::Position>, max : Option<U::Position>);
/// Set the limits for the minimum and maximum angles that the component can reach, note that the limit will
/// be converted and transfered to the parent component if this component has one.
///
/// The difference to [SyncActuator::set_pos_limits()] is that this function **overwrites** the current limits set.
///
/// ```rust
/// use syact::prelude::*;
///
/// // Limits
/// const LIM_MAX : PositionRad = PositionRad(1.0);
/// const LIM_MIN : PositionRad = PositionRad(-2.0);
///
/// const LIM_MIN_LOWER : PositionRad = PositionRad(-3.0);
///
/// // Create a new gear bearing (implements SyncActuator)
/// let mut gear = Gear::new(
/// // A demo actuator (implements SyncActuator)
/// DemoActuator::new(),
/// 0.5); // Ratio is set to 0.5, which means for each radian the motor moves, the bearing moves for half a radian
///
/// gear.set_pos_limits(Some(LIM_MIN), Some(LIM_MAX));
///
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(1.5)), Radians(0.5)); // Over the maximum
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(0.5)), Radians::ZERO); // In range
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(-4.0)), Radians(-2.0)); // Under the minimum
///
/// gear.set_pos_limits(Some(LIM_MIN_LOWER), None); // Overwriting only `min` limit
///
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(1.5)), Radians(0.5)); // Over the maximum
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(0.5)), Radians::ZERO); // In range
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(-4.0)), Radians(-1.0)); // Under the minimum, but less
///
/// gear.overwrite_pos_limits(Some(LIM_MIN_LOWER), None); // Overwriting only both limits with [overwrite_pos_limits()]
///
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(1.5)), Radians::ZERO); // In range, as the `max` limit has been deleted
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(0.5)), Radians::ZERO); // In range
/// assert_eq!(gear.resolve_pos_limits_for_abs_pos(PositionRad(-4.0)), Radians(-1.0)); // Under the minimum, but less
/// ```
fn overwrite_pos_limits(&mut self, min : Option<U::Position>, max : Option<U::Position>);
//
}
//
// #########################################
// # SyncActuator - Extention traits #
// #########################################
// Movement
/// Further defines a `SyncActuator`, extending it with blocking movement functions
pub trait SyncActuatorBlocking<U : UnitSet = Rotary> : SyncActuator<U> {
// State
/// Returns a reference to the actuators `SyncActuatorState`
fn state(&self) -> &dyn SyncActuatorState<U>;
/// Returns an `Arc` reference counter to the given S
fn clone_state(&self) -> Arc<dyn SyncActuatorState<U>>;
//
/// Moves the component by the relative distance as fast as possible, blocks the script until the movement is finshed
fn drive_rel_blocking(&mut self, rel_dist : U::Distance, speed : Factor) -> Result<(), ActuatorError<U>>;
/// Moves the component to the absolute position as fast as possible, blocks the script until the movement is finshed
#[inline]
fn drive_abs_blocking(&mut self, pos : U::Position, speed : Factor) -> Result<(), ActuatorError<U>> {
let rel_dist = pos - self.pos();
self.drive_rel_blocking(rel_dist, speed)
}
/// Starts the movement process of the component in the given direction with a given `speed` factor
fn drive_factor(&mut self, speed : Factor, direction : Direction) -> Result<(), ActuatorError<U>>;
/// Start the movement process of the component with the given velocity `speed`, positive values for `speed` mean CW movement
fn drive_speed(&mut self, speed : U::Velocity) -> Result<(), ActuatorError<U>>;
}
/// Further defines a `SyncActuator`, extending it with non-blocking movement functions
pub trait SyncActuatorNB<U : UnitSet = Rotary> : SyncActuator<U> {
/// Moves the component by the relative distance as fast as possible, blocks the script until the movement is finshed
fn drive_rel_nb(&mut self, rel_dist : U::Distance, speed : Factor) -> Result<(), ActuatorError<U>>;
/// Moves the component to the absolute position as fast as possible, blocks the script until the movement is finshed
fn drive_abs_nb(&mut self, pos : U::Position, speed : Factor) -> Result<(), ActuatorError<U>> {
let rel_dist = pos - self.pos();
self.drive_rel_nb(rel_dist, speed)
}
}
//
//