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..9858597ee --- /dev/null +++ b/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::)); + } +} + +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>, + 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(), + } + } + + 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, + }); + } +} 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,