tianjialong
/
p_boditech_vidas_a8000_Incubator_control_system_copy
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

master
zhaohe 2 years ago
parent
51f61f7608
commit
2f05f230fe
5 changed files with 209 additions and 177 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 185
      src/board/hardware.cpp
  2. 61
      src/board/hardware.hpp
  3. 7
      src/board/project_board.hpp
  4. 104
      src/lncubator_rotating_control_service.cpp
  5. 29
      src/lncubator_rotating_control_service.hpp

185
src/board/hardware.cpp
View File

@ -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();
}
}

61
src/board/hardware.hpp
View File

@ -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

7
src/board/project_board.hpp
View File

@ -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,

104
src/lncubator_rotating_control_service.cpp
View File

@ -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

29
src/lncubator_rotating_control_service.hpp
View File

@ -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
Write Preview
Loading…
Cancel
Save