Program Listing for File dji_motor.hpp

↰ Return to documentation for file (src/tap/motor/dji_motor.hpp)

/*
 * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington <robomstr@uw.edu>
 *
 * This file is part of Taproot.
 *
 * Taproot is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Taproot is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
 */

#ifndef TAPROOT_DJI_MOTOR_HPP_
#define TAPROOT_DJI_MOTOR_HPP_

#include <string>

#include "tap/architecture/timeout.hpp"
#include "tap/communication/can/can_rx_listener.hpp"

#include "modm/math/geometry/angle.hpp"

#include "motor_interface.hpp"

namespace tap::motor
{
enum MotorId : uint32_t
{
    MOTOR1 = 0X201,
    MOTOR2 = 0x202,
    MOTOR3 = 0x203,
    MOTOR4 = 0x204,
    MOTOR5 = 0x205,
    MOTOR6 = 0x206,
    MOTOR7 = 0x207,
    MOTOR8 = 0x208,
};

class DjiMotor : public can::CanRxListener, public MotorInterface
{
public:
    // 0 - 8191 for dji motors
    static constexpr uint16_t ENC_RESOLUTION = 8192;

    // Maximum values for following motors
    // Controller for the M2006, in mA output
    static constexpr uint16_t MAX_OUTPUT_C610 = 10000;
    // Controller for the M3508, in mA output (Mapped to a 20A range though, not 1:1)
    static constexpr uint16_t MAX_OUTPUT_C620 = 16384;
    // Controller for the M3510, in mA output (Not known if mapped to 20A range)
    static constexpr uint16_t MAX_OUTPUT_820R = 32767;

    // Output is in mV
    static constexpr uint16_t MAX_OUTPUT_GM6020 = 25000;
    // Output is in mV
    static constexpr uint16_t MAX_OUTPUT_GM3510 = 29000;

    // Internal gear ratio of the following motors
    static constexpr float GEAR_RATIO_M3508 = 3591.0f / 187.0f;
    static constexpr float GEAR_RATIO_M3510_L1 = 3.7f / 1.0f;
    static constexpr float GEAR_RATIO_M3510_L2 = 5.2f / 1.0f;
    static constexpr float GEAR_RATIO_M3510_L3 = 19.0f / 1.0f;
    static constexpr float GEAR_RATIO_M3510_L4 = 27.0f / 1.0f;
    static constexpr float GEAR_RATIO_M2006 = 36.0f / 1.0f;

    DjiMotor(
        Drivers* drivers,
        MotorId desMotorIdentifier,
        tap::can::CanBus motorCanBus,
        bool isInverted,
        const char* name,
        uint16_t encoderWrapped = ENC_RESOLUTION / 2,
        int64_t encoderRevolutions = 0,
        bool currentControl = false);

    mockable ~DjiMotor();

    void initialize() override;

    float getPositionUnwrapped() const override;

    float getPositionWrapped() const override;

    int64_t getEncoderUnwrapped() const override;

    uint16_t getEncoderWrapped() const override;

    void resetEncoderValue() override;

    DISALLOW_COPY_AND_ASSIGN(DjiMotor)


    void processMessage(const modm::can::Message& message) override;

    void setDesiredOutput(int32_t desiredOutput) override;

    bool isMotorOnline() const override;

    mockable void serializeCanSendData(modm::can::Message* txMessage) const;

    int16_t getOutputDesired() const override;

    mockable uint32_t getMotorIdentifier() const;

    int8_t getTemperature() const override;

    int16_t getTorque() const override;

    int16_t getShaftRPM() const override;

    mockable bool isMotorInverted() const;

    mockable tap::can::CanBus getCanBus() const;

    mockable const char* getName() const;

    mockable bool isInCurrentControl() const;

    template <typename T>
    static void assertEncoderType()
    {
        constexpr bool good_type =
            std::is_same<typename std::decay<T>::type, std::int64_t>::value ||
            std::is_same<typename std::decay<T>::type, std::uint16_t>::value;
        static_assert(good_type, "x is not of the correct type");
    }

    template <typename T>
    static T degreesToEncoder(float angle)
    {
        assertEncoderType<T>();
        return static_cast<T>((ENC_RESOLUTION * angle) / 360);
    }

    template <typename T>
    static float encoderToDegrees(T encoder)
    {
        assertEncoderType<T>();
        return (360.0f * static_cast<float>(encoder)) / ENC_RESOLUTION;
    }

private:
    // wait time before the motor is considered disconnected, in milliseconds
    static const uint32_t MOTOR_DISCONNECT_TIME = 100;

    const char* motorName;

    void updateEncoderValue(uint16_t newEncWrapped);

    Drivers* drivers;

    uint32_t motorIdentifier;

    tap::can::CanBus motorCanBus;

    int16_t desiredOutput;

    int16_t shaftRPM;

    int8_t temperature;

    int16_t torque;

    bool motorInverted;

    uint16_t encoderWrapped;

    int64_t encoderRevolutions;

    uint16_t encoderHomePosition;

    tap::arch::MilliTimeout motorDisconnectTimeout;

    bool currentControl;
};

}  // namespace tap::motor

#endif  // TAPROOT_DJI_MOTOR_HPP_