Implement abstract encoders

include/lemlib/chassis/trackingWheel.hpp

@@ -64,6 +64,7 @@ class TrackingWheel {
          * @param rpm theoretical maximum rpm of the drivetrain wheels
          */
         TrackingWheel(pros::Motor_Group* motors, float wheelDiameter, float distance, float rpm);
+
         /**
          * @brief Reset the tracking wheel position to 0
          *

include/lemlib/devices/encoder.hpp

@@ -0,0 +1,30 @@
+#pragma once
+
+#include "pros/rotation.hpp"
+#include "pros/adi.hpp"
+#include "pros/motors.hpp"
+
+namespace lemlib {
+class Encoder {
+    public:
+        /**
+         * @brief Construct a new Encoder
+         *
+         */
+        Encoder() {}
+
+        /**
+         * @brief Get the angle rotated by the encoder, in radians
+         *
+         * @return float angle rotated by the encoder, in radians
+         */
+        virtual float getAngle() = 0;
+        /**
+         * @brief Reset the encoder
+         *
+         * @return true encoder calibration failed
+         * @return false encoder calibration succeeded
+         */
+        virtual bool reset() = 0;
+};
+} // namespace lemlib

include/lemlib/devices/motor.hpp

@@ -0,0 +1,36 @@
+#pragma once
+
+#include "pros/motors.hpp"
+#include "lemlib/devices/encoder.hpp"
+
+namespace lemlib {
+class MotorEncoder : public Encoder {
+    public:
+        /**
+         * @brief Construct a new Motor Encoder
+         *
+         * @note change to reference instead of pointer when PROS 4 releases
+         *
+         * @param motors pointer to the motor group to be used
+         * @param rpm output rpm
+         */
+        MotorEncoder(pros::MotorGroup* motors, float rpm);
+
+        /**
+         * @brief Get the angle rotated by the motor encoders, in radians
+         *
+         * @return float angle rotated by the motor encoders, in radians
+         */
+        float getAngle() override;
+        /**
+         * @brief Reset the motor encoder
+         *
+         * @return true calibration failed
+         * @return false calibration succeeded
+         */
+        bool reset() override;
+    private:
+        pros::MotorGroup* motors;
+        const float rpm;
+};
+} // namespace lemlib

include/lemlib/devices/optical.hpp

@@ -0,0 +1,35 @@
+#pragma once
+
+#include "pros/adi.hpp"
+#include "lemlib/devices/encoder.hpp"
+
+namespace lemlib {
+class OpticalEncoder : public Encoder {
+        /**
+         * @brief Construct a new Optical Encoder
+         *
+         * @param topPort the top port of the optical encoder. 'a' to 'h'
+         * @param bottomPort the bottom of the optical encoder 'a' to 'h'
+         * @param reversed true if the optical encoder should be reversed, false otherwise
+         * @param ratio the gear ratio to use. Input / Output
+         */
+        OpticalEncoder(char topPort, char bottomPort, bool reversed, float ratio);
+
+        /**
+         * @brief Get the angle rotated by the optical encoder, in radians
+         *
+         * @return float angle rotated by the optical encoder, in radians
+         */
+        float getAngle() override;
+        /**
+         * @brief Reset the optical encoder
+         *
+         * @return true calibration failed
+         * @return false calibration succeeded
+         */
+        bool reset() override;
+    private:
+        const float ratio;
+        pros::ADIEncoder optical;
+};
+} // namespace lemlib

include/lemlib/devices/rotation.hpp

@@ -0,0 +1,35 @@
+#pragma once
+
+#include "pros/rotation.hpp"
+#include "lemlib/devices/encoder.hpp"
+
+namespace lemlib {
+class RotationEncoder : public Encoder {
+    public:
+        /**
+         * @brief Construct a new Rotation Encoder
+         *
+         * @param port the port of the rotation sensor
+         * @param reversed true if the rotation sensor should be reversed, false otherwise
+         * @param ratio the gear ratio to use. Input / Output
+         */
+        RotationEncoder(int port, bool reversed, float ratio);
+
+        /**
+         * @brief Get the angle rotated by the rotation sensor, in radians
+         *
+         * @return float angle rotated by the rotation sensor, in radians
+         */
+        float getAngle() override;
+        /**
+         * @brief Reset the rotation sensor
+         *
+         * @return true calibration failed
+         * @return false calibration succeeded
+         */
+        bool reset() override;
+    private:
+        const float ratio;
+        pros::Rotation rotation;
+};
+} // namespace lemlib

src/lemlib/devices/motor.cpp

@@ -0,0 +1,49 @@
+#include <cmath>
+#include "lemlib/util.hpp"
+#include "lemlib/devices/motor.hpp"
+
+/**
+ * Construct a new motor encoder object
+ *
+ * This is a derived class of the Encoder abstract class.
+ * Currently, a pointer to a MotorGroup is passed instead of just using
+ * a reference, due to limitations in PROS 3. This is fixed in PROS 4, but
+ * we have to deal with this for now.
+ */
+lemlib::MotorEncoder::MotorEncoder(pros::MotorGroup* motors, float rpm)
+    : motors(motors),
+      rpm(rpm) {}
+
+/**
+ * Get the angle the motors rotated by, in radians
+ *
+ * Since the motors in the group may have different cartridges, we need some
+ * extra logic to calculate the geared output. All we do is get a vector
+ * of all the gearboxes, and calculate the appropriate ratio by diving the
+ * output rpm by the input rpm. Then we just multiply the output by 2 pi
+ * to get angle in radians.
+ */
+float lemlib::MotorEncoder::getAngle() {
+    // get gearboxes and encoder position for each motor in the group
+    std::vector<pros::motor_gearset_e_t> gearsets = this->motors->get_gearing();
+    std::vector<double> positions = this->motors->get_positions();
+    std::vector<float> angles;
+    // calculate ratio'd output for each motor
+    for (int i = 0; i < this->motors->size(); i++) {
+        float in;
+        switch (gearsets[i]) {
+            case pros::E_MOTOR_GEARSET_36: in = 100; break;
+            case pros::E_MOTOR_GEARSET_18: in = 200; break;
+            case pros::E_MOTOR_GEARSET_06: in = 600; break;
+            default: in = 200; break;
+        }
+        angles.push_back(positions[i] * (rpm / in) * 2 * M_PI);
+    }
+    // return average of elements in the angles vector
+    return avg(angles);
+}
+
+/**
+ * Reset the motor encoders.
+ */
+bool lemlib::MotorEncoder::reset() { return (motors->tare_position()) ? 0 : 1; }

src/lemlib/devices/optical.cpp

@@ -0,0 +1,26 @@
+#include <cmath>
+#include "lemlib/devices/optical.hpp"
+
+/**
+ * Construct a new Optical Encoder
+ *
+ * This is a derived class of the Encoder abstract class.
+ * This abstraction is easy as there is only a single sensor which needs to be
+ * checked.
+ */
+lemlib::OpticalEncoder::OpticalEncoder(char topPort, char bottomPort, bool reversed, float ratio)
+    : optical(pros::ADIEncoder(topPort, bottomPort, reversed)),
+      ratio(ratio) {}
+
+/**
+ * Get the angle the optical encoder rotated by, in radians
+ *
+ * Pretty straightforward, raw value from the encoder gets converted to rotations
+ * which gets converted to radians
+ */
+float lemlib::OpticalEncoder::getAngle() { return (float(optical.get_value()) * (2 * M_PI) / 360) / ratio; }
+
+/**
+ * Reset/calibrate the optical encoder
+ */
+bool lemlib::OpticalEncoder::reset() { return (optical.reset()) ? 0 : 1; }

src/lemlib/devices/rotation.cpp

@@ -0,0 +1,26 @@
+#include <cmath>
+#include "lemlib/devices/rotation.hpp"
+
+/**
+ * Construct a new Rotation Encoder
+ *
+ * This is a class derived from the Encoder class.
+ * This abstraction is pretty easy because there is only 1 sensor that needs
+ * to be checked.
+ */
+lemlib::RotationEncoder::RotationEncoder(int port, bool reversed, float ratio)
+    : rotation(pros::Rotation(port, reversed)),
+      ratio(ratio) {}
+
+/**
+ * Get the angle the rotation sensor rotated by, in radians
+ *
+ * Pretty straightforward, raw value from the rotation sensor gets converted to rotations
+ * which gets converted to radians
+ */
+float lemlib::RotationEncoder::getAngle() { return (float(rotation.get_position()) / 36000) * (2 * M_PI) / ratio; }
+
+/**
+ * Reset/calibrate the optical encoder
+ */
+bool lemlib::RotationEncoder::reset() { return (rotation.reset_position()) ? 0 : 1; }