#21 Added tracking wheel support

tick_per_inch now changes how it calculates depending on if trakcing wheels are used or not. there are 3 drive constructors, one for internal encoders, one for tracking wheels in brain, and one for tracking wheels in an expander. left_sensor and right_sensor output motor/tracking wheel depending on the constructor used.
EZ-Robotics · Nov 20, 2021 · a3549b2 · a3549b2
1 parent 66cd30a
commit a3549b2
Show file tree

Hide file tree

Showing 3 changed files with 203 additions and 24 deletions.
diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp
@@ -46,6 +46,9 @@ class drive {
     */
     pros::Controller master;
 
+    pros::ADIEncoder left_tracker;
+    pros::ADIEncoder right_tracker;
+
     /**
      * PID objects.
     */
@@ -65,12 +68,74 @@ class drive {
 
 
     /**
-     * Creates a controller.
-     *
-     * Give Sensor Ports, Motor Ports (give a negative port if motor is reversed), Drive Measurements
-     * Set PID Constants
+     * Creates a Drive Controller using internal encoders.
+     * 
+     * \param left_motor_ports
+     *        Input {1, -2...}.  Make ports negative if reversed!
+     * \param right_motor_ports
+     *        Input {-3, 4...}.  Make ports negative if reversed!
+     * \param imu_port
+     *        Port the IMU is plugged into. 
+     * \param wheel_diameter
+     *        Diameter of your drive wheels.  Remember 4" is 4.125"!
+     * \param ticks
+     *        Motor cartidge RPM
+     * \param ratio
+     *        External gear ratio, wheel gear / motor gear.
+    */
+    drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio);
+
+    /**
+     * Creates a Drive Controller using encoders plugged into the brain.
+     * 
+     * \param left_motor_ports
+     *        Input {1, -2...}.  Make ports negative if reversed!
+     * \param right_motor_ports
+     *        Input {-3, 4...}.  Make ports negative if reversed!
+     * \param imu_port
+     *        Port the IMU is plugged into. 
+     * \param wheel_diameter
+     *        Diameter of your sensored wheels.  Remember 4" is 4.125"!
+     * \param ticks
+     *        Ticks per revolution of your encoder.
+     * \param ratio
+     *        External gear ratio, wheel gear / sensor gear.
+     * \param left_tracker_ports
+     *        Input {1, 2}.  Make ports negative if reversed!
+     * \param right_tracker_ports
+     *        Input {3, 4}.  Make ports negative if reversed!
+    */
+    drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector<int> left_tracker_ports, std::vector<int> right_tracker_ports);
+
+
+    /**
+     * Creates a Drive Controller using encoders plugged into a 3 wire expander. 
+     * 
+     * \param left_motor_ports
+     *        Input {1, -2...}.  Make ports negative if reversed!
+     * \param right_motor_ports
+     *        Input {-3, 4...}.  Make ports negative if reversed!
+     * \param imu_port
+     *        Port the IMU is plugged into. 
+     * \param wheel_diameter
+     *        Diameter of your sensored wheels.  Remember 4" is 4.125"!
+     * \param ticks
+     *        Ticks per revolution of your encoder.
+     * \param ratio
+     *        External gear ratio, wheel gear / sensor gear.
+     * \param left_tracker_ports
+     *        Input {1, 2}.  Make ports negative if reversed!
+     * \param right_tracker_ports
+     *        Input {3, 4}.  Make ports negative if reversed!
+     * \param expander_smart_port
+     *        Port the expander is plugged into. 
     */
-    drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports, int imu_port, double wheel_diameter, double motor_cartridge, double ratio);
+    drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector<int> left_tracker_ports, std::vector<int> right_tracker_ports, int expander_smart_port);
+
+    /**
+     * Sets drive defaults. 
+    */
+    void set_defaults();
 
 
 
@@ -419,7 +484,6 @@ class drive {
     void set_exit_condition(exit_condition_ &type, int p_small_exit_time, int p_small_error, int p_big_exit_time, int p_big_error, int p_velocity_exit_timeint, int p_mA_timeout);
 
 
-
   private:  // !Auton
 
 
@@ -486,6 +550,9 @@ class drive {
     // Is tank drive running?
     bool is_tank;
 
+    // Is tracker?
+    bool is_tracker = false;
+
     // Save input curve to SD card
     void save_l_curve_sd();
     void save_r_curve_sd();

diff --git a/src/EZ-Template/drive/drive.cpp b/src/EZ-Template/drive/drive.cpp
@@ -10,12 +10,18 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 using namespace ez;
 
 
-drive::drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports, int imu_port, double wheel_diameter, double motor_cartridge, double ratio)
- :
- imu (imu_port),
- master(pros::E_CONTROLLER_MASTER),
- ez_auto([this]{ this->ez_auto_task(); })
+// Constructor for integrated encoders
+drive::drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports, 
+    int imu_port, double wheel_diameter, double ticks, double ratio)
+  :
+  imu (imu_port),
+  master(pros::E_CONTROLLER_MASTER),
+  left_tracker  (-1, -1, false), // Default value
+  right_tracker (-1, -1, false), // Default value
+  ez_auto([this]{ this->ez_auto_task(); })
 {
+  is_tracker = false;
+
   // Set ports to a global vector
   for(auto i : left_motor_ports)
   {
@@ -31,9 +37,81 @@ drive::drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_por
   // Set constants for tick_per_inch caluclation
   WHEEL_DIAMETER = wheel_diameter;
   RATIO = ratio;
-  CARTRIDGE = motor_cartridge;
+  CARTRIDGE = ticks;
   TICK_PER_INCH = get_tick_per_inch();
 
+  set_defaults();
+}
+
+// Constructor for tracking wheels plugged into the brain
+drive::drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports, 
+    int imu_port, double wheel_diameter, double ticks, double ratio, 
+    std::vector<int> left_tracker_ports, std::vector<int> right_tracker_ports)
+  :
+  imu (imu_port),
+  master(pros::E_CONTROLLER_MASTER),
+  left_tracker (abs(left_tracker_ports[0]),  abs(left_tracker_ports[1]),  util::is_reversed(left_tracker_ports[0])),
+  right_tracker(abs(right_tracker_ports[0]), abs(right_tracker_ports[1]), util::is_reversed(right_tracker_ports[0])),
+  ez_auto([this]{ this->ez_auto_task(); })
+{
+  is_tracker = true;
+
+  // Set ports to a global vector
+  for(auto i : left_motor_ports)
+  {
+    pros::Motor temp(abs(i), util::is_reversed(i));
+    left_motors.push_back(temp);
+  }
+  for(auto i : right_motor_ports)
+  {
+    pros::Motor temp(abs(i), util::is_reversed(i));
+    right_motors.push_back(temp);
+  }
+
+  // Set constants for tick_per_inch caluclation
+  WHEEL_DIAMETER = wheel_diameter;
+  RATIO = ratio;
+  CARTRIDGE = ticks;
+  TICK_PER_INCH = get_tick_per_inch();
+
+  set_defaults();
+}
+
+// Constructor for tracking wheels plugged into a 3 wire expander
+drive::drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports, 
+    int imu_port, double wheel_diameter, double ticks, double ratio, 
+    std::vector<int> left_tracker_ports, std::vector<int> right_tracker_ports, int expander_smart_port)
+  :
+  imu (imu_port),
+  master(pros::E_CONTROLLER_MASTER),
+  left_tracker ({ expander_smart_port, abs(left_tracker_ports[0]),  abs(left_tracker_ports[1]) }, util::is_reversed(left_tracker_ports[0])),
+  right_tracker({ expander_smart_port, abs(right_tracker_ports[0]), abs(right_tracker_ports[1])}, util::is_reversed(right_tracker_ports[0])),
+  ez_auto([this]{ this->ez_auto_task(); })
+{
+  is_tracker = true;
+
+  // Set ports to a global vector
+  for(auto i : left_motor_ports)
+  {
+    pros::Motor temp(abs(i), util::is_reversed(i));
+    left_motors.push_back(temp);
+  }
+  for(auto i : right_motor_ports)
+  {
+    pros::Motor temp(abs(i), util::is_reversed(i));
+    right_motors.push_back(temp);
+  }
+
+  // Set constants for tick_per_inch caluclation
+  WHEEL_DIAMETER = wheel_diameter;
+  RATIO = ratio;
+  CARTRIDGE = ticks;
+  TICK_PER_INCH = get_tick_per_inch();
+
+  set_defaults();
+}
+
+void drive::set_defaults() {
   // PID Constants
   headingPID = {11, 0, 20, 0};
   forward_drivePID = {0.45, 0, 5, 0};
@@ -61,8 +139,13 @@ drive::drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_por
 }
 
 double drive::get_tick_per_inch() {
-  TICK_PER_REV  = (50.0*(3600.0/CARTRIDGE)) * RATIO; // with no cart, the encoder reads 50 counts per rotation
-  CIRCUMFERENCE = WHEEL_DIAMETER*M_PI;
+  CIRCUMFERENCE = WHEEL_DIAMETER*M_PI; 
+
+  if (is_tracker) 
+    TICK_PER_REV = CARTRIDGE * RATIO;
+  else 
+    TICK_PER_REV  = (50.0*(3600.0/CARTRIDGE)) * RATIO; // with no cart, the encoder reads 50 counts per rotation
+
   TICK_PER_INCH = (TICK_PER_REV/CIRCUMFERENCE);
   return TICK_PER_INCH;
 }
@@ -86,16 +169,29 @@ void drive::set_tank(int left, int right) {
 
 // Motor telemetry
 void drive::reset_drive_sensor() {
-  left_motors.front().tare_position();
+  if (is_tracker) {
+    left_tracker. reset();
+    right_tracker.reset();
+    return;
+  }
+  left_motors.front(). tare_position();
   right_motors.front().tare_position();
 }
 
-int drive::right_sensor()   { return right_motors.front().get_position(); }
+int drive::right_sensor() { 
+  if (is_tracker)
+    return right_tracker.get_value();
+  return right_motors.front().get_position(); 
+}
 int drive::right_velocity() { return right_motors.front().get_actual_velocity(); }
 double drive::right_mA()    { return right_motors.front().get_current_draw(); }
 bool drive::right_over_current() { return right_motors.front().is_over_current(); }
 
-int drive::left_sensor()   { return left_motors.front().get_position(); }
+int drive::left_sensor() { 
+    if (is_tracker)
+    return left_tracker.get_value();
+  return left_motors.front().get_position(); 
+}
 int drive::left_velocity() { return left_motors.front().get_actual_velocity(); }
 double drive::left_mA()    { return left_motors.front().get_current_draw(); }
 bool drive::left_over_current() { return left_motors.front().is_over_current(); }

diff --git a/src/main.cpp b/src/main.cpp
@@ -8,22 +8,38 @@
 // Chassis constructor
 drive chassis (
   // Left Chassis Ports (negative port will reverse it!)
-  {-11, -5, -7},
+  {-11, -5, -7}
 
   // Right Chassis Ports (negative port will reverse it!)
-  {3, 2, 17},
+  ,{3, 2, 17}
 
   // IMU Port
-  18,
+  ,18
 
   // Wheel Diameter (Remember, 4" wheels are actually 4.125!)
-  3.25,
+  //    (or tracking wheel diameter)
+  ,3.25
 
   // Cartridge RPM
-  600,
+  //   (pr tick per rotation if using tracking wheels)
+  ,600
 
-  // External Gear Ratio
-  1.66666666667
+  // External Gear Ratio 
+  //    (or gear ratio of tracking wheel)
+  ,1.66666666667
+
+  // Uncomment if using tracking wheels
+  /*
+  // Left Tracking Wheel Ports
+  ,{1, 2}
+
+  // Right Tracking Wheel Ports
+  ,{3, 4}
+  */
+
+  // Uncomment if tracking wheels are plugged into a 3 wire expander
+  // 3 Wire Port Expander Smart Port
+  // ,1
 );