cu_lewansoul/
lib.rs

1use bincode::de::Decoder;
2use bincode::enc::Encoder;
3use bincode::error::{DecodeError, EncodeError};
4use bincode::{Decode, Encode};
5use cu29::prelude::*;
6use serde::Serialize;
7use serialport::{DataBits, FlowControl, Parity, SerialPort, StopBits};
8use std::io::{self, Read, Write};
9use std::time::Duration;
10use uom::si::angle::{degree, radian};
11use uom::si::f32::Angle;
12
13#[allow(dead_code)]
14mod servo {
15    // From "lx-16a LewanSoul Bus Servo Communication Protocol.pdf"
16    pub const SERVO_MOVE_TIME_WRITE: u8 = 1; // 7 bytes
17    pub const SERVO_MOVE_TIME_READ: u8 = 2; // 3 bytes
18    pub const SERVO_MOVE_TIME_WAIT_WRITE: u8 = 7; // 7 bytes
19    pub const SERVO_MOVE_TIME_WAIT_READ: u8 = 8; // 3 bytes
20    pub const SERVO_MOVE_START: u8 = 11; // 3 bytes
21    pub const SERVO_MOVE_STOP: u8 = 12; // 3 bytes
22    pub const SERVO_ID_WRITE: u8 = 13; // 4 bytes
23    pub const SERVO_ID_READ: u8 = 14; // 3 bytes
24    pub const SERVO_ANGLE_OFFSET_ADJUST: u8 = 17; // 4 bytes
25    pub const SERVO_ANGLE_OFFSET_WRITE: u8 = 18; // 3 bytes
26    pub const SERVO_ANGLE_OFFSET_READ: u8 = 19; // 3 bytes
27    pub const SERVO_ANGLE_LIMIT_WRITE: u8 = 20; // 7 bytes
28    pub const SERVO_ANGLE_LIMIT_READ: u8 = 21; // 3 bytes
29    pub const SERVO_VIN_LIMIT_WRITE: u8 = 22; // 7 bytes
30    pub const SERVO_VIN_LIMIT_READ: u8 = 23; // 3 bytes
31    pub const SERVO_TEMP_MAX_LIMIT_WRITE: u8 = 24; // 4 bytes
32    pub const SERVO_TEMP_MAX_LIMIT_READ: u8 = 25; // 3 bytes
33    pub const SERVO_TEMP_READ: u8 = 0x1A; // 26 -> 3 bytes
34    pub const SERVO_VIN_READ: u8 = 0x1B; // 27 -> 3 bytes
35    pub const SERVO_POS_READ: u8 = 28; // 3 bytes
36    pub const SERVO_OR_MOTOR_MODE_WRITE: u8 = 29; // 7 bytes
37    pub const SERVO_OR_MOTOR_MODE_READ: u8 = 30; // 3 bytes
38    pub const SERVO_LOAD_OR_UNLOAD_WRITE: u8 = 31; // 4 bytes
39    pub const SERVO_LOAD_OR_UNLOAD_READ: u8 = 32; // 3 bytes
40    pub const SERVO_LED_CTRL_WRITE: u8 = 33; // 4 bytes
41    pub const SERVO_LED_CTRL_READ: u8 = 34; // 3 bytes
42    pub const SERVO_LED_ERROR_WRITE: u8 = 35; // 4 bytes
43    pub const SERVO_LED_ERROR_READ: u8 = 36; // 3 bytes
44}
45
46const SERIAL_SPEED: u32 = 115200; // only this speed is supported by the servos
47const TIMEOUT: Duration = Duration::from_secs(1); // TODO: add that as a parameter in the config
48
49const MAX_SERVOS: usize = 8; // in theory it could be higher. Revisit if needed.
50
51/// Compute the checksum for the given data.
52/// The spec is "Checksum:The calculation method is as follows:
53// Checksum=~(ID+ Length+Cmd+ Prm1+...PrmN)If the numbers in the
54// brackets are calculated and exceeded 255,Then take the lowest one byte, "~"
55// means Negation."
56#[inline]
57fn compute_checksum(data: impl Iterator<Item = u8>) -> u8 {
58    let mut checksum: u8 = 0;
59    for byte in data {
60        checksum = checksum.wrapping_add(byte);
61    }
62    !checksum
63}
64
65// angle in degrees, returns position in 0.24 degrees
66#[inline]
67#[allow(dead_code)]
68fn angle_to_position(angle: Angle) -> i16 {
69    let angle = angle.get::<degree>();
70    (angle * 1000.0 / 240.0) as i16
71}
72
73/// This is a driver for the LewanSoul LX-16A, LX-225 etc.  Serial Bus Servos.
74pub struct Lewansoul {
75    port: Box<dyn SerialPort>,
76    #[allow(dead_code)]
77    ids: [u8; 8], // TODO: WIP
78}
79
80impl Lewansoul {
81    fn send_packet(&mut self, id: u8, command: u8, data: &[u8]) -> io::Result<()> {
82        let mut packet = vec![0x55, 0x55, id, data.len() as u8 + 3, command];
83        packet.extend(data.iter());
84        let checksum = compute_checksum(packet[2..].iter().cloned());
85        packet.push(checksum);
86
87        // println!("Packet: {:02x?}", packet);
88        self.port.write_all(&packet)?;
89        Ok(())
90    }
91
92    /// This should only be use at HW setup. I leave it there for you to make a tool around it if needed.
93    /// See the unit test how you can reuse independently those functions
94    #[allow(dead_code)]
95    fn reassign_servo_id(&mut self, id: u8, new_id: u8) -> io::Result<()> {
96        self.send_packet(id, servo::SERVO_ID_WRITE, &[new_id])?;
97        self.read_response()?;
98        Ok(())
99    }
100
101    #[allow(dead_code)]
102    fn read_current_position(&mut self, id: u8) -> io::Result<f32> {
103        self.send_packet(id, servo::SERVO_POS_READ, &[])?;
104        let response = self.read_response()?;
105        Ok((i16::from_le_bytes([response.2[0], response.2[1]])) as f32 * 240.0 / 1000.0)
106    }
107
108    #[allow(dead_code)]
109    fn read_present_voltage(&mut self, id: u8) -> io::Result<f32> {
110        self.send_packet(id, servo::SERVO_VIN_READ, &[])?;
111        let response = self.read_response()?;
112        Ok(u16::from_le_bytes([response.2[0], response.2[1]]) as f32 / 1000.0)
113    }
114
115    #[allow(dead_code)]
116    fn read_temperature(&mut self, id: u8) -> io::Result<u8> {
117        self.send_packet(id, servo::SERVO_TEMP_READ, &[])?;
118        let response = self.read_response()?;
119        Ok(response.2[0])
120    }
121
122    #[allow(dead_code)]
123    fn read_angle_limits(&mut self, id: u8) -> io::Result<(f32, f32)> {
124        self.send_packet(id, servo::SERVO_ANGLE_LIMIT_READ, &[])?;
125        let response = self.read_response()?;
126        Ok((
127            (i16::from_le_bytes([response.2[0], response.2[1]]) as f32) * 240.0 / 1000.0,
128            (i16::from_le_bytes([response.2[2], response.2[3]]) as f32) * 240.0 / 1000.0,
129        ))
130    }
131
132    #[allow(dead_code)]
133    fn ping(&mut self, id: u8) -> CuResult<()> {
134        self.send_packet(id, servo::SERVO_ID_READ, &[])
135            .map_err(|e| CuError::new_with_cause("IO Error trying to write to the SBUS", &e))?;
136        let response = self.read_response().map_err(|e| {
137            CuError::new_with_cause("IO Error trying to read the ping response from SBUS", &e)
138        })?;
139
140        if response.2[0] == id {
141            Ok(())
142        } else {
143            Err(format!(
144                "The servo ID {} did not respond to ping got {} as ID instead.",
145                id, response.2[0]
146            )
147            .into())
148        }
149    }
150
151    fn read_response(&mut self) -> io::Result<(u8, u8, Vec<u8>)> {
152        let mut header = [0; 5];
153        self.port.read_exact(&mut header)?;
154        if header[0] != 0x55 || header[1] != 0x55 {
155            return Err(io::Error::other("Invalid header"));
156        }
157        let id = header[2];
158        let length = header[3];
159        let command = header[4];
160        let mut remaining = vec![0; length as usize - 2]; // -2 for length itself already read + command already read
161        self.port.read_exact(&mut remaining)?;
162        let checksum = compute_checksum(
163            &mut header[2..]
164                .iter()
165                .chain(remaining[..remaining.len() - 1].iter())
166                .cloned(),
167        );
168        if checksum != *remaining.last().unwrap() {
169            return Err(io::Error::other("Invalid checksum"));
170        }
171        Ok((id, command, remaining[..remaining.len() - 1].to_vec()))
172    }
173}
174
175impl Freezable for Lewansoul {
176    // This driver is stateless as the IDs are recreate at new time, we keep the default implementation.
177}
178
179#[derive(Debug, Clone, Default, Serialize)]
180pub struct ServoPositionsPayload {
181    pub positions: [Angle; MAX_SERVOS],
182}
183
184impl Encode for ServoPositionsPayload {
185    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
186        let angles: [f32; MAX_SERVOS] = self.positions.map(|a| a.value);
187        bincode::Encode::encode(&angles, encoder)
188    }
189}
190
191impl Decode<()> for ServoPositionsPayload {
192    fn decode<D: Decoder>(decoder: &mut D) -> Result<Self, DecodeError> {
193        let angles: [f32; 8] = Decode::decode(decoder)?;
194        let positions: [Angle; 8] = angles.map(Angle::new::<radian>);
195        Ok(ServoPositionsPayload { positions })
196    }
197}
198
199impl CuSinkTask for Lewansoul {
200    type Input<'m> = input_msg!(ServoPositionsPayload);
201
202    fn new(config: Option<&ComponentConfig>) -> CuResult<Self>
203    where
204        Self: Sized,
205    {
206        let ComponentConfig(kv) =
207            config.ok_or("RPGpio needs a config, None was passed as ComponentConfig")?;
208
209        let serial_dev: String = kv
210            .get("serial_dev")
211            .expect(
212                "Lewansoul expects a serial_dev config entry pointing to the serial device to use.",
213            )
214            .clone()
215            .into();
216
217        let mut ids = [0u8; 8];
218        for (i, id) in ids.iter_mut().enumerate() {
219            let servo = kv.get(format!("servo{i}").as_str());
220            if servo.is_none() {
221                if i == 0 {
222                    return Err(
223                        "You need to specify at least one servo ID to address (as \"servo0\")"
224                            .into(),
225                    );
226                }
227                break;
228            }
229            *id = servo.unwrap().clone().into();
230        }
231
232        let port = serialport::new(serial_dev.as_str(), SERIAL_SPEED)
233            .data_bits(DataBits::Eight)
234            .flow_control(FlowControl::None)
235            .parity(Parity::None)
236            .stop_bits(StopBits::One)
237            .timeout(TIMEOUT)
238            .open()
239            .map_err(|e| format!("Error opening serial port: {e:?}"))?;
240
241        Ok(Lewansoul { port, ids })
242    }
243
244    fn process(&mut self, _clock: &RobotClock, _input: &Self::Input<'_>) -> CuResult<()> {
245        todo!()
246    }
247}
248
249#[cfg(test)]
250mod tests {
251    use super::*;
252
253    #[test]
254    #[ignore]
255    fn end2end_2_servos() {
256        let mut config = ComponentConfig::default();
257        config
258            .0
259            .insert("serial_dev".to_string(), "/dev/ttyACM0".to_string().into());
260
261        config.0.insert("servo0".to_string(), 1.into());
262        config.0.insert("servo1".to_string(), 2.into());
263
264        let mut lewansoul = Lewansoul::new(Some(&config)).unwrap();
265        let _position = lewansoul.read_current_position(1).unwrap();
266
267        let _angle_limits = lewansoul.read_angle_limits(1).unwrap();
268    }
269}
cu_lewansoul/lib.rs

cu_lewansoul/
lib.rs
