Merge branch 'encoder' of https://github.com/ut-ras/robomaster into e…

…ncoder

ut-robomaster/src/board.hpp

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "tap/board/board.hpp"
+
+namespace Board
+{
+using I2cSda = DigitalInPinPF0;
+using I2cScl = DigitalInPinPF1;
+using I2cMaster = I2cMaster2;
+
+inline void initialize_i2c()
+{
+    I2cMaster::connect<I2cSda::Sda, I2cScl::Scl>(I2cMaster::PullUps::Internal);
+    I2cMaster::initialize<SystemClock>();
+}
+}  // namespace Board

ut-robomaster/src/drivers.hpp

@@ -4,9 +4,11 @@
 #include "tap/drivers.hpp"
 
 #include "communication/cv_board.hpp"
-#include "drivers/encoder.hpp"
+#include "drivers/as5600.hpp"
 #include "utils/robot_comms.hpp"
 
+#include "board.hpp"
+
 namespace src
 {
 class Drivers : public tap::Drivers
@@ -16,12 +18,11 @@ class Drivers : public tap::Drivers
 #ifdef ENV_UNIT_TESTS
 public:
 #endif
-    Drivers() : tap::Drivers(), cvBoard(this), terminal(this), encoder() {}
+    Drivers() : tap::Drivers(), cvBoard(this), terminal(this) {}
 
 public:
     communication::CVBoard cvBoard;
     comms::RobotComms terminal;
-    encoder::Encoder<> encoder;
     bool isKillSwitched() { return !remote.isConnected(); }
 };  // class Drivers
 

ut-robomaster/src/drivers/as5600.hpp

@@ -0,0 +1,60 @@
+#pragma once
+
+#include "modm/architecture/interface/i2c_device.hpp"
+#include "modm/processing/protothread/protothread.hpp"
+
+#include "encoder.hpp"
+
+namespace driver
+{
+template <class I2cMaster>
+class As5600 : public Encoder, public modm::I2cDevice<I2cMaster, 1>, public modm::pt::Protothread
+{
+public:
+    As5600() : modm::I2cDevice<I2cMaster, 1>(ADDRESS){};
+
+    void update() { run(); }
+
+    bool run()
+    {
+        PT_BEGIN();
+
+        while (true)
+        {
+            buffer[0] = uint8_t(Register::RAWANGLE);
+            PT_WAIT_UNTIL(this->startWriteRead(buffer, 1, buffer, 2));
+            PT_WAIT_WHILE(this->isTransactionRunning());
+
+            if (this->wasTransactionSuccessful())
+            {
+                angle = (buffer[0] << 8) | buffer[1];
+            }
+        }
+
+        PT_END();
+    }
+
+    float getAngle() override { return angle / 4096.0f; }
+
+protected:
+    enum class Register : uint8_t
+    {
+        ZPOS = 0x01,
+        MPOS = 0x03,
+        CONF = 0x07,
+
+        RAWANGLE = 0x0C,
+        ANGLE = 0x0E,
+
+        STATUS = 0x0B,
+        AGC = 0x1A,
+        MAG = 0x1B,
+    };
+
+private:
+    static const uint8_t ADDRESS = 0x36;
+    uint16_t angle = 0;
+    uint8_t buffer[2];
+};
+
+};  // namespace driver

ut-robomaster/src/drivers/encoder.hpp

@@ -1,101 +1,14 @@
 #pragma once
 
-#include "tap/board/board.hpp"  //clock_src
-
-#include "modm/architecture/interface/i2c_device.hpp"  //i2c implementation
-#include "modm/platform/i2c/i2c_master_2.hpp"          //i2c master
-
-// convert angle to 1.5 bytes
-//(angle / 360) * 4096 = N
-// value written = N
-inline uint16_t calculate_Angle(uint16_t angle) { return (((angle * 1000) / 360) * 4096) / 1000; }
-
-namespace encoder
+namespace driver
 {
-
-// make own read/write transaction
-struct Encoder_I2cWriteReadTransaction : public modm::I2cWriteReadTransaction
-{
-public:
-    Encoder_I2cWriteReadTransaction(uint8_t address) : modm::I2cWriteReadTransaction(address){};
-};
-
-template <class I2cMaster = modm::platform::I2cMaster2>
-class Encoder : public modm::I2cDevice<I2cMaster, 10, modm::I2cWriteReadTransaction>
+class Encoder
 {
 public:
-    // init a i2c device from modm
-    Encoder(uint8_t address = slave_address)
-        : modm::I2cDevice<I2cMaster, 10, modm::I2cWriteReadTransaction>(address){};
-
-    void set_startAngle(uint16_t angle)
-    {
-        Encoder::Encoder_R address = Encoder::Encoder_R::ZPOS;
-        std::fill(std::begin(dataBuffer), std::end(dataBuffer), 0);
-        dataBuffer[0] = (uint8_t)address;
-
-        uint16_t N = calculate_Angle(angle);
-        dataBuffer[1] = N & 0x0F00;  // bits [11:8]
-        dataBuffer[2] = N & 0x00FF;  // bits [7:0]
-
-        this->startWrite(dataBuffer, sizeof(dataBuffer));
-    }
-
-    void set_stopAngle(uint16_t angle)
-    {
-        Encoder::Encoder_R address = Encoder::Encoder_R::MPOS;
-        std::fill(std::begin(dataBuffer), std::end(dataBuffer), 0);
+    virtual ~Encoder() {}
 
-        dataBuffer[0] = (uint8_t)address;
-        uint16_t N = calculate_Angle(angle);
-        dataBuffer[1] = N & 0x0F00;  // bits [11:8]
-        dataBuffer[2] = N & 0x00FF;  // bits [7:0]
-
-        this->startWrite(dataBuffer, sizeof(dataBuffer));
-    }
-
-    uint16_t read_Angle()
-    {
-        Encoder::Encoder_R address = Encoder::Encoder_R::ANGLE;
-        std::fill(std::begin(dataBuffer), std::end(dataBuffer), 0);
-        dataBuffer[0] = (uint8_t)address;
-
-        this->startRead(dataBuffer, sizeof(dataBuffer));
-
-        // returns two bytes from MSB [1] to LSB [2],  always return MSB first.
-        curr_angle = (dataBuffer[1] << 8) | dataBuffer[2];
-        return curr_angle;
-    }
-
-protected:
-    static const uint8_t slave_address = 0x36;  // address of the AS6500
-
-    // enum of the different register address
-    enum class Encoder_R : uint8_t
-    {
-
-        // WRITE CONFIGURATION INPUTS i.e. bits[3:0] indicates a...
-        ZPOS = 0x01,
-        MPOS = 0x03,
-        CONF = 0x07,
-
-        RwANGLE = 0x0C,
-        ANGLE = 0x0E,
-
-        STATUS = 0x0B,
-        AGC = 0x1A,
-        MAG = 0x1B,
-    };
-    enum class Command_Bit : uint8_t
-    {
-        WRITE = 0x00,
-        READ = 0x01
-    };
-
-private:
-    uint16_t curr_angle;
-    const uint16_t max_angle = 360;  // specific to our application
-    uint8_t dataBuffer[4];
+    /// @brief Get current measured angle of the encoder
+    /// @return Angle (revs)
+    virtual float getAngle() = 0;
 };
-
-};  // namespace encoder
+}  // namespace driver

ut-robomaster/src/main.cpp

@@ -6,11 +6,11 @@
 #include "tap/motor/motorsim/sim_handler.hpp"
 #endif
 
-#include "tap/board/board.hpp"
-
 #include "modm/architecture/interface/delay.hpp"
 #include "modm/platform.hpp"
 
+#include "board.hpp"
+
 /* arch includes ------------------------------------------------------------*/
 #include "tap/architecture/periodic_timer.hpp"
 #include "tap/architecture/profiler.hpp"
@@ -58,6 +58,7 @@ int main()
 
     Board::initialize();
     initializeIo(drivers);
+    Board::initialize_i2c();
     control::initSubsystemCommands(drivers);
 
 #ifdef PLATFORM_HOSTED
@@ -100,9 +101,6 @@ static void initializeIo(src::Drivers *drivers)
     drivers->djiMotorTerminalSerialHandler.init();
     drivers->bmi088.initialize(IMU_SAMPLE_FREQUENCY, IMU_KP, IMU_KI);
     drivers->bmi088.requestRecalibration();
-
-    modm::platform::I2cMaster2::connect<GpioF0::Sda, GpioF1::Scl>();
-    modm::platform::I2cMaster2::initialize<Board::SystemClock>();
 }
 
 static void updateIo(src::Drivers *drivers)

ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp

@@ -1,6 +1,7 @@
 #include "chassis_subsystem.hpp"
 
 #include "tap/algorithms/math_user_utils.hpp"
+#include "tap/communication/sensors/buzzer/buzzer.hpp"
 
 #include "robots/robot_constants.hpp"
 
@@ -38,9 +39,6 @@ void ChassisSubsystem::refresh()
     }
 
     limitChassisPower();
-
-    // ENCODER_DEBUG
-    drivers->encoder.read_Angle();
 }
 
 void ChassisSubsystem::limitChassisPower()

ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp

@@ -1,6 +1,7 @@
 #ifndef CHASSIS_SUBSYSTEM_HPP_
 #define CHASSIS_SUBSYSTEM_HPP_
 
+#include "tap/architecture/periodic_timer.hpp"
 #include "tap/control/subsystem.hpp"
 
 #include "modm/math/geometry.hpp"

ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp

@@ -0,0 +1,63 @@
+#include "double_yaw_motor.hpp"
+
+#include "tap/algorithms/math_user_utils.hpp"
+#include "tap/motor/dji_motor.hpp"
+
+namespace subsystems::turret
+{
+
+DoubleYawMotor::DoubleYawMotor(
+    src::Drivers *drivers,
+    MotorInterface *motor1,
+    MotorInterface *motor2,
+    Encoder *encoder,
+    const SmoothPidConfig &pidConfig)
+    : drivers(drivers),
+      motor1(motor1),
+      motor2(motor2),
+      encoder(encoder),
+      pid(pidConfig),
+      setpoint(0.0f, 0.0f, M_TWOPI),
+      currentAngle(0.0f, 0.0f, M_TWOPI)
+{
+}
+
+void DoubleYawMotor::initialize()
+{
+    motor1->initialize();
+    motor2->initialize();
+}
+
+void DoubleYawMotor::reset()
+{
+    motor1->setDesiredOutput(0);
+    motor2->setDesiredOutput(0);
+    pid.reset();
+}
+
+void DoubleYawMotor::updateMotorAngle()
+{
+    float encoderAngle = encoder->getAngle() * M_TWOPI;
+    currentAngle.setValue(encoderAngle);
+}
+
+void DoubleYawMotor::setAngle(float desiredAngle, float dt)
+{
+    setpoint.setValue(desiredAngle);
+
+    float positionControllerError =
+        ContiguousFloat(currentAngle.getValue(), 0, M_TWOPI).difference(setpoint.getValue());
+    float rpm1 = motor1->getShaftRPM();                       // rev / m
+    float rpm2 = motor2->getShaftRPM();                       // rev / m
+    float avgRps = (rpm1 + rpm2) / 2.0f * (M_TWOPI / 60.0f);  // rad / s
+    float output = pid.runController(positionControllerError, avgRps, dt);
+
+    output = limitVal(output, -GM6020_MAX_OUTPUT, GM6020_MAX_OUTPUT);
+    motor1->setDesiredOutput(output);
+    motor2->setDesiredOutput(output);
+}
+
+float DoubleYawMotor::getAngle() { return currentAngle.getValue(); }
+
+float DoubleYawMotor::getSetpoint() { return setpoint.getValue(); }
+}  // namespace subsystems::turret

ut-robomaster/src/subsystems/turret/double_yaw_motor.hpp

@@ -0,0 +1,45 @@
+#pragma once
+
+#include "tap/algorithms/contiguous_float.hpp"
+#include "tap/algorithms/smooth_pid.hpp"
+#include "tap/motor/motor_interface.hpp"
+
+#include "drivers/encoder.hpp"
+
+#include "drivers.hpp"
+
+namespace subsystems::turret
+{
+using namespace tap::algorithms;
+using namespace tap::motor;
+using driver::Encoder;
+
+class DoubleYawMotor
+{
+public:
+    DoubleYawMotor(
+        src::Drivers *drivers,
+        MotorInterface *motor1,
+        MotorInterface *motor2,
+        Encoder *encoder,
+        const SmoothPidConfig &pidConfig);
+
+    void initialize();
+    void reset();
+    void updateMotorAngle();
+    void setAngle(float desiredAngle, float dt);
+    float getAngle();
+    float getSetpoint();
+
+private:
+    src::Drivers *drivers;
+    MotorInterface *motor1;
+    MotorInterface *motor2;
+    Encoder *encoder;
+    SmoothPid pid;
+    ContiguousFloat setpoint;
+    ContiguousFloat currentAngle;
+
+    static constexpr float GM6020_MAX_OUTPUT = 30000.0f;
+};
+}  // namespace subsystems::turret