From 081f822a6f72ead8f970e408b4bc546d6fca064a Mon Sep 17 00:00:00 2001
From: Mactar Gueye <64631549+Mactar1233@users.noreply.github.com>
Date: Wed, 24 Jan 2024 11:46:05 -0800
Subject: [PATCH] Added Ability for user to reverse Drive in opcontrol (#80)

* Update drive.hpp

weird changes

* Added Reversal to opcontrol driving

* mistake deleting active break logic, added back
---
 include/EZ-Template/drive/drive.hpp  | 20 +++++++++++++++++++-
 src/EZ-Template/drive/user_input.cpp |  9 ++++++++-
 2 files changed, 27 insertions(+), 2 deletions(-)

diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp
index 6dcf0196..ed24b632 100644
--- a/include/EZ-Template/drive/drive.hpp
+++ b/include/EZ-Template/drive/drive.hpp
@@ -562,6 +562,18 @@ class Drive {
    */
   bool opcontrol_joystick_practicemode_toggle_get();
 
+     /**
+   * Reversal for drivetrain in opcontrol that flips the left and right side and the direction of the drive
+   *
+   * @param toggle True if you want your drivetrain reversed and False if you do not.
+   */
+  void opcontrol_drive_reverse_set(bool toggle);
+
+  /**
+   * Gets current state of the toggle.  Reversal for drivetrain in opcontrol that flips the left and right side and the direction of the drive.
+   */
+  bool opcontrol_drive_reverse_get();
+  
   /////
   //
   // Autonomous Functions
@@ -578,7 +590,7 @@ class Drive {
    * \param slew_on
    *        ramp up from slew_min to speed over slew_distance.  only use when you're going over about 14"
    * \param toggle_heading
-   *        toggle for heading correction
+   *        toggle for heading correction 
    */
   void pid_drive_set(okapi::QLength p_target, int speed, bool slew_on = false, bool toggle_heading = true);
 
@@ -1086,4 +1098,10 @@ class Drive {
   void l_increase();
   void r_decrease();
   void r_increase();
+
+     /**
+   * Boolean to flip which side is the front of the robot for driver control.
+   */
+   bool is_reversed = false;
+
 };
diff --git a/src/EZ-Template/drive/user_input.cpp b/src/EZ-Template/drive/user_input.cpp
index d72fe181..9aa6b8ef 100644
--- a/src/EZ-Template/drive/user_input.cpp
+++ b/src/EZ-Template/drive/user_input.cpp
@@ -214,13 +214,19 @@ void Drive::opcontrol_drive_sensors_reset() {
 void Drive::opcontrol_joystick_practicemode_toggle(bool toggle) { practice_mode_is_on = toggle; }
 bool Drive::opcontrol_joystick_practicemode_toggle_get() { return practice_mode_is_on; }
 
+void Drive::opcontrol_drive_reverse_set(bool toggle) { is_reversed = toggle; }
+bool Drive::opcontrol_drive_reverse_get() { return is_reversed; }
+
 void Drive::opcontrol_joystick_threshold_iterate(int l_stick, int r_stick) {
   // Check the motors are being set to power
   if (abs(l_stick) > 0 || abs(r_stick) > 0) {
     if (practice_mode_is_on && (abs(l_stick) > 120 || abs(r_stick) > 120))
       drive_set(0, 0);
     else
-      drive_set(l_stick, r_stick);
+      if(is_reversed == true)
+        drive_set(-r_stick, -l_stick);
+      else
+         drive_set(l_stick, r_stick);
     if (active_brake_kp != 0) drive_sensor_reset();
   }
   // When joys are released, run active brake (P) on drive
@@ -229,6 +235,7 @@ void Drive::opcontrol_joystick_threshold_iterate(int l_stick, int r_stick) {
   }
 }
 
+
 // Clip joysticks based on joystick threshold
 int Drive::clipped_joystick(int joystick) { return abs(joystick) < JOYSTICK_THRESHOLD ? 0 : joystick; }