Cleaned user_input.cpp #31

EZ-Robotics · Dec 29, 2021 · 8c373b5 · 8c373b5
1 parent fcf3aed
commit 8c373b5
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 31 deletions.
diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp
@@ -525,6 +525,7 @@ class Drive {
 
  private:  // !Auton
   void reset_drive_sensors_opcontrol();
+  void joy_thresh_opcontrol(int l_stick, int r_stick);
 
   // Heading toggle
   bool heading_on = true;

diff --git a/src/EZ-Template/drive/user_input.cpp b/src/EZ-Template/drive/user_input.cpp
@@ -189,6 +189,18 @@ void Drive::reset_drive_sensors_opcontrol() {
   }
 }
 
+void Drive::joy_thresh_opcontrol(int l_stick, int r_stick) {
+  // Threshold if joysticks don't come back to perfect 0
+  if (abs(l_stick) > JOYSTICK_THRESHOLD || abs(r_stick) > JOYSTICK_THRESHOLD) {
+    set_tank(l_stick, r_stick);
+    if (active_brake_kp != 0) reset_drive_sensor();
+  }
+  // When joys are released, run active brake (P) on drive
+  else {
+    set_tank((0 - left_sensor()) * active_brake_kp, (0 - right_sensor()) * active_brake_kp);
+  }
+}
+
 // Tank control
 void Drive::tank() {
   mode = DISABLE;
@@ -202,15 +214,8 @@ void Drive::tank() {
   int l_stick = left_curve_function(master.get_analog(ANALOG_LEFT_Y));
   int r_stick = left_curve_function(master.get_analog(ANALOG_RIGHT_Y));
 
-  // Threshold if joysticks don't come back to perfect 0
-  if (abs(l_stick) > JOYSTICK_THRESHOLD || abs(r_stick) > JOYSTICK_THRESHOLD) {
-    set_tank(l_stick, r_stick);
-    reset_drive_sensor();
-  }
-  // When joys are released, run active brake (P) on drive
-  else {
-    set_tank((0 - left_sensor()) * active_brake_kp, (0 - right_sensor()) * active_brake_kp);
-  }
+  // Set robot to l_stick and r_stick, check joystick threshold, set active brake
+  joy_thresh_opcontrol(l_stick, r_stick);
 }
 
 // Arcade standard
@@ -234,15 +239,8 @@ void Drive::arcade_standard(e_type stick_type) {
     r_stick = right_curve_function(master.get_analog(ANALOG_LEFT_X));
   }
 
-  // Threshold if joysticks don't come back to perfect 0
-  if (abs(l_stick) > JOYSTICK_THRESHOLD || abs(r_stick) > JOYSTICK_THRESHOLD) {
-    set_tank(l_stick + r_stick, l_stick - r_stick);
-    reset_drive_sensor();
-  }
-  // When joys are released, run active brake (P) on drive
-  else {
-    set_tank((0 - left_sensor()) * active_brake_kp, (0 - right_sensor()) * active_brake_kp);
-  }
+  // Set robot to l_stick and r_stick, check joystick threshold, set active brake
+  joy_thresh_opcontrol(l_stick, r_stick);
 }
 
 // Arcade control flipped
@@ -266,13 +264,6 @@ void Drive::arcade_flipped(e_type stick_type) {
     l_stick = left_curve_function(master.get_analog(ANALOG_RIGHT_X));
   }
 
-  // Threshold if joysticks don't come back to perfect 0
-  if (abs(l_stick) > JOYSTICK_THRESHOLD || abs(r_stick) > JOYSTICK_THRESHOLD) {
-    set_tank(r_stick + l_stick, r_stick - l_stick);
-    reset_drive_sensor();
-  }
-  // When joys are released, run active brake (P) on drive
-  else {
-    set_tank((0 - left_sensor()) * active_brake_kp, (0 - right_sensor()) * active_brake_kp);
-  }
+  // Set robot to l_stick and r_stick, check joystick threshold, set active brake
+  joy_thresh_opcontrol(l_stick, r_stick);
 }
diff --git a/src/main.cpp b/src/main.cpp
@@ -1,6 +1,6 @@
 #include "main.h"
 
-/*
+
 // Chassis constructor
 Drive chassis (
   // Left Chassis Ports (negative port will reverse it!)
@@ -27,7 +27,7 @@ Drive chassis (
   // eg. if your drive is 84:36 where the 36t is powered, your RATIO would be 2.333.
   // eg. if your drive is 36:60 where the 60t is powered, your RATIO would be 0.6.
   ,2
-  */
+
 
   // Uncomment if using tracking wheels
   /*
@@ -41,8 +41,8 @@ Drive chassis (
   // Uncomment if tracking wheels are plugged into a 3 wire expander
   // 3 Wire Port Expander Smart Port
   // ,1
-//);
-
+);
+/*
 // Chassis constructor with rotation sensor
 Drive chassis (
   // Left Chassis Ports (negative port will reverse it!)
@@ -72,6 +72,7 @@ Drive chassis (
   // Left Rotation Port (negative port will reverse it!)
   ,7
 );
+*/