Remote control fixes

crates/control/src/motion/head_motion.rs

@@ -2,6 +2,7 @@ use std::f32::consts::PI;
 
 use color_eyre::Result;
 use context_attribute::context;
+use filtering::low_pass_filter::LowPassFilter;
 use framework::MainOutput;
 use serde::{Deserialize, Serialize};
 use types::{
@@ -16,6 +17,7 @@ use types::{
 #[derive(Default, Deserialize, Serialize)]
 pub struct HeadMotion {
     last_positions: HeadJoints<f32>,
+    lowpass_filter: LowPassFilter<HeadJoints<f32>>,
 }
 
 #[context]
@@ -28,6 +30,7 @@ pub struct CycleContext {
     maximum_velocity: Parameter<HeadJoints<f32>, "head_motion.maximum_velocity">,
     outer_maximum_pitch: Parameter<f32, "head_motion.outer_maximum_pitch">,
     outer_yaw: Parameter<f32, "head_motion.outer_yaw">,
+    injected_head_joints: Parameter<Option<HeadJoints<f32>>, "head_motion.injected_head_joints?">,
 
     look_around: Input<HeadJoints<f32>, "look_around">,
     look_at: Input<HeadJoints<f32>, "look_at">,
@@ -48,10 +51,22 @@ impl HeadMotion {
     pub fn new(_context: CreationContext) -> Result<Self> {
         Ok(Self {
             last_positions: Default::default(),
+            lowpass_filter: LowPassFilter::with_smoothing_factor(Default::default(), 0.075),
         })
     }
 
     pub fn cycle(&mut self, context: CycleContext) -> Result<MainOutputs> {
+        if let Some(injected_head_joints) = context.injected_head_joints.copied() {
+            self.lowpass_filter.update(injected_head_joints);
+
+            return Ok(MainOutputs {
+                head_joints_command: MotorCommands {
+                    positions: self.lowpass_filter.state(),
+                    stiffnesses: HeadJoints::fill(0.8),
+                }
+                .into(),
+            });
+        }
         if context.motion_selection.dispatching_motion.is_some() {
             return Ok(MainOutputs {
                 head_joints_command: MotorCommands {

crates/types/src/players.rs

@@ -58,24 +58,24 @@ impl<T> IndexMut<PlayerNumber> for Players<T> {
     }
 }
 
+fn get_penalty(team_state: &TeamState, index: usize) -> Option<Penalty> {
+    team_state
+        .players
+        .get(index)
+        .map(|player| player.penalty)
+        .unwrap_or_default()
+}
+
 impl From<TeamState> for Players<Option<Penalty>> {
     fn from(team_state: TeamState) -> Self {
         Self {
-            one: team_state.players[0].penalty,
-            two: team_state.players[1].penalty,
-            three: team_state.players[2].penalty,
-            four: team_state.players[3].penalty,
-            five: team_state.players[4].penalty,
-            six: team_state
-                .players
-                .get(5)
-                .map(|player| player.penalty)
-                .unwrap_or_default(),
-            seven: team_state
-                .players
-                .get(6)
-                .map(|player| player.penalty)
-                .unwrap_or_default(),
+            one: get_penalty(&team_state, 0),
+            two: get_penalty(&team_state, 1),
+            three: get_penalty(&team_state, 2),
+            four: get_penalty(&team_state, 3),
+            five: get_penalty(&team_state, 4),
+            six: get_penalty(&team_state, 5),
+            seven: get_penalty(&team_state, 6),
         }
     }
 }

etc/parameters/default.json

@@ -999,7 +999,8 @@
     },
     "outer_maximum_pitch": 0.0,
     "inner_maximum_pitch": 0.61,
-    "outer_yaw": 1.3
+    "outer_yaw": 1.3,
+    "injected_head_joints": null
   },
   "look_at": {
     "glance_angle": 0.25,

tools/twix/src/panels/remote.rs

@@ -1,10 +1,13 @@
-use std::sync::Arc;
+use std::{
+    sync::Arc,
+    time::{Duration, SystemTime},
+};
 
 use communication::messages::TextOrBinary;
 use eframe::egui::Widget;
 use gilrs::{Axis, Button, Gamepad, GamepadId, Gilrs};
 use serde_json::{json, Value};
-use types::step_plan::Step;
+use types::{joints::head::HeadJoints, step_plan::Step};
 
 use crate::{nao::Nao, panel::Panel};
 
@@ -13,6 +16,7 @@ pub struct RemotePanel {
     gilrs: Gilrs,
     active_gamepad: Option<GamepadId>,
     enabled: bool,
+    last_update: SystemTime,
 }
 
 impl Panel for RemotePanel {
@@ -28,6 +32,7 @@ impl Panel for RemotePanel {
             gilrs,
             active_gamepad,
             enabled,
+            last_update: SystemTime::now(),
         }
     }
 
@@ -41,27 +46,43 @@ fn get_axis_value(gamepad: Gamepad, axis: Axis) -> Option<f32> {
 }
 
 impl RemotePanel {
+    fn reset(&self) {
+        self.update_step(Value::Null);
+        self.update_look_at_angle(Value::Null);
+    }
+
     fn update_step(&self, step: Value) {
         self.nao.write(
             "parameters.step_planner.injected_step",
             TextOrBinary::Text(step),
         )
     }
+
+    fn update_look_at_angle(&self, joints: Value) {
+        self.nao.write(
+            "parameters.head_motion.injected_head_joints",
+            TextOrBinary::Text(joints),
+        )
+    }
 }
 
 impl Widget for &mut RemotePanel {
     fn ui(self, ui: &mut eframe::egui::Ui) -> eframe::egui::Response {
+        const UPDATE_DELAY: Duration = Duration::from_millis(100);
+        const HEAD_PITCH_SCALE: f32 = 1.0;
+        const HEAD_YAW_SCALE: f32 = 1.0;
+
         self.gilrs.inc();
 
         if ui.checkbox(&mut self.enabled, "Enabled (Start)").changed() {
-            self.update_step(Value::Null);
+            self.reset();
         };
 
         while let Some(event) = self.gilrs.next_event() {
             if let gilrs::EventType::ButtonPressed(Button::Start, _) = event.event {
                 self.enabled = !self.enabled;
                 if !self.enabled {
-                    self.update_step(Value::Null)
+                    self.reset();
                 }
             };
             self.active_gamepad = Some(event.id);
@@ -70,10 +91,26 @@ impl Widget for &mut RemotePanel {
         if let Some(gamepad) = self.active_gamepad.map(|id| self.gilrs.gamepad(id)) {
             let right = get_axis_value(gamepad, Axis::LeftStickX).unwrap_or(0.0);
             let forward = get_axis_value(gamepad, Axis::LeftStickY).unwrap_or(0.0);
-            let turn_right = get_axis_value(gamepad, Axis::RightStickX).unwrap_or(0.0);
 
             let left = -right;
-            let turn = -turn_right;
+
+            let turn_right = gamepad
+                .button_data(Button::RightTrigger2)
+                .map(|button| button.value())
+                .unwrap_or_default();
+            let turn_left = gamepad
+                .button_data(Button::LeftTrigger2)
+                .map(|button| button.value())
+                .unwrap_or_default();
+            let turn = turn_left - turn_right;
+
+            let head_pitch = get_axis_value(gamepad, Axis::RightStickY).unwrap_or(0.0);
+            let head_yaw = -get_axis_value(gamepad, Axis::RightStickX).unwrap_or(0.0);
+
+            let injected_head_joints = HeadJoints {
+                yaw: head_yaw * HEAD_YAW_SCALE,
+                pitch: head_pitch * HEAD_PITCH_SCALE,
+            };
 
             let step = Step {
                 forward,
@@ -82,9 +119,25 @@ impl Widget for &mut RemotePanel {
             };
 
             if self.enabled {
-                self.update_step(serde_json::to_value(step).unwrap());
+                let now = SystemTime::now();
+                if now
+                    .duration_since(self.last_update)
+                    .expect("Time ran backwards")
+                    > UPDATE_DELAY
+                {
+                    self.last_update = now;
+                    self.update_step(serde_json::to_value(step).unwrap());
+                    self.update_look_at_angle(serde_json::to_value(injected_head_joints).unwrap());
+                }
             }
-            ui.label(&format!("{:#?}", step))
+
+            ui.vertical(|ui| {
+                let label_1 = ui.label(&format!("{:#?}", step));
+                let label_2 = ui.label(&format!("{:#?}", injected_head_joints));
+
+                label_1.union(label_2)
+            })
+            .inner
         } else {
             ui.label("No controller found")
         }