Added pto support #12

include/EZ-Template/drive/drive.hpp

@@ -12,6 +12,7 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
 #include "EZ-Template/PID.hpp"
 #include "EZ-Template/util.hpp"
+#include "pros/motors.h"
 
 using namespace ez;
 
@@ -22,6 +23,16 @@ class Drive {
    */
   int JOYSTICK_THRESHOLD;
 
+  /**
+   * Global current brake mode. 
+   */
+  pros::motor_brake_mode_e_t CURRENT_BRAKE = pros::E_MOTOR_BRAKE_COAST;
+
+  /**
+   * Global current mA. 
+   */
+  int CURRENT_MA = 2500;
+
   /**
    * Current swing type.
    */
@@ -37,6 +48,11 @@ class Drive {
    */
   std::vector<pros::Motor> right_motors;
 
+  /**
+   * Vector of pros motors that are disconnected from the drive. 
+   */
+  std::vector<int> pto_active;
+
   /**
    * Inertial sensor.
    */
@@ -278,6 +294,46 @@ class Drive {
    */
   void set_joystick_threshold(int threshold);
 
+  /////
+  //
+  // PTO 
+  //
+  /////
+
+  /**
+   * Checks if the motor is currently in pto_list.  Returns true if it's already in pto_list. 
+   *
+   * \param check_if_pto
+   *        motor to check. 
+   */
+  bool pto_check(pros::Motor check_if_pto);
+
+  /**
+   * Adds motors to the pto list, removing them from the drive. 
+   *
+   * \param pto_list
+   *        list of motors to remove from the drive. 
+   */
+  void pto_add(std::vector<pros::Motor> pto_list);
+
+  /**
+   * Removes motors from the pto list, adding them to the drive.  You cannot use the first index in a pto.
+   *
+   * \param pto_list
+   *        list of motors to add to the drive. 
+   */
+  void pto_remove(std::vector<pros::Motor> pto_list);
+
+  /**
+   * Adds/removes motors from drive.  You cannot use the first index in a pto.
+   *
+   * \param pto_list
+   *        list of motors to add/remove from the drive. 
+   * \param toggle
+   *        if true, adds to list.  if false, removes from list. 
+   */
+  void pto_toggle(std::vector<pros::Motor> pto_list, bool toggle);
+
   /////
   //
   // PROS Wrapers

src/EZ-Template/drive/drive.cpp

@@ -4,9 +4,10 @@ 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 "drive.hpp"
+
 #include <list>
 
-#include "drive.hpp"
 #include "main.h"
 #include "pros/llemu.hpp"
 #include "pros/screen.hpp"
@@ -185,9 +186,11 @@ void Drive::set_tank(int left, int right) {
   if (pros::millis() < 1500) return;
 
   for (auto i : left_motors) {
+    if (pto_check(i)) break;  // If the motor is in the pto list, don't do anything to the motor.
     i.move_voltage(left * (12000.0 / 127.0));
   }
   for (auto i : right_motors) {
+    if (pto_check(i)) break;  // If the motor is in the pto list, don't do anything to the motor.
     i.move_voltage(right * (12000.0 / 127.0));
   }
 }
@@ -196,10 +199,13 @@ void Drive::set_drive_current_limit(int mA) {
   if (abs(mA) > 2500) {
     mA = 2500;
   }
+  CURRENT_MA = mA;
   for (auto i : left_motors) {
+    if (pto_check(i)) break;  // If the motor is in the pto list, don't do anything to the motor.
     i.set_current_limit(abs(mA));
   }
   for (auto i : right_motors) {
+    if (pto_check(i)) break;  // If the motor is in the pto list, don't do anything to the motor.
     i.set_current_limit(abs(mA));
   }
 }
@@ -212,8 +218,7 @@ void Drive::reset_drive_sensor() {
     left_tracker.reset();
     right_tracker.reset();
     return;
-  }
-  else if (is_tracker == DRIVE_ROTATION) {
+  } else if (is_tracker == DRIVE_ROTATION) {
     left_rotation.reset();
     right_rotation.reset();
     return;
@@ -254,24 +259,24 @@ void Drive::imu_loading_display(int iter) {
 
   // Create the boarder
   pros::screen::set_pen(COLOR_WHITE);
-  for (int i = 1; i<3;i++) {
-    pros::screen::draw_rect(boarder+i, boarder+i, 480-boarder-i, 240-boarder-i);
+  for (int i = 1; i < 3; i++) {
+    pros::screen::draw_rect(boarder + i, boarder + i, 480 - boarder - i, 240 - boarder - i);
   }
 
   // While IMU is loading
   if (iter < 1440) {
     static int last_x1 = boarder;
-    pros::screen::set_pen(0x00FF6EC7); // EZ Pink
-    int x1 = (iter*((480-(boarder*2))/1440.0)) + boarder;
-    pros::screen::fill_rect(last_x1, boarder, x1, 240-boarder);
+    pros::screen::set_pen(0x00FF6EC7);  // EZ Pink
+    int x1 = (iter * ((480 - (boarder * 2)) / 1440.0)) + boarder;
+    pros::screen::fill_rect(last_x1, boarder, x1, 240 - boarder);
     last_x1 = x1;
   }
   // Failsafe time
   else {
     static int last_x1 = boarder;
-    pros::screen::set_pen(COLOR_RED); 
-    int x1 = ((iter-1440)*((480-(boarder*2))/1560.0)) + boarder;
-    pros::screen::fill_rect(last_x1, boarder, x1, 240-boarder);
+    pros::screen::set_pen(COLOR_RED);
+    int x1 = ((iter - 1440) * ((480 - (boarder * 2)) / 1560.0)) + boarder;
+    pros::screen::fill_rect(last_x1, boarder, x1, 240 - boarder);
     last_x1 = x1;
   }
 }
@@ -299,10 +304,13 @@ bool Drive::imu_calibrate() {
 
 // Brake modes
 void Drive::set_drive_brake(pros::motor_brake_mode_e_t brake_type) {
+  CURRENT_BRAKE = brake_type;
   for (auto i : left_motors) {
+    if (pto_check(i)) break;  // If the motor is in the pto list, don't do anything to the motor.
     i.set_brake_mode(brake_type);
   }
   for (auto i : right_motors) {
+    if (pto_check(i)) break;  // If the motor is in the pto list, don't do anything to the motor.
     i.set_brake_mode(brake_type);
   }
 }
@@ -311,4 +319,4 @@ void Drive::initialize() {
   init_curve_sd();
 
   imu_calibrate();
-}
+}

src/EZ-Template/drive/pto.cpp

@@ -0,0 +1,54 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+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 <algorithm>
+#include <vector>
+
+#include "drive.hpp"
+#include "main.h"
+
+bool Drive::pto_check(pros::Motor check_if_pto) {
+  auto does_exist = std::find(pto_active.begin(), pto_active.end(), check_if_pto.get_port());
+  if (does_exist != pto_active.end())
+    return true;  // Motor is in the list
+  return false;   // Motor isn't in the list
+}
+
+void Drive::pto_add(std::vector<pros::Motor> pto_list) {
+  for (auto i : pto_list) {
+    // Return if the motor is already in the list
+    if (pto_check(i)) return;
+
+    // Return if the first index was used (this motor is used for velocity)
+    if (i.get_port() == left_motors[0].get_port() || i.get_port() == right_motors[0].get_port()) {
+      printf("You cannot PTO the first index!\n");
+      return;
+    }
+
+    pto_active.push_back(i.get_port());
+  }
+}
+
+void Drive::pto_remove(std::vector<pros::Motor> pto_list) {
+  for (auto i : pto_list) {
+    auto does_exist = std::find(pto_active.begin(), pto_active.end(), i.get_port());
+    // Return if the motor isn't in the list
+    if (does_exist == pto_active.end()) return;
+
+    // Find index of motor
+    int index = std::distance(pto_active.begin(), does_exist);
+    pto_active.erase(pto_active.begin() + index);
+    i.set_brake_mode(CURRENT_BRAKE);  // Set the motor to the brake type of the drive
+    i.set_current_limit(CURRENT_MA);  // Set the motor to the mA of the drive
+  }
+}
+
+void Drive::pto_toggle(std::vector<pros::Motor> pto_list, bool toggle) {
+  if (toggle)
+    pto_add(pto_list);
+  else
+    pto_remove(pto_list);
+}

src/main.cpp

@@ -5,11 +5,11 @@
 Drive chassis (
   // Left Chassis Ports (negative port will reverse it!)
   //   the first port is the sensored port (when trackers are not used!)
-  {-15, -16}
+  {-15, -16, 5, 6}
 
   // Right Chassis Ports (negative port will reverse it!)
   //   the first port is the sensored port (when trackers are not used!)
-  ,{5, 6}
+  ,{12, 13, 11}
 
   // IMU Port
   ,20
@@ -42,37 +42,6 @@ Drive chassis (
   // 3 Wire Port Expander Smart Port
   // ,1
 );
-/*
-// Chassis constructor with rotation sensor
-Drive chassis (
-  // Left Chassis Ports (negative port will reverse it!)
-  //   the first port is the sensored port (when trackers are not used!)
-  {1, 2}
-
-  // Right Chassis Ports (negative port will reverse it!)
-  //   the first port is the sensored port (when trackers are not used!)
-  ,{-3, -4}
-
-  // IMU Port
-  ,5
-
-  // Wheel Diameter (Remember, 4" wheels are actually 4.125!)
-  //    (or tracking wheel diameter)
-  ,2.75
-
-  // External Gear Ratio (MUST BE DECIMAL)
-  //    (or gear ratio of tracking wheel)
-  // 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.
-  ,1
-
-  // Left Rotation Port (negative port will reverse it!)
-  ,-6
-
-  // Left Rotation Port (negative port will reverse it!)
-  ,7
-);
-*/
 
 
 
@@ -181,7 +150,7 @@ void opcontrol() {
   chassis.set_drive_brake(MOTOR_BRAKE_COAST);
 
   while (true) {
-
+    
     chassis.tank(); // Tank control
     // chassis.arcade_standard(ez::SPLIT); // Standard split arcade
     // chassis.arcade_standard(ez::SINGLE); // Standard single arcade