Break out serial interface to be used in other stages.

Signed-off-by: Henner Zeller <h.zeller@acm.org>

hostcontrol/src/stage.rs

@@ -22,46 +22,47 @@ fn new_delta_into_cartesian(flex_h: f64, flex_a: f64, flex_b: f64) -> nalgebra::
     ]
 }
 
-pub struct Stage {
+#[derive(Error, Debug)]
+pub enum StageIOError {
+    #[error("could not connect to stage controller")]
+    Open(#[from] serialport::Error),
+    #[error("error when sending data to the stage controller")]
+    Write(std::io::Error),
+    #[error("error when receiving data from the stage controller")]
+    Read(std::io::Error),
+}
+
+#[derive(Error, Debug)]
+pub enum StageError {
+    #[error("Interface issue")]
+    Communication(#[from] StageIOError),
+    #[error("protocol error")]
+    Mumble,
+}
+
+pub struct StageIO {
     serial: Box<dyn serialport::SerialPort>,
+}
+
+pub struct Stage {
+    stage_io: StageIO,
     delta_pos: nalgebra::Vector3<i32>,
     cartesian_to_delta: nalgebra::Matrix3<f64>,
     delta_to_cartesian: nalgebra::Matrix3<f64>,
 }
 
-#[derive(Error, Debug)]
-pub enum StageError {
-    #[error("could not connect to stage controller")]
-    Open(serialport::Error),
-    #[error("error when sending data to the stage controller")]
-    Write(std::io::Error),
-    #[error("error when receiving data from the stage controller")]
-    Read(std::io::Error),
-    #[error("protocol error")]
-    Mumble,
-}
-
-pub type Result<T> = std::result::Result<T, StageError>;
-
-impl Stage {
-    pub fn new(port: &str, camera_rotation: f64) -> Result<Self> {
-        // TODO: non-blocking moves
-        let serial = serialport::new(port, 115200).timeout(Duration::from_secs(60)).open().map_err(|e| StageError::Open(e))?;
-        let delta_to_cartesian = new_camera_rotation(camera_rotation).try_inverse().unwrap() * new_delta_into_cartesian(80.0, 50.0, 50.0);
-        let cartesian_to_delta = delta_to_cartesian.try_inverse().unwrap();
-        let mut res = Self { serial, delta_pos: nalgebra::vector![0, 0, 0], cartesian_to_delta, delta_to_cartesian };
-
-        let pos = res.communicate("p?")?.split(" ").map(|v| v.parse::<i32>().map_err(|_| StageError::Mumble)).collect::<std::result::Result<Vec<_>, _>>()?;
-        res.delta_pos = nalgebra::vector![pos[0], pos[1], pos[2]];
-        println!("Initialized stage at delta positions {}/{}/{}", pos[0], pos[1], pos[2]);
-        Ok(res)
+pub type StageIOResult<T> = std::result::Result<T, StageIOError>;
+impl StageIO {
+    pub fn new(port: &str, speed: u32) -> StageIOResult<Self> {
+	let serial = serialport::new(port, speed).timeout(Duration::from_secs(60)).open()?;
+	Ok(Self { serial })
     }
 
-    fn receive_until<S: AsRef<str>>(&mut self, needle: S) -> Result<String> {
+    fn receive_until<S: AsRef<str>>(&mut self, needle: S) -> StageIOResult<String> {
         let mut res: Vec<u8> = vec![];
         loop {
             let mut buf = [0u8; 1];
-            self.serial.read_exact(&mut buf).map_err(|e| StageError::Read(e))?;
+            self.serial.read_exact(&mut buf).map_err(|e| StageIOError::Read(e))?;
             res.push(buf[0]);
             if res.ends_with(needle.as_ref().as_bytes()) {
                 return Ok(String::from_utf8_lossy(&res).to_string());
@@ -69,11 +70,28 @@ impl Stage {
         }
     }
 
-    fn communicate<S: AsRef<str>>(&mut self, command: S) -> Result<String> {
-        self.serial.write_all(command.as_ref().as_bytes()).map_err(|e| StageError::Write(e))?;
+    // Send request and wait for response deliminated with a newline
+    fn send_request<S: AsRef<str>>(&mut self, command: S) -> StageIOResult<String> {
+        self.serial.write_all(command.as_ref().as_bytes()).map_err(|e| StageIOError::Write(e))?;
         let res = self.receive_until("\n")?;
         Ok(res.trim().to_string())
     }
+}
+
+pub type Result<T> = std::result::Result<T, StageError>;
+impl Stage {
+    pub fn new(stage_io: StageIO, camera_rotation: f64) -> Result<Self> {
+        // TODO: non-blocking moves
+        let delta_to_cartesian = new_camera_rotation(camera_rotation).try_inverse().unwrap() * new_delta_into_cartesian(80.0, 50.0, 50.0);
+        let cartesian_to_delta = delta_to_cartesian.try_inverse().unwrap();
+        let mut res = Self { stage_io, delta_pos: nalgebra::vector![0, 0, 0], cartesian_to_delta, delta_to_cartesian };
+
+        let pos = res.stage_io.send_request("p?")?.split(" ").map(|v| v.parse::<i32>().map_err(|_| StageError::Mumble)).collect::<std::result::Result<Vec<_>, _>>()?;
+        res.delta_pos = nalgebra::vector![pos[0], pos[1], pos[2]];
+        println!("Initialized stage at delta positions {}/{}/{}", pos[0], pos[1], pos[2]);
+        Ok(res)
+    }
+
 
     pub fn get_position_delta(&self) -> nalgebra::Vector3<i32> {
         self.delta_pos.clone()
@@ -85,7 +103,7 @@ impl Stage {
     }
 
     pub fn move_relative_delta(&mut self, d: nalgebra::Vector3<i32>) -> Result<()> {
-        self.communicate(format!("mr {} {} {}\n", d[0], d[1], d[2]))?;
+        self.stage_io.send_request(format!("mr {} {} {}\n", d[0], d[1], d[2]))?;
         self.delta_pos += nalgebra::vector![d[0], d[1], d[2]];
         Ok(())
     }
@@ -121,6 +139,6 @@ impl Stage {
     }
 
     pub fn version(&mut self) -> Result<String> {
-        self.communicate("version\n")
+        Ok(self.stage_io.send_request("version\n")?)
     }
 }