ev3_dc/utils/
mod.rs

1//! Utility functions for ev3-dc
2//!
3//! ### Terminology
4//!  - **Layer**: Index of daisy-chained EV3. i.e a single EV3 brick is a master which has layer of 0
5
6use super::{ Encoding::*, encode, ValError };
7
8#[derive(Default)]
9/// Chainable byte vector
10/// # Example
11/// Create u8 vector with chainable method to modify value
12/// ```
13/// // Compared to standard rust vector operation
14/// let mut byte = ChainByte::new(); // let mut byte: Vec<u8> = vec![];
15/// byte.push(0x81) // byte.push(0x81);
16///     .add(vec![0x1B, 0x00]) // byte.extend(vec![0x1B, 0x00]);
17/// println!("Vector: {:02X?}", byte.bytes); // println!("Vector: {:02X?}", byte);
18/// ```
19pub struct ChainByte {
20    /// Result vector
21    pub bytes: Vec<u8>
22}
23
24const LEN_MAX: usize = 1000; // LIMIT: Practical limit is 1000 for some reason.
25
26// maybe use velcro crate instead
27impl ChainByte {
28    pub fn new() -> Self {
29        ChainByte { bytes: vec![] }
30    }
31    /// Same as [`Vec::push`], but chainable
32    pub fn push(&mut self, byte: u8) -> &mut Self {
33        self.bytes.push(byte);
34        self
35    }
36    /// [`Vec::extend`], but chainable
37    pub fn add(&mut self, bytes: Vec<u8>) -> &mut Self {
38        self.bytes.extend(bytes);
39        self
40    }
41}
42
43/// Encode local constant based on integer value
44pub fn auto_const(val: i32) -> Result<Vec<u8>, ValError> {
45    match val.abs() {
46        0..32 => encode(LC0(val as i8)),
47        32..128 => encode(LC1(val as i8)),
48        128..32768 => encode(LC2(val as i16)),
49        _ => encode(LC4(val))
50    }
51}
52
53/// Merge vector of small bytecode to vector of larger bytecode vector.
54/// Max length is set to 1000
55pub fn package_bytes(bytecodes: &[Vec<u8>]) -> Vec<Vec<u8>> {
56    let mut packets: Vec<Vec<u8>> = vec![];
57    let mut buffer: Vec<u8> = Vec::with_capacity(LEN_MAX);
58    let mut size: usize = 0;
59    for bytes in bytecodes {
60       if size + bytes.len() > LEN_MAX {
61            packets.push(buffer.clone());
62            buffer.clear();
63            size = 0;
64        }
65        buffer.extend(bytes);
66        size += bytes.len();
67    }
68    packets.push(buffer);
69    packets
70}
71
72/// Run-Length-Encoding on 1D 178x128 image array.
73/// Return (x1, y1, x2, y2) line bytecode.
74pub fn run_length(image: &[u8]) -> Result<Vec<(u8, u8, u8, u8)>, ValError> {
75    if image.len() != (178 * 128) { return Err(ValError::InvalidValue(image.len() as i32, 178 * 128)) } 
76    let mut state;
77    let mut buffer: Vec<(u8, u8, u8, u8)> = vec![];
78    let mut line: (u8, u8, u8, u8) = (0, 0, 0, 0);
79    for y in 0..128 {
80        state = false;
81        for x in 0..178 {
82            if image[178 * y + x] == 1 && !state {
83                state = true;
84                line.0 = x as u8;
85                line.1 = y as u8;
86            }else if image[178 * y + x] == 0 && state {
87                state = false;
88                line.2 = (x - 1) as u8;
89                line.3 = y as u8;
90                buffer.push(line);
91            }else if image[178 * y + x] == 1 && x == 177 && state {
92                line.2 = 177;
93                line.3 = y as u8;
94                buffer.push(line);
95            }
96        }
97    }
98    Ok(buffer)
99}
100
101/// Convert vector of points from [`run_length`] to direct commands
102/// Return vector of small line / dot bytecodes
103/// # Example
104/// create RLE line vector from 178x128 binary vector and create line bytecode
105/// ```
106/// let img: [u8; 22784] = [1; 22784]; // add stuff here
107/// let lines = run_length(&img).unwrap();
108/// let code = printer(&lines);
109/// println("Bytecode: {:02X?}", code);
110/// ```
111pub fn printer(lines: &[(u8, u8, u8, u8)]) -> Vec<Vec<u8>> {
112    let mut packets: Vec<Vec<u8>> = vec![];
113    for line in lines {
114        let mut bytecode = ChainByte::new();
115        if line.0 == line.2 {
116            bytecode.add(vec![0x84, 0x02, 0x01])
117                .add(encode(LC2(line.0 as i16)).unwrap())
118                .add(encode(LC2(line.1 as i16)).unwrap());
119        }else {
120            bytecode.add(vec![0x84, 0x03, 0x01])
121                .add(encode(LC2(line.0 as i16)).unwrap())
122                .add(encode(LC2(line.1 as i16)).unwrap())
123                .add(encode(LC2(line.2 as i16)).unwrap())
124                .add(encode(LC2(line.3 as i16)).unwrap());
125        }
126        packets.push(bytecode.bytes);
127    }
128    packets
129}
130
131/// Return name of device id
132// will probably get replaced soon
133pub fn device_id(byte: u8) -> String {
134    String::from(match byte {
135        7 => "Large-Motor",
136        8 => "Medium-Motor",
137        10 => "Unknown", // Try re-plugging
138        16 => "Touch-Sensor",
139        29 => "Color-Sensor",
140        30 => "Ultrasonic-Sensor",
141        32 => "Gyro-Sensor",
142        33 => "IR-Sensor",
143        126 => "None",
144        127 => "Port-Error",
145        _ => todo!("ID: {} Unimplemented!", byte) // for now, only support EV3 devices
146    })
147}
148
149/// Read port from u8 slice. 0-3 are inputs, 4-7 are outputs
150/// # Example
151/// read ports of the master / first EV3 brick
152/// ```
153/// let buf: [u8; 32] = [0x7E, 0X7E, 0x08]; // Reply memory from opInput_Device_List OpCode
154/// let res: [u8; 8] = port_read(&buf, 0);
155/// println!("Input device ids: {:?}, Output device ids: {:?}", res[0..4], res[4..8]);
156/// ```
157pub fn port_read(port: &[u8], layer: u8) -> Result<[u8; 8], ValError> {
158    let mut ports = [0_u8; 8];
159    if layer > 3 { return Err(ValError::InvalidRange(layer as i32, 0, 3)) }
160    if port.len() < (20 + (layer * 4)) as usize { return Err(ValError::InvalidRange(port.len() as i32, (20 + (layer * 4)) as i32, 32)); }
161    ports[0..4].copy_from_slice(&port[((layer * 4) as usize)..(((layer + 1) * 4) as usize)]);
162    ports[4..8].copy_from_slice(&port[(16 + (layer * 4) as usize)..(16 + ((layer + 1) * 4) as usize)]);
163    Ok(ports)
164}
165
166/// Read null-terminated string
167pub fn read_string(bytes: &[u8]) -> Option<&str> {
168    str::from_utf8(bytes).unwrap().split_terminator("\0").next()
169}
ev3_dc/utils/mod.rs

ev3_dc/utils/
mod.rs
