m5stack/StackChan
1
0
Fork 0
mirror of https://github.com/m5stack/StackChan.git synced 2026-04-27 11:02:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

update firmware source code to v0.18 (#9)

Browse Source
This commit is contained in:
Forairaaaaa
2026-03-25 11:11:14 +08:00
committed by GitHub
parent 5001b7081b
commit 605b575fcc
123 changed files with 24590 additions and 1899 deletions
Download Patch File Download Diff File Expand all files Collapse all files
firmware/main/hal/hal_servo.cpp
+91 -34
View File
@@ -15,47 +15,83 @@ using namespace stackchan::motion;
static SCSCL _scs_bus;
struct ServoConfig_t {
int id = -1;
int defaultZeroPos = 0;
Vector2i angleLimit;
Vector2i rawPosLimit;
std::string settingNs;
std::string settingZeroPositionKey;
bool enablePwmMode = false;
};
class ScsServo : public Servo {
public:
static inline const std::string _tag = "ScsServo";
ScsServo(int id, Vector2i angleLimit, const std::string& settingNs, const std::string& zeroPosKey,
bool enablePwmMode = false)
: _id(id), _setting_ns(settingNs), _zero_pos_key(zeroPosKey), _enable_pwm_mode(enablePwmMode)
ScsServo(const ServoConfig_t& config) : _config(config)
{
set_angle_limit(angleLimit);
}
Settings settings(_setting_ns, false);
_zero_pos = settings.GetInt(_zero_pos_key, 0);
void init() override
{
set_angle_limit(_config.angleLimit);
get_zero_pos_from_nvs();
Servo::init();
}
mclog::tagInfo(_tag, "id: {} get zero pos: {} from settings", _id, _zero_pos);
void get_zero_pos_from_nvs()
{
_zero_pos = _config.defaultZeroPos;
bool is_valid = false;
{
Settings settings(_config.settingNs, false);
int nvs_zero_pos = settings.GetInt(_config.settingZeroPositionKey, -1);
// Limit check
if (nvs_zero_pos >= _config.rawPosLimit.x && nvs_zero_pos <= _config.rawPosLimit.y) {
_zero_pos = nvs_zero_pos;
is_valid = true;
mclog::tagInfo(_tag, "id: {} get zero pos: {} from settings", _config.id, _zero_pos);
} else {
is_valid = false;
mclog::tagWarn(_tag, "id: {} get invalid zero pos: {} from settings", _config.id, nvs_zero_pos);
}
}
if (!is_valid) {
_zero_pos = _config.defaultZeroPos;
mclog::tagInfo(_tag, "id: {} override zero pos to default: {}", _config.id, _zero_pos);
Settings settings(_config.settingNs, true);
settings.SetInt(_config.settingZeroPositionKey, _zero_pos);
}
}
void set_angle_impl(int angle) override
{
int mapped_angle = _zero_pos + angle * 16 / 5 / 10; // 一步对应 0.3125度, 0.3125 = 5/16
if (mapped_angle < 0) {
mapped_angle = 0;
}
mapped_angle = uitk::clamp(mapped_angle, _config.rawPosLimit.x, _config.rawPosLimit.y);
// mclog::tagInfo(_tag, "id: {} mapped angle: {}", _id, mapped_angle);
check_mode(Mode::Position);
_scs_bus.WritePos(_id, mapped_angle, 20, 0);
_scs_bus.WritePos(_config.id, mapped_angle, 20, 0);
}
int getCurrentAngle() override
{
int current_pos = _scs_bus.ReadPos(_id);
int current_pos = _scs_bus.ReadPos(_config.id);
int angle = (current_pos - _zero_pos) * 5 * 10 / 16;
angle = uitk::clamp(angle, getAngleLimit().x, getAngleLimit().y);
// mclog::tagInfo(_tag, "id: {} current pos: {} angle: {}", _id, current_pos, angle);
return angle;
}
bool isMoving() override
bool is_moving_impl() override
{
int moving = _scs_bus.ReadMove(_id);
int moving = _scs_bus.ReadMove(_config.id);
// mclog::tagInfo(_tag, "id: {} moving: {}", _id, moving);
return moving != 0;
}
@@ -63,53 +99,57 @@ public:
void setTorqueEnabled(bool enabled) override
{
Servo::setTorqueEnabled(enabled);
_scs_bus.EnableTorque(_id, enabled ? 1 : 0);
_scs_bus.EnableTorque(_config.id, enabled ? 1 : 0);
// mclog::tagInfo(_tag, "id: {} set torque: {}", _id, enabled);
}
bool getTorqueEnabled() override
{
int torque_enable = _scs_bus.ReadToqueEnable(_id);
int torque_enable = _scs_bus.ReadToqueEnable(_config.id);
// mclog::tagInfo(_tag, "id: {} torque enable: {}", _id, torque_enable);
return torque_enable > 0;
}
void setCurrentAngleAsZero() override
{
_zero_pos = _scs_bus.ReadPos(_id);
_zero_pos = _scs_bus.ReadPos(_config.id);
Settings settings(_setting_ns, true);
settings.SetInt(_zero_pos_key, _zero_pos);
Settings settings(_config.settingNs, true);
settings.SetInt(_config.settingZeroPositionKey, _zero_pos);
mclog::tagInfo(_tag, "id: {} set zero pos: {} to settings", _id, _zero_pos);
mclog::tagInfo(_tag, "id: {} set zero pos: {} to settings", _config.id, _zero_pos);
}
void resetZeroCalibration() override
{
_zero_pos = _config.defaultZeroPos;
Settings settings(_config.settingNs, true);
settings.SetInt(_config.settingZeroPositionKey, _zero_pos);
mclog::tagInfo(_tag, "id: {} set zero pos: {} to settings", _config.id, _zero_pos);
}
void rotate(int velocity) override
{
velocity = uitk::clamp(velocity, -1000, 1000);
if (!_enable_pwm_mode) {
if (!_config.enablePwmMode) {
return;
}
int mapped_velocity = map_range(velocity, 0, 1000, 0, 1023);
check_mode(Mode::PWM);
_scs_bus.WritePWM(_id, mapped_velocity);
_scs_bus.WritePWM(_config.id, mapped_velocity);
}
private:
enum class Mode { Position = 0, PWM = 1 };
int _id = -1;
int _zero_pos = 0;
std::string _setting_ns;
std::string _zero_pos_key;
ServoConfig_t _config;
int _zero_pos = 0;
Mode _current_mode = Mode::Position;
bool _enable_pwm_mode;
uint32_t _last_true_move_ms = 0;
const uint32_t STOP_DEBOUNCE_MS = 250;
void check_mode(Mode targetMode)
{
@@ -117,7 +157,7 @@ private:
return;
}
_scs_bus.SwitchMode(_id, static_cast<uint8_t>(targetMode));
_scs_bus.SwitchMode(_config.id, static_cast<uint8_t>(targetMode));
_current_mode = targetMode;
}
};
@@ -128,9 +168,26 @@ void Hal::servo_init()
_scs_bus.begin(UART_NUM_1, 1000000, 6, 7);
auto motion =
std::make_unique<Motion>(std::make_unique<ScsServo>(1, Vector2i(-1280, 1280), "servo", "zero_pos_1", true),
std::make_unique<ScsServo>(2, Vector2i(0, 900), "servo", "zero_pos_2"));
ServoConfig_t yaw_servo_config;
yaw_servo_config.id = 1;
yaw_servo_config.defaultZeroPos = 450;
yaw_servo_config.angleLimit = Vector2i(-1280, 1280);
yaw_servo_config.rawPosLimit = Vector2i(0, 1000);
yaw_servo_config.settingNs = "servo";
yaw_servo_config.settingZeroPositionKey = "zero_pos_1";
yaw_servo_config.enablePwmMode = true;
ServoConfig_t pitch_servo_config;
pitch_servo_config.id = 2;
pitch_servo_config.defaultZeroPos = 125;
pitch_servo_config.angleLimit = Vector2i(0, 900);
pitch_servo_config.rawPosLimit = Vector2i(0, 1000);
pitch_servo_config.settingNs = "servo";
pitch_servo_config.settingZeroPositionKey = "zero_pos_2";
auto yaw_servo = std::make_unique<ScsServo>(yaw_servo_config);
auto pitch_servo = std::make_unique<ScsServo>(pitch_servo_config);
auto motion = std::make_unique<Motion>(std::move(yaw_servo), std::move(pitch_servo));
motion->init();
GetStackChan().attachMotion(std::move(motion));