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

st3215/
st3215.rs
