Squashed commit of the following:

commit c40828d Author: mablin7 <[email protected]> Date: Fri Apr 5 14:18:25 2024 +0200 fix: formatting commit 3096877 Author: Renyi Yang <[email protected]> Date: Sat Mar 30 01:39:53 2024 +0100 fix commit 8bd571c Author: Renyi Yang <[email protected]> Date: Sat Mar 30 00:59:50 2024 +0100 fix checkstyle feat: stop controller commit ad3b721 Author: Renyi Yang <[email protected]> Date: Fri Mar 29 19:33:15 2024 +0100 feat: halt controller commit 9045ef3 Author: Renyi Yang <[email protected]> Date: Fri Mar 29 18:45:16 2024 +0100 feat: playercontroller commit 85f4ce2 Author: mablin7 <[email protected]> Date: Thu Mar 28 20:40:49 2024 +0100 Squashed commit of the following: commit 0770d99 Author: mablin7 <[email protected]> Date: Wed Mar 27 15:18:37 2024 +0100 fix ball detection commit c7d9861 Author: mablin7 <[email protected]> Date: Fri Mar 22 15:25:57 2024 +0100 fix: main.rs commit 21b47d0 Merge: a01cf44 1353d76 Author: mablin7 <[email protected]> Date: Fri Mar 22 15:22:23 2024 +0100 Merge branch 'main' into simulator commit a01cf44 Author: mablin7 <[email protected]> Date: Thu Mar 21 20:37:25 2024 +0100 feat: sim Co-authored-by: Teodor Neagoe <[email protected]> commit a96fbe9 Author: Teodor Neagoe <[email protected]> Date: Thu Mar 21 17:47:19 2024 +0100 step-1-simualtor commit 1353d76 Merge: 75b1407 e1ceccd Author: Balint Magyar <[email protected]> Date: Thu Mar 21 15:32:21 2024 +0100 Merge pull request #13 from DelftMercurians/teo-optimize-run-on-server Teo optimize run on server commit e1ceccd Merge: 2c54253 75b1407 Author: Balint Magyar <[email protected]> Date: Thu Mar 21 15:32:03 2024 +0100 Merge branch 'main' into teo-optimize-run-on-server commit 2c54253 Author: mablin7 <[email protected]> Date: Thu Mar 21 15:31:20 2024 +0100 fix commit 75b1407 Merge: 89746a5 a6befa4 Author: Balint Magyar <[email protected]> Date: Wed Mar 20 16:48:15 2024 +0100 Merge pull request #12 from DelftMercurians/filter Feat(world): Kalman Filter commit a6befa4 Author: mablin7 <[email protected]> Date: Wed Mar 20 16:47:04 2024 +0100 fix: fmt commit 708fb76 Author: mablin7 <[email protected]> Date: Wed Mar 20 15:41:19 2024 +0100 sim commit 434988d Author: Renyi Yang <[email protected]> Date: Tue Mar 19 21:00:18 2024 +0100 misc commit 5ba2ab4 Author: Renyi Yang <[email protected]> Date: Tue Mar 19 20:51:12 2024 +0100 . commit 95622fd Author: Renyi Yang <[email protected]> Date: Wed Mar 13 01:47:37 2024 +0100 fix: test3d longer duration commit 253bccb Author: Renyi Yang <[email protected]> Date: Wed Mar 13 01:41:51 2024 +0100 document commit 40e9b3e Author: Renyi Yang <[email protected]> Date: Wed Mar 13 00:46:38 2024 +0100 misc: checkstyle naming, dead code commit e95a0de Author: Renyi Yang <[email protected]> Date: Wed Mar 13 00:22:14 2024 +0100 refactor: remove builder commit 089da8c Author: Renyi Yang <[email protected]> Date: Tue Mar 12 23:20:11 2024 +0100 refactor: switch to smatrix commit 99e78b8 Author: Renyi Yang <[email protected]> Date: Tue Mar 12 19:32:01 2024 +0100 feat: update method not guarantee it functions. still need to test. commit 4d08d42 Author: Renyi Yang <[email protected]> Date: Tue Mar 12 17:15:40 2024 +0100 outlier detect commit 79ec7e9 Author: Renyi Yang <[email protected]> Date: Tue Mar 12 13:52:13 2024 +0100 test: builder test commit d149803 Author: Renyi Yang <[email protected]> Date: Tue Mar 12 01:40:16 2024 +0100 feat: new build for ball filter commit 31f2d46 Author: Renyi Yang <[email protected]> Date: Mon Mar 11 23:40:11 2024 +0100 misc: var instead of std commit 53154bc Author: mablin7 <[email protected]> Date: Wed Mar 6 20:20:33 2024 +0100 fix commit 89746a5 Author: mablin7 <[email protected]> Date: Wed Mar 6 22:08:28 2024 +0100 feat: velocity measurement commit 259c4af Author: Teodor Neagoe <[email protected]> Date: Wed Mar 6 19:40:48 2024 +0100 test-run-on-remote commit f1d5eab Author: mablin7 <[email protected]> Date: Mon Mar 4 19:09:14 2024 +0100 ci: fix deps commit 165247f Author: Renyi Yang <[email protected]> Date: Mon Mar 4 13:06:23 2024 +0100 check style commit 20ad046 Author: Renyi Yang <[email protected]> Date: Sat Mar 2 23:48:12 2024 +0100 unitest commit 1136805 Author: Renyi Yang <[email protected]> Date: Sat Mar 2 22:24:36 2024 +0100 builder feat: buidler commit 8836224 Author: Renyi Yang <[email protected]> Date: Sat Mar 2 20:37:44 2024 +0100 feat: algorithm commit 903cbb2 Author: Renyi Yang <[email protected]> Date: Sat Mar 2 18:13:06 2024 +0100 feat: matrix generator commit 9e1ff1f Merge: 16c5810 002ebe9 Author: Balint Magyar <[email protected]> Date: Mon Mar 4 08:04:39 2024 +0100 Merge pull request #11 from DelftMercurians/alexandru/webui Alexandru/webui commit a838560 Author: Teodor Neagoe <[email protected]> Date: Wed Feb 28 19:37:53 2024 +0100 untested remove dirs commit a1cd9e0 Author: Teodor Neagoe <[email protected]> Date: Wed Feb 28 18:53:18 2024 +0100 added TODO to run-on-server commit 16c5810 Author: mablin7 <[email protected]> Date: Wed Feb 28 18:44:38 2024 +0100 feat: move run on server into workspace commit 002ebe9 Merge: adf9639 f112108 Author: Alexandru Lolea <[email protected]> Date: Wed Feb 28 17:28:35 2024 +0100 Solved conflicts commit adf9639 Author: Alexandru Lolea <[email protected]> Date: Wed Feb 28 17:02:25 2024 +0100 Fixed prop reference in Bot.vue commit fa4d0a3 Author: Alexandru Lolea <[email protected]> Date: Wed Feb 28 16:54:44 2024 +0100 Fixed errors regarding null possibilities commit f0eefc3 Author: mablin7 <[email protected]> Date: Wed Feb 28 18:01:27 2024 +0100 wip: pid
DelftMercurians · Apr 5, 2024 · 44166e0 · 44166e0
1 parent f4f8cc5
commit 44166e0
Show file tree

Hide file tree

Showing 16 changed files with 596 additions and 158 deletions.
diff --git a/Cargo.lock b/Cargo.lock
diff --git a/crates/dies-cli/Cargo.toml b/crates/dies-cli/Cargo.toml
@@ -21,4 +21,4 @@ tracing-subscriber = { version = "0.3.18", features = ["json"] }
 tracing-appender = "0.2.3"
 nalgebra = "0.32.4"
 dies-protos = { version = "0.1.0", path = "../dies-protos" }
-npy = "0.4.0"
+dies-control = { version = "0.1.0", path = "../dies-control" }
diff --git a/crates/dies-cli/src/executor.rs b/crates/dies-cli/src/executor.rs
@@ -6,12 +6,14 @@ use std::{
 
 use anyhow::{Context, Result};
 
-use dies_core::PlayerCmd;
+use dies_control::TeamController;
+use dies_core::{PlayerCmd, WorldData};
 use dies_python_rt::{PyRuntime, PyRuntimeConfig, RuntimeEvent, RuntimeMsg};
 use dies_serial_client::{SerialClient, SerialClientConfig};
 use dies_ssl_client::{VisionClient, VisionClientConfig};
 use dies_webui::spawn_webui;
 use dies_world::{WorldConfig, WorldTracker};
+use nalgebra::Vector2;
 
 pub struct ExecutorConfig {
     pub py_config: Option<PyRuntimeConfig>,
@@ -49,6 +51,8 @@ pub async fn run(config: ExecutorConfig) -> Result<()> {
     };
     let robot_ids = config.robot_ids;
 
+    let mut team_controller = TeamController::new();
+
     // Launch webui
     let (webui_sender, mut webui_cmd_rx, webui_handle) = if config.webui {
         let (webui_sender, webui_cmd_rx, webui_handle) = spawn_webui();
@@ -62,6 +66,7 @@ pub async fn run(config: ExecutorConfig) -> Result<()> {
 
     let fail: HashMap<u32, bool> = HashMap::new();
     let mut robots: HashSet<u32> = HashSet::new();
+    let mut world_data: Option<WorldData> = None;
     loop {
         let runtime_msg_fut = async {
             if let Some(runtime) = &mut runtime {
@@ -96,7 +101,8 @@ pub async fn run(config: ExecutorConfig) -> Result<()> {
                 match vision_msg {
                     Ok(vision_msg) => {
                         tracker.update_from_vision(&vision_msg);
-                        if let Some(world_data) = tracker.get() {
+                        if let Some(new_world_data) = tracker.get() {
+                            world_data = Some(new_world_data.clone());
                             // Failsafe: if one of our robots is not detected, we send stop to runtime
                             // for player in world_data.own_players.iter() {
                             //     if  SystemTime::now().duration_since(UNIX_EPOCH).unwrap().as_secs() as f64 - player.timestamp > 0.5 {
@@ -138,12 +144,14 @@ pub async fn run(config: ExecutorConfig) -> Result<()> {
 
                             // Send update to runtime
                             if let Some(runtime) = &mut runtime {
-                                let _ = runtime.send(&RuntimeMsg::World(world_data.clone())).await;
+                                if let Some(world_data) = &world_data {
+                                    let _ = runtime.send(&RuntimeMsg::World(world_data.clone())).await;
+                                }
                             }
 
                             // Send update to webui
                             if let Some(ref webui_sender) = webui_sender {
-                                if let Err(err) = webui_sender.send(world_data) {
+                                if let Err(err) = webui_sender.send(new_world_data) {
                                     tracing::error!("Failed to send world data to webui: {}", err);
                                 }
                             }
@@ -171,6 +179,11 @@ pub async fn run(config: ExecutorConfig) -> Result<()> {
                             tracing::error!("Received player cmd but serial is not configured");
                         }
                     }
+                    Ok(RuntimeEvent::PlayerPosCmd(cmd)) => {
+                        let rid = *robot_ids.get(&cmd.id).unwrap_or(&0);
+                        robots.insert(rid);
+                        team_controller.set_target_pos(cmd.id, Vector2::new(cmd.x, cmd.y));
+                    }
                     Ok(RuntimeEvent::Debug { msg }) => {
                         tracing::debug!("Runtime debug: {}", msg);
                     }

diff --git a/crates/dies-cli/src/main.rs b/crates/dies-cli/src/main.rs
@@ -56,7 +56,7 @@ struct Args {
     #[clap(long, default_value = "224.5.23.2:10006")]
     vision_addr: SocketAddr,
 
-    #[clap(long, default_value = "info")]
+    #[clap(long, default_value = "debug")]
     log_level: String,
 
     #[clap(long, default_value = "auto")]

diff --git a/crates/dies-control/Cargo.toml b/crates/dies-control/Cargo.toml
@@ -0,0 +1,12 @@
+[package]
+name = "dies-control"
+version = "0.1.0"
+edition.workspace = true
+
+# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+
+[dependencies]
+dies-core = { version = "0.1.0", path = "../dies-core" }
+log = "0.4.20"
+nalgebra = "0.32.4"
+num-traits = "0.2.18"
diff --git a/crates/dies-control/src/lib.rs b/crates/dies-control/src/lib.rs
@@ -0,0 +1,7 @@
+mod pid;
+mod player_controller;
+mod team_controller;
+mod variable;
+
+pub use player_controller::PlayerController;
+pub use team_controller::TeamController;
diff --git a/crates/dies-control/src/pid.rs b/crates/dies-control/src/pid.rs
@@ -0,0 +1,85 @@
+use std::time::Instant;
+
+use crate::variable::Variable;
+
+pub struct PID<T> {
+    kp: f32,
+    ki: f32,
+    kd: f32,
+    setpoint: Option<T>,
+    integral: T,
+    last_error: Option<T>,
+    last_time: Instant,
+}
+
+impl<T> PID<T>
+where
+    T: Variable,
+{
+    pub fn new(kp: f32, ki: f32, kd: f32) -> Self {
+        Self {
+            kp,
+            ki,
+            kd,
+            setpoint: None,
+            integral: T::zero(),
+            last_error: None,
+            last_time: Instant::now(),
+        }
+    }
+
+    pub fn set_setpoint(&mut self, setpoint: T) {
+        self.setpoint = Some(setpoint);
+    }
+
+    pub fn update(&mut self, input: T) -> T {
+        if let Some(setpoint) = self.setpoint {
+            let error = setpoint - input;
+            let dt = self.last_time.elapsed().as_secs_f32();
+            self.last_time = Instant::now();
+
+            self.integral += error * dt;
+
+            let last_error = self.last_error.unwrap_or(error);
+            let derivative = (error - last_error) / dt;
+            self.last_error = Some(error);
+
+            error * self.kp + self.integral * self.ki + derivative * self.kd
+        } else {
+            T::zero()
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use nalgebra::Vector2;
+
+    #[test]
+    fn test_pid_update_without_setpoint() {
+        let mut pid = PID::<f32>::new(1.0, 1.0, 1.0);
+
+        let output = pid.update(1.0);
+
+        assert_eq!(output, 0.0);
+    }
+
+    #[test]
+    fn test_pid_update_with_setpoint() {
+        let mut pid = PID::<f32>::new(1.0, 1.0, 1.0);
+        pid.set_setpoint(2.0);
+
+        let output = pid.update(1.0);
+        assert_ne!(output, 0.0);
+    }
+
+    #[test]
+    fn test_update_with_vector() {
+        let mut pid = PID::new(1.0, 1.0, 1.0);
+        pid.set_setpoint(Vector2::new(2.0, 2.0));
+
+        let output = pid.update(Vector2::new(1.0, 1.0));
+        assert_ne!(output, Vector2::zeros());
+    }
+}
diff --git a/crates/dies-control/src/player_controller.rs b/crates/dies-control/src/player_controller.rs
@@ -0,0 +1,135 @@
+use crate::pid::PID;
+use dies_core::{FieldGeometry, GameState, PlayerCmd, PlayerData};
+use nalgebra::Vector2;
+use std::time::{Duration, Instant};
+
+pub struct PlayerController {
+    id: u32,
+    position_pid: PID<Vector2<f32>>,
+    heading_pid: PID<f32>,
+    kick_timer: Option<Instant>,
+    last_pos: Vector2<f32>,
+    last_orientation: f32,
+    frame_missings: u32,
+}
+
+impl PlayerController {
+    /// Create a new player controller with the given ID.
+    pub fn new(id: u32) -> Self {
+        let mut heading_pid = PID::new(2.0, 0.002, 0.0);
+        // Fix the heading for now
+        heading_pid.set_setpoint(0.0);
+        Self {
+            id,
+            position_pid: PID::new(0.7, 0.0, 0.0),
+            heading_pid,
+            kick_timer: None,
+            last_pos: Vector2::new(0.0, 0.0),
+            last_orientation: 0.0,
+            frame_missings: 0,
+        }
+    }
+
+    /// Increment the missing frame count, return whether it's missing for too long
+    pub fn increment_frame_missings(&mut self) -> bool {
+        let threshold: u32 = 100; // for example 1.5s
+        self.frame_missings += 1;
+        self.frame_missings > threshold
+    }
+
+    /// Set the target position for the player.
+    pub fn set_target_pos(&mut self, setpoint: Vector2<f32>) {
+        self.position_pid.set_setpoint(setpoint);
+    }
+
+    /// Keep track of current position from the frame
+    pub fn update_current_pos(&mut self, state: &PlayerData) {
+        self.frame_missings = 0;
+        self.last_pos = state.position;
+        self.last_orientation = state.orientation;
+    }
+
+    /// Update the controller with the current state of the player.
+    pub fn update(
+        &mut self,
+        state: &PlayerData,
+        is_dribbling: bool,
+        is_kicking: bool,
+    ) -> PlayerCmd {
+        let mut cmd: PlayerCmd = PlayerCmd {
+            id: self.id,
+            ..Default::default()
+        };
+        self.set_speed(&mut cmd);
+
+        self.handle_dribbling(&mut cmd, is_dribbling);
+        self.handle_kicking(&mut cmd, is_kicking);
+
+        cmd
+    }
+
+    /// set speed of the robot based on pids
+    pub fn set_speed(&mut self, cmd: &mut PlayerCmd) {
+        let pos_u = self.position_pid.update(self.last_pos);
+        let head_u = self.heading_pid.update(self.last_orientation);
+        let local_u = rotate_vector(Vector2::new(pos_u.x, pos_u.y), -self.last_orientation);
+        cmd.sx = local_u.x;
+        cmd.sy = local_u.y;
+        cmd.w = head_u;
+    }
+
+    /// handle the dribbling.
+    pub fn handle_dribbling(&mut self, cmd: &mut PlayerCmd, is_dribbling: bool) {
+        // for example, 60 rad/s
+        if is_dribbling {
+            cmd.dribble_speed = 60.0;
+        }
+    }
+
+    /// handle the kicking.
+    pub fn handle_kicking(&mut self, cmd: &mut PlayerCmd, is_kicking: bool) {
+        if !is_kicking {
+            return;
+        }
+        if self.kick_timer.is_none() {
+            self.kick_timer = Some(Instant::now());
+            cmd.arm = true;
+        } else {
+            let elapsed = self.kick_timer.unwrap().elapsed();
+            if elapsed >= Duration::from_millis(1000) {
+                cmd.kick = true;
+                self.kick_timer = None;
+            }
+        }
+    }
+
+    /// speed limit check
+    pub fn speed_limit_check(&self, cmd: &mut PlayerCmd, velocity: Vector2<f32>) {
+        // for example 1500 mm/s
+        let max_speed = 1500.0;
+        let speed = (velocity.x.powi(2) + velocity.y.powi(2)).sqrt();
+        if speed > max_speed {
+            let scale = max_speed / speed;
+            cmd.sx *= scale;
+            cmd.sy *= scale;
+        }
+    }
+
+    /// out of bound detection
+    pub fn out_of_bound_detection(
+        &self,
+        cmd: &mut PlayerCmd,
+        game_state: GameState,
+        field_geometry: FieldGeometry,
+    ) {
+        // TODO:
+        // what is the excepted behavior of this?? I think pid will drag it back when
+        // it is out of bound, and since pid is the only way to control the robot, it may be unnecessary
+        // to do this check.
+    }
+}
+
+fn rotate_vector(v: Vector2<f32>, angle: f32) -> Vector2<f32> {
+    let rot = nalgebra::Rotation2::new(angle);
+    rot * v
+}