diff --git a/.gitignore b/.gitignore index 6386794c..df06b638 100644 --- a/.gitignore +++ b/.gitignore @@ -7,6 +7,7 @@ # Ignore the build directory build build/ +.pio/ # Ignore the output binaries *.elf diff --git a/src/main.cpp b/src/main.cpp index b929e704..0c5c0ba4 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -209,6 +209,7 @@ int main() { // hid_incoming.get_target_state_map(target_state_map); target_state_map.from_comms_packet(comms_layer.get_hive_data().target_state_data.state); last_feed = target_state_map[Cfg::StateName::Feeder].get_position(); + } // override temp state if needed. Dont override in teensy mode so the sentry doesnt move during inspection diff --git a/src/sensors/transmitter/dr16.cpp b/src/sensors/transmitter/dr16.cpp index 52fb8f76..cee08124 100644 --- a/src/sensors/transmitter/dr16.cpp +++ b/src/sensors/transmitter/dr16.cpp @@ -322,12 +322,15 @@ DR16Data DR16::get_dr16_data(){ void DR16::manual_controls(const RobotStateMap& estimated_state_map, RobotStateMap& target_state_map, bool not_safety_mode, float& feed, float& last_feed) { float delta = control_input_timer.delta(); - transmitter_pos_x += mouse_x * 0.05 * delta; - transmitter_pos_y += mouse_y * 0.05 * delta; - - vtm_pos_x += ref.ref_data.kbm_interaction.mouse_speed_x * 0.05 * delta; - vtm_pos_y += ref.ref_data.kbm_interaction.mouse_speed_y * 0.05 * delta; - + // Used for determing whether we are using vtm or usb to dr16 + auto merge_input = [](auto primary_val, auto secondary_val) { + // If primary isn't zero, use it. Else use secondary. + return (primary_val != 0) ? primary_val : secondary_val; + }; + + pos_x = merge_input(mouse_x,ref.ref_data.kbm_interaction.mouse_speed_x) * 0.05 * delta; + pos_y = merge_input(mouse_y,ref.ref_data.kbm_interaction.mouse_speed_y) * 0.05 * delta; + l_mouse_button = merge_input(l_mouse_button,ref.ref_data.kbm_interaction.button_left); float pitch_min = estimated_state_map[Cfg::StateName::GimbalPitch].config().reference_limits.position.min; float pitch_max = estimated_state_map[Cfg::StateName::GimbalPitch].config().reference_limits.position.max; float pitch_average = 0.5 * (pitch_min + pitch_max); @@ -335,17 +338,11 @@ void DR16::manual_controls(const RobotStateMap& estimated_state_map, RobotStateM pitch_max -= pitch_average; // clamp to pitch limits - if (transmitter_pos_y < pitch_min) { - transmitter_pos_y = pitch_min; - } - if (transmitter_pos_y > pitch_max) { - transmitter_pos_y = pitch_max; - } - if (vtm_pos_y < pitch_min) { - vtm_pos_y = pitch_min; + if (pos_y < pitch_min) { + pos_y = pitch_min; } - if (vtm_pos_y > pitch_max) { - vtm_pos_y = pitch_max; + if (pos_y > pitch_max) { + pos_y = pitch_max; } float chassis_vel_x = 0; @@ -355,15 +352,13 @@ void DR16::manual_controls(const RobotStateMap& estimated_state_map, RobotStateM if (estimated_state_map[Cfg::StateName::ChassisX].config().governor_type == Cfg::StateOrder::Velocity) { // if we should be controlling velocity - chassis_vel_x = get_l_stick_y() * 5.4 + - (-ref.ref_data.kbm_interaction.key_w + ref.ref_data.kbm_interaction.key_s) * 2.5; + chassis_vel_x = get_l_stick_y() * 5.4; + + chassis_vel_x += merge_input((-ref.ref_data.kbm_interaction.key_w + ref.ref_data.kbm_interaction.key_s), (-keys.w + keys.s)) * 2.5; - chassis_vel_x += (-keys.w + keys.s) * 2.5; - - chassis_vel_y = -(get_l_stick_x() * 5.4) + - (ref.ref_data.kbm_interaction.key_d - ref.ref_data.kbm_interaction.key_a) * 2.5; - - chassis_vel_y += (keys.d - keys.a) * 2.5; + chassis_vel_y = -(get_l_stick_x() * 5.4); + + chassis_vel_y += merge_input((ref.ref_data.kbm_interaction.key_d - ref.ref_data.kbm_interaction.key_a),(keys.d - keys.a)) * 2.5; } else if (estimated_state_map[Cfg::StateName::ChassisX].config().governor_type == Cfg::StateOrder::Position) { // if we should be controlling position chassis_pos_x = get_l_stick_x() * 2 + pos_offset_x; @@ -371,14 +366,14 @@ void DR16::manual_controls(const RobotStateMap& estimated_state_map, RobotStateM } float chassis_spin = get_wheel() * 25; - float pitch_target = 1.57 + -get_r_stick_y() * 0.3 + vtm_pos_y; - float yaw_target = -get_r_stick_x() * 1.5 - vtm_pos_x; + float pitch_target = 1.57 + -get_r_stick_y() * 0.3 + pos_y; + float yaw_target = -get_r_stick_x() * 1.5 - pos_x; // This was setup with vtm_pos before which never functioned so, if bug its here float fly_wheel_target = (get_r_switch() == SwitchPos::FORWARD || get_r_switch() == SwitchPos::MIDDLE) ? 18 : 0; // m/s // if the right switch is forward, and either the left mouse button is pressed or the right switch is not // backward, set the feeder to something. Otherwise, set it to 0 - float feeder_target = (((ref.ref_data.kbm_interaction.button_left) && + float feeder_target = ((l_mouse_button && get_r_switch() != SwitchPos::BACKWARD) || get_r_switch() == SwitchPos::FORWARD) ? 10 : 0; if (estimated_state_map[Cfg::StateName::Feeder].config().governor_type == Cfg::StateOrder::Position) { float dt2 = timer.delta(); @@ -413,4 +408,4 @@ void DR16::manual_controls(const RobotStateMap& estimated_state_map, RobotStateM pos_offset_y = estimated_state_map[Cfg::StateName::ChassisY].get_position(); feed = last_feed; } -} \ No newline at end of file +} diff --git a/src/sensors/transmitter/dr16.hpp b/src/sensors/transmitter/dr16.hpp index b66fdf84..4adfb415 100644 --- a/src/sensors/transmitter/dr16.hpp +++ b/src/sensors/transmitter/dr16.hpp @@ -217,14 +217,10 @@ class DR16 : public Transmitter { Keys keys; // manual controls - /// @brief Mouse x axis position - float transmitter_pos_x = 0; + /// @brief Mouse x axis position + float pos_x = 0; /// @brief Mouse y axis position - float transmitter_pos_y = 0; - /// @brief Mouse x axis position from ref - float vtm_pos_x = 0; - /// @brief Mouse y axis position from ref - float vtm_pos_y = 0; + float pos_y = 0; /// @brief Position offset for chassis x (so the sentry doesn't drive to 0,0) float pos_offset_x = 0; /// @brief Position offset for chassis y (so the sentry doesn't drive to 0,0)