st3215/
st3215.rs

1use crate::group_sync_write::GroupSyncWrite;
2use crate::port_handler::PortHandler;
3use crate::protocol_packet_handler::ProtocolPacketHandler;
4use crate::values::*;
5use std::collections::HashMap;
6use std::sync::{Arc, Mutex};
7use std::thread;
8use std::time::Duration;
9
10pub struct ST3215 {
11    port_handler: Arc<Mutex<PortHandler>>,
12    #[allow(dead_code)]
13    group_sync_write: Arc<Mutex<GroupSyncWrite>>,
14}
15
16impl ST3215 {
17    /// Créer une nouvelle instance ST3215
18    pub fn new(device: &str) -> Result<Self, String> {
19        let mut port_handler = PortHandler::new(device);
20        port_handler.open_port()?;
21
22        let group_sync_write = GroupSyncWrite::new(STS_ACC, 7);
23
24        Ok(Self {
25            port_handler: Arc::new(Mutex::new(port_handler)),
26            group_sync_write: Arc::new(Mutex::new(group_sync_write)),
27        })
28    }
29
30    /// Vérifier la présence d'un servo
31    pub fn ping_servo(&self, sts_id: u8) -> bool {
32        let mut port = self.port_handler.lock().unwrap();
33        let mut handler = ProtocolPacketHandler::new(&mut *port);
34        let (model, comm, error) = handler.ping(sts_id);
35        comm.is_success() && model != 0 && error == 0
36    }
37
38    /// Scanner le bus pour déterminer tous les servos présents
39    pub fn list_servos(&self) -> Vec<u8> {
40        let mut servos = Vec::new();
41        for id in 0..254 {
42            if self.ping_servo(id) {
43                servos.push(id);
44            }
45        }
46        servos
47    }
48
49    /// Lire la charge du servo (en pourcentage)
50    pub fn read_load(&self, sts_id: u8) -> Option<f32> {
51        let mut port = self.port_handler.lock().unwrap();
52        let mut handler = ProtocolPacketHandler::new(&mut *port);
53        let (load, comm, error) = handler.read_1byte_tx_rx(sts_id, STS_PRESENT_LOAD_L);
54        if comm.is_success() && error == 0 {
55            Some(load as f32 * 0.1)
56        } else {
57            None
58        }
59    }
60
61    /// Lire la tension actuelle du servo (en V)
62    pub fn read_voltage(&self, sts_id: u8) -> Option<f32> {
63        let mut port = self.port_handler.lock().unwrap();
64        let mut handler = ProtocolPacketHandler::new(&mut *port);
65        let (voltage, comm, error) = handler.read_1byte_tx_rx(sts_id, STS_PRESENT_VOLTAGE);
66        if comm.is_success() && error == 0 {
67            Some(voltage as f32 * 0.1)
68        } else {
69            None
70        }
71    }
72
73    /// Lire le courant actuel du servo (en mA)
74    pub fn read_current(&self, sts_id: u8) -> Option<f32> {
75        let mut port = self.port_handler.lock().unwrap();
76        let mut handler = ProtocolPacketHandler::new(&mut *port);
77        let (current, comm, error) = handler.read_1byte_tx_rx(sts_id, STS_PRESENT_CURRENT_L);
78        if comm.is_success() && error == 0 {
79            Some(current as f32 * 6.5)
80        } else {
81            None
82        }
83    }
84
85    /// Lire la température actuelle du servo (en °C)
86    pub fn read_temperature(&self, sts_id: u8) -> Option<u8> {
87        let mut port = self.port_handler.lock().unwrap();
88        let mut handler = ProtocolPacketHandler::new(&mut *port);
89        let (temperature, comm, error) = handler.read_1byte_tx_rx(sts_id, STS_PRESENT_TEMPERATURE);
90        if comm.is_success() && error == 0 {
91            Some(temperature)
92        } else {
93            None
94        }
95    }
96
97    /// Lire la valeur d'accélération actuelle du servo
98    pub fn read_acceleration(&self, sts_id: u8) -> Option<u8> {
99        let mut port = self.port_handler.lock().unwrap();
100        let mut handler = ProtocolPacketHandler::new(&mut *port);
101        let (acc, comm, error) = handler.read_1byte_tx_rx(sts_id, STS_ACC);
102        if comm.is_success() && error == 0 {
103            Some(acc)
104        } else {
105            None
106        }
107    }
108
109    /// Lire le mode actuel du servo
110    /// - 0: Mode position
111    /// - 1: Mode vitesse constante
112    /// - 2: Mode PWM
113    /// - 3: Mode servo pas à pas
114    pub fn read_mode(&self, sts_id: u8) -> Option<u8> {
115        let mut port = self.port_handler.lock().unwrap();
116        let mut handler = ProtocolPacketHandler::new(&mut *port);
117        let (mode, comm, error) = handler.read_1byte_tx_rx(sts_id, STS_MODE);
118        if comm.is_success() && error == 0 {
119            Some(mode)
120        } else {
121            None
122        }
123    }
124
125    /// Lire la correction de position actuelle du servo
126    pub fn read_correction(&self, sts_id: u8) -> Option<i16> {
127        let mut port = self.port_handler.lock().unwrap();
128        let mut handler = ProtocolPacketHandler::new(&mut *port);
129        let (correction, comm, error) = handler.read_2byte_tx_rx(sts_id, STS_OFS_L);
130        if comm.is_success() && error == 0 {
131            let mask = 0x07FFF;
132            let mut bits = correction & mask;
133            if (correction & 0x0800) != 0 {
134                bits = bits & 0x7FF;
135                Some(-(bits as i16))
136            } else {
137                Some(bits as i16)
138            }
139        } else {
140            None
141        }
142    }
143
144    /// Le servo est-il en mouvement ?
145    pub fn is_moving(&self, sts_id: u8) -> Option<bool> {
146        let mut port = self.port_handler.lock().unwrap();
147        let mut handler = ProtocolPacketHandler::new(&mut *port);
148        let (moving, comm, error) = handler.read_1byte_tx_rx(sts_id, STS_MOVING);
149        if comm.is_success() && error == 0 {
150            Some(moving != 0)
151        } else {
152            None
153        }
154    }
155
156    /// Configurer la valeur d'accélération pour le servo
157    /// acc: Valeur d'accélération (0-254). Unité: 100 step/s²
158    pub fn set_acceleration(&self, sts_id: u8, acc: u8) -> Option<bool> {
159        let mut port = self.port_handler.lock().unwrap();
160        let mut handler = ProtocolPacketHandler::new(&mut *port);
161        let (comm, error) = handler.write_tx_rx(sts_id, STS_ACC, &[acc]);
162        if comm.is_success() && error == 0 {
163            Some(true)
164        } else {
165            None
166        }
167    }
168
169    /// Configurer la valeur de vitesse pour le servo
170    /// speed: Valeur de vitesse (0-3400). Unité: Step/s
171    pub fn set_speed(&self, sts_id: u8, speed: u16) -> Option<bool> {
172        let mut port = self.port_handler.lock().unwrap();
173        let mut handler = ProtocolPacketHandler::new(&mut *port);
174        let (comm, error) = handler.write_2byte_tx_rx(sts_id, STS_GOAL_SPEED_L, speed);
175        if comm.is_success() && error == 0 {
176            Some(true)
177        } else {
178            None
179        }
180    }
181
182    /// Désactiver le torque du servo (Mettre le couple à 0)
183    pub fn disable_torque(&self, sts_id: u8) -> Result<(), String> {
184        let mut port = self.port_handler.lock().unwrap();
185        let mut handler = ProtocolPacketHandler::new(&mut *port);
186        let (comm, error) = handler.write_tx_rx(sts_id, STS_TORQUE_ENABLE, &[0]);
187        if comm.is_success() && error == 0 {
188            Ok(())
189        } else {
190            Err(format!("Failed to disable torque for servo {}: comm={:?}, error={}", sts_id, comm, error))
191        }
192    }
193
194    /// Activer le torque du servo (Mettre le couple à 1)
195    pub fn enable_torque(&self, sts_id: u8) -> Result<(), String> {
196        let mut port = self.port_handler.lock().unwrap();
197        let mut handler = ProtocolPacketHandler::new(&mut *port);
198        let (comm, error) = handler.write_tx_rx(sts_id, STS_TORQUE_ENABLE, &[1]);
199        if comm.is_success() && error == 0 {
200            Ok(())
201        } else {
202            Err(format!("Failed to enable torque for servo {}: comm={:?}, error={}", sts_id, comm, error))
203        }
204    }
205
206    /// Arrêter le servo (Mettre le couple à 0)
207    /// 
208    /// **Deprecated:** Utilisez `disable_torque` à la place
209    #[deprecated(since = "0.1.0", note = "Utilisez disable_torque à la place")]
210    pub fn stop_servo(&self, sts_id: u8) -> Option<bool> {
211        self.disable_torque(sts_id).ok().map(|_| true)
212    }
213
214    /// Démarrer le servo (Mettre le couple à 1)
215    /// 
216    /// **Deprecated:** Utilisez `enable_torque` à la place
217    #[deprecated(since = "0.1.0", note = "Utilisez enable_torque à la place")]
218    pub fn start_servo(&self, sts_id: u8) -> Result<(), String> {
219        self.enable_torque(sts_id)
220    }
221
222    /// Configurer le mode opérationnel du servo
223    /// mode: ID du mode (0, 1, 2 ou 3)
224    pub fn set_mode(&self, sts_id: u8, mode: u8) -> Result<(), String> {
225        let mut port = self.port_handler.lock().unwrap();
226        let mut handler = ProtocolPacketHandler::new(&mut *port);
227        let (comm, _error) = handler.write_tx_rx(sts_id, STS_MODE, &[mode]);
228        if comm.is_success() {
229            Ok(())
230        } else {
231            Err("Failed to set mode".to_string())
232        }
233    }
234
235    /// Ajouter une correction de position
236    /// correction: correction (en steps, peut être négatif)
237    pub fn correct_position(&self, sts_id: u8, correction: i16) -> Result<(), String> {
238        let mut corr = correction.abs() as u16;
239        if corr > MAX_CORRECTION {
240            corr = MAX_CORRECTION;
241        }
242
243        let mut port = self.port_handler.lock().unwrap();
244        let mut handler = ProtocolPacketHandler::new(&mut *port);
245        
246        let lo = handler.sts_lobyte(corr);
247        let mut hi = handler.sts_hibyte(corr);
248
249        if correction < 0 {
250            hi |= 1 << 3;
251        }
252
253        let (comm, _error) = handler.write_tx_rx(sts_id, STS_OFS_L, &[lo, hi]);
254        if comm.is_success() {
255            Ok(())
256        } else {
257            Err("Failed to correct position".to_string())
258        }
259    }
260
261    /// Commencer la rotation
262    /// speed: vitesse du servo (peut être négatif, si oui rotation dans le sens inverse)
263    pub fn rotate(&self, sts_id: u8, speed: i16) -> Result<(), String> {
264        self.set_mode(sts_id, 1)?;
265
266        let abs_speed = speed.abs() as u16;
267        let abs_speed = if abs_speed > MAX_SPEED {
268            MAX_SPEED
269        } else {
270            abs_speed
271        };
272
273        let mut port = self.port_handler.lock().unwrap();
274        let mut handler = ProtocolPacketHandler::new(&mut *port);
275        
276        let lo = handler.sts_lobyte(abs_speed);
277        let mut hi = handler.sts_hibyte(abs_speed);
278
279        if speed < 0 {
280            hi |= 1 << 7;
281        }
282
283        let (comm, _error) = handler.write_tx_rx(sts_id, STS_GOAL_SPEED_L, &[lo, hi]);
284        if comm.is_success() {
285            Ok(())
286        } else {
287            Err("Failed to rotate".to_string())
288        }
289    }
290
291    /// Obtenir la prochaine position bloquante
292    fn get_block_position(&self, sts_id: u8) -> Option<u16> {
293        let mut stop_matches = 0;
294        loop {
295            let moving = self.is_moving(sts_id)?;
296
297            if !moving {
298                let position = self.read_position(sts_id);
299                let _ = self.set_mode(sts_id, 0);
300                let _ = self.disable_torque(sts_id);
301
302                if let Some(pos) = position {
303                    stop_matches += 1;
304                    if stop_matches > 4 {
305                        return Some(pos);
306                    }
307                } else {
308                    return None;
309                }
310            } else {
311                stop_matches = 0;
312            }
313
314            thread::sleep(Duration::from_millis(20));
315        }
316    }
317
318    /// Définir la position 2048 (Mettre le couple à 128)
319    pub fn define_middle(&self, sts_id: u8) -> Option<bool> {
320        let mut port = self.port_handler.lock().unwrap();
321        let mut handler = ProtocolPacketHandler::new(&mut *port);
322        let (comm, error) = handler.write_tx_rx(sts_id, STS_TORQUE_ENABLE, &[128]);
323        if comm.is_success() && error == 0 {
324            Some(true)
325        } else {
326            None
327        }
328    }
329
330    /// Étalonner un servo: Trouver ses positions min et max, puis configurer la nouvelle position 0
331    /// ATTENTION: Ne doit être utilisé que pour un servo ayant au moins une position bloquante
332    pub fn tare_servo(&self, sts_id: u8) -> (Option<u16>, Option<u16>) {
333        if self.correct_position(sts_id, 0).is_err() {
334            return (None, None);
335        }
336
337        thread::sleep(Duration::from_millis(500));
338
339        self.set_acceleration(sts_id, 100);
340        let _ = self.rotate(sts_id, -250);
341        thread::sleep(Duration::from_millis(500));
342
343        let min_position = self.get_block_position(sts_id);
344
345        let _ = self.rotate(sts_id, 250);
346        thread::sleep(Duration::from_millis(500));
347
348        let max_position = self.get_block_position(sts_id);
349
350        if let (Some(mut min_pos), Some(mut max_pos)) = (min_position, max_position) {
351            let distance = if min_pos >= max_pos {
352                ((MAX_POSITION - min_pos + max_pos) / 2) as i16
353            } else {
354                ((max_pos - min_pos) / 2) as i16
355            };
356
357            let corr = if min_pos > MAX_POSITION / 2 {
358                min_pos as i16 - MAX_POSITION as i16 - 1
359            } else {
360                min_pos as i16
361            };
362
363            if self.correct_position(sts_id, corr).is_ok() {
364                min_pos = 0;
365                max_pos = (distance * 2) as u16;
366                thread::sleep(Duration::from_millis(500));
367
368                self.move_to(sts_id, distance as u16, 2400, 50, false);
369            }
370
371            return (Some(min_pos), Some(max_pos));
372        }
373
374        (None, None)
375    }
376
377    /// Déplacer le servo vers une position prédéfinie
378    /// position: Nouvelle position du servo
379    /// speed: Vitesse de déplacement en step/s (facultatif, 2400 par défaut)
380    /// acc: Vitesse d'accélération en step/s² (facultatif, 50 par défaut)
381    /// wait: Attendre que la position soit atteinte avant le retour de la fonction
382    pub fn move_to(&self, sts_id: u8, position: u16, speed: u16, acc: u8, wait: bool) -> Option<bool> {
383        self.set_mode(sts_id, 0).ok()?;
384        self.set_acceleration(sts_id, acc)?;
385        self.set_speed(sts_id, speed)?;
386
387        let curr_pos = self.read_position(sts_id)?;
388
389        self.write_position(sts_id, position)?;
390
391        if wait {
392            let distance = (position as i32 - curr_pos as i32).abs() as f64;
393            let time_to_speed = speed as f64 / (acc as f64 * 100.0);
394            let distance_acc = 0.5 * (acc as f64 * 100.0) * time_to_speed.powi(2);
395
396            let time_wait = if distance_acc >= distance {
397                (2.0 * distance / acc as f64).sqrt()
398            } else {
399                let remain_distance = distance - distance_acc;
400                time_to_speed + (remain_distance / speed as f64)
401            };
402
403            thread::sleep(Duration::from_secs_f64(time_wait));
404        }
405
406        Some(true)
407    }
408
409    /// Écrire la position
410    pub fn write_position(&self, sts_id: u8, position: u16) -> Option<bool> {
411        let mut port = self.port_handler.lock().unwrap();
412        let mut handler = ProtocolPacketHandler::new(&mut *port);
413        let (comm, error) = handler.write_2byte_tx_rx(sts_id, STS_GOAL_POSITION_L, position);
414        if comm.is_success() && error == 0 {
415            Some(true)
416        } else {
417            None
418        }
419    }
420
421    /// Obtenir le statut des capteurs
422    pub fn read_status(&self, sts_id: u8) -> Option<HashMap<String, bool>> {
423        let mut port = self.port_handler.lock().unwrap();
424        let mut handler = ProtocolPacketHandler::new(&mut *port);
425        let (status_byte, comm, error) = handler.read_1byte_tx_rx(sts_id, STS_STATUS);
426        
427        if !comm.is_success() || error != 0 {
428            return None;
429        }
430
431        let status_bits = [
432            "Voltage",
433            "Sensor",
434            "Temperature",
435            "Current",
436            "Angle",
437            "Overload",
438        ];
439
440        let mut status = HashMap::new();
441        for (i, name) in status_bits.iter().enumerate() {
442            status.insert(name.to_string(), (status_byte & (1 << i)) == 0);
443        }
444
445        Some(status)
446    }
447
448    /// Obtenir la position actuelle
449    pub fn read_position(&self, sts_id: u8) -> Option<u16> {
450        let mut port = self.port_handler.lock().unwrap();
451        let mut handler = ProtocolPacketHandler::new(&mut *port);
452        let (position, comm, error) = handler.read_2byte_tx_rx(sts_id, STS_PRESENT_POSITION_L);
453        if comm.is_success() && error == 0 {
454            Some(position)
455        } else {
456            None
457        }
458    }
459
460    /// Obtenir la vitesse actuelle
461    pub fn read_speed(&self, sts_id: u8) -> Option<i16> {
462        let mut port = self.port_handler.lock().unwrap();
463        let mut handler = ProtocolPacketHandler::new(&mut *port);
464        let (speed, comm, error) = handler.read_2byte_tx_rx(sts_id, STS_PRESENT_SPEED_L);
465        if comm.is_success() && error == 0 {
466            Some(handler.sts_tohost(speed, 15))
467        } else {
468            None
469        }
470    }
471
472    /// Verrouiller l'EEPROM du servo
473    pub fn lock_eprom(&self, sts_id: u8) -> CommResult {
474        let mut port = self.port_handler.lock().unwrap();
475        let mut handler = ProtocolPacketHandler::new(&mut *port);
476        handler.write_1byte_tx_only(sts_id, STS_LOCK, 1)
477    }
478
479    /// Déverrouiller l'EEPROM du servo
480    pub fn unlock_eprom(&self, sts_id: u8) -> CommResult {
481        let mut port = self.port_handler.lock().unwrap();
482        let mut handler = ProtocolPacketHandler::new(&mut *port);
483        handler.write_1byte_tx_only(sts_id, STS_LOCK, 0)
484    }
485
486    /// Changer l'ID d'un servo
487    /// sts_id: ID actuel du servo (1 pour un servo neuf)
488    /// new_id: Nouvel ID pour le servo (0-253)
489    pub fn change_id(&self, sts_id: u8, new_id: u8) -> Result<(), String> {
490        if new_id > 253 {
491            return Err("new_id must be between 0 and 253".to_string());
492        }
493
494        if !self.ping_servo(sts_id) {
495            return Err(format!("Could not find servo: {}", sts_id));
496        }
497
498        if !self.unlock_eprom(sts_id).is_success() {
499            return Err("Could not unlock Eprom".to_string());
500        }
501
502        let mut port = self.port_handler.lock().unwrap();
503        let mut handler = ProtocolPacketHandler::new(&mut *port);
504        if !handler.write_1byte_tx_only(sts_id, STS_ID, new_id).is_success() {
505            return Err("Could not change Servo ID".to_string());
506        }
507
508        drop(port);
509        let _ = self.lock_eprom(sts_id);
510        
511        Ok(())
512    }
513}
st3215/st3215.rs

st3215/
st3215.rs
