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

完成逻辑，待测试

master
zhaohe 2 years ago
parent
3f1893a7de
commit
da2d58d39b
7 changed files with 267 additions and 82 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. 26
      app/MDK-ARM/app.uvguix.h_zha
  2. 2
      dep/libiflytop_micro
  3. 7
      src/board/hardware.cpp
  4. 60
      src/board/project_board.hpp
  5. 102
      src/lncubator_rotating_control_service.cpp
  6. 31
      src/lncubator_rotating_control_service.hpp
  7. 121
      src/umain.cpp

26
app/MDK-ARM/app.uvguix.h_zha
File diff suppressed because it is too large
View File

2
dep/libiflytop_micro

@ -1 +1 @@
Subproject commit 0e0fbea74859e71d34d2ccf6169b0da763fb95a4
Subproject commit cffbfd7402411697415c57cc1dae7ce82d197827

7
src/board/hardware.cpp
View File

@ -335,8 +335,8 @@ void Hardware::tmc_init() {
#endif
m_tmc.tmc4361motor1.initialize(MOTOR_1_TMC4361A_CHANNEL, TMC4361A::IC_TMC2160, tmc4361aconfig);
m_tmc.tmc4361motor1.setMaximumAcceleration(300000);
m_tmc.tmc4361motor1.setMaximumDeceleration(300000);
m_tmc.tmc4361motor1.setMaximumAcceleration(REG_MOTOR_CTRL_ACCELERATION);
m_tmc.tmc4361motor1.setMaximumDeceleration(REG_MOTOR_CTRL_DECELERATION);
HAL_Delay(100);
// 使能电机
@ -348,9 +348,8 @@ void Hardware::tmc_init() {
if (ic4361Version != 2) {
ZLOGE(TAG, "TMC4361 or TMC2160 is not normal");
}
m_tmc.tmc4361motor1.rotate(300000);
#if TEST_TMC_ENCODER_DIRECTION
m_tmc.tmc4361motor1.rotate(300000);
while (true) {
HAL_Delay(100);
ZLOGI(TAG, "TMC4361A: XACTUAL:%d ENC_POS:%d DIFF:%d", //

60
src/board/project_board.hpp
View File

@ -59,23 +59,23 @@ typedef enum {
0x1029 温度传感器4-数值
#endif
#define REG_REBOOT_FLAG 0x0000 // 重启标识
#define REG_EXCEPTION_FLAG 0x0100 // 异常标志位
#define REG_TEMPERATURE_CTRL_KP 0x1000 // 温度控制PID:P (单位0.01)
#define REG_TEMPERATURE_CTRL_KI 0x1001 // 温度控制PID:I (单位0.01)
#define REG_TEMPERATURE_CTRL_KD 0x1002 // 温度控制PID:D (单位0.01)
#define REG_TEMPERATURE_CTRL_ERROR_LIMIT 0x1003 // 积分数值限制
#define REG_TEMPERATURE_CTRL_MAX_OUTPUT 0x1004 // PID输出上限
#define REG_TEMPERATURE_CTRL_MIN_OUTPUT 0x1005 // PID输出下限
#define REG_TEMPERATURE_CTRL_MAIN_FAN_SPEED_AUTO 0x1006 // 自动控温时主风扇风速
#define REG_TEMPERATURE_CTRL_SUB_FAN_SPEED_AUTO 0x1007 // 自动控温时次风扇风速
#define REG_REBOOT_FLAG 0x0000 // 重启标识
#define REG_EXCEPTION_FLAG 0x0100 // 异常标志位
#define REG_TEMPERATURE_CTRL_KP 0x1000 // 温度控制PID:P (单位0.01)
#define REG_TEMPERATURE_CTRL_KI 0x1001 // 温度控制PID:I (单位0.01)
#define REG_TEMPERATURE_CTRL_KD 0x1002 // 温度控制PID:D (单位0.01)
#define REG_TEMPERATURE_CTRL_ERROR_LIMIT 0x1003 // 积分数值限制
#define REG_TEMPERATURE_CTRL_MAX_OUTPUT 0x1004 // PID输出上限
#define REG_TEMPERATURE_CTRL_MIN_OUTPUT 0x1005 // PID输出下限
#define REG_TEMPERATURE_CTRL_MAIN_FAN_SPEED_AUTO 0x1006 // 自动控温时主风扇风速
#define REG_TEMPERATURE_CTRL_SUB_FAN_SPEED_AUTO 0x1007 // 自动控温时次风扇风速
#define REG_TEMPERATURE_CTRL_TARGET_TEMPERATURE 0x1011 // 控温区域-目标温度
#define REG_TEMPERATURE_CTRL_MODE 0x1012 // 控温区域-控制模式
#define REG_TEMPERATURE_CTRL_MANUAL_HEAT_PWM 0x1013 // 手动控制加热片PWM数值
#define REG_TEMPERATURE_CTRL_MANUAL_MAIN_FAN_PWM 0x1014 // 手动控制主风扇PWM数值
#define REG_TEMPERATURE_CTRL_MANUAL_SUB_FAN_PWM 0x1015 // 手动控制次风扇PWM数值
#define REG_TEMPERATURE_CTRL_DUMPVALUE 0x1016 // 是否打印PID调试信息
#define REG_TEMPERATURE_CTRL_TARGET_TEMPERATURE 0x1011 // 控温区域-目标温度
#define REG_TEMPERATURE_CTRL_MODE 0x1012 // 控温区域-控制模式
#define REG_TEMPERATURE_CTRL_MANUAL_HEAT_PWM 0x1013 // 手动控制加热片PWM数值
#define REG_TEMPERATURE_CTRL_MANUAL_MAIN_FAN_PWM 0x1014 // 手动控制主风扇PWM数值
#define REG_TEMPERATURE_CTRL_MANUAL_SUB_FAN_PWM 0x1015 // 手动控制次风扇PWM数值
#define REG_TEMPERATURE_CTRL_DUMPVALUE 0x1016 // 是否打印PID调试信息
#define REG_TEMPERATURE_CTRL_MAIN_FAN_SPEED 0x1021 // 控温区域-散热区大风扇转速
#define REG_TEMPERATURE_CTRL_SUB_FAN_SPEED 0x1022 // 控温区域-散热区小风扇转速
@ -87,6 +87,24 @@ typedef enum {
#define REG_TEMPERATURE_CTRL_SENSOR3_TEMPERATURE 0x1028 // 温度传感器3-数值
#define REG_TEMPERATURE_CTRL_SENSOR4_TEMPERATURE 0x1029 // 温度传感器4-数值
#define REG_MOTOR_CTRL_TARGET_POSITION1 0x2001 // 设置相对于入口目标位置（转到第几个子仓位，从0开始）
#define REG_MOTOR_CTRL_TARGET_POSITION2 0x2002 // 设置相对于出口目标位置（转到第几个子仓位，从0开始）
#define REG_MOTOR_CTRL_BACK_TO_HOME 0x2003 // BackToHome，写1触发动作
#define REG_MOTOR_CTRL_ROATE_PULSE 0x2004 // 转多少个脉冲(正数为顺时针转，负数为逆时针转)
#define REG_MOTOR_CTRL_STOP 0x2005 // 停止电机运动
#define REG_MOTOR_CTRL_CALIBRATE_POSITION 0x2006 // 设置当前位置为0号子仓位对其入口
#define REG_MOTOR_CTRL_MAX_SPEED 0x2010 // 最高转速
#define REG_MOTOR_CTRL_ENTRY_POSITION 0x2011 // 入口相对于零点的位置（出口相对于零点位置，出口位置会自己计算出来）
#define REG_MOTOR_CTRL_EXIT_POSITION 0x2012 // 出口相对于零点的位置
#define REG_MOTOR_CTRL_ACCELERATION 0x2013 // 加速度
#define REG_MOTOR_CTRL_DECELERATION 0x2014 // 减速度
#define REG_MOTOR_CTRL_CURRENT_POSITION1 0x2021 // 当前位置(相对于入口处于第几个子仓位)
#define REG_MOTOR_CTRL_CURRENT_POSITION2 0x2022 // 当前位置(相对于出口口处于第几个子仓位)
#define REG_MOTOR_CTRL_CURRENT_POSITION3 0x2023 // 当前位置(相对于零点，单位是脉冲数)
#define REG_MOTOR_CTRL_CURRENT_POSITION4 0x2024 // 当前位置(单位是脉冲)
#define REG_MOTOR_CTRL_CURRENT_ACTION 0x2025 // 当前正在执行的动作
#define REG_MOTOR_CTRL_CURRENT_STATE 0x2026 // 当前动作是否完成 0:空闲,1:正在执行,2:执行完成
/*******************************************************************************
* 温度控制相关常数 *
*******************************************************************************/
@ -101,3 +119,13 @@ typedef enum {
#define TEMPERATURE_CTRL_MIN_OUTPUT (-50) // 限制制冷功率，如果出现制冷的时候，反而制热了，则增大这个值
#define TEMPERATURE_CTRL_DUMPVALUE (false) // 是否打印PID调试信息
/*******************************************************************************
* 孵育区转盘相关常数 *
*******************************************************************************/
#define ONECIRCLE_USTEP_NUM (256 * 200 * 150) // 256:步长,200:电机分辨率,150:减速比
#define REG_MOTOR_CTRL_MAX_SPEED_DEFAULT (500000) // 500kpps
#define SUB_POSITION_NUM (20) // 子仓位数量
#define ONE_SUB_POSITION_WIDTH ((ONECIRCLE_USTEP_NUM) / SUB_POSITION_NUM) //
#define REG_MOTOR_CTRL_ACCELERATION_DEFAULT 300000
#define REG_MOTOR_CTRL_DECELERATION_DEFAULT 300000

102
src/lncubator_rotating_control_service.cpp
View File

@ -5,32 +5,45 @@ using namespace iflytop;
#define UPDATE_ZERO_POSITION_ALWAYS 1
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_workfinished = true;
m_dowhat = kidle;
m_maxVelocity = mc_default_velocity;
m_maxVelocity = 0;
m_inletPosition = 0;
m_outletPosition = m_inletPosition + mc_one_sub_position_width / 2;
m_outletPosition = computeOutletPosition(m_inletPosition);
m_jobStep = 0;
m_hasCalibratedHomePosition = false;
}
int32_t LncubatorRotatingControlService::computeOutletPosition(int32_t inletPosition) {
int32_t outletPosition = normalizePosition(inletPosition + ONECIRCLE_USTEP_NUM / 2);
return outletPosition;
}
LncubatorRotatingControlService::~LncubatorRotatingControlService() {}
int32_t LncubatorRotatingControlService::getVelocity() { return m_maxVelocity; }
int32_t LncubatorRotatingControlService::getInletPosition() { return m_inletPosition; }
int32_t LncubatorRotatingControlService::getOutletPosition() { return m_outletPosition; }
void LncubatorRotatingControlService::setInletPosition(int32_t inletPosition) {
m_inletPosition = inletPosition;
m_outletPosition = computeOutletPosition(m_inletPosition);
}
void LncubatorRotatingControlService::setoutletPosition(int32_t outletPosition) { //
m_outletPosition = outletPosition;
}
void LncubatorRotatingControlService::initialize(Hardware* hardware) { //
m_hardware = hardware;
m_motor = m_hardware->tmc_get_motor1();
m_os = m_hardware;
m_workfinished = false;
m_workfinished = true;
m_dowhat = kidle;
m_maxVelocity = mc_default_velocity;
m_maxVelocity = 0;
m_inletPosition = 0;
m_outletPosition = m_inletPosition + mc_one_sub_position_width / 2;
m_outletPosition = computeOutletPosition(m_inletPosition);
/**
* @brief 电机必须工作在位置模式
@ -44,13 +57,40 @@ void LncubatorRotatingControlService::setVelocity(int32_t velocityMax) { m_maxVe
void LncubatorRotatingControlService::registerListener(LncubatorRotatingControlServiceListener* listener) { //
m_listener = listener;
}
int32_t LncubatorRotatingControlService::getPositionPulse() {
int32_t position = tmc_getPosition();
return position;
}
int32_t LncubatorRotatingControlService::computePositionIndex(int32_t position) { //
position = normalizePosition(position);
int index = position / ONE_SUB_POSITION_WIDTH;
int remainder = position % ONE_SUB_POSITION_WIDTH;
if (remainder > ONE_SUB_POSITION_WIDTH / 2) {
index++;
}
return normalizeIndex(index);
}
int32_t LncubatorRotatingControlService::getPositionIndex(refpoint_t refpoint) {
int32_t position = tmc_getPosition();
if (refpoint == REFPOINT_TYPE_INLET) {
return computePositionIndex(position - m_inletPosition);
} else if (refpoint == REFPOINT_TYPE_OUTLET) {
return computePositionIndex(position - m_outletPosition);
}
return 0;
}
void LncubatorRotatingControlService::calibrationPosition(refpoint_t refpoint) {
/**
* @brief 此时是孵育船1对其了入口
*/
if (refpoint == REFPOINT_TYPE_INLET) {
m_inletPosition = tmc_getPosition();
m_outletPosition = normalizePosition(m_inletPosition + mc_onecircle_ustep_num / 2);
m_outletPosition = computeOutletPosition(m_inletPosition);
} else if (refpoint == REFPOINT_TYPE_INLET) {
m_outletPosition = tmc_getPosition();
}
}
/*******************************************************************************
@ -66,18 +106,35 @@ void LncubatorRotatingControlService::tmc_roateToPosition(int32_t position, uint
}
int32_t LncubatorRotatingControlService::normalizePosition(int32_t position) {
while (true) {
if (position > mc_onecircle_ustep_num)
position = position - mc_onecircle_ustep_num;
if (position >= ONECIRCLE_USTEP_NUM)
position = position - ONECIRCLE_USTEP_NUM;
else if (position < 0)
position = position + mc_onecircle_ustep_num;
position = position + ONECIRCLE_USTEP_NUM;
else
break;
}
return position;
}
void LncubatorRotatingControlService::tmc_moveBy(int32_t relativePosition, uint32_t velocity) { m_motor->moveBy(relativePosition, velocity); }
bool LncubatorRotatingControlService::hardware_isHomeRefSwitchTriggered() { return m_hardware->tmc_home_ref_switch_get_state(); }
int32_t LncubatorRotatingControlService::normalizeIndex(int32_t index) {
while (true) {
if (index >= SUB_POSITION_NUM)
index = index - SUB_POSITION_NUM;
else if (index < 0)
index = index + SUB_POSITION_NUM;
else
break;
}
return index;
}
void LncubatorRotatingControlService::tmc_moveBy( //
int32_t relativePosition, uint32_t velocity) { //
m_motor->moveBy(relativePosition, velocity);
}
bool LncubatorRotatingControlService::hardware_isHomeRefSwitchTriggered() { //
return m_hardware->tmc_home_ref_switch_get_state();
}
/*******************************************************************************
* JobBasic *
@ -115,6 +172,7 @@ void LncubatorRotatingControlService::changeState(dowhat_t tostate) {
m_workfinished = true;
} else {
m_workfinished = false;
if (m_listener) m_listener->LncubatorRotatingControlService_startWorking(tostate);
}
}
@ -138,7 +196,7 @@ void LncubatorRotatingControlService::doRotateToHomeJobStep1() {
*/
ZLOGI(TAG, "doRotateToHomeJobStep1, rotate to +10 degree,leave from home");
m_jobStep = 1;
tmc_moveBy(mc_onecircle_ustep_num / 36, m_maxVelocity); // 此时处于零点位置，先顺时针转10度
tmc_moveBy(ONECIRCLE_USTEP_NUM / 10, m_maxVelocity); // 此时处于零点位置，先顺时针转10度
}
void LncubatorRotatingControlService::doRotateToHomeJobStep2() {
/**
@ -146,7 +204,7 @@ void LncubatorRotatingControlService::doRotateToHomeJobStep2() {
*/
ZLOGI(TAG, "doRotateToHomeJobStep2, rotate to home");
m_jobStep = 2;
tmc_moveBy(-mc_onecircle_ustep_num, m_maxVelocity);
tmc_moveBy(-ONECIRCLE_USTEP_NUM, m_maxVelocity);
}
void LncubatorRotatingControlService::doRotateToHomeJob(interevent_t event) {
if (event == kPeriodicJobEvent) {
@ -179,10 +237,10 @@ void LncubatorRotatingControlService::moveToSubPosition(refpoint_t refpoint, int
changeState(kmoveSubPositionJob);
// 出口
if (refpoint == REFPOINT_TYPE_OUTLET) {
int32_t targetPosition = m_outletPosition + positionIndex * mc_one_sub_position_width;
int32_t targetPosition = m_outletPosition + positionIndex * ONE_SUB_POSITION_WIDTH;
tmc_roateToPosition(targetPosition, m_maxVelocity);
} else if (refpoint == REFPOINT_TYPE_INLET) {
int32_t targetPosition = m_inletPosition + positionIndex * mc_one_sub_position_width;
int32_t targetPosition = m_inletPosition + positionIndex * ONE_SUB_POSITION_WIDTH;
tmc_roateToPosition(targetPosition, m_maxVelocity);
} else {
ZLOGE(TAG, "refpoint error");
@ -210,8 +268,8 @@ void LncubatorRotatingControlService::moveBy(int32_t pluseCount) {
position += pluseCount;
if (position <= 0) {
position = 0;
} else if (position > mc_onecircle_ustep_num) {
position = mc_onecircle_ustep_num;
} else if (position > ONECIRCLE_USTEP_NUM) {
position = ONECIRCLE_USTEP_NUM;
}
tmc_roateToPosition(position, m_maxVelocity);
}

31
src/lncubator_rotating_control_service.hpp
View File

@ -30,6 +30,7 @@ class LncubatorRotatingControlServiceListener {
virtual void LncubatorRotatingControlService_onMoveToHomeJobFinished() = 0;
virtual void LncubatorRotatingControlService_onMoveSubPositionJobFinished() = 0;
virtual void LncubatorRotatingControlService_onMoveByJobFinished() = 0;
virtual void LncubatorRotatingControlService_startWorking(int32_t dowhat) = 0;
};
class LncubatorRotatingControlService : public HardwareListener, //
@ -40,20 +41,14 @@ class LncubatorRotatingControlService : public HardwareListener, //
REFPOINT_TYPE_INLET, // 入口
REFPOINT_TYPE_ZERO, // 零点
} refpoint_t;
/*******************************************************************************
* 常量配置 *
*******************************************************************************/
static const uint32_t mc_onecircle_ustep_num;
static const uint32_t mc_default_velocity;
static const uint32_t mc_one_sub_position_width;
private:
typedef enum {
kidle,
kmoveSubPositionJob,
kmoveToHomeJob,
kmoveByJob,
} dowhat_t;
private:
typedef enum {
kPeriodicJobEvent,
kHomeSwitchTriggeredEvent,
@ -88,10 +83,17 @@ class LncubatorRotatingControlService : public HardwareListener, //
void periodicJob();
void setVelocity(int32_t velocityMax);
void setInletPositionAndOutletPosition(int32_t inletPosition, int32_t outletPosition);
int32_t getVelocity() { return m_maxVelocity; }
int32_t getInletPosition() { return m_inletPosition; }
int32_t getOutletPosition() { return m_outletPosition; }
int32_t getVelocity();
void setInletPosition(int32_t inletPosition);
int32_t getInletPosition();
void setoutletPosition(int32_t outletPosition);
int32_t getOutletPosition();
int32_t getPositionPulse();
int32_t getPositionIndex(refpoint_t refpoint);
bool isWorkFinished() { return m_workfinished; }
dowhat_t getDoWhat() { return m_dowhat; }
void moveToHome();
void moveBy(int32_t pluseCount);
@ -104,7 +106,6 @@ class LncubatorRotatingControlService : public HardwareListener, //
* @param positionIndex 第几个位置
*/
void calibrationPosition(refpoint_t refpoint);
/*******************************************************************************
* OVERRIDE HardwareListener *
*******************************************************************************/
@ -119,7 +120,6 @@ class LncubatorRotatingControlService : public HardwareListener, //
void doRotateToHomeJob(interevent_t event);
void doRotateToHomeJobStep1();
void doRotateToHomeJobStep2();
void doMoveToSubPositionJob(interevent_t event);
void doMoveByJob(interevent_t event);
void changeState(dowhat_t state);
@ -129,6 +129,9 @@ class LncubatorRotatingControlService : public HardwareListener, //
void tmc_roateToPosition(int32_t position, uint32_t velocity);
void tmc_moveBy(int32_t relativePosition, uint32_t velocity);
int32_t normalizePosition(int32_t position);
int32_t normalizeIndex(int32_t index);
int32_t computePositionIndex(int32_t position);
int32_t computeOutletPosition(int32_t inletPosition);
bool hardware_isHomeRefSwitchTriggered();
};

121
src/umain.cpp
View File

@ -6,6 +6,8 @@
#include "libtrinamic/src/ic/tmc2160.hpp"
#include "libtrinamic\src\ic\tmc4361A.hpp"
//
#include <stdlib.h>
#include "board/hardware.hpp"
#include "i2c.h"
#include "libiflytop_micro\stm32\basic\zsignal.hpp"
@ -15,14 +17,14 @@
#include "lncubator_temperature_control_service.hpp"
#define TAG "main"
#define ACTIVE_REG(reg, mask, initval) IflytopCanProtocolStackProcesser::activeReg(&IflytopCanStackConfig, reg, mask, initval)
#define UPDATE_REG(reg, value, periodms) \
{ \
static uint32_t lastReportTicket = 0; \
if (m_hardware.hasPassedMS(lastReportTicket) > periodms) { \
lastReportTicket = m_hardware.getTicket(); \
m_protocolStack.writeRegValue(reg, value, false); \
} \
#define ACTIVE_REG(reg, mask, initval) IflytopCanProtocolStackProcesser::activeReg(&IflytopCanStackConfig, reg, mask, (initval))
#define UPDATE_REG(reg, value, periodms) \
{ \
static uint32_t lastReportTicket = 0; \
if (m_hardware.hasPassedMS(lastReportTicket) > (periodms + random(3))) { \
lastReportTicket = m_hardware.getTicket(); \
m_protocolStack.writeRegValue(reg, value, false); \
} \
}
using namespace iflytop;
@ -51,6 +53,7 @@ class Main : public HardwareListener, //
virtual void LncubatorRotatingControlService_onMoveToHomeJobFinished();
virtual void LncubatorRotatingControlService_onMoveSubPositionJobFinished();
virtual void LncubatorRotatingControlService_onMoveByJobFinished();
virtual void LncubatorRotatingControlService_startWorking(int32_t dowhat);
/*******************************************************************************
* override HardwareListener *
@ -67,9 +70,14 @@ class Main : public HardwareListener, //
virtual icps::error_t IflytopCanProtocolStack_onHostRegisterReadEvent( //
icps::Reg_t *reg, int32_t &value);
virtual void IflytopCanProtocolStack_onRegisterValueAutoReportEvent(icps::Reg_t *reg, int32_t &value);
virtual void IflytopCanProtocolStack_onRegisterValueAutoReportEvent(icps::Reg_t *reg, int32_t &value);
};
int random(int max) {
// TODO:使用STM32的随机数功能生成随机数
return 0 % max;
}
/*******************************************************************************
* CODE IMPL *
*******************************************************************************/
@ -139,6 +147,53 @@ icps::error_t Main::IflytopCanProtocolStack_onHostRegisterWriteEvent( //
} else if (reg->add == REG_TEMPERATURE_CTRL_DUMPVALUE) {
m_tempCtrl.setDumpValue((bool)newvalue);
}
// 赋予转盘控制
else if (reg->add == REG_MOTOR_CTRL_TARGET_POSITION1) {
if (m_rotatingCtrl.isWorkFinished()) {
m_rotatingCtrl.moveToSubPosition(m_rotatingCtrl.REFPOINT_TYPE_INLET, newvalue);
} else {
return icps::kDeviceBusy;
}
newvalue = 0;
} else if (reg->add == REG_MOTOR_CTRL_TARGET_POSITION2) {
if (m_rotatingCtrl.isWorkFinished()) {
m_rotatingCtrl.moveToSubPosition(m_rotatingCtrl.REFPOINT_TYPE_INLET, newvalue);
} else {
return icps::kDeviceBusy;
}
newvalue = 0;
} else if (reg->add == REG_MOTOR_CTRL_BACK_TO_HOME) {
if (m_rotatingCtrl.isWorkFinished()) {
m_rotatingCtrl.moveToHome();
} else {
return icps::kDeviceBusy;
}
} else if (reg->add == REG_MOTOR_CTRL_ROATE_PULSE) {
if (m_rotatingCtrl.getDoWhat() == m_rotatingCtrl.kmoveByJob || //
m_rotatingCtrl.getDoWhat() == m_rotatingCtrl.kidle) {
m_rotatingCtrl.moveBy(newvalue);
} else {
return icps::kDeviceBusy;
}
} else if (reg->add == REG_MOTOR_CTRL_STOP) {
m_rotatingCtrl.stop();
} else if (reg->add == REG_MOTOR_CTRL_CALIBRATE_POSITION) {
if (newvalue == 0) {
m_rotatingCtrl.calibrationPosition(m_rotatingCtrl.REFPOINT_TYPE_INLET);
} else if (newvalue == 1) {
m_rotatingCtrl.calibrationPosition(m_rotatingCtrl.REFPOINT_TYPE_OUTLET);
}
} else if (reg->add == REG_MOTOR_CTRL_MAX_SPEED) {
m_rotatingCtrl.setVelocity(newvalue);
} else if (reg->add == REG_MOTOR_CTRL_ENTRY_POSITION) {
m_rotatingCtrl.setInletPosition(newvalue);
} else if (reg->add == REG_MOTOR_CTRL_EXIT_POSITION) {
m_rotatingCtrl.setoutletPosition(newvalue);
} else if (reg->add == REG_MOTOR_CTRL_ACCELERATION) {
m_hardware.tmc_get_motor1()->setMaximumAcceleration(newvalue);
} else if (reg->add == REG_MOTOR_CTRL_DECELERATION) {
m_hardware.tmc_get_motor1()->setMaximumDeceleration(newvalue);
}
return icps::kSuccess;
}
@ -183,14 +238,47 @@ void Main::initializeIflytopCanProtocolStackProcesser() {
ACTIVE_REG(REG_TEMPERATURE_CTRL_SENSOR2_TEMPERATURE /* */, icps::kr, 0);
ACTIVE_REG(REG_TEMPERATURE_CTRL_SENSOR3_TEMPERATURE /* */, icps::kr, 0);
ACTIVE_REG(REG_TEMPERATURE_CTRL_SENSOR4_TEMPERATURE /* */, icps::kr, 0);
/*******************************************************************************
* 孵育区转盘控制 *
*******************************************************************************/
ACTIVE_REG(REG_MOTOR_CTRL_TARGET_POSITION1 /* */, icps::kw, 0);
ACTIVE_REG(REG_MOTOR_CTRL_TARGET_POSITION2 /* */, icps::kw, 0);
ACTIVE_REG(REG_MOTOR_CTRL_BACK_TO_HOME /* */, icps::kw, 0);
ACTIVE_REG(REG_MOTOR_CTRL_ROATE_PULSE /* */, icps::kw, 0);
ACTIVE_REG(REG_MOTOR_CTRL_CALIBRATE_POSITION /* */, icps::kw, 0);
ACTIVE_REG(REG_MOTOR_CTRL_MAX_SPEED /* */, icps::kwr, REG_MOTOR_CTRL_MAX_SPEED_DEFAULT);
ACTIVE_REG(REG_MOTOR_CTRL_ENTRY_POSITION /* */, icps::kwr, 0);
ACTIVE_REG(REG_MOTOR_CTRL_EXIT_POSITION /* */, icps::kwr, ONECIRCLE_USTEP_NUM / 2);
ACTIVE_REG(REG_MOTOR_CTRL_ACCELERATION /* */, icps::kwr, REG_MOTOR_CTRL_ACCELERATION_DEFAULT);
ACTIVE_REG(REG_MOTOR_CTRL_DECELERATION /* */, icps::kwr, REG_MOTOR_CTRL_DECELERATION_DEFAULT);
ACTIVE_REG(REG_MOTOR_CTRL_CURRENT_POSITION1 /* */, icps::kr, 0);
ACTIVE_REG(REG_MOTOR_CTRL_CURRENT_POSITION2 /* */, icps::kr, SUB_POSITION_NUM / 2);
ACTIVE_REG(REG_MOTOR_CTRL_CURRENT_POSITION3 /* */, icps::kr, 0);
ACTIVE_REG(REG_MOTOR_CTRL_CURRENT_POSITION4 /* */, icps::kr, 0);
ACTIVE_REG(REG_MOTOR_CTRL_CURRENT_ACTION /* */, icps::kr, 0);
ACTIVE_REG(REG_MOTOR_CTRL_CURRENT_STATE /* */, icps::kr, 0);
}
void Main::LncubatorRotatingControlService_onException(exception_id_t exception) {}
void Main::LncubatorRotatingControlService_onMoveToHomeJobFinished() {}
void Main::LncubatorRotatingControlService_onMoveSubPositionJobFinished() {}
void Main::LncubatorRotatingControlService_onMoveByJobFinished() {}
void Main::LncubatorRotatingControlService_onMoveToHomeJobFinished() { //
m_protocolStack.writeRegValue(REG_MOTOR_CTRL_CURRENT_STATE, 1, false);
}
void Main::LncubatorRotatingControlService_onMoveSubPositionJobFinished() { //
m_protocolStack.writeRegValue(REG_MOTOR_CTRL_CURRENT_STATE, 1, false);
}
void Main::LncubatorRotatingControlService_onMoveByJobFinished() { //
m_protocolStack.writeRegValue(REG_MOTOR_CTRL_CURRENT_STATE, 1, false);
}
void Main::LncubatorRotatingControlService_startWorking(int32_t dowhat) { //
m_protocolStack.writeRegValue(REG_MOTOR_CTRL_CURRENT_STATE, 0, false);
m_protocolStack.writeRegValue(REG_MOTOR_CTRL_CURRENT_ACTION, dowhat, false);
}
void Main::updateRegList() { //
UPDATE_REG(REG_TEMPERATURE_CTRL_MAIN_FAN_SPEED, m_hardware.fanReadState4(), 1000);
UPDATE_REG(REG_TEMPERATURE_CTRL_SUB_FAN_SPEED, m_hardware.fanReadState0to3(), 1000);
UPDATE_REG(REG_TEMPERATURE_CTRL_HEAT_TEMPERATURE, 0, 1000);
@ -200,6 +288,13 @@ void Main::updateRegList() { //
UPDATE_REG(REG_TEMPERATURE_CTRL_SENSOR2_TEMPERATURE, m_hardware.temperature_get_temp(1) * 100, 1000);
UPDATE_REG(REG_TEMPERATURE_CTRL_SENSOR3_TEMPERATURE, m_hardware.temperature_get_temp(2) * 100, 1000);
UPDATE_REG(REG_TEMPERATURE_CTRL_SENSOR4_TEMPERATURE, m_hardware.temperature_get_temp(3) * 100, 1000);
UPDATE_REG(REG_MOTOR_CTRL_CURRENT_POSITION1, m_rotatingCtrl.getPositionIndex(m_rotatingCtrl.REFPOINT_TYPE_INLET), 100);
UPDATE_REG(REG_MOTOR_CTRL_CURRENT_POSITION2, m_rotatingCtrl.getPositionIndex(m_rotatingCtrl.REFPOINT_TYPE_OUTLET), 100);
// UPDATE_REG(REG_MOTOR_CTRL_CURRENT_POSITION3, m_rotatingCtrl.getPositionIndex(m_rotatingCtrl.REFPOINT_TYPE_HOME), 100);
UPDATE_REG(REG_MOTOR_CTRL_CURRENT_POSITION4, m_rotatingCtrl.getPositionPulse(), 100);
UPDATE_REG(REG_MOTOR_CTRL_CURRENT_ACTION, m_rotatingCtrl.getDoWhat(), 33);
UPDATE_REG(REG_MOTOR_CTRL_CURRENT_STATE, m_rotatingCtrl.isWorkFinished(), 33);
}
void Main::main(int argc, char const *argv[]) {
@ -223,6 +318,8 @@ void Main::main(int argc, char const *argv[]) {
// 初始化转盘控制器
m_rotatingCtrl.initialize(&m_hardware);
m_rotatingCtrl.registerListener(this);
m_rotatingCtrl.setVelocity(m_protocolStack.readRegValue(REG_MOTOR_CTRL_MAX_SPEED));
m_rotatingCtrl.setInletPosition(m_protocolStack.readRegValue(REG_MOTOR_CTRL_ENTRY_POSITION));
while (true) {
m_hardware.periodicJob();

Write Preview
Loading…
Cancel
Save