Implemented position control

src/lib.rs

@@ -13,6 +13,7 @@ use protocol::{parse_from_bytes, Message};
 use protocol::messages_robocup_ssl_wrapper::SSL_WrapperPacket;
 use protocol::grSim_Packet::grSim_Packet;
 use protocol::grSim_Commands::grSim_Robot_Command;
+use protocol::messages_robocup_ssl_detection::SSL_DetectionFrame;
 use net2::UdpSocketExt;
 use time::{Duration, SteadyTime, precise_time_s};
 
@@ -22,6 +23,8 @@ pub use subproc::{CommandExt, ChildExt};
 mod error;
 mod subproc;
 
+
+
 /// - One thread will bind to 0.0.0.0 and join the multicast to receive and dispatch vision packets
 ///   in a loop;
 /// - Another thread will buffer the dispatched packets and if more than 1s passes after the last
@@ -80,9 +83,42 @@ pub fn demo() {
 
         let mut packets: Vec<SSL_WrapperPacket> = Vec::new();
         let mut last_time = SteadyTime::now();
+        struct Position{x: f32, y: f32, orientation: f32};
+        impl Copy for Position {}
+	impl Clone for Position { fn clone(&self) -> Position { *self } }
+	impl std::ops::Sub<Position> for Position {
+            type Output = Position;
+
+            fn sub(self, _rhs: Position) -> Position {
+                let result = Position{x: self.x - _rhs.x, y: self.y - _rhs.y, orientation: self.orientation - _rhs.orientation};
+		//let result = Position{x:0.0, y: 0.0, orientation:0.0};
+		result
+            }
+        }
+        struct Velocity{normal: f32, tangent: f32, angular: f32};
+        let hold_pos = Position{x: 2000.0, y: 40.0, orientation: 0.0};
+        let mut pos = Position{x: 0.0, y: 0.0, orientation: 0.0};
+	let mut answer = Velocity{normal: 0.0, tangent: 0.0, angular: 0.0};
         loop {
-            packets.push(rx.recv().unwrap());
-
+            let packet: SSL_WrapperPacket = rx.recv().unwrap();
+            {
+	        let frame = packet.get_detection();
+		let robots =  frame.get_robots_blue();
+		pos.x = robots[0].get_x();
+		pos.y = robots[0].get_y();
+                pos.orientation = robots[0].get_orientation();
+            }
+            packets.push(packet);
+            {
+		//let dif_pos = Position{x:hold_pos.x - pos.x, y: hold_pos.y - pos.y, orientation: hold_pos.orientation - pos.orientation};
+		let dif_pos = hold_pos - pos;
+		answer.tangent = 0.0004*( dif_pos.y*(pos.orientation.sin())); 
+		answer.normal = 0.0004*( dif_pos.y*(pos.orientation.cos())); 
+		answer.normal += 0.0004*( -dif_pos.x*(pos.orientation.sin())); 
+		answer.tangent += 0.0004*( dif_pos.x*(pos.orientation.cos())); 
+		answer.angular = 0.8*(dif_pos.orientation);
+                println!("Pos x: {}, pos y: {}, orientation {}", dif_pos.x, dif_pos.y, dif_pos.orientation);
+            }
             let mut packet = grSim_Packet::new();
             {
                 let commands = packet.mut_commands();
@@ -95,9 +131,9 @@ pub fn demo() {
                 robot_command.set_id(0);
                 robot_command.set_kickspeedx(0.0);
                 robot_command.set_kickspeedz(0.0);
-                robot_command.set_veltangent((2.0 * (timestamp * 3.0).cos()) as f32);
-                robot_command.set_velnormal(0.0);
-                robot_command.set_velangular(0.0);
+                robot_command.set_veltangent(answer.tangent);
+                robot_command.set_velnormal(answer.normal);
+                robot_command.set_velangular(answer.angular);
                 robot_command.set_spinner(false);
                 robot_command.set_wheelsspeed(false);
 
@@ -137,7 +173,7 @@ pub fn demo() {
         };
         println!("send socket bound to {}", addr);
 
-        let grsim_ip = Ipv4Addr::new(192, 168, 91, 92);
+        let grsim_ip = Ipv4Addr::new(0, 0, 0, 0);
         let grsim_addr = SocketAddrV4::new(grsim_ip, 20011);
 
         let mut last_time = SteadyTime::now();