driver_station/
driverstation.rs

1use std::mem::size_of;
2use std::net::UdpSocket;
3
4// NOTE: Produces ip addresses with leading zeros. This should NOT be a problem according to the rust docs. https://doc.rust-lang.org/std/net/struct.Ipv4Addr.html#textual-representation
5pub fn team_number_to_ip(team_number: u16) -> String {
6    let mut tn = team_number.to_string();
7    tn = "0".repeat(4 - tn.len()) + &tn;
8    return "10.".to_owned() + &tn[0..2] + "." + &tn[2..4] + ".2";
9}
10
11macro_rules! from_be_bytes {
12    ($t:ty, $v:expr) => {
13        {
14            let mut bytes = [0u8; size_of::<$t>()];
15            bytes.clone_from_slice($v);
16
17            <$t>::from_be_bytes(bytes)
18        }
19    }
20}
21
22pub struct DriverStation {
23    team_number: u16,
24    rio_socket: UdpSocket,
25    mode: Mode,
26    enabled: bool,
27    sequence: u16,
28    e_stopped: bool,
29    fms_connected: bool,
30    alliance: Alliance,
31    restart_rio: bool,
32    restart_code: bool,
33}
34
35impl DriverStation {
36    pub async fn new(team: u16) -> Self {
37        return Self {
38            team_number: team,
39            rio_socket: UdpSocket::bind("172.0.0.1:4000").expect("Error: Failed to bind socket"),
40            mode: Mode::Teleop,
41            enabled: false,
42            sequence: 0,
43            e_stopped: false,
44            fms_connected: false,
45            alliance: Alliance::Red { val: 1 },
46            restart_rio: false,
47            restart_code: false,
48        }
49    }
50
51    pub async fn receive_rio_udp(self) -> FromRioUdpPacket {
52        let mut buf = [0u8; 1500];
53        let (num_bytes, _) = self.rio_socket.recv_from(&mut buf).unwrap();
54        
55        return FromRioUdpPacket::from_packet(&buf[..num_bytes]);
56    }
57
58    pub async fn send_rio_udp(self) -> Result<usize, std::io::Error> {
59        let mut control: u8 = 0;
60
61        control += if self.e_stopped { 0b10000000 } else { 0 }; 
62        control += if self.fms_connected { 0b1000 } else { 0 };
63        control += if self.enabled { 0b100 } else { 0 };
64        control += match self.mode {
65            Mode::Teleop => 0,
66            Mode::Test => 1,
67            Mode::Autonomous => 2,
68            _ => 3,
69        };
70
71        let mut request: u8 = 0;
72
73        request += if self.restart_rio { 0b1000 } else { 0 };
74        request += if self.restart_code { 0b100 } else { 0 };
75
76        let main_buf: Vec<u8> = vec![
77            u16::to_be_bytes(self.sequence)[0],
78            u16::to_be_bytes(self.sequence)[1],
79            0x01,
80            control,
81            request,
82            alliance_to_int(self.alliance),
83        ];
84
85        return self.rio_socket.send_to(&main_buf[..], "10.82.30.2:1510");
86    }
87}
88
89pub struct FromRioUdpPacket {
90    sequence: u16,
91    e_stopped: bool,
92    brownout: bool,
93    code_start: bool,
94    enabled: bool,
95    mode: Mode,
96    robot_code: bool,
97    is_rio: bool,
98    test: bool,
99    autonomous: bool,
100    teleop_code: bool,
101    disabled: bool,
102    battery: f32,
103    request_date: bool,
104    tags: Vec<FromRioTag>,
105}
106
107impl FromRioUdpPacket {
108    pub fn from_packet(buf: &[u8]) -> Self {
109        let mut result =  Self {
110            sequence: u16::from_be_bytes(buf[..2].try_into().unwrap()),
111            e_stopped: buf[3] & 0b10000000 != 0,
112            brownout: buf[3] & 0b10000 != 0,
113            code_start: buf[3] & 0b1000 != 0,
114            enabled: buf[3] & 0b100 != 0,
115            mode: match buf[3] % 0b100 {
116                0 => Mode::Teleop,
117                1 => Mode::Test,
118                2 => Mode::Autonomous,
119                _ => Mode::Invalid,
120            },
121            robot_code: buf[4] & 0b100000 != 0,
122            is_rio: buf[4] & 0b10000 != 0,
123            test: buf[4] & 0b1000 != 0,
124            autonomous: buf[4] & 0b100 != 0,
125            teleop_code: buf[4] & 0b10 != 0,
126            disabled: buf[4] % 0b10 != 0,
127            battery: buf[5] as f32 + ( buf[6] as f32 / 256f32 ), 
128            request_date: buf[7] != 0,
129            tags: Vec::new(),
130        };
131
132        let mut tags: Vec<u8> = buf[7..].to_vec();
133
134        while !tags.is_empty() {
135            let mut tag = FromRioTag {
136                size: tags[0],
137                joystick: None,
138                disk: None,
139                cpu: None,
140                ram: None,
141                pdp: None,
142                unknown: None,
143                can: None,
144            };
145
146             match tags[1] {
147                 0x01 => tag.joystick = Some(JoystickOutput {
148                    joysticks: from_be_bytes!(u32, &tags[2..6]),
149                    left_rumble: from_be_bytes!(u16, &tags[6..8]),
150                    right_rumble: from_be_bytes!(u16, &tags[8..10]),
151                 }),
152                 0x04 => tag.disk = Some(DiskInfo {
153                    bytes_available: from_be_bytes!(u32, &tags[2..6]),
154                 }),
155                 0x05 => tag.cpu = Some(CpuInfo {
156                     cpu_count: from_be_bytes!(f32, &tags[2..6]),
157                     cpu_time_critical_per: from_be_bytes!(f32, &tags[6..10]),
158                     cpu_above_normal_per: from_be_bytes!(f32, &tags[10..14]),
159                     cpu_normal_per: from_be_bytes!(f32, &tags[14..18]),
160                     cpu_low_per: from_be_bytes!(f32, &tags[18..22]),
161                 }),
162                 0x06 => tag.ram = Some(RamInfo {
163                     block: from_be_bytes!(u32, &tags[2..6]),
164                     free_space: from_be_bytes!(u32, &tags[6..10]),
165                 }),
166                 0x08 => tag.pdp = Some(PdpLog {
167                     unknown: tags[2],
168                     pdp_stats_1: from_be_bytes!(u64, &tags[3..11]),
169                     pdp_stats_2: from_be_bytes!(u64, &tags[11..19]),
170                     pdp_stats_3: from_be_bytes!(u32, &tags[19..23]),
171                     pdp_stats_4: tags[23],
172                     unknown_2: from_be_bytes!(u16, &tags[24..26]),
173                     unknown_3: tags[26],
174                 }),
175                 0x09 => tag.unknown = Some(UnknownTag {
176                    sec_1: from_be_bytes!(u64, &tags[2..10]),
177                    sec_2: tags[10],
178                 }),
179                 0x0e => tag.can = Some(CanMetrics {
180                     utilization: from_be_bytes!(f32, &tags[2..6]),
181                     bus_off: from_be_bytes!(u32, &tags[6..10]),
182                     tx_full: from_be_bytes!(u32, &tags[10..14]),
183                     rx_errors: tags[14],
184                     tx_errors: tags[15],
185                 }),
186                _ => ()
187            };
188
189            for _ in 0..tag.size {
190                tags.remove(0);
191            }
192
193            result.tags.append(&mut vec![tag]);
194        }
195
196        return result;
197    }
198}
199
200
201pub struct FromRioTag {
202    size: u8,
203    joystick: Option<JoystickOutput>,
204    disk: Option<DiskInfo>,
205    cpu: Option<CpuInfo>,
206    ram: Option<RamInfo>,
207    pdp: Option<PdpLog>,
208    unknown: Option<UnknownTag>,
209    can: Option<CanMetrics>
210        ,
211}
212
213pub struct JoystickOutput {
214    joysticks: u32,
215    left_rumble: u16,
216    right_rumble: u16,
217}
218
219pub struct DiskInfo {
220    bytes_available: u32,
221}
222
223pub struct CpuInfo {
224    cpu_count: f32, // 0x02 on RoboRIO
225    cpu_time_critical_per: f32,
226    cpu_above_normal_per: f32,
227    cpu_normal_per: f32,
228    cpu_low_per: f32,
229}
230
231
232pub struct RamInfo {
233    block: u32,
234    free_space: u32,
235}
236
237// TODO: Figure ut how this behaves with REV PDH
238pub struct PdpLog {
239    unknown: u8,
240    pdp_stats_1: u64,
241    pdp_stats_2: u64,
242    pdp_stats_3: u32,
243    pdp_stats_4: u8,
244    unknown_2: u16,
245    unknown_3: u8,
246}
247
248pub struct UnknownTag {
249    sec_1: u64,
250    sec_2: u8,
251}
252
253pub struct CanMetrics {
254    utilization: f32,
255    bus_off: u32,
256    tx_full: u32,
257    rx_errors: u8,
258    tx_errors: u8,
259}
260
261#[repr(u8)]
262pub enum Mode {
263    Teleop = 0,
264    Test = 1,
265    Autonomous = 2,
266    Invalid = 3,
267}
268
269pub enum Alliance {
270    Red{ val: u8 },
271    Blue{ val: u8 },
272}
273
274fn alliance_to_int(alliance: Alliance) -> u8 {
275    return match alliance {
276        Alliance::Red { val } => (val - 1) % 3,
277        Alliance::Blue { val } => ((val - 1) % 3) + 3,
278    }
279}
280
281fn alliance_from_int(num: u8) -> Alliance {
282    return if num < 3 {
283        Alliance::Red { val: num % 3 + 1 }
284    } else {
285        Alliance::Blue { val:  num % 3 + 1 }
286    }
287}
driver_station/driverstation.rs

driver_station/
driverstation.rs
