cocube_rs/
lib.rs

1use std::collections::HashMap;
2use std::net::{UdpSocket, SocketAddr};
3use std::sync::{Arc, Mutex};
4use std::thread;
5use std::time::{Duration, Instant};
6use tracing::info;
7use uuid::Uuid;
8#[derive(Debug,Clone)]
9pub struct CoCube {
10    #[allow(dead_code)]
11    robot_id: u32,
12    #[allow(dead_code)]
13    robot_addr: SocketAddr,
14    sock_send: Arc<UdpSocket>,
15    state: Arc<Mutex<RobotState>>,
16}
17
18#[derive(Debug)]
19struct RobotState {
20    pos_x: i32,
21    pos_y: i32,
22    pos_m: [i32; 2],
23    yaw: i32,
24    pos_direction: i32,
25    uuid_func: HashMap<String, String>,
26    uuid_result: HashMap<String, String>,
27}
28
29impl CoCube {
30    pub fn new(robot_id: u32, gateway: &str, local_ip: &str, ip_prefix: u32, udp_port: u16) -> std::io::Result<Self> {
31        let robot_ip = format!("{}.{}", gateway.split('.').take(3).collect::<Vec<&str>>().join("."), ip_prefix + robot_id);
32        let robot_port = udp_port + robot_id as u16;
33        
34        let sock_listen = Arc::new(UdpSocket::bind((local_ip, robot_port))?);
35        sock_listen.set_read_timeout(Some(Duration::from_secs(3)))?;
36        
37        let sock_send = Arc::new(UdpSocket::bind((local_ip, 0))?);
38        sock_send.connect((robot_ip.as_str(), udp_port))?;
39
40        let state = Arc::new(Mutex::new(RobotState {
41            pos_x: 0,
42            pos_y: 0,
43            pos_m: [0, 0],
44            yaw: 0,
45            pos_direction: 0,
46            uuid_func: HashMap::new(),
47            uuid_result: HashMap::new(),
48        }));
49
50        let state_clone = Arc::clone(&state);
51        let sock_listen_clone = Arc::clone(&sock_listen);
52        
53        thread::spawn(move || {
54            let mut buf = [0u8; 1024];
55            loop {
56                match sock_listen_clone.recv_from(&mut buf) {
57                    Ok((size, _)) => {
58                        if let Ok(data) = String::from_utf8(buf[..size].to_vec()) {
59                            let parts: Vec<&str> = data.split(',').collect();
60                            let mut state = state_clone.lock().unwrap();
61                            
62                            match parts.first().map(|s| s.trim()) {
63                                Some("res") if parts.len() >= 3 => {
64                                    let uuid = parts[1].trim().to_string();
65                                    let result = parts[2].trim().to_string();
66                                    state.uuid_result.insert(uuid.clone(), result);
67                                    state.uuid_func.remove(&uuid);
68                                },
69                                Some("pos") if parts.len() >= 4 => {
70                                    if let (Ok(x), Ok(y), Ok(dir)) = (
71                                        parts[1].trim().parse::<i32>(),
72                                        parts[2].trim().parse::<i32>(),
73                                        parts[3].trim().parse::<i32>(),
74                                    ) {
75                                        state.pos_x = (x as f64 / 64.0) as i32;
76                                        state.pos_y = (y as f64 / 64.0) as i32;
77                                        state.pos_m = [(state.pos_y as f64 * 1.35 * 0.001 )as i32, (state.pos_x as f64 * 1.35 * 0.001) as i32];
78                                        state.pos_direction = dir;
79                                        // state.yaw = if dir < 180 {
80                                        //     dir as f64 * PI / 180.0
81                                        // } else {
82                                        //     (dir - 360) as f64 * PI / 180.0
83                                        // };
84                                    }
85                                },
86                                _ => (),
87                            }
88                        }
89                    },
90                    Err(e) => eprintln!("Error receiving data: {}", e),
91                }
92            }
93        });
94
95        Ok(Self {
96            robot_id,
97            robot_addr: format!("{}:{}", robot_ip, udp_port).parse().unwrap(),
98            sock_send: Arc::clone(&sock_send),
99            state,
100        })
101    }
102
103    pub fn get_position(&self) -> [i32; 3] {
104        let state = self.state.lock().unwrap();
105        [state.pos_x, state.pos_y, state.pos_direction as i32]
106    }
107
108    fn process_data(&self, func: &str, params: &[&str], if_return_uuid: bool, timeout: Duration) -> Option<String> {
109        let uuid = Uuid::new_v4().to_string().chars().take(6).collect::<String>();
110        let if_return = if if_return_uuid { "1" } else { "0" };
111        
112        let params_str = params.iter()
113            .map(|p| if p.parse::<i32>().is_ok() { p.to_string() } else { format!("\"{}\"", p) })
114            .collect::<Vec<String>>()
115            .join(",");
116
117        let message = if params.is_empty() {
118            format!("{},{},{}", if_return, uuid, func)
119        } else {
120            format!("{},{},{},{}", if_return, uuid, func, params_str)
121        };
122        info!("message: {}", message);
123        if if_return_uuid {
124            let mut state = self.state.lock().unwrap();
125            state.uuid_func.insert(uuid.clone(), func.to_string());
126        }
127
128        if let Err(e) = self.sock_send.send(message.as_bytes()) {
129            eprintln!("Failed to send data: {}", e);
130            return None;
131        }
132
133        if if_return_uuid {
134            let start = Instant::now();
135            loop {
136                if start.elapsed() > timeout {
137                    return None;
138                }
139                
140                let mut state = self.state.lock().unwrap();
141                if let Some(result) = state.uuid_result.remove(&uuid) {
142                    return Some(result);
143                }
144                
145                thread::sleep(Duration::from_millis(100));
146            }
147        }
148        
149        None
150    }
151
152    // Motion control methods
153    pub fn set_tft_backlight(&self, flag: u8) {
154        self.process_data("CoCube set TFT backlight", &[&flag.to_string()], false, Duration::from_secs(3));
155    }
156
157    pub fn draw_aruco_marker_on_tft(&self, id: u8) {
158        self.process_data("CoCube draw Aruco Marker on TFT", &[&id.to_string()], false, Duration::from_secs(3));
159    }
160
161    pub fn move_robot(&self, direction: &str, speed: u8) {
162        self.process_data("CoCube move", &[direction, &speed.to_string()], false, Duration::from_secs(3));
163    }
164
165    pub fn rotate_robot(&self, direction: &str, speed: u8) {
166        self.process_data("CoCube rotate", &[direction, &speed.to_string()], false, Duration::from_secs(3));
167    }
168
169    pub fn move_millisecs(&self, direction: &str, speed: u8, duration: u64) -> Option<String> {
170        self.process_data(
171            "CoCube move for millisecs",
172            &[direction, &speed.to_string(), &duration.to_string()],
173            true,
174            Duration::from_secs(3) + Duration::from_millis(duration)
175        )
176    }
177
178    pub fn rotate_millisecs(&self, direction: &str, speed: u8, duration: u64) -> Option<String> {
179        self.process_data(
180            "CoCube rotate for millisecs",
181            &[direction, &speed.to_string(), &duration.to_string()],
182            true,
183            Duration::from_secs(3) + Duration::from_millis(duration)
184        )
185    }
186
187    pub fn move_by_steps(&self, direction: &str, speed: u8, step: u16) -> Option<String> {
188        self.process_data(
189            "CoCube move by step",
190            &[direction, &speed.to_string(), &step.to_string()],
191            true,
192            Duration::from_secs(3)
193        )
194    }
195
196    pub fn rotate_by_degree(&self, direction: &str, speed: u8, degree: u16) -> Option<String> {
197        self.process_data(
198            "CoCube rotate by degree",
199            &[direction, &speed.to_string(), &degree.to_string()],
200            true,
201            Duration::from_secs(3)
202        )
203    }
204
205    pub fn set_wheel_speed(&self, left: u8, right: u8) {
206        self.process_data(
207            "CoCube set wheel",
208            &[&left.to_string(), &right.to_string()],
209            false,
210            Duration::from_secs(3)
211        );
212    }
213
214    pub fn wheels_stop(&self) {
215        self.process_data("CoCube wheels stop", &[], false, Duration::from_secs(3));
216    }
217
218    pub fn wheels_break(&self) {
219        self.process_data("CoCube wheels break", &[], false, Duration::from_secs(3));
220    }
221
222    pub fn rotate_to_angle(&self, angle: i16, speed: u8) {
223        self.process_data(
224            "CoCube rotate to angle",
225            &[&angle.to_string(), &speed.to_string()],
226            false,
227            Duration::from_secs(3)
228        );
229    }
230
231    pub fn rotate_to_target(&self, x: i32, y: i32, speed: u8) {
232        self.process_data(
233            "CoCube rotate to target",
234            &[&x.to_string(), &y.to_string(), &speed.to_string()],
235            false,
236            Duration::from_secs(3)
237        );
238    }
239
240    pub fn move_to(&self, x: i32, y: i32, speed: u8) {
241        self.process_data(
242            "CoCube move to",
243            &[&x.to_string(), &y.to_string(), &speed.to_string()],
244            false,
245            Duration::from_secs(3)
246        );
247    }
248
249
250    pub fn move_to_target(&self, x: i32, y: i32, speed: u8) {
251        self.process_data(
252            "myreturn_uuid move to target",
253            &[&x.to_string(), &y.to_string(), &speed.to_string()],
254            false,
255            Duration::from_secs(3)
256        );
257    }
258
259    // Module control methods
260    pub fn power_on_module(&self) {
261        self.process_data("Power on module", &[], false, Duration::from_secs(3));
262    }
263
264    pub fn power_off_module(&self) {
265        self.process_data("Power off module", &[], false, Duration::from_secs(3));
266    }
267
268    pub fn gripper_open(&self) {
269        self.process_data("ccmodule_gripper open", &[], false, Duration::from_secs(3));
270    }
271
272    pub fn gripper_close(&self) {
273        self.process_data("ccmodule_gripper close", &[], false, Duration::from_secs(3));
274    }
275
276    pub fn gripper_degree(&self, degree: i8) {
277        self.process_data("Gripper degree", &[&degree.to_string()], false, Duration::from_secs(3));
278    }
279
280    // NeoPixel control
281    pub fn attach_neo_pixel(&self) {
282        self.process_data("attach NeoPixel", &[], false, Duration::from_secs(3));
283    }
284
285    pub fn set_all_neo_pixels_color(&self, r: u8, g: u8, b: u8) {
286        let color = (r as u32) << 16 | (g as u32) << 8 | b as u32;
287        self.process_data("set all NeoPixels color", &[&color.to_string()], false, Duration::from_secs(3));
288    }
289
290    pub fn clear_neo_pixels(&self) {
291        self.process_data("clear NeoPixels", &[], false, Duration::from_secs(3));
292    }
293
294    // Display control
295    pub fn mb_display(&self, code: u32) -> Option<String> {
296        self.process_data("[display:mbDisplay]", &[&code.to_string()], true, Duration::from_secs(3))
297    }
298
299    pub fn mb_display_off(&self) -> Option<String> {
300        self.process_data("[display:mbDisplayOff]", &[], true, Duration::from_secs(3))
301    }
302
303    pub fn set_display_color(&self, r: u8, g: u8, b: u8) {
304        let color = (r as u32) << 16 | (g as u32) << 8 | b as u32;
305        self.process_data("set display color", &[&color.to_string()], false, Duration::from_secs(3));
306    }
307}
cocube_rs/lib.rs

cocube_rs/
lib.rs
