Browse Source

update

master
zhaohe 1 year ago
parent
commit
b49806e465
  1. 18
      components/mini_servo_motor/feite_servo_motor.cpp

18
components/mini_servo_motor/feite_servo_motor.cpp

@ -14,7 +14,7 @@ using namespace feite;
return false; \
}
#define DEBUG_MODE 1
#define DEBUG_MODE 0
static void dumphex(const char* tag, uint8_t* data, uint8_t len) {
#if DEBUG_MODE
@ -171,7 +171,7 @@ bool FeiTeServoMotor::stop(uint8_t id) {
} else {
DO(write_reg(id, kRegServoTorqueSwitch, 0));
DO(write_reg(id, kRegServoRunMode, 0));
DO(write_reg(id, kRegServoRunSpeed, 0));
DO(write_reg(id, kRegServoTorqueSwitch, 1));
return true;
}
@ -219,8 +219,13 @@ bool FeiTeServoMotor::runInMode0(uint8_t id, int32_t limitTorque, int32_t speed,
// stop(id);
int32_t val = 0;
int32_t nowmode = 0;
DO(read_reg(id, kRegServoRunMode, &nowmode));
if (nowmode != 0) {
DO(write_reg(id, kRegServoRunMode, (uint8_t)0));
}
DO(write_reg(id, kRegServoTorqueSwitch, 1));
DO(write_reg(id, kRegServoRunMode, (uint8_t)0));
DO(write_reg(id, kRegServoTorqueLimit, limitTorque));
DO(write_reg(id, kRegServoRunSpeed, speed));
DO(write_reg(id, kRegServoTargetPos, motor_pos));
@ -266,8 +271,11 @@ bool FeiTeServoMotor::runInMode1(uint8_t id, int32_t limitTorque, int32_t speed)
if (limitTorque <= 0) limitTorque = 0;
if (limitTorque > 1000) limitTorque = 1000;
stop(id);
DO(write_reg(id, kRegServoRunMode, (uint8_t)1));
int32_t nowmode = 0;
DO(read_reg(id, kRegServoRunMode, &nowmode));
if (nowmode != 1) {
DO(write_reg(id, kRegServoRunMode, (uint8_t)1));
}
DO(write_reg(id, kRegServoTorqueLimit, limitTorque));
DO(write_reg(id, kRegServoRunSpeed, speed));
return true;

Loading…
Cancel
Save