mks_servo42/
lib.rs

1#![no_std]
2
3mod response;
4pub mod direction;
5mod errors;
6
7pub use errors::Error;
8
9/// The core object in the API. Represents a single motor.
10///
11/// Has a Default impl which uses the default address, but if you want to stack them on a bus you
12/// should use `with_id`.
13pub struct Driver {
14    address: u8,
15    bytes: [u8; 8], // This is our buffer that we're going to load out with commands in it
16}
17
18type Result<T> = core::result::Result<T, crate::Error>;
19
20impl Default for Driver {
21    fn default() -> Self {
22        Driver {
23            address: 0xe0,
24            bytes: Default::default(),
25        }
26    }
27}
28
29impl Driver {
30    /// Create a driver with a different ID.
31    pub fn with_id(address: u8) -> Self {
32        Driver {
33            address,
34            .. Default::default()
35        }
36    }
37
38    // TODO(richo) This speed param is everywhere we should make a constructor that refuses to do
39    // the wrong thing.
40    /// Start the motor rotating.
41    ///
42    /// It will continue doing this until stopped. Direction is relative to the configured
43    /// direction in the motor.
44    ///
45    /// Speed is a value between 0 and 0x80. The values can be calculated with more infomration
46    /// about how the motor is configured but that's not implemented.
47    pub fn rotate<'a>(&'a mut self, direction: direction::Direction, speed: u8) -> Result<&'a [u8]> {
48        if speed > 0x80 {
49            return Err(Error::InvalidValue);
50        }
51
52        Ok(self.set_bytes(&[self.address, 0xf6, speed | direction as u8]))
53    }
54
55    /// Stop the motor.
56    pub fn stop<'a>(&'a mut self) -> Result<&'a [u8]> {
57        Ok(self.set_bytes(&mut [self.address, 0xf7]))
58    }
59
60    pub fn rotate_to<'a>(&'a mut self, direction: direction::Direction, speed: u8, value: u32) -> Result<&'a [u8]> {
61        if speed > 0x80 {
62            return Err(Error::InvalidValue);
63        }
64
65        // I'll figure out some fancy pants way to do this later
66        Ok(self.set_bytes(&[self.address, 0xfd, speed | direction as u8,
67                ((value & 0xff000000) >> 24) as u8,
68                ((value & 0x00ff0000) >> 16) as u8,
69                ((value & 0x0000ff00) >> 8) as u8,
70                ((value & 0x000000ff) >> 0) as u8]))
71
72    }
73
74    pub fn zero<'a>(&'a mut self) -> Result<&'a [u8]> {
75        Ok(self.set_bytes(&[self.address, 0x94, 0x00]))
76    }
77
78    // TODO(richo) this u8 is a lie
79    pub fn set_zero_speed<'a>(&'a mut self, speed: u8) -> Result<&'a [u8]> {
80        Ok(self.set_bytes(&[self.address, 0x92, speed]))
81    }
82
83    /// Setup these bytes in the internal buffer, build the checksum, and then return the correct
84    /// slice.
85    fn set_bytes(&mut self, cmd: &[u8]) -> &[u8] {
86        let len = cmd.len();
87        self.bytes[..len].clone_from_slice(&cmd);
88        self.bytes[len] = checksum(&cmd);
89        &self.bytes[..len+1]
90    }
91}
92
93fn checksum(bytes: &[u8]) -> u8 {
94    let mut total: u64 = 0;
95    for b in bytes {
96        total += *b as u64;
97    }
98    return (total & 0xff) as u8;
99}
100
101
102#[cfg(test)]
103mod tests {
104    use super::*;
105
106    #[test]
107    fn checksums() {
108        assert_eq!(0xd7, checksum(&[0xe0, 0xf6, 0x01]));
109    }
110}
mks_servo42/lib.rs

mks_servo42/
lib.rs
