diff --git a/src/board/hardware.cpp b/src/board/hardware.cpp index 67cf704..bee4d44 100644 --- a/src/board/hardware.cpp +++ b/src/board/hardware.cpp @@ -12,14 +12,6 @@ int fputc(int ch, FILE *stream) { } } -int32_t Hardware::port_tmc4361_get_version(uint8_t channel) { - int value; - uint8_t data[5]; - data[0] = 0x7f; - tmc_motor_spi_write_and_read(channel, &data[0], 5); - value = ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4]; - return value; -} void Hardware::registerListener(HardwareListener *listener) { if (listener && m_listenerNum < MAX_HARDWARE_LISTENER_NUM) { m_listener[m_listenerNum] = listener; @@ -37,35 +29,28 @@ void Hardware::hardwareinit() { *******************************************************************************/ debug_light_init(); -/******************************************************************************* - * CANSLAVEService初始化 * - *******************************************************************************/ -#if ENABLE_CAN + /******************************************************************************* + * CANSLAVEService初始化 * + *******************************************************************************/ IflytopCanSlave::iflytop_can_slave_config_t *config = canSlaveService.createDefaultConfig(DEVICE_ID); canSlaveService.initialize(this, config); canSlaveService.activateRxIT(); canSlaveService.registerListener(this); -#endif /******************************************************************************* * 帕尔贴初始化 * *******************************************************************************/ -#if 0 peltier_init(); -#endif /******************************************************************************* * 温度传感器初始化 * *******************************************************************************/ -#if 0 tmp117[0].initializate(&hi2c1, TMP117::ID0); tmp117[1].initializate(&hi2c1, TMP117::ID1); tmp117[2].initializate(&hi2c1, TMP117::ID2); tmp117[3].initializate(&hi2c1, TMP117::ID3); -#endif /******************************************************************************* * 电机初始化 * *******************************************************************************/ -#if 0 tmc_init(); tmc_extern_clk_enable(); // 4361初始化 @@ -93,20 +78,16 @@ void Hardware::hardwareinit() { } // 期望 4361Version:2 ic2160Version:30 // tmc4361motor1.stop(); -#if 1 // 测试编码器方向 - tmc4361motor1.rotate(-300000); - while (true) { - HAL_Delay(100); - ZLOGI(TAG, "TMC4361A: XACTUAL:%d ENC_POS:%d DIFF:%d", tmc4361motor1.getXACTUAL(), tmc4361motor1.getENC_POS(), tmc4361motor1.getENC_POS_DEV()); - } -#endif + // tmc4361motor1.rotate(-300000); + // while (true) { + // HAL_Delay(100); + // ZLOGI(TAG, "TMC4361A: XACTUAL:%d ENC_POS:%d DIFF:%d", tmc4361motor1.getXACTUAL(), tmc4361motor1.getENC_POS(), tmc4361motor1.getENC_POS_DEV()); + // } -#endif -/******************************************************************************* - * 风扇初始化 * - *******************************************************************************/ -#if 0 + /******************************************************************************* + * 风扇初始化 * + *******************************************************************************/ fanInit(1000); /** * @brief 风扇反馈初始化 @@ -118,13 +99,20 @@ void Hardware::hardwareinit() { * FAN4_FB_INT PC7 * FAN5_FB_INT PC10 */ - m_fanStateMonitor[0].initialize(m_os, GPIOC, GPIO_PIN_1, fanGetPowerState(0)); - m_fanStateMonitor[1].initialize(m_os, GPIOC, GPIO_PIN_4, fanGetPowerState(1)); - m_fanStateMonitor[2].initialize(m_os, GPIOC, GPIO_PIN_5, fanGetPowerState(2)); - m_fanStateMonitor[3].initialize(m_os, GPIOC, GPIO_PIN_6, fanGetPowerState(3)); - m_fanStateMonitor[4].initialize(m_os, GPIOC, GPIO_PIN_7, fanGetPowerState(4)); - m_fanStateMonitor[5].initialize(m_os, GPIOC, GPIO_PIN_10, fanGetPowerState(5)); -#endif + m_fanStateMonitor[0].initialize(this, GPIOC, GPIO_PIN_1, fanGetPowerState(0)); + m_fanStateMonitor[1].initialize(this, GPIOC, GPIO_PIN_4, fanGetPowerState(1)); + m_fanStateMonitor[2].initialize(this, GPIOC, GPIO_PIN_5, fanGetPowerState(2)); + m_fanStateMonitor[3].initialize(this, GPIOC, GPIO_PIN_6, fanGetPowerState(3)); + m_fanStateMonitor[4].initialize(this, GPIOC, GPIO_PIN_7, fanGetPowerState(4)); + m_fanStateMonitor[5].initialize(this, GPIOC, GPIO_PIN_10, fanGetPowerState(5)); +} +int32_t Hardware::port_tmc4361_get_version(uint8_t channel) { + int value; + uint8_t data[5]; + data[0] = 0x7f; + tmc_motor_spi_write_and_read(channel, &data[0], 5); + value = ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4]; + return value; } void Hardware::periodicJob() { #if 0 @@ -137,7 +125,7 @@ void Hardware::periodicJob() { #endif do_debug_light_state(); #if ENABLE_CAN - testCanSlaveTxAndRx(); + // testCanSlaveTxAndRx(); #endif } void Hardware::do_debug_light_state() { @@ -147,65 +135,6 @@ void Hardware::do_debug_light_state() { HAL_GPIO_TogglePin(DEBUG_LIGHT_PORT, DEBUG_LIGHT_PIN); } } -/******************************************************************************* - * 硬件测试 * - *******************************************************************************/ -void Hardware::testTmp117() { - static uint32_t lastcall; - if (hasPassedMS(lastcall) > 1000) { - lastcall = getTicket(); - for (size_t i = 0; i < 4; i++) { - float temp = tmp117[i].getTemperature(); - if (tmp117[i].getLastCallStatus() == HAL_OK) { - ZLOGI(TAG, "tmp117_%d:%f", i, temp); - } else { - ZLOGI(TAG, "tmp117_%d:read fail", i); - } - } - } -} - -/** - * @brief 测试CAN的发送和接收 - */ -void Hardware::testCanSlaveTxAndRx() { - { - static uint32_t lastcall; - static uint8_t tx[8] = {1, 2, 3, 4, 5, 6, 7, 8}; - if (hasPassedMS(lastcall) > 1000) { - lastcall = getTicket(); - // canSlaveService.translate(0x01, tx, 8, 2); - canSlaveService.sendReport(1, 2, 3, 100); - if (canSlaveService.getLastTransmitStatus() == HAL_OK) { - ZLOGI(TAG, "send ok"); - } else { - ZLOGI(TAG, "send fail"); - } - } - } - - /** - * @brief 接收CAN消息 - */ - if (m_canOnRxDataFlag) { - m_canOnRxDataFlag = false; - static CAN_RxHeaderTypeDef packetHeader; - static uint8_t packetData[8]; - // while (canSlaveService.getRxMessage(&packetHeader, packetData)) { - // ZLOGI(TAG, "rx packet:"); - // ZLOGI(TAG, "\tid:%x len:%d", packetHeader.StdId, packetHeader.DLC); - // ZLOGI_HEX(TAG, packetData, packetHeader.DLC); - // } - - static IflytopCanSlave::packet_t packet; - while (canSlaveService.getRxPacket(&packet)) { - ZLOGI(TAG, "rx packet:"); - ZLOGI(TAG, "\ttype:%01x, targetId:%01x, sourceId:%02x, seq:%02x, regAdd:%02x, regValue:%04x", // - packet.type, packet.targetId, packet.sourceId, packet.seq, packet.regAdd, packet.regValue); - } - canSlaveService.activateRxIT(); - } -} void Hardware::TMC4361APort_setResetPinState(uint16_t channel, bool state) { tmc_nRESET_pin_set_state(channel, state); } void Hardware::TMC4361APort_setFREEZEPinState(uint16_t channel, bool state) { tmc_nFREEZE_pin_set_state(channel, state); } void Hardware::TMC4361APort_setENNPinState(uint16_t channel, bool state) { tmc_ENN_pin_set_state(channel, state); } @@ -213,7 +142,6 @@ bool Hardware::TMC4361APort_getTargetReachedPinState(uint16_t channel) { return void Hardware::TMC4361APort_sleepus(int32_t us) { sleepus(us); } void Hardware::TMC4361APort_readWriteArray(uint8_t *data, size_t length) { tmc_motor_spi_write_and_read(0, data, length); } void Hardware::TMC4361APort_setSubICENNPinState(uint16_t channel, bool state) { tmc_subic_ENN_pin_set_state(channel, state); } - void Hardware::STM32_HAL_onGPIO_EXTI_Callback(uint16_t gpioNum) { /** * @brief 赋予盘零位光电触发 @@ -391,7 +319,66 @@ void Hardware::tmc_subic_ENN_pin_set_state(uint8_t channel, bool state) { } } void Hardware::tmc_nRESET_pin_set_state(uint8_t channel, bool state) {} - void Hardware::home_ref_switch_init() { // STM32_HAL::gpioInitAsEXIT(TMC_HOME_REF_GPIO_PORT, TMC_HOME_REF_GPIO_PIN, GPIO_NOPULL, TMC_HOME_REF_GPIO_IRQ_MODE); } +bool Hardware::home_ref_switch_get_state() { return (HAL_GPIO_ReadPin(TMC_HOME_REF_GPIO_PORT, TMC_HOME_REF_GPIO_PIN) == TMC_HOME_REF_GPIO_USEFUL_STATE); } +/******************************************************************************* + * 硬件测试 * + *******************************************************************************/ +void Hardware::testTmp117() { + static uint32_t lastcall; + if (hasPassedMS(lastcall) > 1000) { + lastcall = getTicket(); + for (size_t i = 0; i < 4; i++) { + float temp = tmp117[i].getTemperature(); + if (tmp117[i].getLastCallStatus() == HAL_OK) { + ZLOGI(TAG, "tmp117_%d:%f", i, temp); + } else { + ZLOGI(TAG, "tmp117_%d:read fail", i); + } + } + } +} + +/** + * @brief 测试CAN的发送和接收 + */ +void Hardware::testCanSlaveTxAndRx() { + { + static uint32_t lastcall; + static uint8_t tx[8] = {1, 2, 3, 4, 5, 6, 7, 8}; + if (hasPassedMS(lastcall) > 1000) { + lastcall = getTicket(); + // canSlaveService.translate(0x01, tx, 8, 2); + canSlaveService.sendReport(1, 2, 3, 100); + if (canSlaveService.getLastTransmitStatus() == HAL_OK) { + ZLOGI(TAG, "send ok"); + } else { + ZLOGI(TAG, "send fail"); + } + } + } + + /** + * @brief 接收CAN消息 + */ + if (m_canOnRxDataFlag) { + m_canOnRxDataFlag = false; + static CAN_RxHeaderTypeDef packetHeader; + static uint8_t packetData[8]; + // while (canSlaveService.getRxMessage(&packetHeader, packetData)) { + // ZLOGI(TAG, "rx packet:"); + // ZLOGI(TAG, "\tid:%x len:%d", packetHeader.StdId, packetHeader.DLC); + // ZLOGI_HEX(TAG, packetData, packetHeader.DLC); + // } + + static IflytopCanSlave::packet_t packet; + while (canSlaveService.getRxPacket(&packet)) { + ZLOGI(TAG, "rx packet:"); + ZLOGI(TAG, "\ttype:%01x, targetId:%01x, sourceId:%02x, seq:%02x, regAdd:%02x, regValue:%04x", // + packet.type, packet.targetId, packet.sourceId, packet.seq, packet.regAdd, packet.regValue); + } + canSlaveService.activateRxIT(); + } +} diff --git a/src/board/hardware.hpp b/src/board/hardware.hpp index f47f0fc..a97cee4 100644 --- a/src/board/hardware.hpp +++ b/src/board/hardware.hpp @@ -28,19 +28,23 @@ class Hardware : public IflytopCanSlaveListener, // private: /* data */ - private: - TMC4361A tmc4361motor1; + public: + /*motor*/ + TMC4361A tmc4361motor1; + /*temperature*/ + TMP117 tmp117[4]; + /*fan*/ FanStateMonitor m_fanStateMonitor[6]; + /*can*/ IflytopCanSlave canSlaveService; bool m_canOnRxDataFlag; /* data */ - bool m_fanState[6]; + bool m_fanState[6]; + HardwareListener *m_listener[MAX_HARDWARE_LISTENER_NUM]; int m_listenerNum; public: - TMP117 tmp117[4]; - public: /******************************************************************************* * ListenerImpl * @@ -62,6 +66,32 @@ class Hardware : public IflytopCanSlaveListener, // int32_t port_tmc4361_get_version(uint8_t channel); TMC4361A *getMotor1() { return &tmc4361motor1; } + void debug_light_init(); + + void fanInit(int freq); + void fanSetDutyCycle(int fanIndex, uint16_t dutyCycle); + void fanSetState0to3(uint16_t dutyCycle); + void fanSetState4(uint16_t dutyCycle); + void fanSetState5(uint16_t dutyCycle); + bool *fanGetPowerState(int fanIndex); + + void peltier_cold_ctr_pwm(int pwm); + void peltier_hot_ctr_pwm(int pwm); + void peltier_init(); + void peltier_set_pwm(int pwm); + + void tmc_init(); + void tmc_motor_spi_select(int channel, bool state); + void tmc_motor_spi_write_and_read(int channel, uint8_t *data, size_t length); + void tmc_extern_clk_enable(); + void tmc_nFREEZE_pin_set_state(uint8_t channel, bool state); + void tmc_ENN_pin_set_state(uint8_t channel, bool state); + void tmc_subic_ENN_pin_set_state(uint8_t channel, bool state); + void tmc_nRESET_pin_set_state(uint8_t channel, bool state); + + void home_ref_switch_init(); + bool home_ref_switch_get_state(); + /******************************************************************************* * 测试 * *******************************************************************************/ @@ -96,27 +126,6 @@ class Hardware : public IflytopCanSlaveListener, // virtual void STM32_HAL_onGPIO_EXTI_Callback(uint16_t GPIO_Pin); public: - void debug_light_init(); - void fanInit(int freq); - void fanSetDutyCycle(int fanIndex, uint16_t dutyCycle); - void fanSetState0to3(uint16_t dutyCycle); - void fanSetState4(uint16_t dutyCycle); - void fanSetState5(uint16_t dutyCycle); - void peltier_cold_ctr_pwm(int pwm); - void peltier_hot_ctr_pwm(int pwm); - void peltier_init(); - void peltier_set_pwm(int pwm); - void tmc_init(); - void tmc_motor_spi_select(int channel, bool state); - void tmc_motor_spi_write_and_read(int channel, uint8_t *data, size_t length); - void tmc_extern_clk_enable(); - void tmc_nFREEZE_pin_set_state(uint8_t channel, bool state); - void tmc_ENN_pin_set_state(uint8_t channel, bool state); - void tmc_subic_ENN_pin_set_state(uint8_t channel, bool state); - void tmc_nRESET_pin_set_state(uint8_t channel, bool state); - bool *fanGetPowerState(int fanIndex); - - void home_ref_switch_init(); }; } // namespace iflytop diff --git a/src/board/project_board.hpp b/src/board/project_board.hpp index 7fc5e61..88a103e 100644 --- a/src/board/project_board.hpp +++ b/src/board/project_board.hpp @@ -20,9 +20,10 @@ // 电机通道编号 #define MOTOR_1_TMC4361A_CHANNEL 1 -#define TMC_HOME_REF_GPIO_PIN GPIO_PIN_3 -#define TMC_HOME_REF_GPIO_PORT GPIOD -#define TMC_HOME_REF_GPIO_IRQ_MODE GPIO_MODE_IT_RISING +#define TMC_HOME_REF_GPIO_PIN GPIO_PIN_3 +#define TMC_HOME_REF_GPIO_PORT GPIOD +#define TMC_HOME_REF_GPIO_IRQ_MODE GPIO_MODE_IT_RISING +#define TMC_HOME_REF_GPIO_USEFUL_STATE GPIO_PIN_SET typedef enum { kok = 0, diff --git a/src/lncubator_rotating_control_service.cpp b/src/lncubator_rotating_control_service.cpp index 3d8d77c..79cc7e0 100644 --- a/src/lncubator_rotating_control_service.cpp +++ b/src/lncubator_rotating_control_service.cpp @@ -5,19 +5,21 @@ using namespace iflytop; #define UPDATE_ZERO_POSITION_ALWAYS 1 -const uint32_t LncubatorRotatingControlService::mc_onecircle_ustep_num = (256 * 200 * 50); +const uint32_t LncubatorRotatingControlService::mc_onecircle_ustep_num = (256 * 200 * 150); const uint32_t LncubatorRotatingControlService::mc_default_velocity = 500000; // 500kpps const uint32_t LncubatorRotatingControlService::mc_one_sub_position_width = (256 * 200 * 50) / 20; LncubatorRotatingControlService::LncubatorRotatingControlService() { - m_hardware = NULL; - m_motor = NULL; - m_os = NULL; - m_workfinished = false; - m_dowhat = kidle; - m_maxVelocity = mc_default_velocity; - m_inletPosition = 0; - m_outletPosition = m_inletPosition + mc_one_sub_position_width / 2; + m_hardware = NULL; + m_motor = NULL; + m_os = NULL; + m_workfinished = false; + m_dowhat = kidle; + m_maxVelocity = mc_default_velocity; + m_inletPosition = 0; + m_outletPosition = m_inletPosition + mc_one_sub_position_width / 2; + m_jobStep = 0; + m_hasCalibratedHomePosition = false; } LncubatorRotatingControlService::~LncubatorRotatingControlService() {} void LncubatorRotatingControlService::initialize(Hardware* hardware) { // @@ -58,24 +60,25 @@ int32_t LncubatorRotatingControlService::tmc_getPosition() { int32_t position = m_motor->getENC_POS(); return normalizePosition(position); } -void LncubatorRotatingControlService::tmc_roateToPosition(int32_t position) { +void LncubatorRotatingControlService::tmc_roateToPosition(int32_t position, uint32_t velocity) { int32_t _position = normalizePosition(position); - m_motor->moveTo(position, m_maxVelocity); + m_motor->moveTo(position, velocity); } int32_t LncubatorRotatingControlService::normalizePosition(int32_t position) { while (true) { - if (position > mc_onecircle_ustep_num / 2) + if (position > mc_onecircle_ustep_num) position = position - mc_onecircle_ustep_num; - else if (position < mc_onecircle_ustep_num / 2) + else if (position < 0) position = position + mc_onecircle_ustep_num; else break; } - if (position == mc_onecircle_ustep_num / 2) position = position - 1; - if (position == -mc_onecircle_ustep_num / 2) position = position + 1; return position; } -void LncubatorRotatingControlService::tmc_moveBy(int32_t relativePosition) { m_motor->moveBy(relativePosition, m_maxVelocity); } +void LncubatorRotatingControlService::tmc_moveBy(int32_t relativePosition, uint32_t velocity) { m_motor->moveBy(relativePosition, velocity); } + +bool LncubatorRotatingControlService::hardware_isHomeRefSwitchTriggered() { return m_hardware->home_ref_switch_get_state(); } + /******************************************************************************* * JobBasic * *******************************************************************************/ @@ -121,22 +124,50 @@ void LncubatorRotatingControlService::changeState(dowhat_t tostate) { void LncubatorRotatingControlService::moveToHome() { if (m_dowhat != kidle) return; changeState(kmoveToHomeJob); - moveBy(mc_onecircle_ustep_num * 2); + if (hardware_isHomeRefSwitchTriggered()) { + // 设备当前处于零点，脱离零点位置 + doRotateToHomeJobStep1(); + } else { + // 逆时针找零点 + doRotateToHomeJobStep2(); + } +} +void LncubatorRotatingControlService::doRotateToHomeJobStep1() { + /** + * @brief 设备当前处于零点，脱离零点位置 + */ + ZLOGI(TAG, "doRotateToHomeJobStep1, rotate to +10 degree,leave from home"); + m_jobStep = 1; + tmc_moveBy(mc_onecircle_ustep_num / 36, m_maxVelocity); // 此时处于零点位置，先顺时针转10度 +} +void LncubatorRotatingControlService::doRotateToHomeJobStep2() { + /** + * @brief 逆时针找零点 + */ + ZLOGI(TAG, "doRotateToHomeJobStep2, rotate to home"); + m_jobStep = 2; + tmc_moveBy(-mc_onecircle_ustep_num, m_maxVelocity); } void LncubatorRotatingControlService::doRotateToHomeJob(interevent_t event) { if (event == kPeriodicJobEvent) { } else if (event == kHomeSwitchTriggeredEvent) { - m_motor->stop(); - m_motor->setZeroPoint(); - changeState(kidle); - ZLOGI(TAG, "find Zero Point"); - if (m_listener) m_listener->LncubatorRotatingControlService_onMoveToHomeJobFinished(); + if (m_jobStep == 2) { + m_motor->stop(); + m_motor->setZeroPoint(); + changeState(kidle); + ZLOGI(TAG, "find Zero Point"); + m_hasCalibratedHomePosition = true; + if (m_listener) m_listener->LncubatorRotatingControlService_onMoveToHomeJobFinished(); + } } else if (event == kTMC4361AReachTargetEvent) { - /** - * @brief 电机转了两圈依然没有找到零点 - */ - if (m_listener) m_listener->LncubatorRotatingControlService_onException(kGoHomeFail); - changeState(kidle); + if (m_jobStep == 1) { + // 已经脱离零点位置，逆时针找零点 + doRotateToHomeJobStep2(); + } else if (m_jobStep == 2) { + // 电机转了一整圈依然没有找到零点，报错 + if (m_listener) m_listener->LncubatorRotatingControlService_onException(kGoHomeFail); + changeState(kidle); + } } } @@ -149,10 +180,10 @@ void LncubatorRotatingControlService::moveToSubPosition(refpoint_t refpoint, int // 出口 if (refpoint == REFPOINT_TYPE_OUTLET) { int32_t targetPosition = m_outletPosition + positionIndex * mc_one_sub_position_width; - tmc_roateToPosition(targetPosition); + tmc_roateToPosition(targetPosition, m_maxVelocity); } else if (refpoint == REFPOINT_TYPE_INLET) { int32_t targetPosition = m_inletPosition + positionIndex * mc_one_sub_position_width; - tmc_roateToPosition(targetPosition); + tmc_roateToPosition(targetPosition, m_maxVelocity); } else { ZLOGE(TAG, "refpoint error"); } @@ -160,9 +191,6 @@ void LncubatorRotatingControlService::moveToSubPosition(refpoint_t refpoint, int void LncubatorRotatingControlService::doMoveToSubPositionJob(interevent_t event) { if (event == kPeriodicJobEvent) { } else if (event == kHomeSwitchTriggeredEvent) { -#if UPDATE_ZERO_POSITION_ALWAYS - m_motor->setZeroPoint(); -#endif } else if (event == kTMC4361AReachTargetEvent) { /** * @brief moveToTarget @@ -178,15 +206,19 @@ void LncubatorRotatingControlService::doMoveToSubPositionJob(interevent_t event) void LncubatorRotatingControlService::moveBy(int32_t pluseCount) { if (m_dowhat == kidle || m_dowhat == kmoveByJob) { changeState(kmoveByJob); - tmc_moveBy(pluseCount); + int32_t position = tmc_getPosition(); + position += pluseCount; + if (position <= 0) { + position = 0; + } else if (position > mc_onecircle_ustep_num) { + position = mc_onecircle_ustep_num; + } + tmc_roateToPosition(position, m_maxVelocity); } } void LncubatorRotatingControlService::doMoveByJob(interevent_t event) { if (event == kPeriodicJobEvent) { } else if (event == kHomeSwitchTriggeredEvent) { -#if UPDATE_ZERO_POSITION_ALWAYS - m_motor->setZeroPoint(); -#endif } else if (event == kTMC4361AReachTargetEvent) { /** * @brief moveToTarget diff --git a/src/lncubator_rotating_control_service.hpp b/src/lncubator_rotating_control_service.hpp index c6884de..01e36db 100644 --- a/src/lncubator_rotating_control_service.hpp +++ b/src/lncubator_rotating_control_service.hpp @@ -9,20 +9,15 @@ namespace iflytop { * @brief * 孵育盘 * - * 1. 孵育船位X对其孵育盘出口 - * 2. 孵育船位X对其孵育盘入口 - * 3. 找零点 - * 4. 电机顺时针转x度 - * 5. 电机逆时针转x度 - * 6. 校准位置([0]参考标识类型:出口，入口，零点，[1]第几个孵育船); - * 7. 读取校准值 - * 8. 写入校准值 * * 约定: - * 一圈位置的位置为 (-256*200/2)<0<(-256*200/2) - * - * + * 一圈位置的位置为 0 <= position < (256*200*150) (150是减速比) * + * 运动逻辑: + * 1. 找零点 + * 2. 转到第几个子仓位 + * 3. 正转或反转一定的角度 + * 4. 校准当前位置 */ #define LNCUBATOR_MAX_POSITION_COUNT 20 @@ -78,6 +73,9 @@ class LncubatorRotatingControlService : public HardwareListener, // int32_t m_maxVelocity = 0; bool m_workfinished; bool m_homeKeyTriggered; + int m_jobStep; + + bool m_hasCalibratedHomePosition; LncubatorRotatingControlServiceListener* m_listener; @@ -119,14 +117,19 @@ class LncubatorRotatingControlService : public HardwareListener, // private: void triggerEvent(interevent_t event); void doRotateToHomeJob(interevent_t event); + void doRotateToHomeJobStep1(); + void doRotateToHomeJobStep2(); + void doMoveToSubPositionJob(interevent_t event); void doMoveByJob(interevent_t event); void changeState(dowhat_t state); private: int32_t tmc_getPosition(); - void tmc_roateToPosition(int32_t position); - void tmc_moveBy(int32_t position); + void tmc_roateToPosition(int32_t position, uint32_t velocity); + void tmc_moveBy(int32_t relativePosition, uint32_t velocity); int32_t normalizePosition(int32_t position); + + bool hardware_isHomeRefSwitchTriggered(); }; } // namespace iflytop