Receiving udp packets successfully and also using bitflags now.

grebneerg · Nov 29, 2018 · aac24ea · aac24ea
1 parent 21a1697
commit aac24ea
Show file tree

Hide file tree

Showing 6 changed files with 61 additions and 36 deletions.
diff --git a/Cargo.lock b/Cargo.lock
diff --git a/Cargo.toml b/Cargo.toml
@@ -6,3 +6,4 @@ authors = ["Jack Greenberg <theProgrammerJack@gmail.com>"]
 [dependencies]
 chrono = "0.4.6"
 byteorder = "1.2.7"
+bitflags = "1.0.4"
diff --git a/src/connection.rs b/src/connection.rs
@@ -31,10 +31,12 @@ impl DSConnection {
 
         let t = thread::spawn(move || {
             println!("udp start");
-            let udp = UdpSocket::bind("169.254.65.205:1150").unwrap();
-            println!("udp 2");
+            let udp = UdpSocket::bind("169.254.65.205:1149").unwrap();
+
             udp.connect(SocketAddr::new(addr.clone(), 1110)).unwrap();
-            udp.set_nonblocking(true).unwrap();
+			println!("udp 2");
+			let udp_recv = UdpSocket::bind("169.254.65.205:1150").unwrap();
+            udp_recv.set_nonblocking(true).unwrap();
             println!("udp started");
 
             udp.send(state.lock().unwrap().udp_packet().as_ref())
@@ -51,16 +53,15 @@ impl DSConnection {
                 }
 
                 let mut udp_buf = vec![0u8; 100];
-                match udp.recv_from(&mut udp_buf) {
-                    Ok((n, f)) => println!("packet received: {:?}", udp_buf),
+                match udp_recv.recv_from(&mut udp_buf) {
+                    Ok((n, f)) => if let Some(packet) = RioUdpPacket::from_bytes(Vec::from(&udp_buf[0..n])) {
+						state.lock().unwrap().update_from_udp(packet);
+					},
                     Err(e) => {
                         if e.kind() != io::ErrorKind::WouldBlock {
                             if let Err(e) = sender_res.send(Err(e)) {
                                 break;
                             }
-                            println!("uh oh");
-                        } else {
-                            println!("got nothing");
                         }
                     }
                 }

diff --git a/src/ds.rs b/src/ds.rs
@@ -32,7 +32,7 @@ impl DriverStationState {
         self.sequence_num += 1;
 
         packet.write_u8(0x01); // comm version
-        packet.write_u8(self.control_byte()); // control byte
+        packet.write_u8(self.control_byte().bits()); // control byte
         packet.write_u8(0); // TODO: actually restart code or rio with this byte.
         packet.write_u8(self.alliance.to_position_u8()); // alliance
 
@@ -64,17 +64,17 @@ impl DriverStationState {
         packet.into_vec()
     }
 
-    fn control_byte(&self) -> u8 {
-        let mut byte: u8 = 0;
+    fn control_byte(&self) -> Control {
+        let mut byte = Control::empty();
         if self.estop {
-            byte |= 0b1000_0000;
+            byte |= Control::ESTOP;
         }
         // fms is never connected, but if it were that would go here
         if self.enabled {
-            byte |= 0b0000_0100;
+            byte |= Control::ENABLED;
         }
 
-        byte |= self.mode as u8;
+        byte |= Control::from_bits(self.mode as u8).unwrap();
 
         byte
     }
@@ -121,3 +121,29 @@ impl Default for DriverStationState {
         }
     }
 }
+
+bitflags! {
+	pub struct Control: u8 {
+		const ESTOP = 0b1000_0000;
+        const FMS_CONNECTED = 0b0000_1000;
+        const ENABLED = 0b0000_0100;
+
+		const TELEOP = 0b00;
+        const TEST = 0b01;
+        const AUTO = 0b10;
+	}
+}
+
+impl Control {
+	pub fn robot_mode(&self) -> Option<RobotMode> {
+		return if self.contains(Control::AUTO) {
+			Some(RobotMode::Auto)
+		} else if self.contains(Control::TELEOP) {
+			Some(RobotMode::Teleop)
+		} else if self.contains(Control::TEST) {
+			Some(RobotMode::Test)
+		} else {
+			None
+		}
+	}
+}
diff --git a/src/lib.rs b/src/lib.rs
@@ -1,5 +1,7 @@
 extern crate byteorder;
 extern crate chrono;
+#[macro_use]
+extern crate bitflags;
 
 use byteorder::{ByteOrder, NetworkEndian};
 use chrono::prelude::*;

diff --git a/src/messages.rs b/src/messages.rs
@@ -3,27 +3,15 @@ use std::convert::From;
 use packet::PacketReader;
 use states::RobotMode;
 
-pub struct Trace {
-    robot_code: bool,
-    is_roborio: bool,
-    // these modes don't seem to quite line up with what we send
-    test_mode: bool,
-    auto_mode: bool,
-    teleop_code: bool,
-    disabled: bool,
-}
-
-impl From<u8> for Trace {
-    fn from(byte: u8) -> Self {
-        Trace {
-            robot_code: byte & 0b0010_0000 != 0,
-            is_roborio: byte & 0b0001_0000 != 0,
-            test_mode: byte & 0b0000_1000 != 0,
-            auto_mode: byte & 0b0000_0100 != 0,
-            teleop_code: byte & 0b0000_0010 != 0,
-            disabled: byte & 0b0000_0001 != 0,
-        }
-    }
+bitflags! {
+	pub struct Trace: u8 {
+		const ROBOT_CODE = 0b0010_0000;
+		const IS_ROBORIO = 0b0001_0000;
+		const TEST_MODE = 0b0000_1000;
+		const AUTO_MODE = 0b0000_0100;
+		const TELEOP_CODE = 0b0000_0010;
+		const DISABLED = 0b0000_0001;
+	}
 }
 
 pub struct Status {
@@ -66,7 +54,7 @@ impl RioUdpPacket {
                 sequence_num: packet.next_u16().unwrap(),
                 comm_version: packet.next_u8().unwrap(),
                 status: packet.next_u8().unwrap().into(),
-                trace: packet.next_u8().unwrap().into(),
+                trace: Trace::from_bits(packet.next_u8().unwrap()).unwrap(),
                 battery_voltage: f32::from(packet.next_u8().unwrap())
                     + f32::from(packet.next_u8().unwrap()) / 256.0,
                 request_date: packet.next_u8().unwrap() == 0x01,