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

st3215/
st3215.rs
