|
| 1 | +#pragma once |
| 2 | + |
| 3 | +#pragma once |
| 4 | + |
| 5 | +#include <string> |
| 6 | +#include <vector> |
| 7 | + |
| 8 | +#include "base_component.hpp" |
| 9 | +#include "bldc_driver.hpp" |
| 10 | +#include "bldc_motor.hpp" |
| 11 | +#include "i2c.hpp" |
| 12 | +#include "mt6701.hpp" |
| 13 | +#include "simple_lowpass_filter.hpp" |
| 14 | + |
| 15 | +namespace espp { |
| 16 | +/// This class acts as a board support component for the TinyS3 BLDC Test Stand. |
| 17 | +/// It provides a high-level interface to the system's functionality. |
| 18 | +/// |
| 19 | +/// High level overview of the system |
| 20 | +/// - ESP32s3 module, using TinyS3 |
| 21 | +/// - TMC6300-BOB motor driver on a breakout board |
| 22 | +/// - One MT6701 magnetic encoder connected via I2C |
| 23 | +class TinyS3TestStand : public BaseComponent { |
| 24 | +public: |
| 25 | + using Encoder = espp::Mt6701<>; |
| 26 | + using BldcMotor = espp::BldcMotor<espp::BldcDriver, Encoder>; |
| 27 | + |
| 28 | + /// Constructor |
| 29 | + /// \param verbosity The verbosity level for the logger of the MotorGo-Mini |
| 30 | + /// and its components |
| 31 | + explicit TinyS3TestStand(espp::Logger::Verbosity verbosity = espp::Logger::Verbosity::WARN) |
| 32 | + : BaseComponent("TinyS3 Test Stand", verbosity) { |
| 33 | + init(); |
| 34 | + } |
| 35 | + |
| 36 | + /// Get a reference to the encoder |
| 37 | + /// \return A reference to the encoder |
| 38 | + Encoder &encoder() { return encoder_; } |
| 39 | + |
| 40 | + /// Get a reference to the motor driver |
| 41 | + /// \return A reference to the motor driver |
| 42 | + espp::BldcDriver &motor_driver() { return motor_driver_; } |
| 43 | + |
| 44 | + /// Get a reference to the motor |
| 45 | + /// \return A reference to the motor |
| 46 | + BldcMotor &motor() { return motor_; } |
| 47 | + |
| 48 | +protected: |
| 49 | + static constexpr auto I2C_PORT = I2C_NUM_0; |
| 50 | + static constexpr auto I2C_SDA_PIN = GPIO_NUM_8; |
| 51 | + static constexpr auto I2C_SCL_PIN = GPIO_NUM_9; |
| 52 | + |
| 53 | + static constexpr uint64_t core_update_period_us = 1000; // 1 ms |
| 54 | + |
| 55 | + static constexpr auto MOTOR_A_H = GPIO_NUM_1; |
| 56 | + static constexpr auto MOTOR_A_L = GPIO_NUM_2; |
| 57 | + static constexpr auto MOTOR_B_H = GPIO_NUM_3; |
| 58 | + static constexpr auto MOTOR_B_L = GPIO_NUM_4; |
| 59 | + static constexpr auto MOTOR_C_H = GPIO_NUM_5; |
| 60 | + static constexpr auto MOTOR_C_L = GPIO_NUM_21; |
| 61 | + static constexpr auto MOTOR_ENABLE = GPIO_NUM_34; |
| 62 | + static constexpr auto MOTOR_FAULT = GPIO_NUM_36; |
| 63 | + |
| 64 | + void init() { |
| 65 | + init_encoder(); |
| 66 | + init_motor(); |
| 67 | + } |
| 68 | + |
| 69 | + void init_encoder() { |
| 70 | + bool run_task = true; |
| 71 | + std::error_code ec; |
| 72 | + encoder_.initialize(run_task, ec); |
| 73 | + if (ec) { |
| 74 | + logger_.error("Could not initialize encoder: {}", ec.message()); |
| 75 | + } |
| 76 | + } |
| 77 | + |
| 78 | + void init_motor() { motor_.initialize(); } |
| 79 | + |
| 80 | + /// I2C bus for external communication |
| 81 | + I2c i2c_{{ |
| 82 | + .port = I2C_PORT, |
| 83 | + .sda_io_num = I2C_SDA_PIN, |
| 84 | + .scl_io_num = I2C_SCL_PIN, |
| 85 | + .sda_pullup_en = GPIO_PULLUP_ENABLE, |
| 86 | + .scl_pullup_en = GPIO_PULLUP_ENABLE, |
| 87 | + .clk_speed = 1 * 1000 * 1000, // MT6701 supports 1 MHz I2C |
| 88 | + }}; |
| 89 | + |
| 90 | + // Encoder |
| 91 | + Encoder encoder_{ |
| 92 | + {.write = std::bind(&espp::I2c::write, &i2c_, std::placeholders::_1, std::placeholders::_2, |
| 93 | + std::placeholders::_3), |
| 94 | + .read = std::bind(&espp::I2c::read, &i2c_, std::placeholders::_1, std::placeholders::_2, |
| 95 | + std::placeholders::_3), |
| 96 | + .update_period = std::chrono::duration<float>(core_update_period_us / 1e6f), |
| 97 | + .auto_init = false, // we have to initialize the SPI first before we can use the encoder |
| 98 | + .log_level = get_log_level()}}; |
| 99 | + |
| 100 | + // Driver |
| 101 | + espp::BldcDriver motor_driver_{{.gpio_a_h = MOTOR_A_H, |
| 102 | + .gpio_a_l = MOTOR_A_L, |
| 103 | + .gpio_b_h = MOTOR_B_H, |
| 104 | + .gpio_b_l = MOTOR_B_L, |
| 105 | + .gpio_c_h = MOTOR_C_H, |
| 106 | + .gpio_c_l = MOTOR_C_L, |
| 107 | + .gpio_enable = MOTOR_ENABLE, |
| 108 | + .gpio_fault = MOTOR_FAULT, |
| 109 | + .power_supply_voltage = 5.0f, |
| 110 | + .limit_voltage = 5.0f, |
| 111 | + .log_level = get_log_level()}}; |
| 112 | + |
| 113 | + // Filters |
| 114 | + espp::SimpleLowpassFilter motor_velocity_filter_{{.time_constant = 0.005f}}; |
| 115 | + espp::SimpleLowpassFilter motor_angle_filter_{{.time_constant = 0.001f}}; |
| 116 | + |
| 117 | + // Motor |
| 118 | + BldcMotor motor_{{ |
| 119 | + .num_pole_pairs = 7, |
| 120 | + .phase_resistance = 5.0f, |
| 121 | + .kv_rating = 320, |
| 122 | + .current_limit = 1.0f, |
| 123 | + .foc_type = espp::detail::FocType::SPACE_VECTOR_PWM, |
| 124 | + // create shared_ptr from raw pointer to ensure shared_ptr doesn't delete the object |
| 125 | + .driver = |
| 126 | + std::shared_ptr<espp::BldcDriver>(std::shared_ptr<espp::BldcDriver>{}, &motor_driver_), |
| 127 | + // create shared_ptr from raw pointer to ensure shared_ptr doesn't delete the object |
| 128 | + .sensor = std::shared_ptr<Encoder>(std::shared_ptr<Encoder>{}, &encoder_), |
| 129 | + .velocity_pid_config = |
| 130 | + { |
| 131 | + .kp = 0.010f, |
| 132 | + .ki = 1.000f, |
| 133 | + .kd = 0.000f, |
| 134 | + .integrator_min = -1.0f, // same scale as output_min (so same scale as current) |
| 135 | + .integrator_max = 1.0f, // same scale as output_max (so same scale as current) |
| 136 | + .output_min = -1.0, // velocity pid works on current (if we have phase resistance) |
| 137 | + .output_max = 1.0, // velocity pid works on current (if we have phase resistance) |
| 138 | + }, |
| 139 | + .angle_pid_config = |
| 140 | + { |
| 141 | + .kp = 7.000f, |
| 142 | + .ki = 0.300f, |
| 143 | + .kd = 0.010f, |
| 144 | + .integrator_min = -10.0f, // same scale as output_min (so same scale as velocity) |
| 145 | + .integrator_max = 10.0f, // same scale as output_max (so same scale as velocity) |
| 146 | + .output_min = -20.0, // angle pid works on velocity (rad/s) |
| 147 | + .output_max = 20.0, // angle pid works on velocity (rad/s) |
| 148 | + }, |
| 149 | + .velocity_filter = [this](auto v) { return motor_velocity_filter_(v); }, |
| 150 | + .angle_filter = [this](auto v) { return motor_angle_filter_(v); }, |
| 151 | + .auto_init = false, // we have to initialize the SPI first before we can use the encoder |
| 152 | + .log_level = get_log_level(), |
| 153 | + }}; |
| 154 | +}; |
| 155 | +} // namespace espp |
0 commit comments