From e9ed0787f4e48d8a51b1037d49073e169a1aa6b0 Mon Sep 17 00:00:00 2001 From: Julia Blaauboer Date: Fri, 7 Mar 2025 15:58:19 +0100 Subject: [PATCH 1/2] conjure demons --- yggdrasil/src/behavior/behaviors/mod.rs | 2 + yggdrasil/src/behavior/behaviors/possessed.rs | 48 +++++++++++++++++++ yggdrasil/src/behavior/engine.rs | 14 ++++-- yggdrasil/src/behavior/primary_state.rs | 13 ++++- yggdrasil/src/main.rs | 3 +- 5 files changed, 74 insertions(+), 6 deletions(-) create mode 100644 yggdrasil/src/behavior/behaviors/possessed.rs diff --git a/yggdrasil/src/behavior/behaviors/mod.rs b/yggdrasil/src/behavior/behaviors/mod.rs index f38da5619..64035fb20 100644 --- a/yggdrasil/src/behavior/behaviors/mod.rs +++ b/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}; diff --git a/yggdrasil/src/behavior/behaviors/possessed.rs b/yggdrasil/src/behavior/behaviors/possessed.rs new file mode 100644 index 000000000..9a57d72f8 --- /dev/null +++ b/yggdrasil/src/behavior/behaviors/possessed.rs @@ -0,0 +1,48 @@ +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::)); + } +} + +const POSSESSED_WALK_SPEED: f32 = 0.045; +const POSSESSED_SIDE_SPEED: f32 = 0.045; +const POSSESSED_TURN_SPEED: f32 = 0.2; + +#[derive(Resource)] +pub struct Possessed; + +impl Behavior for Possessed { + const STATE: BehaviorState = BehaviorState::Possessed; +} + +pub fn possessed( + gamepad: Query<&Gamepad>, + gait: Res>, + mut step_context: ResMut, +) { + let gamepad = gamepad.single(); + + if gamepad.just_pressed(GamepadButton::West) { + match **gait { + Gait::Sitting => step_context.request_stand(), + _ => step_context.request_sit(), + } + } + + if !matches!(**gait, Gait::Sitting) { + step_context.request_walk(Step { + forward: POSSESSED_WALK_SPEED * gamepad.left_stick().y, + left: -POSSESSED_SIDE_SPEED * gamepad.left_stick().x, + turn: POSSESSED_TURN_SPEED * gamepad.right_stick().angle_to(Vec2::Y), + }); + } +} diff --git a/yggdrasil/src/behavior/engine.rs b/yggdrasil/src/behavior/engine.rs index 5ab60392a..e1591c547 100644 --- a/yggdrasil/src/behavior/engine.rs +++ b/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 { diff --git a/yggdrasil/src/behavior/primary_state.rs b/yggdrasil/src/behavior/primary_state.rs index 23dd80b3e..728f81428 100644 --- a/yggdrasil/src/behavior/primary_state.rs +++ b/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, game_controller_message: Option>, @@ -95,6 +98,7 @@ pub fn update_primary_state( config: Res, player_config: Res, whistle: Res, + 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 { diff --git a/yggdrasil/src/main.rs b/yggdrasil/src/main.rs index 9e16822f5..62d5b08da 100644 --- a/yggdrasil/src/main.rs +++ b/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, From fdfdf73ff2d76dfbc94251ab05d9f7e4c1230c03 Mon Sep 17 00:00:00 2001 From: Julia Blaauboer Date: Fri, 7 Mar 2025 19:03:30 +0100 Subject: [PATCH 2/2] death --- yggdrasil/src/behavior/behaviors/possessed.rs | 20 ++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/yggdrasil/src/behavior/behaviors/possessed.rs b/yggdrasil/src/behavior/behaviors/possessed.rs index 9a57d72f8..9858597ee 100644 --- a/yggdrasil/src/behavior/behaviors/possessed.rs +++ b/yggdrasil/src/behavior/behaviors/possessed.rs @@ -16,6 +16,8 @@ impl Plugin for PossessedBehaviorPlugin { 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; @@ -38,11 +40,23 @@ pub fn possessed( } } + 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 * gamepad.left_stick().y, - left: -POSSESSED_SIDE_SPEED * gamepad.left_stick().x, - turn: POSSESSED_TURN_SPEED * gamepad.right_stick().angle_to(Vec2::Y), + forward: POSSESSED_WALK_SPEED * left_stick.y, + left: -POSSESSED_SIDE_SPEED * left_stick.x, + turn: POSSESSED_TURN_SPEED * right_stick, }); } }