1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75

use anyhow::Result;
use clap::Parser;
use gilrs::Gilrs;
use hopper_rust::{hopper_config, motion_controller, utilities};
use log::*;
use motion_controller::{visualizer::GroundType, walking::MoveCommand};
use nalgebra::Vector2;
use std::path::Path;
use std::{thread::sleep, time::Duration};

/// Visualize Hopper
#[derive(Parser)]
#[clap(author, version)]
struct Args {
    /// Sets path to body config file (.yaml)
    /// If unset uses default value.
    #[clap(long)]
    body_config: Option<String>,
    /// type of floor to draw in visualizer
    #[clap(short, long, default_value = "ChessBoard")]
    ground: GroundType,
    /// Sets the level of verbosity
    #[clap(short, parse(from_occurrences))]
    verbose: u8,
}

#[tokio::main]
async fn main() -> Result<()> {
    let args: Args = Args::parse();
    utilities::start_loggers(None, args.verbose)?;
    info!("Started main visualizer");

    let _config = args
        .body_config
        .map(|path| hopper_config::HopperConfig::load(Path::new(&path)))
        .unwrap_or_else(|| Ok(hopper_config::HopperConfig::default()))?;

    let visualizer = motion_controller::visualizer::HopperVisualizer::new(args.ground);
    let mut motion_controller =
        motion_controller::MotionController::new(Box::new(visualizer)).await?;

    // gamepad
    let mut gilrs = Gilrs::new().unwrap();
    loop {
        trace!("Read gamepad");
        while gilrs.next_event().is_some() {}
        if let Some((_, gamepad)) = gilrs.gamepads().next() {
            if gamepad.is_connected() {
                let x = gamepad.value(gilrs::Axis::LeftStickY);
                let x = if x.abs() > 0.2 { x } else { 0.0 };
                // y direction needs to be inverted
                let y = -gamepad.value(gilrs::Axis::LeftStickX);
                let y = if y.abs() > 0.2 { y } else { 0.0 };

                let yaw = -gamepad.value(gilrs::Axis::RightStickX);
                let yaw = if yaw.abs() > 0.2 { yaw } else { 0.0 };

                if gamepad.is_pressed(gilrs::Button::South) {
                    info!("Settings desired state to Standing");
                    motion_controller.set_body_state(motion_controller::BodyState::Standing);
                }
                if gamepad.is_pressed(gilrs::Button::East) {
                    info!("Settings desired state to Grounded");
                    motion_controller.set_body_state(motion_controller::BodyState::Grounded);
                }

                let move_command =
                    MoveCommand::new(Vector2::new(0.06 * x, 0.06 * y), 10_f32.to_radians() * yaw);
                trace!("{:?}", move_command);
                motion_controller.set_command(move_command);
            }
        }
        sleep(Duration::from_millis(20));
    }
}