diff --git a/components/eq_20_asb_motor/eq20_servomotor.cpp b/components/eq_20_asb_motor/eq20_servomotor.cpp index daf81a4..2fe1a4d 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.cpp +++ b/components/eq_20_asb_motor/eq20_servomotor.cpp @@ -9,7 +9,7 @@ using namespace iflytop; #define TAG "Eq20ServoMotor" -void Eq20ServoMotor::init(ModbusBlockHost *modbusBlockHost, int moduleid, int motorid = 1) { +void Eq20ServoMotor::init(ModbusBlockHost *modbusBlockHost, int moduleid, int motorid) { // this->com = com; m_modbusBlockHost = modbusBlockHost; m_deviceId = motorid; @@ -230,18 +230,28 @@ int32_t Eq20ServoMotor::module_get_last_exec_status(int32_t *status) { *status = 0; return 0; } -int32_t Eq20ServoMotor::module_get_status(int32_t *status) {} +int32_t Eq20ServoMotor::module_get_status(int32_t *status) { + *status = 0; + servo_internal_status_t internal_state; + int32_t ecode = get_servo_internal_state(internal_state); + if (ecode != 0) { + return ecode; + } + if (!internal_state.data.sbit.is_reach_target) { + *status = 1; + } + return 0; +} int32_t Eq20ServoMotor::module_get_error(int32_t *iserror) { *iserror = 0; - - // get_servo_internal_state(); servo_internal_status_t internal_state; int32_t ecode = get_servo_internal_state(internal_state); if (ecode != 0) { return ecode; } - *status = internal_state.data.sbit.servo_warning | internal_state.data.sbit.servo_warning; - return 0; + if (internal_state.data.sbit.servo_warning | internal_state.data.sbit.servo_warning) { + *iserror = err::kmodule_error; + } return 0; } int32_t Eq20ServoMotor::module_clear_error() { return err::koperation_not_support; } @@ -254,7 +264,15 @@ int32_t Eq20ServoMotor::module_writeio(int32_t io) { return err::koperation_not_ int32_t Eq20ServoMotor::module_read_adc(int32_t adcindex, int32_t *adc) { return err::koperation_not_support; } int32_t Eq20ServoMotor::motor_enable(int32_t varenable) { return enable(varenable); } -int32_t Eq20ServoMotor::motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) { return rotate(direction * motor_velocity, acc); } + +int32_t Eq20ServoMotor::motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) { return err::koperation_not_support; } int32_t Eq20ServoMotor::motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) { return err::koperation_not_support; } -int32_t Eq20ServoMotor::motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) {} -int32_t Eq20ServoMotor::motor_move_to_with_torque(int32_t pos, int32_t torque) {} \ No newline at end of file +int32_t Eq20ServoMotor::motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) { return err::koperation_not_support; } +int32_t Eq20ServoMotor::motor_move_to_with_torque(int32_t pos, int32_t torque) { return err::koperation_not_support; } + +int32_t Eq20ServoMotor::motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) { return rotate(direction * motor_velocity, acctime); }; +int32_t Eq20ServoMotor::motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) { return move_by(distance, motor_velocity, acctime); }; +int32_t Eq20ServoMotor::motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) { return move_to(position, motor_velocity, acctime); } + +int32_t Eq20ServoMotor::motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return move_to_zero_forward(findzerospeed, findzeroedge_speed, 30); } +int32_t Eq20ServoMotor::motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return move_to_zero_backward(findzerospeed, findzeroedge_speed, 30); } diff --git a/components/eq_20_asb_motor/eq20_servomotor.hpp b/components/eq_20_asb_motor/eq20_servomotor.hpp index d088892..558442f 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.hpp +++ b/components/eq_20_asb_motor/eq20_servomotor.hpp @@ -120,6 +120,13 @@ class Eq20ServoMotor : public ZIModule, public ZIMotor { virtual int32_t motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) override; virtual int32_t motor_move_to_with_torque(int32_t pos, int32_t torque) override; + virtual int32_t motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) override; + virtual int32_t motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) override; + virtual int32_t motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) override; + + virtual int32_t motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; + virtual int32_t motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; + public: int32_t write_reg(uint32_t regadd, int32_t value); int32_t read_reg(uint32_t regadd, int32_t &value); diff --git a/components/hardware/uart/zuart_helper.cpp b/components/hardware/uart/zuart_helper.cpp index 4a93c5d..4712a9a 100644 --- a/components/hardware/uart/zuart_helper.cpp +++ b/components/hardware/uart/zuart_helper.cpp @@ -3,8 +3,8 @@ #include "sdk\components\zprotocols\errorcode\errorcode.hpp" using namespace iflytop; -#define DEBUG 0 -#if DEBUG +#define __DEBUG 0 +#if __DEBUG static void dumphex(char* tag, uint8_t* data, uint8_t len) { printf("%s:", tag); for (int i = 0; i < len; i++) { @@ -30,7 +30,7 @@ void ZUARTHelper::cleanRxBuff() { } int32_t ZUARTHelper::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, uint16_t overtimems) { uint32_t enter_ticket = HAL_GetTick(); -#if DEBUG +#if __DEBUG dumphex("tx:", tx, txdatalen); #endif @@ -43,7 +43,7 @@ int32_t ZUARTHelper::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint osDelay(1); int rxsize = expectrxsize - __HAL_DMA_GET_COUNTER(m_hdma_rx); if (rxsize == expectrxsize) { -#if DEBUG +#if __DEBUG dumphex("rx:", rx, expectrxsize); #endif break;