Quadrotor.h

 
#pragma once
 
#include "TinyRobotics/motors/Motors.h"
#include "Vehicle.h"
 
namespace tinyrobotics {
 
/// @brief Enum for quadrotor motor indexing
enum QuadrotorMotorNo {
  FRONT_LEFT = 0,
  FRONT_RIGHT = 1,
  REAR_LEFT = 2,
  REAR_RIGHT = 3
};
 
/**
 * @class Quadrotor
 * @ingroup vehicles
 * @brief Simple quadrotor (quadcopter) model with 4-motor control.
 *
 * This class abstracts the control of a basic quadcopter:
 *  - 4 motors (front left, front right, rear left, rear right) via HBridge
 *  - Throttle, roll, pitch, and yaw control
 *
 * Usage Example (with setPins):
 * @code
 * Quadrotor quad;
 * quad.setPins(FRONT_LEFT, m1_in1, m1_in2); // front left
 * quad.setPins(FRONT_RIGHT, m2_in1, m2_in2); // front right
 * quad.setPins(REAR_LEFT, m3_in1, m3_in2); // rear left
 * quad.setPins(REAR_RIGHT, m4_in1, m4_in2); // rear right
 * quad.setThrottle(60); // 60% throttle
 * quad.setRoll(10);     // roll right
 * quad.setPitch(-5);    // pitch down
 * quad.setYaw(15);      // yaw right
 * @endcode
 */
 
template <typename T=float, typename DriverT = BrushedMotor<T>>
class Quadrotor : public Vehicle<T> {
 public:
  Quadrotor() = default;
 
  /**
   * @brief Set the pins for a specific motor (0=front left, 1=front right,
   * 2=rear left, 3=rear right)
   */
  void setPins(QuadrotorMotorNo motor, int in1, int in2) {
    motors_[motor].setPins(in1, in2);
    motors_[motor].setID((uint8_t)motor);
  }
 
  /** Set throttle (0-100%) for all motors */
  void setThrottle(float percent) {
    throttle_ = constrain(percent, 0.0f, 100.0f);
    updateMotors();
  }
 
  /** Set roll (-100 to 100): positive = right, negative = left */
  void setRoll(float percent) {
    roll_ = constrain(percent, -100.0f, 100.0f);
    updateMotors();
  }
 
  /** Set pitch (-100 to 100): positive = forward, negative = backward */
  void setPitch(float percent) {
    pitch_ = constrain(percent, -100.0f, 100.0f);
    updateMotors();
  }
 
  /** Set yaw (-100 to 100): positive = right, negative = left */
  void setYaw(float percent) {
    yaw_ = constrain(percent, -100.0f, 100.0f);
    updateMotors();
  }
 
  /** Stop all motors and reset state */
  void end() override {
    setThrottle(0);
    setRoll(0);
    setPitch(0);
    setYaw(0);
    // stop all motors
    for (auto& motor : getMotors()) {
      motor.end();
    }
  }
 
  /**
   * @brief Set a calibration gain for a specific motor (default 1.0).
   * @param motor Motor index (QuadrotorMotorNo)
   * @param gain  Gain factor (e.g., 1.05 for +5% output)
   */
  void setMotorGain(QuadrotorMotorNo motor, float gain) {
    motorGain_[motor] = gain;
  }
 
  bool isPinsSet() const {
    for (int i = 0; i < 4; ++i) {
      if (!motors_[i].isPinsSet()) return false;
    }
    return true;
  }
 
  bool onMessage(const Message<float>& msg) override {
    float angle;
    switch (msg.content) {
      case MessageContent::Throttle:
        if (msg.unit != Unit::Percent) return false;
        setThrottle(static_cast<int>(msg.value));
        return true;
      case MessageContent::Pitch:
        angle = msg.value;
        if (!toAngleDegree(angle, msg.unit, angle))
          return false;  // Invalid unit
        setPitch(angle);
        return true;
      case MessageContent::Roll:
        angle = msg.value;
        if (!toAngleDegree(angle, msg.unit, angle))
          return false;  // Invalid unit
        setRoll(angle);
        return true;
      case MessageContent::Yaw:
        angle = msg.value;
        if (!toAngleDegree(angle, msg.unit, angle))
          return false;  // Invalid unit
        setYaw(angle);
        return true;
      default:
        return false;  // Unhandled message content
    }
  }
 
  std::vector<MessageContent> getControls() const override {
    return {MessageContent::Throttle, MessageContent::Pitch,
            MessageContent::Roll, MessageContent::Yaw};
  }
 
  DriverT& getMotor(QuadrotorMotorNo motor) { return motors_[motor]; }
 
  std::vector<IMotor<T>*> getMotors() override {
    std::vector<IMotor<T>*> motors;
    for (int i = 0; i < 4; ++i) {
      motors.push_back(&motors_[i]);
    }
    return motors;
  }
 
 protected:
  DriverT motors_[4];
  float motorGain_[4] = {1.0f, 1.0f, 1.0f, 1.0f};
  float throttle_ = 0;
  float roll_ = 0;
  float pitch_ = 0;
  float yaw_ = 0;
 
  /**
   * @brief Update all motors based on throttle, roll, pitch, and yaw.
   *
   * This is a simple mixer for an X-configuration quadrotor:
   *  - m1: front left
   *  - m2: front right
   *  - m3: rear left
   *  - m4: rear right
   *
   * Each control input is in percent (-100 to 100).
   */
  void updateMotors() {
    // Basic mixing: throttle + pitch/roll/yaw
    int m[4];
    m[0] = throttle_ + pitch_ + roll_ - yaw_;
    m[1] = throttle_ + pitch_ - roll_ + yaw_;
    m[2] = throttle_ - pitch_ + roll_ + yaw_;
    m[3] = throttle_ - pitch_ - roll_ - yaw_;
    for (int i = 0; i < 4; ++i) {
      int calibrated = static_cast<int>(m[i] * motorGain_[i]);
      motors_[i].setSpeed(constrain(calibrated, 0, 100));
    }
    // publish motor speeds as messages for telemetry
    for (int i = 0; i < 4; ++i) {
      Message<float> msg(MessageContent::MotorSpeed, m[i], Unit::Percent);
      msg.origin = MessageOrigin::Motor;
      msg.origin_id = i;  // Motor index
      Vehicle<T>::sendMessage(msg);
    }
  }
};
 
}  // namespace tinyrobotics