Completed small issues (#56, #55, $54, #52, #48)

include/EZ-Template/drive/drive.hpp

@@ -370,7 +370,7 @@ class Drive {
   /////
 
   /**
-   * Sets the chassis to voltage
+   * Sets the chassis to voltage.  Disables PID when called.
    *
    * \param left
    *        voltage for left side, -127 to 127
@@ -707,6 +707,21 @@ class Drive {
   int swing_min = 0;
   int turn_min = 0;
 
+  /**
+   * Sets the chassis to voltage.
+   *
+   * \param left
+   *        voltage for left side, -127 to 127
+   * \param right
+   *        voltage for right side, -127 to 127
+   */
+  void private_set_tank(int left, int right);
+
+  /**
+   * Returns joystick value clipped to JOYSTICK_THRESH
+   */
+  int clipped_joystick(int joystick);
+
   /**
    * Heading bool.
    */

include/EZ-Template/piston.hpp

@@ -25,6 +25,16 @@ class Piston {
    */
   Piston(int input_port, bool default_state = false);
 
+  /**
+   * Piston constructor in 3 wire expander.  The starting position of your piston is FALSE.
+   *
+   * \param input_ports
+   *        The ports of your pistons.
+   * \param default_state
+   *        Starting state of your piston.
+   */
+  Piston(int input_port, int expander_smart_port, bool default_state = false);
+
   /**
    * Sets the piston to the input.
    *

include/EZ-Template/piston_group.hpp

@@ -26,6 +26,17 @@ class PistonGroup {
    */
   PistonGroup(std::vector<int> input_ports, bool default_state = false);
 
+  /**
+   * PistonGroup constructor in 3 wire expander.  This class keeps track of piston state and allows
+   * multiple pistons to set at once.  The starting position of your piston is FALSE.
+   *
+   * \param input_ports
+   *        The ports of your pistons.
+   * \param default_state
+   *        Starting state of your piston.
+   */
+  PistonGroup(std::vector<int> input_ports, int expander_smart_port, bool default_state = false);
+
   /**
    * Sets the pistons to the input.
    *

src/EZ-Template/drive/drive.cpp

@@ -191,7 +191,7 @@ void Drive::set_pid_constants(PID* pid, double p, double i, double d, double p_s
   pid->set_constants(p, i, d, p_start_i);
 }
 
-void Drive::set_tank(int left, int right) {
+void Drive::private_set_tank(int left, int right) {
   if (pros::millis() < 1500) return;
 
   for (auto i : left_motors) {
@@ -202,6 +202,11 @@ void Drive::set_tank(int left, int right) {
   }
 }
 
+void Drive::set_tank(int left, int right) {
+  set_mode(DISABLE);
+  private_set_tank(left, right);
+}
+
 void Drive::set_drive_current_limit(int mA) {
   if (abs(mA) > 2500) {
     mA = 2500;

src/EZ-Template/drive/pid_tasks.cpp

@@ -4,7 +4,7 @@ License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */
 
-#include "main.h"
+#include "EZ-Template/api.hpp"
 #include "pros/misc.hpp"
 
 using namespace ez;
@@ -19,10 +19,14 @@ void Drive::ez_auto_task() {
     else if (get_mode() == SWING)
       swing_pid_task();
 
+    util::AUTON_RAN = get_mode() != DISABLE ? true : false;
+
+    /*
     if (pros::competition::is_autonomous() && !util::AUTON_RAN)
       util::AUTON_RAN = true;
     else if (!pros::competition::is_autonomous())
       set_mode(DISABLE);
+    */
 
     pros::delay(util::DELAY_TIME);
   }
@@ -57,7 +61,7 @@ void Drive::drive_pid_task() {
 
   // Set motors
   if (drive_toggle)
-    set_tank(l_out, r_out);
+    private_set_tank(l_out, r_out);
 }
 
 // Turn PID task
@@ -76,7 +80,7 @@ void Drive::turn_pid_task() {
 
   // Set motors
   if (drive_toggle)
-    set_tank(gyro_out, -gyro_out);
+    private_set_tank(gyro_out, -gyro_out);
 }
 
 // Swing PID task
@@ -96,8 +100,8 @@ void Drive::swing_pid_task() {
   if (drive_toggle) {
     // Check if left or right swing, then set motors accordingly
     if (current_swing == LEFT_SWING)
-      set_tank(swing_out, 0);
+      private_set_tank(swing_out, 0);
     else if (current_swing == RIGHT_SWING)
-      set_tank(0, -swing_out);
+      private_set_tank(0, -swing_out);
   }
 }

src/EZ-Template/drive/user_input.cpp

@@ -177,7 +177,10 @@ double Drive::right_curve_function(double x) {
 }
 
 // Set active brake constant
-void Drive::set_active_brake(double kp) { active_brake_kp = kp; }
+void Drive::set_active_brake(double kp) {
+  active_brake_kp = kp;
+  reset_drive_sensor();
+}
 
 // Set joystick threshold
 void Drive::set_joystick_threshold(int threshold) { JOYSTICK_THRESHOLD = abs(threshold); }
@@ -190,8 +193,8 @@ 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) {
+  // Check the motors are being set to power
+  if (abs(l_stick) > 0 || abs(r_stick) > 0) {
     set_tank(l_stick, r_stick);
     if (active_brake_kp != 0) reset_drive_sensor();
   }
@@ -201,6 +204,9 @@ void Drive::joy_thresh_opcontrol(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; }
+
 // Tank control
 void Drive::tank() {
   is_tank = true;
@@ -210,8 +216,8 @@ void Drive::tank() {
   modify_curve_with_controller();
 
   // Put the joysticks through the curve function
-  int l_stick = left_curve_function(master.get_analog(ANALOG_LEFT_Y));
-  int r_stick = left_curve_function(master.get_analog(ANALOG_RIGHT_Y));
+  int l_stick = left_curve_function(clipped_joystick(master.get_analog(ANALOG_LEFT_Y)));
+  int r_stick = left_curve_function(clipped_joystick(master.get_analog(ANALOG_RIGHT_Y)));
 
   // Set robot to l_stick and r_stick, check joystick threshold, set active brake
   joy_thresh_opcontrol(l_stick, r_stick);
@@ -229,12 +235,12 @@ void Drive::arcade_standard(e_type stick_type) {
   // Check arcade type (split vs single, normal vs flipped)
   if (stick_type == SPLIT) {
     // Put the joysticks through the curve function
-    fwd_stick = left_curve_function(master.get_analog(ANALOG_LEFT_Y));
-    turn_stick = right_curve_function(master.get_analog(ANALOG_RIGHT_X));
+    fwd_stick = left_curve_function(clipped_joystick(master.get_analog(ANALOG_LEFT_Y)));
+    turn_stick = right_curve_function(clipped_joystick(master.get_analog(ANALOG_RIGHT_X)));
   } else if (stick_type == SINGLE) {
     // Put the joysticks through the curve function
-    fwd_stick = left_curve_function(master.get_analog(ANALOG_LEFT_Y));
-    turn_stick = right_curve_function(master.get_analog(ANALOG_LEFT_X));
+    fwd_stick = left_curve_function(clipped_joystick(master.get_analog(ANALOG_LEFT_Y)));
+    turn_stick = right_curve_function(clipped_joystick(master.get_analog(ANALOG_LEFT_X)));
   }
 
   // Set robot to l_stick and r_stick, check joystick threshold, set active brake
@@ -253,12 +259,12 @@ void Drive::arcade_flipped(e_type stick_type) {
   // Check arcade type (split vs single, normal vs flipped)
   if (stick_type == SPLIT) {
     // Put the joysticks through the curve function
-    fwd_stick = right_curve_function(master.get_analog(ANALOG_RIGHT_Y));
-    turn_stick = left_curve_function(master.get_analog(ANALOG_LEFT_X));
+    fwd_stick = right_curve_function(clipped_joystick(master.get_analog(ANALOG_RIGHT_Y)));
+    turn_stick = left_curve_function(clipped_joystick(master.get_analog(ANALOG_LEFT_X)));
   } else if (stick_type == SINGLE) {
     // Put the joysticks through the curve function
-    fwd_stick = right_curve_function(master.get_analog(ANALOG_RIGHT_Y));
-    turn_stick = left_curve_function(master.get_analog(ANALOG_RIGHT_X));
+    fwd_stick = right_curve_function(clipped_joystick(master.get_analog(ANALOG_RIGHT_Y)));
+    turn_stick = left_curve_function(clipped_joystick(master.get_analog(ANALOG_RIGHT_X)));
   }
 
   // Set robot to l_stick and r_stick, check joystick threshold, set active brake

src/EZ-Template/piston.cpp

@@ -12,6 +12,12 @@ Piston::Piston(int input_port, bool default_state)
   reversed = default_state;
 }
 
+// Constructor for one piston plugged into expander
+Piston::Piston(int input_port, int expander_smart_port, bool default_state)
+    : piston({expander_smart_port, input_port}, default_state) {
+  reversed = default_state;
+}
+
 // Set piston
 void Piston::set(bool input) {
   piston.set_value(reversed ? !input : input);

src/EZ-Template/piston_group.cpp

@@ -56,6 +56,15 @@ PistonGroup::PistonGroup(std::vector<int> input_ports, bool default_state) {
   reversed = default_state;
 }
 
+// Constructor for one piston in expander
+PistonGroup::PistonGroup(std::vector<int> input_ports, int expander_smart_port, bool default_state) {
+  for (auto i : input_ports) {
+    pros::ADIDigitalOut temp({expander_smart_port, i}, default_state);
+    pistons.push_back(temp);
+  }
+  reversed = default_state;
+}
+
 // Set piston
 void PistonGroup::set(bool input) {
   for (auto i : pistons) {

src/main.cpp

@@ -148,7 +148,7 @@ void autonomous() {
 void opcontrol() {
   // This is preference to what you like to drive on.
   chassis.set_drive_brake(MOTOR_BRAKE_COAST);
-
+  
   while (true) {
 
     chassis.tank(); // Tank control