飞特舵机支持设置任意位置

a8000_protocol

@@ -1 +1 @@
-Subproject commit 36523bea3565257b6033f75fd1adb343602532e9
+Subproject commit ad55d4474d5fe39585c788d2f5d73832cf782e94

sdk/components/mini_servo_motor/feite_servo_motor.cpp

@@ -102,10 +102,11 @@ static void reginfo_init() {
   /*86*/ REG_INFO_INIT(kRegServoAccMultiple /*          */, 1, UNSIGN, 0);   // 86 加速度倍数
 }
 
-void FeiTeServoMotor::regServo(uint8_t id, feite_servo_model_number_t model_type) {
+void FeiTeServoMotor::regServo(uint8_t id, feite_servo_model_number_t model_type, bool shaft) {
   feite_servo_info_t info;
   info.id         = id;
   info.model_type = model_type;
+  info.shaft      = shaft;
   if (info.model_type == ksts5420m_c001) {
     info.support_mode0 = true;
     info.support_mode1 = true;
@@ -116,8 +117,8 @@ void FeiTeServoMotor::regServo(uint8_t id, feite_servo_model_number_t model_type
 
     info.regDefaultVal_servoMinAngle = 1;
     info.regDefaultVal_servoMaxAngle = 4094;
-
-    info.angleConverCoefficient = 360 / 4096.0;
+    info.range                       = 4096;
+    info.angleConverCoefficient      = 360 / 4096.0;
   } else if (info.model_type == kscs009) {
     info.support_mode0               = true;
     info.support_mode1               = false;
@@ -127,6 +128,7 @@ void FeiTeServoMotor::regServo(uint8_t id, feite_servo_model_number_t model_type
     info.isSCS                       = true;
     info.regDefaultVal_servoMinAngle = 20;
     info.regDefaultVal_servoMaxAngle = 1003;
+    info.range                       = 1024;
     info.angleConverCoefficient      = 300 / 1024.0;
   }
   regServoInfo(&info);
@@ -187,6 +189,12 @@ bool FeiTeServoMotor::runInMode0(uint8_t id, int32_t limitTorque, int32_t speed,
   }
 
   if (info->isSCS) {
+    if (info->shaft) {
+      pos3600 = 3600 - pos3600;
+    }
+    if (pos3600 < 0) {
+      pos3600 = 0;
+    }
     if (limitTorque <= 0) limitTorque = 0;
     if (limitTorque > 1000) limitTorque = 1000;
 
@@ -207,6 +215,12 @@ bool FeiTeServoMotor::runInMode0(uint8_t id, int32_t limitTorque, int32_t speed,
 
     return true;
   } else {
+    if (info->shaft) {
+      pos3600 = 3600 - pos3600;
+    }
+    if (pos3600 < 0) {
+      pos3600 = 0;
+    }
     if (limitTorque <= 0) limitTorque = 0;
     if (limitTorque > 1000) limitTorque = 1000;
 
@@ -233,6 +247,75 @@ bool FeiTeServoMotor::runInMode0(uint8_t id, int32_t limitTorque, int32_t speed,
   }
 }
 
+bool FeiTeServoMotor::setInMode0(uint8_t id) {
+  zlock_guard         l(m_mutex);
+  feite_servo_info_t* info = getServoInfo(id);
+
+  if (!info) {
+    ZLOGE(TAG, "runInMode0 id:%d not find", id);
+    return false;
+  }
+
+  if (info->isSCS) {
+    // 由于SCS型号的舵机如果当前工作在模式2下，
+    //      则 kRegServoRunTime 控制运行方向和扭矩，这里需要将其修改为0，让其停下来
+    stop(id);
+    DO(write_reg(id, kRegServoMinAngle, info->regDefaultVal_servoMinAngle));
+    DO(write_reg(id, kRegServoMaxAngle, info->regDefaultVal_servoMaxAngle));
+    return true;
+  } else {
+    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));
+
+    return true;
+  }
+}
+bool FeiTeServoMotor::setInMode1(uint8_t id) {
+  zlock_guard         l(m_mutex);
+  feite_servo_info_t* info = getServoInfo(id);
+
+  if (!info) {
+    ZLOGE(TAG, "runInMode0 id:%d not find", id);
+    return false;
+  }
+  if (!info->support_mode1) {
+    ZLOGE(TAG, "runInMode1 id:%d not support", id);
+    return false;
+  }
+  if (info->isSCS) {
+    ZLOGE(TAG, "runInMode1 id:%d not support", id);
+    return false;
+  } else {
+    stop(id);
+    int32_t nowmode = 0;
+    DO(read_reg(id, kRegServoRunMode, &nowmode));
+    if (nowmode != 1) {
+      DO(write_reg(id, kRegServoRunMode, (uint8_t)1));
+    }
+    return true;
+  }
+  return false;
+}
+bool FeiTeServoMotor::setInMode2(uint8_t id) {
+  zlock_guard         l(m_mutex);
+  feite_servo_info_t* info = getServoInfo(id);
+
+  if (!info) {
+    ZLOGE(TAG, "runInMode0 id:%d not find", id);
+    return false;
+  }
+  if (!info->support_mode2) {
+    ZLOGE(TAG, "runInMode2 id:%d not support", id);
+    return false;
+  }
+  return setInMode0(id);
+}
+
 bool FeiTeServoMotor::isSupportMode0(uint8_t id) {
   zlock_guard         l(m_mutex);
   feite_servo_info_t* info = getServoInfo(id);
@@ -268,6 +351,10 @@ bool FeiTeServoMotor::runInMode1(uint8_t id, int32_t limitTorque, int32_t speed)
     ZLOGE(TAG, "runInMode1 id:%d not support", id);
     return false;
   } else {
+    if (info->shaft) {
+      speed = -speed;
+    }
+
     if (limitTorque <= 0) limitTorque = 0;
     if (limitTorque > 1000) limitTorque = 1000;
     stop(id);
@@ -282,6 +369,82 @@ bool FeiTeServoMotor::runInMode1(uint8_t id, int32_t limitTorque, int32_t speed)
   }
   return false;
 }
+
+static int16_t getcalibrate(int16_t nowpos, int16_t aftercalibratepos) {
+  int16_t calibrate = nowpos - aftercalibratepos;
+  while (true) {
+    if (calibrate > 2047) {
+      calibrate -= 4094;
+    } else if (calibrate < -2047) {
+      calibrate += 4094;
+    } else {
+      break;
+    }
+  }
+  return calibrate;
+}
+bool FeiTeServoMotor::setCurPos(uint8_t id, int32_t pos3600) {
+  zlock_guard         l(m_mutex);
+  feite_servo_info_t* info = getServoInfo(id);
+
+  if (!info) {
+    ZLOGE(TAG, "runInMode0 id:%d not find", id);
+    return false;
+  }
+  if (info->isSCS) {
+    ZLOGE(TAG, "runInMode1 id:%d not support", id);
+    return false;
+  } else {
+    if (info->range != 4096) {
+      return false;
+    }
+    if (info->shaft) {
+      pos3600 = 3600 - pos3600;
+    }
+    int32_t motor_pos = pos3600 / 10.0 / info->angleConverCoefficient;
+    return setCurPos4096(id, motor_pos);
+  }
+}
+
+bool FeiTeServoMotor::isSupportSetCurPos(uint8_t id) {
+  zlock_guard         l(m_mutex);
+  feite_servo_info_t* info = getServoInfo(id);
+
+  if (!info) {
+    ZLOGE(TAG, "runInMode0 id:%d not find", id);
+    return false;
+  }
+  if (info->isSCS) {
+    ZLOGE(TAG, "runInMode1 id:%d not support", id);
+    return false;
+  } else {
+    if (info->range != 4096) {
+      return false;
+    }
+    return true;
+  }
+}
+
+bool FeiTeServoMotor::setCurPos4096(uint8_t id, int32_t targetpos4095) {
+  DO(setInMode0(id));
+
+  int32_t nowpos            = 0;
+  int32_t nowposcalibration = 0;
+  DO(write_reg(id, feite::reg_add_e::kRegServoTorqueSwitch, 0));
+  DO(write_reg(id, kRegServoCalibration, 0));
+
+  DO(read_reg(id, kRegServoCurrentPos, &nowpos));
+  int16_t newcalibrate = getcalibrate(nowpos, targetpos4095);
+  DO(write_reg(id, kRegServoLockFlag, 0));
+  DO(write_reg(id, kRegServoCalibration, newcalibrate));
+  DO(write_reg(id, kRegServoLockFlag, 1));
+
+  DO(write_reg(id, kRegServoTargetPos, targetpos4095));
+  DO(write_reg(id, feite::reg_add_e::kRegServoTorqueSwitch, 1));
+
+  return true;
+}
+
 bool FeiTeServoMotor::runInMode2(uint8_t id, int32_t torque) {
   zlock_guard         l(m_mutex);
   feite_servo_info_t* info = getServoInfo(id);
@@ -296,6 +459,10 @@ bool FeiTeServoMotor::runInMode2(uint8_t id, int32_t torque) {
   }
 
   if (info->isSCS) {
+    if (info->shaft) {
+      torque = -torque;
+    }
+
     stop(id);
     DO(write_reg(id, kRegServoMinAngle, 0));
     DO(write_reg(id, kRegServoMaxAngle, 0));
@@ -352,6 +519,17 @@ bool FeiTeServoMotor::getPos(uint8_t id, int32_t* pos) {
   // ZLOGI(TAG, "getPos id:%d pos:%d", id, *pos);
 
   *pos = (*pos) * info->angleConverCoefficient * 10 + 0.5;
+
+  if (info->shaft) {
+    *pos = 3600 - *pos;
+  }
+  if (*pos < 0) {
+    *pos = 0;
+  }
+  if (*pos > 3600) {
+    *pos = 3600;
+  }
+
   return true;
 }
 bool FeiTeServoMotor::isMove(uint8_t id, int32_t* move) {

sdk/components/mini_servo_motor/feite_servo_motor.hpp

@@ -34,6 +34,8 @@ typedef struct {
 
   bool isSCS;
 
+  bool shaft;
+
   bool bigend;
   bool support_mode0;  // 角度伺服模式
   bool support_mode1;  // 速度闭环模式
@@ -43,6 +45,8 @@ typedef struct {
   int16_t regDefaultVal_servoMinAngle;
   int16_t regDefaultVal_servoMaxAngle;
 
+  int16_t range;
+
   float angleConverCoefficient;
 
 } feite_servo_info_t;
@@ -163,11 +167,16 @@ class FeiTeServoMotor {
  public:
   void initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx);
 
-  void                regServo(uint8_t id, feite_servo_model_number_t model_type);
+  void                regServo(uint8_t id, feite_servo_model_number_t model_type, bool shaft);
   feite_servo_info_t* getServoInfo(int id);
 
   bool ping(uint8_t id);
   bool stop(uint8_t id);
+
+  bool setInMode0(uint8_t id);
+  bool setInMode1(uint8_t id);
+  bool setInMode2(uint8_t id);
+
   /**
    * @brief
    *
@@ -189,6 +198,10 @@ class FeiTeServoMotor {
   bool getPos(uint8_t id, int32_t* pos);
   bool isMove(uint8_t id, int32_t* move);
 
+  bool setCurPos(uint8_t id, int32_t pos);
+  bool isSupportSetCurPos(uint8_t id);
+  bool setCurPos4096(uint8_t id, int32_t pos);
+
  public:
   bool write_reg(uint8_t id, int regadd, int32_t regval);
   bool read_reg(uint8_t id, int regadd, int32_t* regval);

sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

@@ -3,15 +3,6 @@ using namespace iflytop;
 using namespace std;
 #define TAG "MiniServo"
 
-static int32_t limit_input(int32_t input, int32_t min, int32_t max) {
-  int32_t ret = 0;
-  if (input > max) input = max;
-  if (input < min) input = min;
-
-  ret = input;
-  return ret;
-}
-
 static bool poseq(int32_t a, int32_t b, int32_t err) { return abs(a - b) < err; }
 
 void MiniServoCtrlModule::initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus, config_t *cfg) {
@@ -22,6 +13,7 @@ void MiniServoCtrlModule::initialize(uint16_t module_id, FeiTeServoMotor *bus, u
   m_cfg = *cfg;
 
   m_thread.init("MiniRobotCtrlModule", 1024, osPriorityNormal);
+  m_bus->setInMode0(idinbus);  // 设置成模式0，这样能保证回读的位置是正确的。
   mini_servo_enable(1);
 }
 void MiniServoCtrlModule::create_default_config(config_t &cfg) {
@@ -94,6 +86,14 @@ int32_t MiniServoCtrlModule::mini_servo_rotate(int32_t speed) {
 
 int32_t MiniServoCtrlModule::mini_servo_move_to(int32_t pos3600) {
   ZLOGI(TAG, "%d mini_servo_move_to %d", m_module_id, pos3600);
+  // if (pos3600 > 3600 || pos3600 < 0) {
+  //   return err::kparam_out_of_range;
+  // }
+
+  // if (m_cfg.shaft != 0) {
+  //   pos3600 = 3600 - pos3600;
+  // }
+
   m_thread.stop();
   if (!m_bus->ping(m_idinbus)) return err::ksubdevice_overtime;
   if (!m_state.enable) return err::kmini_servo_not_enable;
@@ -185,6 +185,16 @@ int32_t MiniServoCtrlModule::mini_servo_read_io_state(int32_t ioindex, int32_t *
   return 0;
 }
 
+int32_t MiniServoCtrlModule::mini_servo_set_cur_pos(int32_t pos) {
+  ZLOGI(TAG, "%d mini_servo_set_cur_pos %d", m_module_id, pos);
+  if (!m_bus->isSupportSetCurPos(m_idinbus)) {
+    return err::kmini_servo_mode_not_support;
+  }
+  bool suc = m_bus->setCurPos(m_idinbus, pos);
+  if (!suc) return err::ksubdevice_overtime;
+  return 0;
+}
+
 void MiniServoCtrlModule::befor_motor_move() {
   creg.module_status    = 1;
   creg.module_errorcode = 0;

sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp

@@ -7,10 +7,11 @@
 
 namespace iflytop {
 class MiniServoCtrlModule : public ZIModule, public ZIMiniServo {
-  ENABLE_MODULE(StepMotorCtrlModule, kmini_servo_motor_module,PC_VERSION);
+  ENABLE_MODULE(StepMotorCtrlModule, kmini_servo_motor_module, PC_VERSION);
 
  public:
   typedef struct {
+    int32_t shaft;
     int32_t limit_velocity;     // 0->10000 ,位置模式，有效。
     int32_t limit_torque;       // 0->1000  ,位置模式，速度模式，有效。
     int32_t protective_torque;  // 0->1000  ,暂时为使用
@@ -52,6 +53,7 @@ class MiniServoCtrlModule : public ZIModule, public ZIMiniServo {
 
   virtual int32_t mini_servo_set_mid_point() override;
   virtual int32_t mini_servo_read_io_state(int32_t ioindex, int32_t *val) override;
+  virtual int32_t mini_servo_set_cur_pos(int32_t pos) override;
 
  private:
   void befor_motor_move();

sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp

@@ -160,6 +160,7 @@ int32_t XYRobotCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t&
   if (!read) {
     module_active_cfg();
   }
+  return ret;
 }
 
 int32_t XYRobotCtrlModule::__module_xxx_reg(int32_t param_id, bool read, int32_t& val) {
@@ -560,4 +561,4 @@ int32_t XYRobotCtrlModule::xymotor_read_enc_direct(int32_t* enc1, int32_t* enc2)
   *enc1 = m_stepM1->read_enc_val();
   *enc2 = m_stepM2->read_enc_val();
   return 0;
-}
+}

sdk/components/zcancmder/zcan_protocol_parser.cpp

@@ -53,6 +53,7 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) {
   REGFN(mini_servo_move_to);
   REGFN(mini_servo_rotate);
   REGFN(mini_servo_rotate_with_torque);
+  REGFN(mini_servo_set_cur_pos);
 
   REGFN(extboard_read_inio);
   REGFN(extboard_write_outio);
@@ -413,6 +414,10 @@ int32_t ZCanProtocolParser::mini_servo_rotate_with_torque(cmdcontxt_t* cxt) {
   CHECK_AND_GET_MODULE(1);
   return module->mini_servo_rotate_with_torque(cxt->params[0]);
 }
+int32_t ZCanProtocolParser::mini_servo_set_cur_pos(cmdcontxt_t* cxt) {
+  CHECK_AND_GET_MODULE(1);
+  return module->mini_servo_set_cur_pos(cxt->params[0]);
+}
 
 #undef MODULE_CLASS
 

sdk/components/zcancmder/zcan_protocol_parser.hpp

@@ -79,14 +79,13 @@ class ZCanProtocolParser : public IZCanReceiverListener {
   CMDFN(step_motor_get_subdevice_reg);
   CMDFN(step_motor_easy_reciprocating_motion);
 
-  
-
   CMDFN(mini_servo_enable);
   CMDFN(mini_servo_read_pos);
   CMDFN(mini_servo_active_cfg);
   CMDFN(mini_servo_stop);
   CMDFN(mini_servo_set_mid_point);
   CMDFN(mini_servo_read_io_state);
+  CMDFN(mini_servo_set_cur_pos);
 
   CMDFN(mini_servo_move_to);
   CMDFN(mini_servo_rotate);

usrc/subboards/subboard30_shake_module/subboard30_shake_module.cpp

@@ -38,7 +38,7 @@ void Subboard30ShakeModule::initialize() {
     int                                  subid      = 4;
     int                                  subidInBus = 1;
 
-    feiteservomotor_bus.regServo(subidInBus, feite::ksts5420m_c001);
+    feiteservomotor_bus.regServo(subidInBus, feite::ksts5420m_c001, true);
 
     module.create_default_config(cfg);
     module.initialize(getmoduleId(subid), &feiteservomotor_bus, subidInBus, &cfg);
@@ -51,7 +51,7 @@ void Subboard30ShakeModule::initialize() {
     int                                  subid      = 5;
     int                                  subidInBus = 2;
 
-    feiteservomotor_bus.regServo(subidInBus, feite::ksts5420m_c001);
+    feiteservomotor_bus.regServo(subidInBus, feite::ksts5420m_c001, false);
 
     module.create_default_config(cfg);
     module.initialize(getmoduleId(subid), &feiteservomotor_bus, subidInBus, &cfg);
@@ -64,7 +64,7 @@ void Subboard30ShakeModule::initialize() {
     int                                  subid      = 6;
     int                                  subidInBus = 3;
 
-    feiteservomotor_bus.regServo(subidInBus, feite::ksts5420m_c001);
+    feiteservomotor_bus.regServo(subidInBus, feite::ksts5420m_c001, false);
 
     module.create_default_config(cfg);
     module.initialize(getmoduleId(subid), &feiteservomotor_bus, subidInBus, &cfg);
@@ -77,7 +77,7 @@ void Subboard30ShakeModule::initialize() {
     int                                  subid      = 7;
     int                                  subidInBus = 4;
 
-    feiteservomotor_bus.regServo(subidInBus, feite::kscs009);
+    feiteservomotor_bus.regServo(subidInBus, feite::kscs009, false);
 
     module.create_default_config(cfg);
     module.initialize(getmoduleId(subid), &feiteservomotor_bus, subidInBus, &cfg);

usrc/subboards/subboard90_optical_module/pri_board.h

@@ -92,8 +92,8 @@
 #define MOTOR2_CSN PB1
 #define MOTOR2_ENN PD4
 
-#define MOTOR2_REFL        PD2
-#define MOTOR2_REFR        PD8
+#define MOTOR2_REFL        PD8
+#define MOTOR2_REFR        PD2
 #define MOTOR2_REFL_MIRROR true
 #define MOTOR2_REFR_MIRROR true
 
@@ -102,7 +102,7 @@
 #define MOTOR2_V1     800
 #define MOTOR2_AMAX   300
 
-#define MOTOR2_MOTOR_SHAFT                        true
+#define MOTOR2_MOTOR_SHAFT                        false
 #define MOTOR2_MOTOR_ONE_CIRCLE_PULSE             20
 #define MOTOR2_MOTOR_ONE_CIRCLE_PULSE_DENOMINATOR 1
 #define MOTOR2_STEPMOTOR_IHOLD                    3