From ef53455e2d1a7208c15d2bc9c1d2b270847a1bef Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Sat, 12 Mar 2016 09:59:27 -0800 Subject: [PATCH] Sub: Implement joystick button actions --- ArduSub/Sub.h | 1 + ArduSub/joystick.cpp | 195 +++++++++++++++++++++++++++++++++---------- 2 files changed, 151 insertions(+), 45 deletions(-) diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index e889387cd7..7426f79fab 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -885,6 +885,7 @@ private: void enable_motor_output(); void read_radio(); void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons); + void handle_jsbutton_press(uint8_t button,uint8_t shift=0); void set_throttle_and_failsafe(uint16_t throttle_pwm); void set_throttle_zero_flag(int16_t throttle_control); void init_barometer(bool full_calibration); diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index b9eca0aa06..f587052b4d 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -2,9 +2,18 @@ #include "Sub.h" -// Function that will read the radio data, limit servos and trigger a failsafe +// Functions that will handle joystick/gamepad input // ---------------------------------------------------------------------------- +namespace { + int16_t mode; + int16_t camTilt = 1500; + int16_t lights1 = 1100; + int16_t lights2 = 1100; + int16_t rollTrim = 0; + int16_t pitchTrim = 0; +} + void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) { int16_t channels[10]; @@ -14,58 +23,34 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t float throttleScale = 0.8; int16_t rpyCenter = 1500; int16_t throttleBase = 1500-500*throttleScale; - static int16_t rollTrim = 0; - static int16_t mode; - static int16_t camTilt = 1500; - static int16_t lights = 1100; + uint8_t shift = 0; static uint32_t buttonDebounce; // Debouncing timer if ( tnow_ms - buttonDebounce > 50 ) { buttonDebounce = tnow_ms; + if ( (buttons & (1 << 0)) && g.jbtn_0.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + if ( (buttons & (1 << 1)) && g.jbtn_1.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + if ( (buttons & (1 << 2)) && g.jbtn_2.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + if ( (buttons & (1 << 3)) && g.jbtn_3.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + if ( (buttons & (1 << 4)) && g.jbtn_4.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + if ( (buttons & (1 << 5)) && g.jbtn_5.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + if ( (buttons & (1 << 6)) && g.jbtn_6.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + if ( (buttons & (1 << 7)) && g.jbtn_7.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + if ( (buttons & (1 << 8)) && g.jbtn_8.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + if ( (buttons & (1 << 9)) && g.jbtn_9.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + if ( (buttons & (1 << 10)) && g.jbtn_10.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + if ( (buttons & (1 << 11)) && g.jbtn_11.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + if ( (buttons & (1 << 12)) && g.jbtn_12.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + if ( (buttons & (1 << 13)) && g.jbtn_13.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + if ( (buttons & (1 << 14)) && g.jbtn_14.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + for ( uint8_t i = 0 ; i < 16 ; i++ ) { if ( buttons & (1 << i) ) { -// handle_jsbutton_press(i); + handle_jsbutton_press(i,shift); } } - - // Button logic to arm/disarm motors (Start and back buttons) - if ( buttons & (1 << 4) ) { - init_arm_motors(true); - } else if ( buttons & (1 << 5) ) { - init_disarm_motors(); - } - - // Button logic to change camera tilt (D-pad up and down + left joystick click to center) - if ( buttons & (1 << 0) ) { - camTilt = constrain_float(camTilt-30,800,2200); - } else if ( buttons & (1 << 1) ) { - camTilt = constrain_float(camTilt+30,800,2200); - } else if ( buttons & (1 << 6) ) { - camTilt = 1500; // Reset camera tilt - } - - // Button logic for roll trim (D-pad left and right) - if ( (buttons & ( 1 << 2 )) && rollTrim > -200 ) { - rollTrim -= 10; - } else if ( (buttons & ( 1 << 3 )) && rollTrim < 200 ) { - rollTrim += 10; - } - - // Button logic for mode changes (B for stabilize, Y for altitude hold) - if ( buttons & (1 << 14) ) { - mode = 2000; - } else if ( buttons & (1 << 12)) { - mode = 1000; - } - - // Button logic for lights with dimming (right bumper brighter, left bumper dimmer) - if ( buttons & (1 << 8) ) { - lights = constrain_float(lights-100,1100,1900); - } else if ( buttons & (1 << 9) ) { - lights = constrain_float(lights+100,1100,1900); - } } channels[0] = 1500; // pitch @@ -76,11 +61,131 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t channels[5] = x*rpyScale+rpyCenter; // forward for ROV channels[6] = y*rpyScale+rpyCenter; // strafe for ROV channels[7] = camTilt; // camera tilt - channels[8] = lights; + channels[8] = lights1; channels[9] = 0; // record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation failsafe.rc_override_active = hal.rcin->set_overrides(channels, 10); } -//void Sub::handle_jsbutton_press(uint8_t button); +void Sub::handle_jsbutton_press(uint8_t button, uint8_t shift) { + uint8_t func; + + switch ( button ) { + case 0: if ( shift ) func = g.jbtn_0.function(); else func = g.jbtn_0.function(true); break; + case 1: if ( shift ) func = g.jbtn_1.function(); else func = g.jbtn_1.function(true); break; + case 2: if ( shift ) func = g.jbtn_2.function(); else func = g.jbtn_2.function(true); break; + case 3: if ( shift ) func = g.jbtn_3.function(); else func = g.jbtn_3.function(true); break; + case 4: if ( shift ) func = g.jbtn_4.function(); else func = g.jbtn_4.function(true); break; + case 5: if ( shift ) func = g.jbtn_5.function(); else func = g.jbtn_5.function(true); break; + case 6: if ( shift ) func = g.jbtn_6.function(); else func = g.jbtn_6.function(true); break; + case 7: if ( shift ) func = g.jbtn_7.function(); else func = g.jbtn_7.function(true); break; + case 8: if ( shift ) func = g.jbtn_8.function(); else func = g.jbtn_8.function(true); break; + case 9: if ( shift ) func = g.jbtn_9.function(); else func = g.jbtn_9.function(true); break; + case 10: if ( shift ) func = g.jbtn_10.function(); else func = g.jbtn_10.function(true); break; + case 11: if ( shift ) func = g.jbtn_11.function(); else func = g.jbtn_11.function(true); break; + case 12: if ( shift ) func = g.jbtn_12.function(); else func = g.jbtn_12.function(true); break; + case 13: if ( shift ) func = g.jbtn_13.function(); else func = g.jbtn_13.function(true); break; + case 14: if ( shift ) func = g.jbtn_14.function(); else func = g.jbtn_14.function(true); break; + case 15: if ( shift ) func = g.jbtn_15.function(); else func = g.jbtn_15.function(true); break; + } + + switch ( func ) { + case JSButton::button_function_t::k_arm_toggle: + break; + case JSButton::button_function_t::k_arm: + init_arm_motors(true); + break; + case JSButton::button_function_t::k_disarm: + init_disarm_motors(); + break; + case JSButton::button_function_t::k_mode_toggle: + init_disarm_motors(); + break; + case JSButton::button_function_t::k_mode_1: + mode = 1100; + break; + case JSButton::button_function_t::k_mode_2: + mode = 1300; + break; + case JSButton::button_function_t::k_mode_3: + mode = 1420; + break; + case JSButton::button_function_t::k_mode_4: + mode = 1550; + break; + case JSButton::button_function_t::k_mode_5: + mode = 1690; + break; + case JSButton::button_function_t::k_mode_6: + mode = 1900; + break; + case JSButton::button_function_t::k_mount_center: + camTilt = 1500; + break; + case JSButton::button_function_t::k_mount_tilt_up: + camTilt = constrain_float(camTilt-30,800,2200); + break; + case JSButton::button_function_t::k_mount_tilt_down: + camTilt = constrain_float(camTilt+30,800,2200); + break; + case JSButton::button_function_t::k_camera_trigger: + break; + case JSButton::button_function_t::k_lights1_cycle: + { + static bool increasing = true; + if ( increasing ) { + lights1 = constrain_float(lights1+100,1100,1900); + } else { + lights1 = constrain_float(lights1-100,1100,1900); + } + if ( lights1 >= 1900 || lights1 <= 1100 ) { + increasing = !increasing; + } + } + break; + case JSButton::button_function_t::k_lights1_brighter: + lights1 = constrain_float(lights1+100,1100,1900); + break; + case JSButton::button_function_t::k_lights1_dimmer: + lights1 = constrain_float(lights1-100,1100,1900); + break; + case JSButton::button_function_t::k_lights2_cycle: + { + static bool increasing = true; + if ( increasing ) { + lights2 = constrain_float(lights2+100,1100,1900); + } else { + lights2 = constrain_float(lights2-100,1100,1900); + } + if ( lights2 >= 1900 || lights2 <= 1100 ) { + increasing = !increasing; + } + } + break; + case JSButton::button_function_t::k_lights2_brighter: + lights2 = constrain_float(lights2+100,1100,1900); + break; + case JSButton::button_function_t::k_lights2_dimmer: + lights2 = constrain_float(lights2-100,1100,1900); + break; + case JSButton::button_function_t::k_gain_toggle: + break; + case JSButton::button_function_t::k_gain_inc: + break; + case JSButton::button_function_t::k_gain_dec: + break; + case JSButton::button_function_t::k_trim_roll_inc: + rollTrim = constrain_float(rollTrim+10,-200,200); + break; + case JSButton::button_function_t::k_trim_roll_dec: + rollTrim = constrain_float(rollTrim-10,-200,200); + break; + case JSButton::button_function_t::k_trim_pitch_inc: + pitchTrim = constrain_float(pitchTrim+10,-200,200); + break; + case JSButton::button_function_t::k_trim_pitch_dec: + pitchTrim = constrain_float(pitchTrim-10,-200,200); + break; + } +}