1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
use serialport::SerialPort;
pub enum Error {
InvalidServo(u8),
InvalidPosition { servo: u8, position: u16 },
SerialPortFailure(std::io::Error),
}
pub struct Arm {
port: Box<dyn SerialPort>,
}
impl Arm {
pub fn new(port: Box<dyn SerialPort>) -> Self {
Arm { port }
}
pub fn reset(&mut self) -> Result<(), Error> {
let speed = 1000;
self.set_position([500, 500, 500, 500, 500, 500], speed)
}
pub fn set_position(&mut self, pos: [u16; 6], speed: u16) -> Result<(), Error> {
for (i, v) in pos.iter().enumerate() {
self.set_servo_position((i as u8) + 1, v.to_owned(), speed)?;
}
Ok(())
}
pub fn set_servo_position(&mut self, servo: u8, pos: u16, speed: u16) -> Result<(), Error> {
if servo > 6 {
return Err(Error::InvalidServo(servo));
}
if pos > 1000 {
return Err(Error::InvalidPosition {
servo: servo,
position: pos,
});
}
let mut buf = [0u8; 10];
let pos_bytes = pos.to_le_bytes();
let speed_bytes = speed.to_le_bytes();
buf[0] = 0x55;
buf[1] = 0x55;
buf[2] = 0x08;
buf[3] = 0x03;
buf[4] = 0x01;
buf[5] = speed_bytes[0];
buf[6] = speed_bytes[1];
buf[7] = servo.to_le_bytes()[0];
buf[8] = pos_bytes[0];
buf[9] = pos_bytes[1];
let result = self.port.write_all(&buf);
match result {
Err(e) => Err(Error::SerialPortFailure(e)),
Ok(_) => Ok(()),
}
}
}