diff --git a/Cargo.toml b/Cargo.toml
index 2636469..7292d7d 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -1,6 +1,6 @@
 [package]
 name = "rustypot"
-version = "0.4.2"
+version = "0.5.0"
 edition = "2021"
 license = "Apache-2.0"
 authors = ["Pollen Robotics"]
@@ -19,6 +19,7 @@ log = "0.4.17"
 paste = "1.0.10"
 serialport = "4.2.0"
 clap = { version = "4.0.32", features = ["derive"] }
+proc-macro2 = { version = "=1.0.67", features=["default", "proc-macro"] }
 
 [dev-dependencies]
 env_logger = "0.10.0"
diff --git a/Changelog.md b/Changelog.md
index 6486da7..f2eae5c 100644
--- a/Changelog.md
+++ b/Changelog.md
@@ -1,3 +1,7 @@
+## Version 0.5.0
+
+- Add an post delay option to the read and write method.
+
 ## Version 0.4.0
 
 - Add support for orbita-2dof-foc device.
diff --git a/examples/foc.rs b/examples/foc.rs
index 789b6fd..b7ecb0f 100644
--- a/examples/foc.rs
+++ b/examples/foc.rs
@@ -10,39 +10,67 @@ fn main() -> Result<(), Box<dyn Error>> {
         .timeout(Duration::from_millis(100))
         .open()?;
 
-    let io = DynamixelSerialIO::v1();
+    let io = DynamixelSerialIO::v1().with_post_delay(Duration::from_millis(1));
 
     let id = 70;
-    orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 0)?;
+    let mut state = orbita_foc::read_motors_drivers_states(&io, serial_port.as_mut(), id)?;
+    println!("state {:#010b}", state);
+    println!("state {:?}", state);
+
+    let fw = orbita_foc::read_firmware_version(&io, serial_port.as_mut(), id)?;
+    println!("Firmware version {:?}", fw);
+
+    orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 1)?;
+    thread::sleep(Duration::from_millis(1000));
+    // orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 0)?;
 
-    thread::sleep(Duration::from_millis(100));
+    // thread::sleep(Duration::from_millis(1000));
+    // orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 1)?;
 
+    // thread::sleep(Duration::from_millis(1000));
+
+    state = orbita_foc::read_motors_drivers_states(&io, serial_port.as_mut(), id)?;
+    println!("state {:#010b}", state);
     let now = SystemTime::now();
     // let x = io.ping(serial_port.as_mut(), id);
     // println!("{:?}", x);
-    loop {
+    let mut total = 0.0;
+
+    while total < 10.0 {
         // let x = io.ping(serial_port.as_mut(), id);
         // println!("{:?}", x);
 
         let pos = orbita_foc::read_present_position(&io, serial_port.as_mut(), id)?;
-        println!("{:?}", pos);
+
+        let tophall = orbita_foc::read_top_present_hall(&io, serial_port.as_mut(), id)?;
+        let midhall = orbita_foc::read_middle_present_hall(&io, serial_port.as_mut(), id)?;
+        let bothall = orbita_foc::read_bottom_present_hall(&io, serial_port.as_mut(), id)?;
+        println!(
+            "{:?} tophall: {:?} midhal: {:?} bothall: {:?}",
+            pos, tophall, midhall, bothall
+        );
 
         let t = now.elapsed().unwrap().as_secs_f32();
-        let target = 60.0_f32 * (2.0 * PI * 0.5 * t).sin();
-        orbita_foc::write_top_goal_position(&io, serial_port.as_mut(), id, target)?;
-        // println!("{}", t);
+        let target = 10.0_f32 * (2.0 * PI * 0.25 * t).sin();
 
         orbita_foc::write_goal_position(
             &io,
             serial_port.as_mut(),
             id,
             DiskValue {
-                top: target,
-                middle: target,
-                bottom: target,
+                top: target.to_radians() + pos.top,
+                middle: target.to_radians() + pos.middle,
+                bottom: target.to_radians() + pos.bottom,
+                // top: pos.top,
+                // middle: pos.middle,
+                // bottom: pos.bottom,
             },
         )?;
-
         thread::sleep(Duration::from_millis(10));
+        total += 0.01;
     }
+    orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 0)?;
+
+    thread::sleep(Duration::from_millis(1000));
+    Ok(())
 }
diff --git a/examples/poulpe2d.rs b/examples/poulpe2d.rs
new file mode 100644
index 0000000..946060d
--- /dev/null
+++ b/examples/poulpe2d.rs
@@ -0,0 +1,166 @@
+use std::time::SystemTime;
+use std::{error::Error, thread, time::Duration, time::Instant};
+
+use rustypot::device::orbita2d_poulpe::{self, MotorValue};
+// use rustypot::device::orbita3d_poulpe::{self, MotorValue};
+use rustypot::DynamixelSerialIO;
+
+use clap::Parser;
+
+#[derive(Parser, Debug)]
+#[command(author, version, about, long_about = None)]
+struct Args {
+    /// tty
+    #[arg(short, long, default_value = "/dev/ttyUSB0")]
+    serialport: String,
+    /// baud
+    #[arg(short, long, default_value_t = 2_000_000)]
+    baudrate: u32,
+
+    /// id
+    #[arg(short, long, default_value_t = 43)]
+    id: u8,
+
+    ///sinus amplitude (f64)
+    #[arg(short, long, default_value_t = 1.0)]
+    amplitude: f32,
+
+    ///sinus frequency (f64)
+    #[arg(short, long, default_value_t = 1.0)]
+    frequency: f32,
+}
+
+fn main() -> Result<(), Box<dyn Error>> {
+    let args = Args::parse();
+    let serialportname: String = args.serialport;
+    let baudrate: u32 = args.baudrate;
+    let id: u8 = args.id;
+    let amplitude: f32 = args.amplitude;
+    let frequency: f32 = args.frequency;
+
+    //print the standard ids for the arm motors
+
+    //print all the argument values
+    println!("serialport: {}", serialportname);
+    println!("baudrate: {}", baudrate);
+    println!("id: {}", id);
+    println!("amplitude: {}", amplitude);
+    println!("frequency: {}", frequency);
+
+    let mut serial_port = serialport::new(serialportname, baudrate)
+        .timeout(Duration::from_millis(10))
+        .open()?;
+
+    let now = SystemTime::now();
+
+    let io = DynamixelSerialIO::v1();
+
+    // Ping
+    let x = io.ping(serial_port.as_mut(), id);
+    println!("Ping {:?}: {:?}", id, x);
+    thread::sleep(Duration::from_millis(100));
+
+    let _ = orbita2d_poulpe::write_torque_enable(
+        &io,
+        serial_port.as_mut(),
+        id,
+        MotorValue::<bool> {
+            motor_a: true,
+            motor_b: true,
+        },
+    )?;
+    thread::sleep(Duration::from_millis(1000));
+    // let torque = orbita3d_poulpe::read_torque_enable(&io, serial_port.as_mut(), id)?;
+    // println!("torque: {:?}", torque);
+    // thread::sleep(Duration::from_millis(1000));
+
+    let curr_pos = orbita2d_poulpe::read_current_position(&io, serial_port.as_mut(), id)?;
+
+    println!("curr_pos: {:?} {:?}", curr_pos.motor_a, curr_pos.motor_b);
+
+    // let index_sensor = orbita3d_poulpe::read_index_sensor(&io, serial_port.as_mut(), id)?;
+    // println!("index_sensor: {:?} {:?} {:?}", index_sensor.top, index_sensor.middle, index_sensor.bottom);
+
+    let mut t = now.elapsed().unwrap().as_secs_f32();
+    let mut nberr = 0;
+    let mut nbtot = 0;
+    let mut nbok = 0;
+    let mut target = 0.0;
+    loop {
+        let t0 = Instant::now();
+
+        if t > 3.0 {
+            break;
+        }
+
+        t = now.elapsed().unwrap().as_secs_f32();
+        // let target = amplitude * 180.0_f32.to_radians() * (2.0 * PI * 0.5 * t).sin();
+        target += 0.001;
+        // let feedback = orbita2d_poulpe::write_target_position(&io, serial_port.as_mut(), id, MotorValue::<f32>{motor_a:target+curr_pos.motor_a, motor_b:target+curr_pos.motor_b});
+
+        // let feedback = orbita2d_poulpe::write_target_position(
+        //     &io,
+        //     serial_port.as_mut(),
+        //     id,
+        //     MotorValue::<f32> {
+        //         motor_a: target + curr_pos.motor_a,
+        //         motor_b: curr_pos.motor_b,
+        //     },
+        // );
+
+        let feedback = orbita2d_poulpe::write_target_position(
+            &io,
+            serial_port.as_mut(),
+            id,
+            MotorValue::<f32> {
+                motor_a: target + curr_pos.motor_a,
+                motor_b: target + curr_pos.motor_b,
+            },
+        );
+
+        match feedback {
+            Ok(feedback) => {
+                nbok += 1;
+                println!(
+                    "42 target: {} feedback pos: {} {}",
+                    target, feedback.position.motor_a, feedback.position.motor_b,
+                );
+            }
+            Err(e) => {
+                nberr += 1;
+                println!("error: {:?}", e);
+            }
+        }
+
+        nbtot += 1;
+
+        // thread::sleep(Duration::from_micros(1000-t0.elapsed().as_micros() as u64));
+        // thread::sleep(Duration::from_millis(1));
+        thread::sleep(Duration::from_micros(500));
+        // thread::sleep(Duration::from_micros(4500));
+        println!("ELAPSED: {:?}", t0.elapsed().as_micros());
+    }
+
+    println!(
+        "nberr: {} nbtot: {} nbok: {} ratio: {:?}",
+        nberr,
+        nbtot,
+        nbok,
+        nbok as f32 / nbtot as f32
+    );
+
+    println!("STOP");
+    let _ = orbita2d_poulpe::write_torque_enable(
+        &io,
+        serial_port.as_mut(),
+        id,
+        MotorValue::<bool> {
+            motor_a: false,
+            motor_b: false,
+        },
+    )?;
+
+    thread::sleep(Duration::from_millis(2000));
+
+    Ok(())
+}
diff --git a/examples/poulpe3d.rs b/examples/poulpe3d.rs
new file mode 100644
index 0000000..ea84e3d
--- /dev/null
+++ b/examples/poulpe3d.rs
@@ -0,0 +1,226 @@
+use std::f32::consts::PI;
+use std::time::SystemTime;
+use std::{error::Error, thread, time::Duration, time::Instant};
+
+use rustypot::device::orbita3d_poulpe::{self, MotorValue};
+use rustypot::DynamixelSerialIO;
+
+use clap::Parser;
+
+#[derive(Parser, Debug)]
+#[command(author, version, about, long_about = None)]
+struct Args {
+    /// tty
+    #[arg(short, long, default_value = "/dev/ttyUSB0")]
+    serialport: String,
+    /// baud
+    #[arg(short, long, default_value_t = 2_000_000)]
+    baudrate: u32,
+
+    /// id
+    #[arg(short, long, default_value_t = 42)]
+    id: u8,
+
+    ///sinus amplitude (f64)
+    #[arg(short, long, default_value_t = 10.0)]
+    amplitude: f32,
+
+    ///sinus frequency (f64)
+    #[arg(short, long, default_value_t = 1.0)]
+    frequency: f32,
+}
+
+fn main() -> Result<(), Box<dyn Error>> {
+    let args = Args::parse();
+    let serialportname: String = args.serialport;
+    let baudrate: u32 = args.baudrate;
+    let id: u8 = args.id;
+    let amplitude: f32 = args.amplitude;
+    let frequency: f32 = args.frequency;
+
+    //print the standard ids for the arm motors
+
+    //print all the argument values
+    println!("serialport: {}", serialportname);
+    println!("baudrate: {}", baudrate);
+    println!("id: {}", id);
+    println!("amplitude: {}", amplitude);
+    println!("frequency: {}", frequency);
+
+    let mut serial_port = serialport::new(serialportname, baudrate)
+        .timeout(Duration::from_millis(10))
+        .open()?;
+
+    let now = SystemTime::now();
+
+    let io = DynamixelSerialIO::v1();
+
+    // Ping
+    let x = io.ping(serial_port.as_mut(), id);
+    println!("Ping {:?}: {:?}", id, x);
+
+    let _ = orbita3d_poulpe::write_torque_enable(
+        &io,
+        serial_port.as_mut(),
+        id,
+        MotorValue::<bool> {
+            top: true,
+            middle: true,
+            bottom: true,
+        },
+    )?;
+    // let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::<bool>{top:false, middle:false, bottom:false})?;
+    thread::sleep(Duration::from_millis(1000));
+    // let torque = orbita3d_poulpe::read_torque_enable(&io, serial_port.as_mut(), id)?;
+    // println!("torque: {:?}", torque);
+    // thread::sleep(Duration::from_millis(1000));
+
+    let curr_pos = orbita3d_poulpe::read_current_position(&io, serial_port.as_mut(), id)?;
+
+    // let index_sensor = orbita3d_poulpe::read_index_sensor(&io, serial_port.as_mut(), id)?;
+    // println!("index_sensor: {:?} {:?} {:?}", index_sensor.top, index_sensor.middle, index_sensor.bottom);
+
+    let limit = orbita3d_poulpe::read_velocity_limit(&io, serial_port.as_mut(), id)?;
+    println!(
+        "vel_limit: {:?} {:?} {:?}",
+        limit.top, limit.middle, limit.bottom
+    );
+    let limit = orbita3d_poulpe::read_torque_flux_limit(&io, serial_port.as_mut(), id)?;
+    println!(
+        "tf_limit: {:?} {:?} {:?}",
+        limit.top, limit.middle, limit.bottom
+    );
+    let limit = orbita3d_poulpe::read_uq_ud_limit(&io, serial_port.as_mut(), id)?;
+    println!(
+        "uq_ud_limit: {:?} {:?} {:?}",
+        limit.top, limit.middle, limit.bottom
+    );
+
+    let pid = orbita3d_poulpe::read_flux_pid(&io, serial_port.as_mut(), id)?;
+    println!("flux_pid: {:?} {:?} {:?}", pid.top, pid.middle, pid.bottom);
+    let pid = orbita3d_poulpe::read_torque_pid(&io, serial_port.as_mut(), id)?;
+    println!(
+        "torque_pid: {:?} {:?} {:?}",
+        pid.top, pid.middle, pid.bottom
+    );
+    let pid = orbita3d_poulpe::read_velocity_pid(&io, serial_port.as_mut(), id)?;
+    println!(
+        "velocity_pid: {:?} {:?} {:?}",
+        pid.top, pid.middle, pid.bottom
+    );
+    let pid = orbita3d_poulpe::read_position_pid(&io, serial_port.as_mut(), id)?;
+    println!(
+        "position_pid: {:?} {:?} {:?}",
+        pid.top, pid.middle, pid.bottom
+    );
+
+    let mut t = now.elapsed().unwrap().as_secs_f32();
+    let mut nberr = 0;
+    let mut nbtot = 0;
+    let mut nbok = 0;
+
+    loop {
+        let t0 = Instant::now();
+
+        if t > 100.0 {
+            break;
+        }
+
+        t = now.elapsed().unwrap().as_secs_f32();
+        let target = amplitude * 180.0_f32.to_radians() * (2.0 * PI * 0.5 * t).sin();
+
+        let feedback = orbita3d_poulpe::write_target_position(
+            &io,
+            serial_port.as_mut(),
+            id,
+            MotorValue::<f32> {
+                top: target + curr_pos.top,
+                middle: target + curr_pos.middle,
+                bottom: target + curr_pos.bottom,
+            },
+        );
+        match feedback {
+            Ok(feedback) => {
+                nbok += 1;
+                println!(
+                    "42 target: {} feedback pos: {} {} {}",
+                    target,
+                    feedback.position.top,
+                    feedback.position.middle,
+                    feedback.position.bottom,
+                );
+            }
+            Err(e) => {
+                nberr += 1;
+                println!("error: {:?}", e);
+            }
+        }
+
+        // thread::sleep(Duration::from_micros(500));
+        // let feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), 43, MotorValue::<f32>{top:0.0, middle:0.0, bottom:0.0});
+        // // let feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), 43, MotorValue::<f32>{top:0.0, middle:0.0, bottom:0.0}).unwrap_or_else(MotorPositionSpeedLoad::{position:MotorValue::<f32>{top:0.0, middle:0.0, bottom:0.0}, speed:MotorValue::<f32>{top:0.0, middle:0.0, bottom:0.0}, load:MotorValue::<f32>{top:0.0, middle:0.0, bottom:0.0}});
+        // match feedback {
+        //     Ok(feedback) => {
+        // 	nbok+=1;
+        // 	println!("43 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom);
+        // 		    },
+        //     Err(e) => {
+        // 	nberr+=1;
+        // 	println!("error: {:?}", e);
+        //     }
+        // }
+
+        // println!("43 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom);
+
+        // thread::sleep(Duration::from_micros(500));
+        // let feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), 44, MotorValue::<f32>{top:0.0, middle:0.0, bottom:0.0});
+        // // let feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), 44, MotorValue::<f32>{top:0.0, middle:0.0, bottom:0.0}).unwrap_or_else(MotorPositionSpeedLoad::{position:MotorValue::<f32>{top:0.0, middle:0.0, bottom:0.0}, speed:MotorValue::<f32>{top:0.0, middle:0.0, bottom:0.0}, load:MotorValue::<f32>{top:0.0, middle:0.0, bottom:0.0}});
+        // match feedback {
+        //     Ok(feedback) => {
+        // 	nbok+=1;
+        // 	println!("44 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom);
+        // 		    },
+        //     Err(e) => {
+        // 	nberr+=1;
+        // 	println!("error: {:?}", e);
+        //     }
+        // }
+
+        // nbtot+=3;
+        nbtot += 1;
+        // println!("44 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom);
+
+        // let axis_sensor = orbita3d_poulpe::read_axis_sensor(&io, serial_port.as_mut(), id)?;
+        // println!("axis_sensor: {:6.2} {:6.2} {:6.2}", axis_sensor.top, axis_sensor.middle, axis_sensor.bottom);
+
+        // thread::sleep(Duration::from_micros(1000-t0.elapsed().as_micros() as u64));
+        // thread::sleep(Duration::from_millis(1));
+        thread::sleep(Duration::from_micros(500));
+        // thread::sleep(Duration::from_micros(4500));
+        println!("ELAPSED: {:?}", t0.elapsed().as_micros());
+    }
+
+    println!(
+        "nberr: {} nbtot: {} nbok: {} ratio: {:?}",
+        nberr,
+        nbtot,
+        nbok,
+        nbok as f32 / nbtot as f32
+    );
+
+    println!("STOP");
+    let _ = orbita3d_poulpe::write_torque_enable(
+        &io,
+        serial_port.as_mut(),
+        id,
+        MotorValue::<bool> {
+            top: false,
+            middle: false,
+            bottom: false,
+        },
+    )?;
+
+    thread::sleep(Duration::from_millis(2000));
+
+    Ok(())
+}
diff --git a/src/bin/dxl_scan.rs b/src/bin/dxl_scan.rs
index baaf3d9..23bc5eb 100644
--- a/src/bin/dxl_scan.rs
+++ b/src/bin/dxl_scan.rs
@@ -20,7 +20,7 @@ fn main() -> Result<(), Box<dyn Error>> {
     let args = Args::parse();
 
     println!("Scanning...");
-    let mut serial_port = serialport::new(args.serial_port, 1_000_000)
+    let mut serial_port = serialport::new(args.serial_port, 2_000_000)
         .timeout(Duration::from_millis(10))
         .open()?;
 
diff --git a/src/device/mod.rs b/src/device/mod.rs
index 1187d35..191cfb1 100644
--- a/src/device/mod.rs
+++ b/src/device/mod.rs
@@ -18,6 +18,7 @@ macro_rules! reg_read_only {
             id: u8,
         ) -> Result<$reg_type> {
             let val = io.read(serial_port, id, $addr, size_of::<$reg_type>().try_into().unwrap())?;
+            check_response_size!(&val, $reg_type);
             let val = $reg_type::from_le_bytes(val.try_into().unwrap());
 
             Ok(val)
@@ -30,6 +31,7 @@ macro_rules! reg_read_only {
             ids: &[u8],
         ) -> Result<Vec<$reg_type>> {
             let val = io.sync_read(serial_port, ids, $addr, size_of::<$reg_type>().try_into().unwrap())?;
+            check_response_size!(&val, $reg_type);
             let val = val
                 .iter()
                 .map(|v| $reg_type::from_le_bytes(v.as_slice().try_into().unwrap()))
@@ -78,6 +80,45 @@ macro_rules! reg_write_only {
     };
 }
 
+/// Generates write and sync_write functions with feedback for given register
+#[macro_export]
+macro_rules! reg_write_only_fb {
+    ($name:ident, $addr:expr, $reg_type:ty, $fb_type: ty) => {
+        paste! {
+            #[doc = concat!("Write register with fb *", stringify!($name), "* (addr: ", stringify!($addr), ", type: ", stringify!($reg_type), ")")]
+            pub fn [<write_ $name>](
+                io: &DynamixelSerialIO,
+                serial_port: &mut dyn serialport::SerialPort,
+                id: u8,
+                val: $reg_type,
+            ) -> Result<$fb_type> {
+                let fb=io.write_fb(serial_port, id, $addr, &val.to_le_bytes())?;
+                check_response_size!(&fb, $fb_type);
+                let fb = $fb_type::from_le_bytes(fb.try_into().unwrap());
+                Ok(fb)
+            }
+
+            #[doc = concat!("Sync write register *", stringify!($name), "* (addr: ", stringify!($addr), ", type: ", stringify!($reg_type), ")")]
+            pub fn [<sync_write_ $name>](
+                io: &DynamixelSerialIO,
+                serial_port: &mut dyn serialport::SerialPort,
+                ids: &[u8],
+                values: &[$reg_type],
+            ) -> Result<()> {
+                io.sync_write(
+                    serial_port,
+                    ids,
+                    $addr,
+                    &values
+                        .iter()
+                        .map(|v| v.to_le_bytes().to_vec())
+                        .collect::<Vec<Vec<u8>>>(),
+                )
+            }
+        }
+    };
+}
+
 /// Generates read, sync_read, write and sync_write functions for given register
 #[macro_export]
 macro_rules! reg_read_write {
@@ -87,9 +128,37 @@ macro_rules! reg_read_write {
     };
 }
 
+#[macro_export]
+macro_rules! reg_read_write_fb {
+    ($name:ident, $addr:expr, $reg_type:ty, $fb_type: ty) => {
+        reg_read_only!($name, $addr, $reg_type);
+        reg_write_only_fb!($name, $addr, $reg_type, $fb_type);
+    };
+}
+
+// Check if the response size is correct
+// If not, return an error
+// response is a Vec<u8>
+macro_rules! check_response_size {
+    ($response:expr, $reg_type:ty) => {{
+        let response = $response;
+        if response.len() != std::mem::size_of::<$reg_type>() {
+            let message = format!(
+                "Invalid response size, expected {} received {}",
+                std::mem::size_of::<$reg_type>(),
+                response.len()
+            );
+            return Err(message.into());
+        }
+    }};
+}
+
 pub mod l0_force_fan;
 pub mod mx;
+pub mod orbita2d_poulpe;
 pub mod orbita2dof_foc;
 pub mod orbita_foc;
+
+pub mod orbita3d_poulpe;
 pub mod xl320;
 pub mod xm;
diff --git a/src/device/orbita2d_poulpe.rs b/src/device/orbita2d_poulpe.rs
new file mode 100644
index 0000000..07a32ef
--- /dev/null
+++ b/src/device/orbita2d_poulpe.rs
@@ -0,0 +1,255 @@
+use crate::device::*;
+
+/// Wrapper for a value per motor (A and B)
+#[derive(Clone, Copy, Debug)]
+pub struct MotorValue<T> {
+    pub motor_a: T,
+    pub motor_b: T,
+}
+/// Wrapper for a Position/Speed/Load value for each motor
+#[derive(Clone, Copy, Debug)]
+pub struct MotorPositionSpeedLoad {
+    pub position: MotorValue<f32>,
+    // pub speed: MotorValue<f32>,
+    // pub load: MotorValue<f32>,
+}
+/// Wrapper for PID gains.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct Pid {
+    pub p: i16,
+    pub i: i16,
+}
+
+reg_read_only!(model_number, 0, u16);
+reg_read_only!(firmware_version, 6, u8);
+reg_read_write!(id, 7, u8);
+
+reg_read_write!(velocity_limit, 10, MotorValue::<f32>);
+reg_read_write!(velocity_limit_max, 12, MotorValue::<f32>);
+reg_read_write!(torque_flux_limit, 14, MotorValue::<f32>);
+reg_read_write!(torque_flux_limit_max, 16, MotorValue::<f32>);
+reg_read_write!(uq_ud_limit, 18, MotorValue::<i16>);
+
+reg_read_write!(flux_pid, 20, MotorValue::<Pid>);
+reg_read_write!(torque_pid, 24, MotorValue::<Pid>);
+reg_read_write!(velocity_pid, 28, MotorValue::<Pid>);
+reg_read_write!(position_pid, 32, MotorValue::<Pid>);
+
+reg_read_write!(torque_enable, 40, MotorValue::<bool>);
+
+reg_read_only!(current_position, 50, MotorValue::<f32>);
+reg_read_only!(current_velocity, 51, MotorValue::<f32>);
+reg_read_only!(current_torque, 52, MotorValue::<f32>);
+
+reg_read_write_fb!(
+    target_position,
+    60,
+    MotorValue::<f32>,
+    MotorPositionSpeedLoad
+);
+
+reg_read_write!(board_state, 80, u8);
+
+reg_read_only!(axis_sensor, 90, MotorValue::<f32>);
+
+reg_read_only!(full_state, 100, MotorPositionSpeedLoad);
+
+impl MotorPositionSpeedLoad {
+    pub fn from_le_bytes(bytes: [u8; 8]) -> Self {
+        MotorPositionSpeedLoad {
+            position: MotorValue::<f32>::from_le_bytes(bytes[0..8].try_into().unwrap()),
+            // speed: MotorValue::<f32>::from_le_bytes(bytes[8..16].try_into().unwrap()),
+            // load: MotorValue::<f32>::from_le_bytes(bytes[16..24].try_into().unwrap()),
+        }
+    }
+    // pub fn to_le_bytes(&self) -> [u8; 36] {
+    // 	let mut bytes = Vec::new();
+
+    // 	bytes.extend_from_slice(&self.position.to_le_bytes());
+    // 	bytes.extend_from_slice(&self.speed.to_le_bytes());
+    // 	bytes.extend_from_slice(&self.load.to_le_bytes());
+
+    // 	bytes.try_into().unwrap()
+    // }
+}
+
+impl<T: PartialEq> PartialEq for MotorValue<T> {
+    fn eq(&self, other: &Self) -> bool {
+        self.motor_a == other.motor_a && self.motor_b == other.motor_b
+    }
+}
+
+impl MotorValue<f32> {
+    pub fn from_le_bytes(bytes: [u8; 8]) -> Self {
+        MotorValue {
+            motor_a: f32::from_le_bytes(bytes[0..4].try_into().unwrap()),
+            motor_b: f32::from_le_bytes(bytes[4..8].try_into().unwrap()),
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 8] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&self.motor_a.to_le_bytes());
+        bytes.extend_from_slice(&self.motor_b.to_le_bytes());
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl MotorValue<u32> {
+    pub fn from_le_bytes(bytes: [u8; 8]) -> Self {
+        MotorValue {
+            motor_a: u32::from_le_bytes(bytes[0..4].try_into().unwrap()),
+            motor_b: u32::from_le_bytes(bytes[4..8].try_into().unwrap()),
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 8] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&self.motor_a.to_le_bytes());
+        bytes.extend_from_slice(&self.motor_b.to_le_bytes());
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl MotorValue<i32> {
+    pub fn from_le_bytes(bytes: [u8; 8]) -> Self {
+        MotorValue {
+            motor_a: i32::from_le_bytes(bytes[0..4].try_into().unwrap()),
+            motor_b: i32::from_le_bytes(bytes[4..8].try_into().unwrap()),
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 8] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&self.motor_a.to_le_bytes());
+        bytes.extend_from_slice(&self.motor_b.to_le_bytes());
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl MotorValue<i16> {
+    pub fn from_le_bytes(bytes: [u8; 4]) -> Self {
+        MotorValue {
+            motor_a: i16::from_le_bytes(bytes[0..2].try_into().unwrap()),
+            motor_b: i16::from_le_bytes(bytes[2..4].try_into().unwrap()),
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 4] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&self.motor_a.to_le_bytes());
+        bytes.extend_from_slice(&self.motor_b.to_le_bytes());
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl MotorValue<u16> {
+    pub fn from_le_bytes(bytes: [u8; 4]) -> Self {
+        MotorValue {
+            motor_a: u16::from_le_bytes(bytes[0..2].try_into().unwrap()),
+            motor_b: u16::from_le_bytes(bytes[2..4].try_into().unwrap()),
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 4] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&self.motor_a.to_le_bytes());
+        bytes.extend_from_slice(&self.motor_b.to_le_bytes());
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl MotorValue<bool> {
+    pub fn from_le_bytes(bytes: [u8; 2]) -> Self {
+        MotorValue {
+            motor_a: bytes[0] != 0,
+            motor_b: bytes[1] != 0,
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 2] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&[self.motor_a as u8]);
+        bytes.extend_from_slice(&[self.motor_b as u8]);
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl MotorValue<Pid> {
+    pub fn from_le_bytes(bytes: [u8; 8]) -> Self {
+        MotorValue {
+            motor_a: Pid::from_le_bytes(bytes[0..4].try_into().unwrap()),
+            motor_b: Pid::from_le_bytes(bytes[4..8].try_into().unwrap()),
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 8] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&self.motor_a.to_le_bytes());
+        bytes.extend_from_slice(&self.motor_b.to_le_bytes());
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl Pid {
+    pub fn from_le_bytes(bytes: [u8; 4]) -> Self {
+        Pid {
+            p: i16::from_le_bytes(bytes[0..2].try_into().unwrap()),
+            i: i16::from_le_bytes(bytes[2..4].try_into().unwrap()),
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 4] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&self.p.to_le_bytes());
+        bytes.extend_from_slice(&self.i.to_le_bytes());
+
+        bytes.try_into().unwrap()
+    }
+}
+
+// ///////////////////
+// reg_read_write!(torque_enable, 40, MotorValue::<u8>);
+// reg_read_write!(present_position, 50, MotorValue::<f32>);
+// reg_read_write!(goal_position, 60, MotorValue::<f32>);
+
+// impl MotorValue<u8> {
+//     pub fn from_le_bytes(bytes: [u8; 2]) -> Self {
+//         MotorValue {
+//             motor_a: bytes[0],
+//             motor_b: bytes[1],
+//         }
+//     }
+//     pub fn to_le_bytes(&self) -> [u8; 3] {
+//         let mut bytes = Vec::new();
+
+//         bytes.extend_from_slice(&self.motor_a.to_le_bytes());
+//         bytes.extend_from_slice(&self.motor_b.to_le_bytes());
+
+//         bytes.try_into().unwrap()
+//     }
+// }
+
+// impl MotorValue<f32> {
+//     pub fn from_le_bytes(bytes: [u8; 8]) -> Self {
+//         MotorValue {
+//             motor_a: f32::from_le_bytes(bytes[0..4].try_into().unwrap()),
+//             motor_b: f32::from_le_bytes(bytes[4..8].try_into().unwrap()),
+//         }
+//     }
+//     pub fn to_le_bytes(&self) -> [u8; 8] {
+//         let mut bytes = Vec::new();
+
+//         bytes.extend_from_slice(&self.motor_a.to_le_bytes());
+//         bytes.extend_from_slice(&self.motor_b.to_le_bytes());
+
+//         bytes.try_into().unwrap()
+//     }
+// }
diff --git a/src/device/orbita3d_poulpe.rs b/src/device/orbita3d_poulpe.rs
new file mode 100644
index 0000000..0bbfae4
--- /dev/null
+++ b/src/device/orbita3d_poulpe.rs
@@ -0,0 +1,284 @@
+//! Orbita 3Dof Poulpe version
+
+use crate::device::*;
+
+/// Wrapper for a value per motor
+#[derive(Clone, Copy, Debug)]
+pub struct MotorValue<T> {
+    pub top: T,
+    pub middle: T,
+    pub bottom: T,
+}
+
+/// Wrapper for a 3D vector (x, y, z)
+#[derive(Clone, Copy, Debug)]
+pub struct Vec3d<T> {
+    pub x: T,
+    pub y: T,
+    pub z: T,
+}
+
+/// Wrapper for a Position/Speed/Load value for each motor
+#[derive(Clone, Copy, Debug)]
+pub struct MotorPositionSpeedLoad {
+    pub position: MotorValue<f32>,
+    // pub speed: MotorValue<f32>,
+    // pub load: MotorValue<f32>,
+}
+/// Wrapper for PID gains.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct Pid {
+    pub p: i16,
+    pub i: i16,
+}
+
+reg_read_only!(model_number, 0, u16);
+reg_read_only!(firmware_version, 6, u8);
+reg_read_write!(id, 7, u8);
+
+reg_read_write!(velocity_limit, 10, MotorValue::<f32>);
+reg_read_write!(velocity_limit_max, 12, MotorValue::<f32>);
+reg_read_write!(torque_flux_limit, 14, MotorValue::<f32>);
+reg_read_write!(torque_flux_limit_max, 16, MotorValue::<f32>);
+reg_read_write!(uq_ud_limit, 18, MotorValue::<f32>);
+
+reg_read_write!(flux_pid, 20, MotorValue::<Pid>);
+reg_read_write!(torque_pid, 24, MotorValue::<Pid>);
+reg_read_write!(velocity_pid, 28, MotorValue::<Pid>);
+reg_read_write!(position_pid, 32, MotorValue::<Pid>);
+
+reg_read_write!(torque_enable, 40, MotorValue::<bool>);
+
+reg_read_only!(current_position, 50, MotorValue::<f32>);
+reg_read_only!(current_velocity, 51, MotorValue::<f32>);
+reg_read_only!(current_torque, 52, MotorValue::<f32>);
+
+reg_read_write_fb!(
+    target_position,
+    60,
+    MotorValue::<f32>,
+    MotorPositionSpeedLoad
+);
+reg_read_write!(board_state, 80, u8);
+reg_read_only!(axis_sensor, 90, MotorValue::<f32>);
+reg_read_only!(index_sensor, 99, MotorValue::<u8>);
+reg_read_only!(full_state, 100, MotorPositionSpeedLoad);
+
+impl MotorPositionSpeedLoad {
+    pub fn from_le_bytes(bytes: [u8; 12]) -> Self {
+        MotorPositionSpeedLoad {
+            position: MotorValue::<f32>::from_le_bytes(bytes[0..12].try_into().unwrap()),
+            // speed: MotorValue::<f32>::from_le_bytes(bytes[12..24].try_into().unwrap()),
+            // load: MotorValue::<f32>::from_le_bytes(bytes[24..36].try_into().unwrap()),
+        }
+    }
+    // pub fn to_le_bytes(&self) -> [u8; 36] {
+    // 	let mut bytes = Vec::new();
+
+    // 	bytes.extend_from_slice(&self.position.to_le_bytes());
+    // 	bytes.extend_from_slice(&self.speed.to_le_bytes());
+    // 	bytes.extend_from_slice(&self.load.to_le_bytes());
+
+    // 	bytes.try_into().unwrap()
+    // }
+}
+
+impl<T: PartialEq> PartialEq for MotorValue<T> {
+    fn eq(&self, other: &Self) -> bool {
+        self.top == other.top && self.middle == other.middle && self.bottom == other.bottom
+    }
+}
+
+impl MotorValue<f32> {
+    pub fn from_le_bytes(bytes: [u8; 12]) -> Self {
+        MotorValue {
+            top: f32::from_le_bytes(bytes[0..4].try_into().unwrap()),
+            middle: f32::from_le_bytes(bytes[4..8].try_into().unwrap()),
+            bottom: f32::from_le_bytes(bytes[8..12].try_into().unwrap()),
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 12] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&self.top.to_le_bytes());
+        bytes.extend_from_slice(&self.middle.to_le_bytes());
+        bytes.extend_from_slice(&self.bottom.to_le_bytes());
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl MotorValue<u32> {
+    pub fn from_le_bytes(bytes: [u8; 12]) -> Self {
+        MotorValue {
+            top: u32::from_le_bytes(bytes[0..4].try_into().unwrap()),
+            middle: u32::from_le_bytes(bytes[4..8].try_into().unwrap()),
+            bottom: u32::from_le_bytes(bytes[8..12].try_into().unwrap()),
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 12] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&self.top.to_le_bytes());
+        bytes.extend_from_slice(&self.middle.to_le_bytes());
+        bytes.extend_from_slice(&self.bottom.to_le_bytes());
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl MotorValue<i32> {
+    pub fn from_le_bytes(bytes: [u8; 12]) -> Self {
+        MotorValue {
+            top: i32::from_le_bytes(bytes[0..4].try_into().unwrap()),
+            middle: i32::from_le_bytes(bytes[4..8].try_into().unwrap()),
+            bottom: i32::from_le_bytes(bytes[8..12].try_into().unwrap()),
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 12] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&self.top.to_le_bytes());
+        bytes.extend_from_slice(&self.middle.to_le_bytes());
+        bytes.extend_from_slice(&self.bottom.to_le_bytes());
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl MotorValue<i16> {
+    pub fn from_le_bytes(bytes: [u8; 6]) -> Self {
+        MotorValue {
+            top: i16::from_le_bytes(bytes[0..2].try_into().unwrap()),
+            middle: i16::from_le_bytes(bytes[2..4].try_into().unwrap()),
+            bottom: i16::from_le_bytes(bytes[4..6].try_into().unwrap()),
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 6] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&self.top.to_le_bytes());
+        bytes.extend_from_slice(&self.middle.to_le_bytes());
+        bytes.extend_from_slice(&self.bottom.to_le_bytes());
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl MotorValue<u16> {
+    pub fn from_le_bytes(bytes: [u8; 6]) -> Self {
+        MotorValue {
+            top: u16::from_le_bytes(bytes[0..2].try_into().unwrap()),
+            middle: u16::from_le_bytes(bytes[2..4].try_into().unwrap()),
+            bottom: u16::from_le_bytes(bytes[4..6].try_into().unwrap()),
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 6] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&self.top.to_le_bytes());
+        bytes.extend_from_slice(&self.middle.to_le_bytes());
+        bytes.extend_from_slice(&self.bottom.to_le_bytes());
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl MotorValue<bool> {
+    pub fn from_le_bytes(bytes: [u8; 3]) -> Self {
+        MotorValue {
+            top: bytes[0] != 0,
+            middle: bytes[1] != 0,
+            bottom: bytes[2] != 0,
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 3] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&[self.top as u8]);
+        bytes.extend_from_slice(&[self.middle as u8]);
+        bytes.extend_from_slice(&[self.bottom as u8]);
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl MotorValue<u8> {
+    pub fn from_le_bytes(bytes: [u8; 3]) -> Self {
+        MotorValue {
+            top: bytes[0],
+            middle: bytes[1],
+            bottom: bytes[2],
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 3] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&[self.top]);
+        bytes.extend_from_slice(&[self.middle]);
+        bytes.extend_from_slice(&[self.bottom]);
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl MotorValue<Pid> {
+    pub fn from_le_bytes(bytes: [u8; 12]) -> Self {
+        MotorValue {
+            top: Pid::from_le_bytes(bytes[0..4].try_into().unwrap()),
+            middle: Pid::from_le_bytes(bytes[4..8].try_into().unwrap()),
+            bottom: Pid::from_le_bytes(bytes[8..12].try_into().unwrap()),
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 12] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&self.top.to_le_bytes());
+        bytes.extend_from_slice(&self.middle.to_le_bytes());
+        bytes.extend_from_slice(&self.bottom.to_le_bytes());
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl<T: PartialEq> PartialEq for Vec3d<T> {
+    fn eq(&self, other: &Self) -> bool {
+        self.x == other.x && self.y == other.y && self.z == other.z
+    }
+}
+
+impl Vec3d<f32> {
+    pub fn from_le_bytes(bytes: [u8; 12]) -> Self {
+        Vec3d {
+            x: f32::from_le_bytes(bytes[0..4].try_into().unwrap()),
+            y: f32::from_le_bytes(bytes[4..8].try_into().unwrap()),
+            z: f32::from_le_bytes(bytes[8..12].try_into().unwrap()),
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 12] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&self.x.to_le_bytes());
+        bytes.extend_from_slice(&self.y.to_le_bytes());
+        bytes.extend_from_slice(&self.z.to_le_bytes());
+
+        bytes.try_into().unwrap()
+    }
+}
+
+impl Pid {
+    pub fn from_le_bytes(bytes: [u8; 4]) -> Self {
+        Pid {
+            i: i16::from_le_bytes(bytes[0..2].try_into().unwrap()),
+            p: i16::from_le_bytes(bytes[2..4].try_into().unwrap()),
+        }
+    }
+    pub fn to_le_bytes(&self) -> [u8; 4] {
+        let mut bytes = Vec::new();
+
+        bytes.extend_from_slice(&self.i.to_le_bytes());
+        bytes.extend_from_slice(&self.p.to_le_bytes());
+
+        bytes.try_into().unwrap()
+    }
+}
diff --git a/src/device/orbita_foc.rs b/src/device/orbita_foc.rs
index ab6aec3..62d790a 100644
--- a/src/device/orbita_foc.rs
+++ b/src/device/orbita_foc.rs
@@ -38,7 +38,8 @@ reg_read_only!(firmware_version, 6, u8);
 reg_read_write!(id, 7, u8);
 
 reg_write_only!(system_check, 8, u8);
-reg_read_only!(motors_drivers_states, 9, u8);
+
+reg_read_only!(motors_drivers_states, 159, u8);
 
 reg_read_write!(voltage_limit, 10, f32);
 // reg_read_write!(intensity_limit, 14, f32);
@@ -70,6 +71,11 @@ reg_read_only!(present_position, 71, DiskValue::<f32>);
 reg_read_only!(top_present_position, 71, f32);
 reg_read_only!(middle_present_position, 75, f32);
 reg_read_only!(bottom_present_position, 79, f32);
+
+reg_read_only!(top_present_hall, 160, f32);
+reg_read_only!(middle_present_hall, 164, f32);
+reg_read_only!(bottom_present_hall, 168, f32);
+
 // reg_read_only!(present_speed, 83, DiskValue::<f32>);
 // reg_read_only!(top_present_speed, 83, f32);
 // reg_read_only!(middle_present_speed, 87, f32);
diff --git a/src/protocol/mod.rs b/src/protocol/mod.rs
index 7d10b1e..f5ef071 100644
--- a/src/protocol/mod.rs
+++ b/src/protocol/mod.rs
@@ -1,9 +1,11 @@
 use serialport::SerialPort;
 
 mod v1;
+#[allow(unused_imports)]
 pub use v1::{PacketV1, V1};
 
 mod v2;
+#[allow(unused_imports)]
 pub use v2::{PacketV2, V2};
 
 use crate::{
@@ -31,6 +33,7 @@ pub trait Protocol<P: Packet> {
         self.send_instruction_packet(port, P::write_packet(id, addr, data).as_ref())?;
         self.read_status_packet(port, id).map(|_| ())
     }
+
     fn write_fb(
         &self,
         port: &mut dyn SerialPort,
@@ -42,6 +45,7 @@ pub trait Protocol<P: Packet> {
         self.read_status_packet(port, id)
             .map(|sp| sp.params().to_vec())
     }
+
     fn sync_read(
         &self,
         port: &mut dyn SerialPort,
@@ -131,6 +135,8 @@ pub enum CommunicationErrorKind {
     TimeoutError,
     /// Incorrect response id - different from sender (sender id, response id)
     IncorrectId(u8, u8),
+
+    /// Operation not supported
     Unsupported,
 }
 impl fmt::Display for CommunicationErrorKind {
@@ -142,7 +148,7 @@ impl fmt::Display for CommunicationErrorKind {
             CommunicationErrorKind::IncorrectId(sender_id, resp_id) => {
                 write!(f, "Incorrect id ({} instead of {})", resp_id, sender_id)
             }
-	    CommunicationErrorKind::Unsupported => write!(f, "Operation not supported"),
+            CommunicationErrorKind::Unsupported => write!(f, "Operation not supported"),
         }
     }
 }
diff --git a/src/protocol/v1.rs b/src/protocol/v1.rs
index 1b73421..ff740b1 100644
--- a/src/protocol/v1.rs
+++ b/src/protocol/v1.rs
@@ -148,6 +148,11 @@ impl StatusPacket<PacketV1> for StatusPacketV1 {
         let read_crc = *data.last().unwrap();
         let computed_crc = crc(&data[2..data.len() - 1]);
         if read_crc != computed_crc {
+            println!(
+                "read crc: {}, computed crc: {} data: {:?}",
+                read_crc, computed_crc, data
+            );
+
             return Err(Box::new(CommunicationErrorKind::ChecksumError));
         }