Build a ToolCommand packet from teleop ToolCommand values.
(
axis: f32,
motor: f32,
action_a: bool,
action_b: bool,
sequence: u16,
)
| 186 | |
| 187 | /// Build a ToolCommand packet from teleop ToolCommand values. |
| 188 | pub fn build_tool_command( |
| 189 | axis: f32, |
| 190 | motor: f32, |
| 191 | action_a: bool, |
| 192 | action_b: bool, |
| 193 | sequence: u16, |
| 194 | ) -> [u8; COMMAND_SIZE] { |
| 195 | let axis_i16 = (axis.clamp(-1.0, 1.0) * 32767.0) as i16; |
| 196 | let motor_i16 = (motor.clamp(-1.0, 1.0) * 32767.0) as i16; |
| 197 | let ts = std::time::SystemTime::now() |
| 198 | .duration_since(std::time::UNIX_EPOCH) |
| 199 | .map(|d| d.as_millis() as u32) |
| 200 | .unwrap_or(0); |
| 201 | |
| 202 | EthCommand::tool_command( |
| 203 | axis_i16, |
| 204 | motor_i16, |
| 205 | action_a as u8, |
| 206 | action_b as u8, |
| 207 | sequence, |
| 208 | ts, |
| 209 | ) |
| 210 | .serialize() |
| 211 | } |
| 212 | |
| 213 | /// Build a SetState packet from rover Mode. |
| 214 | pub fn build_set_state(rover_state: u8, sequence: u16) -> [u8; COMMAND_SIZE] { |