Conjure demons

yggdrasil/src/behavior/behaviors/mod.rs

@@ -1,5 +1,6 @@
 mod catchfall;
 mod observe;
+mod possessed;
 mod sitting;
 mod stand;
 mod stand_look;
@@ -11,6 +12,7 @@ mod walk_to_set;
 
 pub use catchfall::{CatchFall, CatchFallBehaviorPlugin};
 pub use observe::{Observe, ObserveBehaviorConfig, ObserveBehaviorPlugin};
+pub use possessed::{Possessed, PossessedBehaviorPlugin};
 pub use sitting::{Sitting, SittingBehaviorPlugin};
 pub use stand::{Stand, StandBehaviorPlugin};
 pub use stand_look::{StandLookAt, StandLookAtBehaviorPlugin};

yggdrasil/src/behavior/behaviors/possessed.rs

@@ -0,0 +1,62 @@
+use bevy::prelude::*;
+
+use crate::{
+    behavior::engine::{in_behavior, Behavior, BehaviorState},
+    motion::walking_engine::{step::Step, step_context::StepContext, Gait},
+};
+
+pub struct PossessedBehaviorPlugin;
+
+impl Plugin for PossessedBehaviorPlugin {
+    fn build(&self, app: &mut App) {
+        app.add_systems(Update, possessed.run_if(in_behavior::<Possessed>));
+    }
+}
+
+const POSSESSED_WALK_SPEED: f32 = 0.045;
+const POSSESSED_SIDE_SPEED: f32 = 0.045;
+const POSSESSED_TURN_SPEED: f32 = 0.2;
+const POSSESSED_LEFT_DEADBAND: f32 = 0.1;
+const POSSESSED_RIGHT_DEADBAND: f32 = 0.1;
+
+#[derive(Resource)]
+pub struct Possessed;
+
+impl Behavior for Possessed {
+    const STATE: BehaviorState = BehaviorState::Possessed;
+}
+
+pub fn possessed(
+    gamepad: Query<&Gamepad>,
+    gait: Res<State<Gait>>,
+    mut step_context: ResMut<StepContext>,
+) {
+    let gamepad = gamepad.single();
+
+    if gamepad.just_pressed(GamepadButton::West) {
+        match **gait {
+            Gait::Sitting => step_context.request_stand(),
+            _ => step_context.request_sit(),
+        }
+    }
+
+    let left_stick = if gamepad.left_stick().length() > POSSESSED_LEFT_DEADBAND {
+        gamepad.left_stick()
+    } else {
+        Vec2::ZERO
+    };
+
+    let right_stick = if gamepad.right_stick().length() > POSSESSED_RIGHT_DEADBAND {
+        Vec2::Y.angle_to(gamepad.right_stick())
+    } else {
+        0.
+    };
+
+    if !matches!(**gait, Gait::Sitting) {
+        step_context.request_walk(Step {
+            forward: POSSESSED_WALK_SPEED * left_stick.y,
+            left: -POSSESSED_SIDE_SPEED * left_stick.x,
+            turn: POSSESSED_TURN_SPEED * right_stick,
+        });
+    }
+}

yggdrasil/src/behavior/engine.rs

@@ -14,10 +14,11 @@ use crate::{
 
 use super::{
     behaviors::{
-        CatchFall, CatchFallBehaviorPlugin, ObserveBehaviorPlugin, Sitting, SittingBehaviorPlugin,
-        Stand, StandBehaviorPlugin, StandLookAt, StandLookAtBehaviorPlugin, Standup,
-        StandupBehaviorPlugin, StartUpBehaviorPlugin, WalkBehaviorPlugin, WalkToBehaviorPlugin,
-        WalkToSet, WalkToSetBehaviorPlugin,
+        CatchFall, CatchFallBehaviorPlugin, ObserveBehaviorPlugin, Possessed,
+        PossessedBehaviorPlugin, Sitting, SittingBehaviorPlugin, Stand, StandBehaviorPlugin,
+        StandLookAt, StandLookAtBehaviorPlugin, Standup, StandupBehaviorPlugin,
+        StartUpBehaviorPlugin, WalkBehaviorPlugin, WalkToBehaviorPlugin, WalkToSet,
+        WalkToSetBehaviorPlugin,
     },
     primary_state::PrimaryState,
     roles::{
@@ -40,6 +41,7 @@ impl Plugin for BehaviorEnginePlugin {
                 WalkBehaviorPlugin,
                 CatchFallBehaviorPlugin,
                 ObserveBehaviorPlugin,
+                PossessedBehaviorPlugin,
                 SittingBehaviorPlugin,
                 StandLookAtBehaviorPlugin,
                 StandupBehaviorPlugin,
@@ -60,6 +62,7 @@ pub enum BehaviorState {
     Stand,
     CatchFall,
     Observe,
+    Possessed,
     Sitting,
     StandLookAt,
     Standup,
@@ -239,6 +242,9 @@ pub fn role_base(
         PrimaryState::Standby | PrimaryState::Finished | PrimaryState::Calibration => {
             commands.set_behavior(Stand);
         }
+        PrimaryState::Possessed => {
+            commands.set_behavior(Possessed);
+        }
         PrimaryState::Initial => {
             orientation.reset();
             commands.set_behavior(StandLookAt {

yggdrasil/src/behavior/primary_state.rs

@@ -62,8 +62,10 @@ pub enum PrimaryState {
     Penalized,
     /// State of the robot when a half is finished
     Finished,
-    /// State the indicates the robot is performing automatic calibration
+    /// State that indicates the robot is performing automatic calibration
     Calibration,
+    /// State for when the robot is controlled by black magic (gamepad).
+    Possessed,
 }
 
 fn is_penalized_by_game_controller(
@@ -87,6 +89,7 @@ fn update_gamecontroller_message(
     }
 }
 
+#[allow(clippy::too_many_arguments)]
 pub fn update_primary_state(
     mut primary_state: ResMut<PrimaryState>,
     game_controller_message: Option<Res<GameControllerMessage>>,
@@ -95,6 +98,7 @@ pub fn update_primary_state(
     config: Res<PrimaryStateConfig>,
     player_config: Res<PlayerConfig>,
     whistle: Res<Whistle>,
+    gamepads: Query<&Gamepad>,
 ) {
     use PrimaryState as PS;
     let next_state = next_primary_state(
@@ -104,6 +108,7 @@ pub fn update_primary_state(
         &head_buttons,
         &player_config,
         &whistle,
+        !gamepads.is_empty(),
     );
 
     match next_state {
@@ -120,6 +125,7 @@ pub fn update_primary_state(
         PS::Penalized => nao_manager.set_chest_led(color::f32::RED, Priority::Critical),
         PS::Finished => nao_manager.set_chest_led(color::f32::GRAY, Priority::Critical),
         PS::Calibration => nao_manager.set_chest_led(color::f32::PURPLE, Priority::Critical),
+        PS::Possessed => nao_manager.set_chest_led(color::f32::MAROON, Priority::Critical),
     };
 
     *primary_state = next_state;
@@ -133,9 +139,14 @@ pub fn next_primary_state(
     head_buttons: &HeadButtons,
     player_config: &PlayerConfig,
     whistle: &Whistle,
+    has_gamepad: bool,
 ) -> PrimaryState {
     use PrimaryState as PS;
 
+    if has_gamepad {
+        return PS::Possessed;
+    }
+
     let mut primary_state = match primary_state {
         PS::Sitting if chest_button.state.is_tapped() => PS::Initial,
         PS::Initial if chest_button.state.is_tapped() => PS::Playing {

yggdrasil/src/main.rs

@@ -1,3 +1,4 @@
+use bevy::input::InputPlugin;
 use bevy::state::app::StatesPlugin;
 use miette::{Context, IntoDiagnostic};
 use tracing::Level;
@@ -18,7 +19,7 @@ fn main() -> Result<()> {
     miette::set_panic_hook();
 
     App::new()
-        .add_plugins((MinimalPlugins, StatesPlugin))
+        .add_plugins((MinimalPlugins, InputPlugin, StatesPlugin))
         .add_plugins((
             schedule::NaoSchedulePlugin,
             game_controller::GameControllerPlugin,