#![warn(missing_docs)]
#![cfg_attr(not(feature = "std"), no_std)]
#[cfg(feature = "std")]
use alloc::sync::Arc;
#[cfg(feature = "std")]
use std::sync::{Mutex, RwLock};
#[cfg(feature = "alloc")]
extern crate alloc;
#[cfg(feature = "alloc")]
use alloc::rc::Rc;
#[cfg(feature = "alloc")]
use alloc::vec::Vec;
#[cfg(any(feature = "alloc", feature = "devices"))]
use core::cell::RefCell;
use core::fmt::Debug;
use core::marker::PhantomData;
use core::ops::{
Add, AddAssign, Deref, DerefMut, Div, DivAssign, Mul, MulAssign, Neg, Not, Sub, SubAssign,
};
mod command;
mod datum;
#[cfg(feature = "devices")]
pub mod devices;
pub mod dimensions;
pub use dimensions::*;
mod motion_profile;
pub mod reference;
mod state;
pub mod streams;
pub use command::*;
pub use datum::*;
pub use motion_profile::*;
#[cfg(feature = "alloc")]
pub use reference::rc_ref_cell_reference;
pub use reference::Reference;
#[cfg(feature = "std")]
pub use reference::{arc_mutex_reference, arc_rw_lock_reference};
pub use state::*;
#[derive(Clone, Copy, Debug, PartialEq)]
#[non_exhaustive]
pub enum Error<O: Copy + Debug> {
FromNone,
Other(O),
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum PositionDerivative {
Position,
Velocity,
Acceleration,
}
#[cfg(any(
feature = "dim_check_release",
all(debug_assertions, feature = "dim_check_debug")
))]
impl TryFrom<Unit> for PositionDerivative {
type Error = ();
fn try_from(was: Unit) -> Result<Self, ()> {
Ok(match was {
MILLIMETER => PositionDerivative::Position,
MILLIMETER_PER_SECOND => PositionDerivative::Velocity,
MILLIMETER_PER_SECOND_SQUARED => PositionDerivative::Acceleration,
_ => return Err(()),
})
}
}
impl From<Command> for PositionDerivative {
fn from(was: Command) -> Self {
match was {
Command::Position(_) => Self::Position,
Command::Velocity(_) => Self::Velocity,
Command::Acceleration(_) => Self::Acceleration,
}
}
}
impl TryFrom<MotionProfilePiece> for PositionDerivative {
type Error = ();
fn try_from(was: MotionProfilePiece) -> Result<Self, ()> {
match was {
MotionProfilePiece::BeforeStart | MotionProfilePiece::Complete => Err(()),
MotionProfilePiece::InitialAcceleration | MotionProfilePiece::EndAcceleration => {
Ok(PositionDerivative::Acceleration)
}
MotionProfilePiece::ConstantVelocity => Ok(PositionDerivative::Velocity),
}
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PIDKValues {
pub kp: f32,
pub ki: f32,
pub kd: f32,
}
impl PIDKValues {
pub const fn new(kp: f32, ki: f32, kd: f32) -> Self {
Self {
kp: kp,
ki: ki,
kd: kd,
}
}
#[inline]
pub fn evaluate(&self, error: f32, error_integral: f32, error_derivative: f32) -> f32 {
self.kp * error + self.ki * error_integral + self.kd * error_derivative
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PositionDerivativeDependentPIDKValues {
pub position: PIDKValues,
pub velocity: PIDKValues,
pub acceleration: PIDKValues,
}
impl PositionDerivativeDependentPIDKValues {
pub const fn new(position: PIDKValues, velocity: PIDKValues, acceleration: PIDKValues) -> Self {
Self {
position: position,
velocity: velocity,
acceleration: acceleration,
}
}
#[inline]
pub fn get_k_values(&self, position_derivative: PositionDerivative) -> PIDKValues {
match position_derivative {
PositionDerivative::Position => self.position,
PositionDerivative::Velocity => self.velocity,
PositionDerivative::Acceleration => self.acceleration,
}
}
#[inline]
pub fn evaluate(
&self,
position_derivative: PositionDerivative,
error: f32,
error_integral: f32,
error_derivative: f32,
) -> f32 {
self.get_k_values(position_derivative)
.evaluate(error, error_integral, error_derivative)
}
}
pub type Output<T, E> = Result<Option<Datum<T>>, Error<E>>;
pub type TimeOutput<E> = Result<Time, Error<E>>;
pub type NothingOrError<E> = Result<(), Error<E>>;
pub trait TimeGetter<E: Copy + Debug>: Updatable<E> {
fn get(&self) -> TimeOutput<E>;
}
pub trait History<T, E: Copy + Debug>: Updatable<E> {
fn get(&self, time: Time) -> Option<Datum<T>>;
}
pub trait Updatable<E: Copy + Debug> {
fn update(&mut self) -> NothingOrError<E>;
}
pub trait Getter<G, E: Copy + Debug>: Updatable<E> {
fn get(&self) -> Output<G, E>;
}
pub struct SettableData<S, E: Copy + Debug> {
following: Option<Reference<dyn Getter<S, E>>>,
last_request: Option<S>,
}
impl<S, E: Copy + Debug> SettableData<S, E> {
pub const fn new() -> Self {
Self {
following: None,
last_request: None,
}
}
}
pub trait Settable<S: Clone, E: Copy + Debug>: Updatable<E> {
fn impl_set(&mut self, value: S) -> NothingOrError<E>;
fn set(&mut self, value: S) -> NothingOrError<E> {
self.impl_set(value.clone())?;
let data = self.get_settable_data_mut();
data.last_request = Some(value);
Ok(())
}
fn get_settable_data_ref(&self) -> &SettableData<S, E>;
fn get_settable_data_mut(&mut self) -> &mut SettableData<S, E>;
fn follow(&mut self, getter: Reference<dyn Getter<S, E>>) {
let data = self.get_settable_data_mut();
data.following = Some(getter);
}
fn stop_following(&mut self) {
let data = self.get_settable_data_mut();
data.following = None;
}
fn update_following_data(&mut self) -> NothingOrError<E> {
let data = self.get_settable_data_ref();
match &data.following {
None => {}
Some(getter) => {
let new_value = getter.borrow().get()?;
match new_value {
None => {
return Ok(());
}
Some(datum) => {
self.set(datum.value)?;
}
}
}
}
Ok(())
}
fn get_last_request(&self) -> Option<S> {
let data = self.get_settable_data_ref();
data.last_request.clone()
}
}
pub struct TimeGetterFromGetter<T: Clone, G: Getter<T, E> + ?Sized, E: Copy + Debug> {
elevator: streams::converters::NoneToError<T, G, E>,
}
impl<T: Clone, G: Getter<T, E> + ?Sized, E: Copy + Debug> TimeGetterFromGetter<T, G, E> {
pub const fn new(stream: Reference<G>) -> Self {
Self {
elevator: streams::converters::NoneToError::new(stream),
}
}
}
impl<T: Clone, G: Getter<T, E> + ?Sized, E: Copy + Debug> TimeGetter<E>
for TimeGetterFromGetter<T, G, E>
{
fn get(&self) -> TimeOutput<E> {
let output = self.elevator.get()?;
let output = output.expect("`NoneToError` made all `Ok(None)`s into `Err(_)`s, and `?` returned all `Err(_)`s, so we're sure this is now an `Ok(Some(_))`.");
return Ok(output.time);
}
}
impl<T: Clone, G: Getter<T, E> + ?Sized, E: Copy + Debug> Updatable<E>
for TimeGetterFromGetter<T, G, E>
{
fn update(&mut self) -> NothingOrError<E> {
Ok(())
}
}
pub struct GetterFromHistory<'a, G, TG: TimeGetter<E>, E: Copy + Debug> {
history: &'a mut dyn History<G, E>,
time_getter: Reference<TG>,
time_delta: Time,
}
impl<'a, G, TG: TimeGetter<E>, E: Copy + Debug> GetterFromHistory<'a, G, TG, E> {
pub fn new_no_delta(history: &'a mut impl History<G, E>, time_getter: Reference<TG>) -> Self {
Self {
history: history,
time_getter: time_getter,
time_delta: Time::default(),
}
}
pub fn new_start_at_zero(
history: &'a mut impl History<G, E>,
time_getter: Reference<TG>,
) -> Result<Self, Error<E>> {
let time_delta = -time_getter.borrow().get()?;
Ok(Self {
history: history,
time_getter: time_getter,
time_delta: time_delta,
})
}
pub fn new_custom_start(
history: &'a mut impl History<G, E>,
time_getter: Reference<TG>,
start: Time,
) -> Result<Self, Error<E>> {
let time_delta = start - time_getter.borrow().get()?;
Ok(Self {
history: history,
time_getter: time_getter,
time_delta: time_delta,
})
}
pub fn new_custom_delta(
history: &'a mut impl History<G, E>,
time_getter: Reference<TG>,
time_delta: Time,
) -> Self {
Self {
history: history,
time_getter: time_getter,
time_delta: time_delta,
}
}
pub fn set_delta(&mut self, time_delta: Time) {
self.time_delta = time_delta;
}
pub fn set_time(&mut self, time: Time) -> NothingOrError<E> {
let time_delta = time - self.time_getter.borrow().get()?;
self.time_delta = time_delta;
Ok(())
}
}
impl<G, TG: TimeGetter<E>, E: Copy + Debug> Updatable<E> for GetterFromHistory<'_, G, TG, E> {
fn update(&mut self) -> NothingOrError<E> {
self.history.update()?;
self.time_getter.borrow_mut().update()?;
Ok(())
}
}
impl<G, TG: TimeGetter<E>, E: Copy + Debug> Getter<G, E> for GetterFromHistory<'_, G, TG, E> {
fn get(&self) -> Output<G, E> {
let time = self.time_getter.borrow().get()?;
Ok(match self.history.get(time + self.time_delta) {
Some(datum) => Some(Datum::new(time, datum.value)),
None => None,
})
}
}
pub struct ConstantGetter<T: Clone, TG: TimeGetter<E> + ?Sized, E: Copy + Debug> {
settable_data: SettableData<T, E>,
time_getter: Reference<TG>,
value: T,
}
impl<T: Clone, TG: TimeGetter<E> + ?Sized, E: Copy + Debug> ConstantGetter<T, TG, E> {
pub const fn new(time_getter: Reference<TG>, value: T) -> Self {
Self {
settable_data: SettableData::new(),
time_getter: time_getter,
value: value,
}
}
}
impl<T: Clone, TG: TimeGetter<E> + ?Sized, E: Copy + Debug> Getter<T, E>
for ConstantGetter<T, TG, E>
{
fn get(&self) -> Output<T, E> {
let time = self.time_getter.borrow().get()?;
Ok(Some(Datum::new(time, self.value.clone())))
}
}
impl<T: Clone, TG: TimeGetter<E> + ?Sized, E: Copy + Debug> Settable<T, E>
for ConstantGetter<T, TG, E>
{
fn get_settable_data_ref(&self) -> &SettableData<T, E> {
&self.settable_data
}
fn get_settable_data_mut(&mut self) -> &mut SettableData<T, E> {
&mut self.settable_data
}
fn impl_set(&mut self, value: T) -> NothingOrError<E> {
self.value = value;
Ok(())
}
}
impl<T: Clone, TG: TimeGetter<E> + ?Sized, E: Copy + Debug> Updatable<E>
for ConstantGetter<T, TG, E>
{
fn update(&mut self) -> NothingOrError<E> {
self.update_following_data()?;
Ok(())
}
}
pub struct NoneGetter;
impl NoneGetter {
pub const fn new() -> Self {
Self
}
}
impl<T, E: Copy + Debug> Getter<T, E> for NoneGetter {
fn get(&self) -> Output<T, E> {
Ok(None)
}
}
impl<E: Copy + Debug> Updatable<E> for NoneGetter {
fn update(&mut self) -> NothingOrError<E> {
Ok(())
}
}
impl<E: Copy + Debug> TimeGetter<E> for Time {
fn get(&self) -> TimeOutput<E> {
Ok(*self)
}
}
impl<E: Copy + Debug> Updatable<E> for Time {
fn update(&mut self) -> NothingOrError<E> {
Ok(())
}
}
#[cfg(feature = "devices")]
pub struct Terminal<'a, E: Copy + Debug> {
settable_data_state: SettableData<Datum<State>, E>,
settable_data_command: SettableData<Datum<Command>, E>,
other: Option<&'a RefCell<Terminal<'a, E>>>,
}
#[cfg(feature = "devices")]
impl<E: Copy + Debug> Terminal<'_, E> {
pub const fn new_raw() -> Self {
Self {
settable_data_state: SettableData::new(),
settable_data_command: SettableData::new(),
other: None,
}
}
pub const fn new() -> RefCell<Self> {
RefCell::new(Self::new_raw())
}
pub fn disconnect(&mut self) {
match self.other {
Some(other) => {
let mut other = other.borrow_mut();
other.other = None;
self.other = None;
}
None => (),
}
}
}
#[cfg(feature = "devices")]
impl<E: Copy + Debug> Settable<Datum<State>, E> for Terminal<'_, E> {
fn get_settable_data_ref(&self) -> &SettableData<Datum<State>, E> {
&self.settable_data_state
}
fn get_settable_data_mut(&mut self) -> &mut SettableData<Datum<State>, E> {
&mut self.settable_data_state
}
fn impl_set(&mut self, _state: Datum<State>) -> NothingOrError<E> {
Ok(())
}
}
#[cfg(feature = "devices")]
impl<E: Copy + Debug> Settable<Datum<Command>, E> for Terminal<'_, E> {
fn get_settable_data_ref(&self) -> &SettableData<Datum<Command>, E> {
&self.settable_data_command
}
fn get_settable_data_mut(&mut self) -> &mut SettableData<Datum<Command>, E> {
&mut self.settable_data_command
}
fn impl_set(&mut self, _command: Datum<Command>) -> NothingOrError<E> {
Ok(())
}
}
#[cfg(feature = "devices")]
impl<E: Copy + Debug> Getter<State, E> for Terminal<'_, E> {
fn get(&self) -> Output<State, E> {
let mut addends: [core::mem::MaybeUninit<Datum<State>>; 2] =
[core::mem::MaybeUninit::uninit(); 2];
let mut addend_count = 0usize;
match self.get_last_request() {
Some(state) => {
addends[0].write(state);
addend_count += 1;
}
None => (),
}
match self.other {
Some(other) => match other.borrow().get_last_request() {
Some(state) => {
addends[addend_count].write(state);
addend_count += 1;
}
None => (),
},
None => (),
}
unsafe {
match addend_count {
0 => return Ok(None),
1 => return Ok(Some(addends[0].assume_init())),
2 => {
return Ok(Some(
(addends[0].assume_init() + addends[1].assume_init()) / 2.0,
))
}
_ => unimplemented!(),
}
}
}
}
#[cfg(feature = "devices")]
impl<E: Copy + Debug> Getter<Command, E> for Terminal<'_, E> {
fn get(&self) -> Output<Command, E> {
let mut maybe_command: Option<Datum<Command>> = None;
match self.get_last_request() {
Some(command) => {
maybe_command = Some(command);
}
None => {}
}
match self.other {
Some(other) => {
match <Terminal<'_, E> as Settable<Datum<Command>, E>>::get_last_request(
&other.borrow(),
) {
Some(gotten_command) => match maybe_command {
Some(command_some) => {
if gotten_command.time > command_some.time {
maybe_command = Some(gotten_command);
}
}
None => {
maybe_command = Some(gotten_command);
}
},
None => (),
}
}
None => (),
}
Ok(maybe_command)
}
}
#[cfg(feature = "devices")]
impl<E: Copy + Debug> Getter<TerminalData, E> for Terminal<'_, E> {
fn get(&self) -> Output<TerminalData, E> {
let command = self.get().expect("Terminal get cannot return Err");
let state = self.get().expect("Terminal get cannot return Err");
let (mut time, command) = match command {
Some(datum_command) => (Some(datum_command.time), Some(datum_command.value)),
None => (None, None),
};
let state = match state {
Some(datum_state) => {
time = Some(datum_state.time);
Some(datum_state.value)
}
None => None,
};
Ok(match time {
Some(time) => Some(Datum::new(
time,
TerminalData {
time: time,
command: command,
state: state,
},
)),
None => None,
})
}
}
#[cfg(feature = "devices")]
impl<E: Copy + Debug> Updatable<E> for Terminal<'_, E> {
fn update(&mut self) -> NothingOrError<E> {
<Terminal<'_, E> as Settable<Datum<Command>, E>>::update_following_data(self)?;
<Terminal<'_, E> as Settable<Datum<State>, E>>::update_following_data(self)?;
Ok(())
}
}
#[cfg(feature = "devices")]
pub fn connect<'a, E: Copy + Debug>(
term1: &'a RefCell<Terminal<'a, E>>,
term2: &'a RefCell<Terminal<'a, E>>,
) {
let mut term1_borrow = term1.borrow_mut();
let mut term2_borrow = term2.borrow_mut();
term1_borrow.disconnect();
term2_borrow.disconnect();
term1_borrow.other = Some(term2);
term2_borrow.other = Some(term1);
}
#[cfg(feature = "devices")]
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct TerminalData {
pub time: Time,
pub command: Option<Command>,
pub state: Option<State>,
}
#[cfg(feature = "devices")]
impl TryFrom<TerminalData> for Datum<Command> {
type Error = ();
fn try_from(value: TerminalData) -> Result<Datum<Command>, ()> {
match value.command {
Some(command) => Ok(Datum::new(value.time, command)),
None => Err(()),
}
}
}
#[cfg(feature = "devices")]
impl TryFrom<TerminalData> for Datum<State> {
type Error = ();
fn try_from(value: TerminalData) -> Result<Datum<State>, ()> {
match value.state {
Some(state) => Ok(Datum::new(value.time, state)),
None => Err(()),
}
}
}
#[cfg(feature = "devices")]
pub trait Device<E: Copy + Debug>: Updatable<E> {
fn update_terminals(&mut self) -> NothingOrError<E>;
}
pub fn latest<T>(dat1: Datum<T>, dat2: Datum<T>) -> Datum<T> {
if dat1.time >= dat2.time {
dat1
} else {
dat2
}
}