diff --git a/.github/workflows/publish.yml b/.github/workflows/publish.yml
new file mode 100644
index 0000000..381c2af
--- /dev/null
+++ b/.github/workflows/publish.yml
@@ -0,0 +1,29 @@
+name: Publish
+
+on:
+  push:
+    branches:
+      - master
+
+env:
+  CARGO_TERM_COLOR: always
+
+jobs:
+  publish:
+    runs-on: ubuntu-latest
+  
+    steps:
+    - uses: actions/checkout@v3
+
+    - name: Deps
+      run: |
+        sudo apt-get update
+        sudo apt install -y libudev-dev
+
+    - uses: actions-rs/toolchain@v1
+      with:
+          toolchain: stable
+          override: true
+    - uses: katyo/publish-crates@v1
+      with:
+          registry-token: ${{ secrets.CARGO_REGISTRY_TOKEN }}
\ No newline at end of file
diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml
new file mode 100644
index 0000000..32cff37
--- /dev/null
+++ b/.github/workflows/rust.yml
@@ -0,0 +1,33 @@
+name: Rust Lint and Test
+
+on: [push, pull_request]
+
+env:
+    CARGO_TERM_COLOR: always
+
+jobs:
+    build:
+        runs-on: ubuntu-latest
+
+        steps:
+        - uses: actions/checkout@v3
+
+        - name: Check
+          run: |
+            cargo check --all --tests
+        
+        - name: Build
+          run: |
+            cargo build
+        
+        - name: Test
+          run: |
+            cargo test
+        
+        - name: Format
+          run: |
+            cargo fmt --all -- --check
+                
+        - name: Lint
+          run: |
+            cargo clippy --all-targets --all-features -- -D warnings
diff --git a/Cargo.toml b/Cargo.toml
new file mode 100644
index 0000000..9f11fec
--- /dev/null
+++ b/Cargo.toml
@@ -0,0 +1,9 @@
+[package]
+name = "motor_toolbox_rs"
+version = "0.1.0"
+edition = "2021"
+
+# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+
+[dependencies]
+serde = { version = "1.0.183", features = ["derive"] }
diff --git a/src/coherency.rs b/src/coherency.rs
new file mode 100644
index 0000000..f200f8b
--- /dev/null
+++ b/src/coherency.rs
@@ -0,0 +1,39 @@
+use crate::motor_controller::Result;
+
+pub trait CoherentResult<T> {
+    fn coherent(self) -> Result<T>;
+}
+
+#[derive(Debug)]
+pub struct IncoherentError;
+impl std::fmt::Display for IncoherentError {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        write!(f, "(incoherent values)",)
+    }
+}
+impl std::error::Error for IncoherentError {}
+
+impl<T, U: Iterator<Item = Result<T>>> CoherentResult<T> for U
+where
+    T: Copy + Clone + PartialEq + std::fmt::Debug,
+{
+    fn coherent(self) -> Result<T> {
+        let mut iter = self;
+
+        if let Some(Ok(first)) = iter.next() {
+            for x in iter {
+                match x {
+                    Ok(x) => {
+                        if x != first {
+                            return Err(Box::new(IncoherentError));
+                        }
+                    }
+                    Err(e) => return Err(e),
+                }
+            }
+            Ok(first)
+        } else {
+            Err(Box::new(IncoherentError))
+        }
+    }
+}
diff --git a/src/fake_motor.rs b/src/fake_motor.rs
new file mode 100644
index 0000000..e40a48f
--- /dev/null
+++ b/src/fake_motor.rs
@@ -0,0 +1,162 @@
+use std::f64::{INFINITY, NAN};
+
+use crate::{MotorController, Result, PID};
+
+/// Fake motor implementation for testing purposes.
+pub struct FakeMotor {
+    torque_on: bool,
+
+    current_position: f64,
+    current_velocity: f64,
+    current_torque: f64,
+
+    target_position: f64,
+
+    velocity_limit: f64,
+    torque_limit: f64,
+    pid: PID,
+}
+
+impl Default for FakeMotor {
+    fn default() -> Self {
+        Self {
+            torque_on: false,
+
+            current_position: 0.0,
+            current_velocity: NAN,
+            current_torque: NAN,
+
+            target_position: 0.0,
+
+            velocity_limit: INFINITY,
+            torque_limit: INFINITY,
+            pid: PID {
+                p: NAN,
+                i: NAN,
+                d: NAN,
+            },
+        }
+    }
+}
+
+impl MotorController for FakeMotor {
+    fn name(&self) -> String {
+        "FakeMotor".to_string()
+    }
+
+    fn is_torque_on(&mut self) -> Result<bool> {
+        Ok(self.torque_on)
+    }
+
+    fn set_torque(&mut self, on: bool) -> Result<()> {
+        if on != self.torque_on {
+            self.current_position = self.target_position;
+        }
+        self.torque_on = on;
+        Ok(())
+    }
+
+    fn get_current_position(&mut self) -> Result<f64> {
+        Ok(self.current_position)
+    }
+
+    fn get_current_velocity(&mut self) -> Result<f64> {
+        Ok(self.current_velocity)
+    }
+
+    fn get_current_torque(&mut self) -> Result<f64> {
+        Ok(self.current_torque)
+    }
+
+    fn get_target_position(&mut self) -> Result<f64> {
+        Ok(self.target_position)
+    }
+
+    fn set_target_position(&mut self, position: f64) -> Result<()> {
+        self.target_position = position;
+
+        if self.torque_on {
+            self.current_position = position;
+        }
+
+        Ok(())
+    }
+
+    fn get_velocity_limit(&mut self) -> Result<f64> {
+        Ok(self.velocity_limit)
+    }
+
+    fn set_velocity_limit(&mut self, velocity: f64) -> Result<()> {
+        self.velocity_limit = velocity;
+        Ok(())
+    }
+
+    fn get_torque_limit(&mut self) -> Result<f64> {
+        Ok(self.torque_limit)
+    }
+
+    fn set_torque_limit(&mut self, torque: f64) -> Result<()> {
+        self.torque_limit = torque;
+        Ok(())
+    }
+
+    fn get_pid_gains(&mut self) -> Result<crate::PID> {
+        Ok(self.pid)
+    }
+
+    fn set_pid_gains(&mut self, pid: crate::PID) -> Result<()> {
+        self.pid = pid;
+        Ok(())
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::{
+        FakeMotor, MotorController, MultipleMotorsController, MultipleMotorsControllerWrapper,
+    };
+
+    #[test]
+    fn check_default() {
+        let mut motor: FakeMotor = FakeMotor::default();
+
+        assert!(!motor.is_torque_on().unwrap());
+        assert_eq!(motor.get_current_position().unwrap(), 0.0);
+        assert_eq!(motor.get_target_position().unwrap(), 0.0);
+    }
+
+    #[test]
+    fn set_target() {
+        let mut motor: FakeMotor = FakeMotor::default();
+
+        // With torque off, the current position should not change
+        motor.set_target_position(0.5).unwrap();
+        assert_eq!(motor.get_target_position().unwrap(), 0.5);
+        assert_eq!(motor.get_current_position().unwrap(), 0.0);
+
+        // Enabling the torque, and reset the target position
+        motor.enable_torque(true).unwrap();
+        assert!(motor.is_torque_on().unwrap());
+        assert_eq!(motor.get_current_position().unwrap(), 0.0);
+        assert_eq!(motor.get_target_position().unwrap(), 0.0);
+
+        // Setting the target position
+        motor.set_target_position(0.5).unwrap();
+        assert_eq!(motor.get_target_position().unwrap(), 0.5);
+        assert_eq!(motor.get_current_position().unwrap(), 0.5);
+    }
+
+    #[test]
+    fn multiple_fake() {
+        let motor1 = Box::<FakeMotor>::default();
+        let motor2 = Box::<FakeMotor>::default();
+        let motor3 = Box::<FakeMotor>::default();
+
+        let controllers: [Box<dyn MotorController>; 3] = [motor1, motor2, motor3];
+
+        let mut wrapper = MultipleMotorsControllerWrapper::new(controllers);
+
+        wrapper.set_torque(true).unwrap();
+        assert!(wrapper.is_torque_on().unwrap());
+    }
+}
diff --git a/src/lib.rs b/src/lib.rs
new file mode 100644
index 0000000..e15ade1
--- /dev/null
+++ b/src/lib.rs
@@ -0,0 +1,16 @@
+mod coherency;
+
+mod fake_motor;
+pub use fake_motor::FakeMotor;
+
+mod limit;
+pub use limit::Limit;
+
+mod motor_controller;
+pub use motor_controller::{MissingResisterErrror, MotorController, Result};
+
+mod multiple_motors_controller;
+pub use multiple_motors_controller::{MultipleMotorsController, MultipleMotorsControllerWrapper};
+
+mod pid;
+pub use pid::PID;
diff --git a/src/limit.rs b/src/limit.rs
new file mode 100644
index 0000000..c84c39c
--- /dev/null
+++ b/src/limit.rs
@@ -0,0 +1,23 @@
+use serde::{Deserialize, Serialize};
+
+#[derive(Debug, Deserialize, Serialize)]
+/// Limit wrapper
+pub struct Limit<T>
+where
+    T: Copy + Ord,
+{
+    /// lower limit
+    pub min: T,
+    /// upper limit
+    pub max: T,
+}
+
+impl<T> Limit<T>
+where
+    T: Copy + Ord,
+{
+    /// Clamp value to limits
+    pub fn clamp(&self, value: T) -> T {
+        value.clamp(self.min, self.max)
+    }
+}
diff --git a/src/motor_controller.rs b/src/motor_controller.rs
new file mode 100644
index 0000000..1db112e
--- /dev/null
+++ b/src/motor_controller.rs
@@ -0,0 +1,78 @@
+use crate::PID;
+
+/// Result generic wrapper using `std::error::Error` trait
+pub type Result<T> = std::result::Result<T, Box<dyn std::error::Error>>;
+
+/// Low level motor controller interface
+pub trait MotorController {
+    /// Name of the controller (used for Debug trait)
+    fn name(&self) -> String;
+
+    /// Check if the motor is ON or OFF
+    fn is_torque_on(&mut self) -> Result<bool>;
+    /// Enable/Disable the torque
+    fn set_torque(&mut self, on: bool) -> Result<()>;
+    /// Enable the torque
+    ///
+    /// # Arguments
+    /// * `reset_target` - If true, reset the target position to the current position
+    fn enable_torque(&mut self, reset_target: bool) -> Result<()> {
+        if reset_target {
+            let position = self.get_current_position()?;
+            self.set_target_position(position)?;
+        }
+
+        self.set_torque(true)?;
+
+        Ok(())
+    }
+    /// Disable the torque
+    fn disable_torque(&mut self) -> Result<()> {
+        self.set_torque(false)
+    }
+
+    /// Get the current position of the motor (in radians)
+    fn get_current_position(&mut self) -> Result<f64>;
+    /// Get the current velocity of the motor (in radians per second)
+    fn get_current_velocity(&mut self) -> Result<f64>;
+    /// Get the current torque of the motor (in Nm)
+    fn get_current_torque(&mut self) -> Result<f64>;
+
+    /// Get the current target position of the motor (in radians)
+    fn get_target_position(&mut self) -> Result<f64>;
+    /// Set the current target position of the motor (in radians)
+    fn set_target_position(&mut self, position: f64) -> Result<()>;
+
+    /// Get the velocity limit of the motor (in radians per second)
+    fn get_velocity_limit(&mut self) -> Result<f64>;
+    /// Set the velocity limit of the motor (in radians per second)
+    fn set_velocity_limit(&mut self, velocity: f64) -> Result<()>;
+
+    /// Get the torque limit of the motor (in Nm)
+    fn get_torque_limit(&mut self) -> Result<f64>;
+    /// Set the torque limit of the motor (in Nm)
+    fn set_torque_limit(&mut self, torque: f64) -> Result<()>;
+
+    /// Get the current PID gains of the motor
+    fn get_pid_gains(&mut self) -> Result<PID>;
+    /// Set the current PID gains of the motor
+    fn set_pid_gains(&mut self, pid: PID) -> Result<()>;
+}
+
+#[derive(Debug)]
+pub struct MissingResisterErrror(pub String);
+impl std::fmt::Display for MissingResisterErrror {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        let name = &self.0;
+        write!(f, "(missing register \"{name}\")",)
+    }
+}
+impl std::error::Error for MissingResisterErrror {}
+
+impl std::fmt::Debug for dyn MotorController {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        f.debug_struct("MotorController")
+            .field("name", &self.name())
+            .finish()
+    }
+}
diff --git a/src/multiple_motors_controller.rs b/src/multiple_motors_controller.rs
new file mode 100644
index 0000000..21992d7
--- /dev/null
+++ b/src/multiple_motors_controller.rs
@@ -0,0 +1,219 @@
+use crate::coherency::IncoherentError;
+use crate::{motor_controller::Result, MotorController, PID};
+
+pub trait MultipleMotorsController<const N: usize> {
+    /// Name of the controller (used for Debug trait)
+    fn name(&self) -> String;
+
+    /// Check if the motor is ON or OFF
+    fn is_torque_on(&mut self) -> Result<bool>;
+    /// Enable/Disable the torque
+    fn set_torque(&mut self, on: bool) -> Result<()>;
+    /// Enable the torque
+    ///
+    /// # Arguments
+    /// * `reset_target` - If true, reset the target position to the current position
+    fn enable_torque(&mut self, reset_target: bool) -> Result<()> {
+        if reset_target {
+            let position = self.get_current_position()?;
+            self.set_target_position(position)?;
+        }
+
+        self.set_torque(true)?;
+
+        Ok(())
+    }
+    /// Disable the torque
+    fn disable_torque(&mut self) -> Result<()> {
+        self.set_torque(false)
+    }
+
+    /// Get the current position of the motor (in radians)
+    fn get_current_position(&mut self) -> Result<[f64; N]>;
+    /// Get the current velocity of the motor (in radians per second)
+    fn get_current_velocity(&mut self) -> Result<[f64; N]>;
+    /// Get the current torque of the motor (in Nm)
+    fn get_current_torque(&mut self) -> Result<[f64; N]>;
+
+    /// Get the current target position of the motor (in radians)
+    fn get_target_position(&mut self) -> Result<[f64; N]>;
+    /// Set the current target position of the motor (in radians)
+    fn set_target_position(&mut self, position: [f64; N]) -> Result<()>;
+
+    /// Get the velocity limit of the motor (in radians per second)
+    fn get_velocity_limit(&mut self) -> Result<[f64; N]>;
+    /// Set the velocity limit of the motor (in radians per second)
+    fn set_velocity_limit(&mut self, velocity: [f64; N]) -> Result<()>;
+
+    /// Get the torque limit of the motor (in Nm)
+    fn get_torque_limit(&mut self) -> Result<[f64; N]>;
+    /// Set the torque limit of the motor (in Nm)
+    fn set_torque_limit(&mut self, torque: [f64; N]) -> Result<()>;
+
+    /// Get the current PID gains of the motor
+    fn get_pid_gains(&mut self) -> Result<[PID; N]>;
+    /// Set the current PID gains of the motor
+    fn set_pid_gains(&mut self, pid: [PID; N]) -> Result<()>;
+}
+
+impl<const N: usize> std::fmt::Debug for dyn MultipleMotorsController<N> {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        f.debug_struct("MultipleMotorsController")
+            .field("name", &self.name())
+            .finish()
+    }
+}
+
+#[derive(Debug)]
+pub struct MultipleMotorsControllerWrapper<const N: usize> {
+    controllers: [Box<dyn MotorController>; N],
+}
+
+impl<const N: usize> MultipleMotorsControllerWrapper<N> {
+    pub fn new(controllers: [Box<dyn MotorController>; N]) -> Self {
+        Self { controllers }
+    }
+}
+
+impl<const N: usize> MultipleMotorsController<N> for MultipleMotorsControllerWrapper<N> {
+    fn name(&self) -> String {
+        "MultipleMotorsController".to_string()
+    }
+
+    fn is_torque_on(&mut self) -> Result<bool> {
+        let mut torques = vec![];
+        for c in self.controllers.iter_mut() {
+            match c.is_torque_on() {
+                Ok(p) => torques.push(p),
+                Err(e) => return Err(e),
+            }
+        }
+
+        let torques: [bool; 3] = torques.try_into().unwrap();
+
+        if torques[0] == torques[1] && torques[1] == torques[2] {
+            Ok(torques[0])
+        } else {
+            Err(Box::new(IncoherentError {}))
+        }
+    }
+
+    fn set_torque(&mut self, on: bool) -> Result<()> {
+        for c in self.controllers.iter_mut() {
+            c.set_torque(on)?;
+        }
+        Ok(())
+    }
+
+    fn get_current_position(&mut self) -> Result<[f64; N]> {
+        let mut pos = vec![];
+        for c in self.controllers.iter_mut() {
+            match c.get_current_position() {
+                Ok(p) => pos.push(p),
+                Err(e) => return Err(e),
+            }
+        }
+
+        Ok(pos.try_into().unwrap())
+    }
+
+    fn get_current_velocity(&mut self) -> Result<[f64; N]> {
+        let mut vel = vec![];
+        for c in self.controllers.iter_mut() {
+            match c.get_current_velocity() {
+                Ok(v) => vel.push(v),
+                Err(e) => return Err(e),
+            }
+        }
+
+        Ok(vel.try_into().unwrap())
+    }
+
+    fn get_current_torque(&mut self) -> Result<[f64; N]> {
+        let mut torque = vec![];
+        for c in self.controllers.iter_mut() {
+            match c.get_current_torque() {
+                Ok(t) => torque.push(t),
+                Err(e) => return Err(e),
+            }
+        }
+
+        Ok(torque.try_into().unwrap())
+    }
+
+    fn get_target_position(&mut self) -> Result<[f64; N]> {
+        let mut pos = vec![];
+        for c in self.controllers.iter_mut() {
+            match c.get_target_position() {
+                Ok(p) => pos.push(p),
+                Err(e) => return Err(e),
+            }
+        }
+
+        Ok(pos.try_into().unwrap())
+    }
+
+    fn set_target_position(&mut self, position: [f64; N]) -> Result<()> {
+        for (c, p) in self.controllers.iter_mut().zip(position.iter()) {
+            c.set_target_position(*p)?;
+        }
+        Ok(())
+    }
+
+    fn get_velocity_limit(&mut self) -> Result<[f64; N]> {
+        let mut vel = vec![];
+        for c in self.controllers.iter_mut() {
+            match c.get_velocity_limit() {
+                Ok(v) => vel.push(v),
+                Err(e) => return Err(e),
+            }
+        }
+
+        Ok(vel.try_into().unwrap())
+    }
+
+    fn set_velocity_limit(&mut self, velocity: [f64; N]) -> Result<()> {
+        for (c, v) in self.controllers.iter_mut().zip(velocity.iter()) {
+            c.set_velocity_limit(*v)?;
+        }
+        Ok(())
+    }
+
+    fn get_torque_limit(&mut self) -> Result<[f64; N]> {
+        let mut torque = vec![];
+        for c in self.controllers.iter_mut() {
+            match c.get_torque_limit() {
+                Ok(t) => torque.push(t),
+                Err(e) => return Err(e),
+            }
+        }
+
+        Ok(torque.try_into().unwrap())
+    }
+
+    fn set_torque_limit(&mut self, torque: [f64; N]) -> Result<()> {
+        for (c, t) in self.controllers.iter_mut().zip(torque.iter()) {
+            c.set_torque_limit(*t)?;
+        }
+        Ok(())
+    }
+
+    fn get_pid_gains(&mut self) -> Result<[PID; N]> {
+        let mut pid_gains = vec![];
+        for c in self.controllers.iter_mut() {
+            match c.get_pid_gains() {
+                Ok(p) => pid_gains.push(p),
+                Err(e) => return Err(e),
+            }
+        }
+
+        Ok(pid_gains.try_into().unwrap())
+    }
+
+    fn set_pid_gains(&mut self, pid: [PID; N]) -> Result<()> {
+        for (c, p) in self.controllers.iter_mut().zip(pid.iter()) {
+            c.set_pid_gains(*p)?;
+        }
+        Ok(())
+    }
+}
diff --git a/src/pid.rs b/src/pid.rs
new file mode 100644
index 0000000..223b958
--- /dev/null
+++ b/src/pid.rs
@@ -0,0 +1,10 @@
+#[derive(Clone, Copy, Debug, PartialEq, PartialOrd)]
+/// PID gains wrapper
+pub struct PID {
+    /// Propotional gain
+    pub p: f64,
+    /// Integral gain
+    pub i: f64,
+    /// Derivative gain
+    pub d: f64,
+}