14 changed files with 739 additions and 97 deletions
-
20components/iflytop_can_slave_modules/device_base_control_service.cpp
-
6components/iflytop_can_slave_modules/device_base_control_service.hpp
-
21components/iflytop_can_slave_modules/idcard_reader_service.cpp
-
6components/iflytop_can_slave_modules/idcard_reader_service.hpp
-
97components/iflytop_can_slave_v1/iflytop_can_slave.cpp
-
45components/iflytop_can_slave_v1/iflytop_can_slave.hpp
-
290components/single_axis_motor_control/single_axis_motor_control.cpp
-
141components/single_axis_motor_control/single_axis_motor_control.hpp
-
25components/single_axis_motor_control_v2/i_iflytop_can_slave_module.cpp
-
8components/single_axis_motor_control_v2/i_iflytop_can_slave_module.hpp
-
2components/single_axis_motor_control_v2/single_axis_motor_control_v2.cpp
-
117components/step_action_scheduler/step_scheduler.cpp
-
57components/step_action_scheduler/step_scheduler.hpp
-
1hal/zhal_core.hpp
@ -0,0 +1,290 @@ |
|||
#include "single_axis_motor_control.hpp"
|
|||
#ifdef HAL_CAN_MODULE_ENABLED
|
|||
using namespace std; |
|||
using namespace iflytop; |
|||
|
|||
void SingleAxisMotorControler::initialize(const char* name, IflytopCanProtocolStackProcesser* processer, int regaddoff, //
|
|||
ZGPIO* homegpio, //
|
|||
ZGPIO* mingpio, //
|
|||
ZGPIO* rightgpio, //
|
|||
IStepperMotor* motor) { |
|||
m_homegpio = homegpio; |
|||
m_minlimitgpio = mingpio; |
|||
m_maxlimitgpio = rightgpio; |
|||
m_motor = motor; |
|||
m_regaddoff = regaddoff; |
|||
m_name = name; |
|||
m_processer = processer; |
|||
|
|||
ZASSERT(motor); |
|||
if (m_homegpio) { |
|||
ZASSERT(m_homegpio->isItRisingAndItFallingEXITGPIO()); |
|||
} |
|||
|
|||
m_slave = m_processer->createICPSSlaveModule(name, this, m_regaddoff); |
|||
|
|||
m_processer->activeReg(m_slave, SAMC_REG_ACT_ROTATE, icps::kw, 0); |
|||
m_processer->activeReg(m_slave, SAMC_REG_ACT_MOVE_TO, icps::kw, 0); |
|||
m_processer->activeReg(m_slave, SAMC_REG_ACT_MOVE_BY, icps::kw, 0); |
|||
m_processer->activeReg(m_slave, SAMC_REG_ACT_RUN_TO_HOME, icps::kw, 0); |
|||
m_processer->activeReg(m_slave, SAMC_REG_ACT_DO_NOTHING, icps::kw, 0); |
|||
m_processer->activeReg(m_slave, SAMC_REG_ACT_STOP, icps::kw, 0); |
|||
m_processer->activeReg(m_slave, SAMC_REG_ACT_CLEAR_EXCEPTION, icps::kw, 0); |
|||
|
|||
m_statusReg = m_processer->activeReg(m_slave, SAMC_REG_STAT_STATUS, icps::kwr, 0); |
|||
m_currentVelocityReg = m_processer->activeReg(m_slave, SAMC_REG_STAT_CURRENT_VELOCITY, icps::kwr, 0); |
|||
m_currentPosReg = m_processer->activeReg(m_slave, SAMC_REG_STAT_CURRENT_POS, icps::kwr, 0); |
|||
m_exceptionReg = m_processer->activeReg(m_slave, SAMC_REG_STAT_EXCEPTION, icps::kwr, 0); |
|||
|
|||
cfg_acc_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_ACC, icps::kwr, 0); |
|||
cfg_dec_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_DEC, icps::kwr, 0); |
|||
cfg_velocity_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_VELOCITY, icps::kwr, 0); |
|||
cfg_zero_shift_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_ZERO_SHIFT, icps::kwr, 0); // 零点偏移
|
|||
cfg_runhome_velocity_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_RUNHOME_VELOCITY, icps::kwr, 0); // 归零速度
|
|||
cfg_runtohome_dec_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_RUNTOHOME_DEC, icps::kwr, 0); // 归零减速度
|
|||
cfg_runtohome_max_distance_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_RUNTOHOME_MAX_DISTANCE, icps::kwr, 0); // 归零最大距离
|
|||
cfg_runtohome_leave_zero_point_distance_reg = |
|||
m_processer->activeReg(m_slave, SAMC_REG_CFG_RUNTOHOME_LEAVE_ZERO_POINT_DISTANCE, icps::kwr, 0); // 离开零点距离
|
|||
cfg_min_pos_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_MIN_POS, icps::kwr, 0); // 最小位置
|
|||
cfg_max_pos_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_MAX_POS, icps::kwr, 0); // 最大位置
|
|||
cfg_runtohome_keep_move_distance_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_RUNTOHOME_KEEP_MOVE_DISTANCE, icps::kwr, 0); // 归零阶段1移动距离
|
|||
} |
|||
|
|||
icps::error_t SingleAxisMotorControler::onHostRegisterWriteEvent(icps::WriteEvent* event) { |
|||
int regoff = event->reg->add - m_regaddoff; |
|||
int val = event->newvalue; |
|||
switch (regoff) { |
|||
case SAMC_REG_ACT_ROTATE: |
|||
return rotate(val); |
|||
case SAMC_REG_ACT_MOVE_TO: |
|||
return moveTo(val); |
|||
case SAMC_REG_ACT_MOVE_BY: |
|||
return moveBy(val); |
|||
case SAMC_REG_ACT_RUN_TO_HOME: |
|||
return runToHome(); |
|||
case SAMC_REG_ACT_DO_NOTHING: |
|||
return icps::kSuccess; |
|||
case SAMC_REG_ACT_STOP: |
|||
return stop(); |
|||
case SAMC_REG_ACT_CLEAR_EXCEPTION: |
|||
return clearException(); |
|||
default: |
|||
break; |
|||
}; |
|||
return icps::kSuccess; |
|||
} |
|||
|
|||
icps::error_t SingleAxisMotorControler::rotate(int32_t velocity) { |
|||
if (!(m_state == kstate_rotate || m_state == kstate_idle)) { |
|||
return icps::kDeviceBusy; |
|||
} |
|||
changeToState(kstate_rotate); |
|||
|
|||
if (velocity == 0 && m_state == kstate_rotate) { |
|||
m_motor->stop(); |
|||
m_step_scheduler.stop(); |
|||
return icps::kSuccess; |
|||
} |
|||
|
|||
m_step_scheduler.reset(); |
|||
m_step_scheduler.pushActionAction([this, velocity](StepActionContext* context) { //
|
|||
motor_rotate(velocity, cfg_acc_reg->getValue(), cfg_dec_reg->getValue()); |
|||
}); |
|||
m_step_scheduler.pushActionIsDone([](StepActionContext* context) { return false; }); |
|||
m_step_scheduler.regDoneCallback([this]() { changeToState(kstate_idle); }); |
|||
m_step_scheduler.regBreakCallback([this]() { changeToState(kstate_idle); }); |
|||
m_step_scheduler.startSchedule(); |
|||
return icps::kSuccess; |
|||
} |
|||
icps::error_t SingleAxisMotorControler::moveTo(int32_t pos) { |
|||
if (!(m_state == kstate_moveTo || m_state == kstate_idle)) { |
|||
return icps::kDeviceBusy; |
|||
} |
|||
changeToState(kstate_moveTo); |
|||
|
|||
m_step_scheduler.reset(); |
|||
m_step_scheduler.pushActionAction([this, pos](StepActionContext* context) { //
|
|||
motor_moveTo(pos, cfg_velocity_reg->getValue(), cfg_acc_reg->getValue(), cfg_dec_reg->getValue()); |
|||
}); |
|||
m_step_scheduler.pushActionIsDone(bind(&SingleAxisMotorControler::motorIsStop, this, std::placeholders::_1)); |
|||
m_step_scheduler.regDoneCallback([this]() { changeToState(kstate_idle); }); |
|||
m_step_scheduler.regBreakCallback([this]() { changeToState(kstate_idle); }); |
|||
m_step_scheduler.startSchedule(); |
|||
|
|||
return icps::kSuccess; |
|||
} |
|||
icps::error_t SingleAxisMotorControler::moveBy(int32_t pos) { |
|||
if (!(m_state == kstate_moveBy || m_state == kstate_idle)) { |
|||
return icps::kDeviceBusy; |
|||
} |
|||
changeToState(kstate_moveBy); |
|||
|
|||
m_step_scheduler.reset(); |
|||
m_step_scheduler.pushActionAction([this, pos](StepActionContext* context) { //
|
|||
motor_moveBy(pos, cfg_velocity_reg->getValue(), cfg_acc_reg->getValue(), cfg_dec_reg->getValue()); |
|||
}); |
|||
m_step_scheduler.pushActionIsDone(bind(&SingleAxisMotorControler::motorIsStop, this, std::placeholders::_1)); |
|||
m_step_scheduler.regDoneCallback([this]() { changeToState(kstate_idle); }); |
|||
m_step_scheduler.regBreakCallback([this]() { changeToState(kstate_idle); }); |
|||
m_step_scheduler.startSchedule(); |
|||
|
|||
return icps::kSuccess; |
|||
} |
|||
icps::error_t SingleAxisMotorControler::runToHome() { |
|||
if (!(m_state == kstate_runToHome || m_state == kstate_idle)) { |
|||
return icps::kDeviceBusy; |
|||
} |
|||
changeToState(kstate_runToHome); |
|||
|
|||
m_step_scheduler.reset(); |
|||
|
|||
/**
|
|||
* @brief 步骤1 如果当前不在零点的位置，则缓慢移动到零点 |
|||
*/ |
|||
if (!m_homegpio->getState()) { |
|||
m_step_scheduler.pushActionAction([this](StepActionContext* context) { //
|
|||
regGpioIrqProcesser([this](ZGPIO* GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent) { |
|||
if (GPIO_Pin != m_homegpio) return; |
|||
m_runToHomeContext.stopByIrq = true; |
|||
m_motor->stop(); |
|||
}); |
|||
|
|||
motor_moveBy(-cfg_runtohome_max_distance_reg->getValue(), //
|
|||
cfg_runhome_velocity_reg->getValue() * 2, //
|
|||
cfg_acc_reg->getValue(), //
|
|||
cfg_runtohome_dec_reg->getValue()); |
|||
}); |
|||
m_step_scheduler.pushActionIsDone([this](StepActionContext* context) { |
|||
if (motorIsStop(context)) { |
|||
if (m_runToHomeContext.stopByIrq) { |
|||
return true; |
|||
} else { |
|||
context->breakflag = true; |
|||
ZLOGE(m_name, "runToHome fail, not reach home point"); |
|||
return false; |
|||
} |
|||
} |
|||
return false; |
|||
}); |
|||
} |
|||
/**
|
|||
* @brief STEP 2 离开原点一定的距离 |
|||
* |
|||
*/ |
|||
m_step_scheduler.pushActionAction([this](StepActionContext* context) { //
|
|||
regGpioIrqProcesser(nullptr); |
|||
motor_moveBy(cfg_runtohome_leave_zero_point_distance_reg->getValue(), //
|
|||
cfg_runhome_velocity_reg->getValue(), //
|
|||
cfg_acc_reg->getValue(), //
|
|||
cfg_dec_reg->getValue()); // 此时处于零点位置，先离开原点一定位置
|
|||
}); |
|||
m_step_scheduler.pushActionIsDone(bind(&SingleAxisMotorControler::motorIsStop, this, std::placeholders::_1)); |
|||
|
|||
/**
|
|||
* @brief STEP 3 返回原点 |
|||
*/ |
|||
m_step_scheduler.pushActionAction([this](StepActionContext* context) { //
|
|||
regGpioIrqProcesser([this](ZGPIO* GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent) { |
|||
if (GPIO_Pin != m_homegpio) return; |
|||
m_runToHomeContext.stopByIrq = true; |
|||
m_runToHomeContext.zeroPointPos = m_motor->getXACTUAL(); |
|||
m_motor->stop(); |
|||
}); |
|||
motor_moveBy(-cfg_runtohome_leave_zero_point_distance_reg->getValue() * 1.5, //
|
|||
cfg_runhome_velocity_reg->getValue(), //
|
|||
cfg_acc_reg->getValue(), //
|
|||
cfg_runtohome_dec_reg->getValue()); // 此时处于零点位置，先离开原点一定位置
|
|||
}); |
|||
m_step_scheduler.pushActionIsDone([this](StepActionContext* context) { |
|||
if (motorIsStop(context)) { |
|||
if (m_runToHomeContext.stopByIrq) { |
|||
return true; |
|||
} else { |
|||
context->breakflag = true; |
|||
ZLOGE(m_name, "runToHome fail, not reach home point"); |
|||
return false; |
|||
} |
|||
} |
|||
return false; |
|||
}); |
|||
m_step_scheduler.pushActionDone([this](StepActionContext* context) { |
|||
int32_t nowposition = m_motor->getXACTUAL(); |
|||
int32_t nowposition_real = nowposition - m_runToHomeContext.zeroPointPos; |
|||
ZLOGI(m_name, "nowposition:%d,zeroPointPosition:%d", nowposition, m_runToHomeContext.zeroPointPos); |
|||
m_motor->setXACTUAL(nowposition_real - cfg_zero_shift_reg->getValue()); |
|||
}); |
|||
|
|||
/**
|
|||
* @brief STEP 4 移动到零点 |
|||
*/ |
|||
m_step_scheduler.pushActionAction([this](StepActionContext* context) { //
|
|||
regGpioIrqProcesser(nullptr); |
|||
motor_moveTo(0, //
|
|||
cfg_runhome_velocity_reg->getValue(), //
|
|||
cfg_acc_reg->getValue(), //
|
|||
cfg_dec_reg->getValue()); // 此时处于零点位置，先离开原点一定位置
|
|||
}); |
|||
m_step_scheduler.pushActionIsDone(bind(&SingleAxisMotorControler::motorIsStop, this, std::placeholders::_1)); |
|||
|
|||
/**
|
|||
* @brief 异常处理 |
|||
*/ |
|||
m_step_scheduler.regDoneCallback([this]() { changeToState(kstate_idle); }); |
|||
m_step_scheduler.regBreakCallback([this]() { |
|||
m_exceptionReg->setValue(kexcep_loseZeroPoint); |
|||
changeToState(kstate_exception); |
|||
}); |
|||
m_step_scheduler.startSchedule(); |
|||
|
|||
return icps::kSuccess; |
|||
} |
|||
|
|||
icps::error_t SingleAxisMotorControler::stop() { |
|||
changeToState(kstate_idle); |
|||
m_motor->stop(); |
|||
m_step_scheduler.stop(); |
|||
return icps::kSuccess; |
|||
} |
|||
icps::error_t SingleAxisMotorControler::clearException() { |
|||
m_exceptionReg->setValue(0); |
|||
changeToState(kstate_idle); |
|||
return icps::kSuccess; |
|||
} |
|||
|
|||
void SingleAxisMotorControler::changeToState(state_t state) { |
|||
m_state = state; |
|||
m_statusReg->setValue(m_state); |
|||
} |
|||
|
|||
void SingleAxisMotorControler::periodJob(ZHALCORE::Context* context) {} |
|||
void SingleAxisMotorControler::motor_moveTo(int32_t pos, int32_t velocity, int32_t acc, int32_t dec) { |
|||
ZLOGI(m_name, "motor_moveTo %d %d %d %d", pos, velocity, acc, dec); |
|||
int32_t position = m_motor->getXACTUAL(); |
|||
// position >= pos ? m_direction = 1 : m_direction = -1;
|
|||
m_motor->setAcceleration(acc); |
|||
m_motor->setDeceleration(dec); |
|||
m_motor->moveTo(pos, velocity); |
|||
} |
|||
void SingleAxisMotorControler::motor_moveBy(int32_t pos, int32_t velocity, int32_t acc, int32_t dec) { |
|||
ZLOGI(m_name, "motor_moveBy %d %d %d %d", pos, velocity, acc, dec); |
|||
// pos >= 0 ? m_direction = 1 : m_direction = -1;
|
|||
m_motor->setAcceleration(acc); |
|||
m_motor->setDeceleration(dec); |
|||
m_motor->moveBy(pos, velocity); |
|||
} |
|||
void SingleAxisMotorControler::motor_rotate(int32_t velocity, int32_t acc, int32_t dec) { |
|||
ZLOGI(m_name, "motor_rotate %d %d %d", velocity, acc, dec); |
|||
// velocity >= 0 ? m_direction = 1 : m_direction = -1;
|
|||
m_motor->setAcceleration(acc); |
|||
m_motor->setDeceleration(dec); |
|||
m_motor->rotate(velocity); |
|||
} |
|||
bool SingleAxisMotorControler::motorIsStop(StepActionContext* context) { |
|||
if (!context) { |
|||
return m_motor->isReachTarget(); |
|||
} |
|||
if (context->coreContext->getScheduleTimes() % 33 == 0 /*33 ms*/) return m_motor->isReachTarget(); |
|||
return false; |
|||
} |
|||
|
|||
#endif
|
|||
@ -0,0 +1,141 @@ |
|||
#pragma once
|
|||
#include "sdk/hal/zhal.hpp"
|
|||
#ifdef HAL_CAN_MODULE_ENABLED
|
|||
#include <stdlib.h>
|
|||
|
|||
#include "sdk/components/step_action_scheduler/step_scheduler.hpp"
|
|||
#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp"
|
|||
#include "sdk\components\tmc\basic\tmc_ic_interface.hpp"
|
|||
|
|||
/**
|
|||
* @brief |
|||
* |
|||
* service: SingleAxisMotorControler |
|||
* |
|||
*/ |
|||
#define SINGLE_AXIS_MOTOR_CTRL_REG_NUM 99
|
|||
|
|||
#define SAMC_REG_ACT_ROTATE (0) // 速度模式控制
|
|||
#define SAMC_REG_ACT_MOVE_TO (1) // 位置模式控制
|
|||
#define SAMC_REG_ACT_MOVE_BY (2) // 相对位置模式控制
|
|||
#define SAMC_REG_ACT_RUN_TO_HOME (3) // 归零
|
|||
#define SAMC_REG_ACT_DO_NOTHING (4) // __
|
|||
#define SAMC_REG_ACT_STOP (8) // 停止
|
|||
#define SAMC_REG_ACT_CLEAR_EXCEPTION (9) // 清空异常
|
|||
|
|||
#define SAMC_REG_STAT_STATUS (10) // 设备状态
|
|||
#define SAMC_REG_STAT_CURRENT_VELOCITY (12) // 当前转速
|
|||
#define SAMC_REG_STAT_CURRENT_POS (13) // 当前位置
|
|||
#define SAMC_REG_STAT_EXCEPTION (14) // 异常状态
|
|||
|
|||
#define SAMC_REG_CFG_ACC (20) // 加速度pps^2
|
|||
#define SAMC_REG_CFG_DEC (21) // 减速度pps^2
|
|||
#define SAMC_REG_CFG_VELOCITY (22) // 速度
|
|||
#define SAMC_REG_CFG_ZERO_SHIFT (23) // 零点偏移
|
|||
#define SAMC_REG_CFG_RUNHOME_VELOCITY (24) // 归零速度
|
|||
#define SAMC_REG_CFG_RUNTOHOME_DEC (25) // 归零减速度
|
|||
#define SAMC_REG_CFG_RUNTOHOME_MAX_DISTANCE (26) // 归零最大移动距离
|
|||
#define SAMC_REG_CFG_RUNTOHOME_LEAVE_ZERO_POINT_DISTANCE (27) // 归零阶段1移动距离
|
|||
#define SAMC_REG_CFG_MIN_POS (28) // 最小位置
|
|||
#define SAMC_REG_CFG_MAX_POS (29) // 最大位置
|
|||
#define SAMC_REG_CFG_RUNTOHOME_KEEP_MOVE_DISTANCE (30) // 最大位置
|
|||
|
|||
namespace iflytop { |
|||
using namespace std; |
|||
|
|||
class SingleAxisMotorControler : public ICPSListener { |
|||
public: |
|||
typedef enum { |
|||
kstate_idle = 0, |
|||
kstate_rotate = 1, |
|||
kstate_moveTo = 2, |
|||
kstate_moveBy = 3, |
|||
kstate_runToHome = 4, |
|||
kstate_exception = 65535, |
|||
} state_t; |
|||
|
|||
typedef enum { |
|||
kexcep_motorBlocking = 1, |
|||
kexcep_loseZeroPoint = 2, |
|||
} exception_id_t; |
|||
|
|||
class RunToHomeContext { |
|||
public: |
|||
bool stopByIrq = false; |
|||
int32_t zeroPointPos; |
|||
}; |
|||
|
|||
private: |
|||
const char* m_name; |
|||
IflytopCanProtocolStackProcesser* m_processer; |
|||
int m_regaddoff; |
|||
// res
|
|||
ZGPIO* m_homegpio; |
|||
ZGPIO* m_maxlimitgpio; |
|||
ZGPIO* m_minlimitgpio; |
|||
IStepperMotor* m_motor; |
|||
|
|||
ICPSSlaveModule* m_slave; |
|||
|
|||
state_t m_state = kstate_idle; |
|||
|
|||
icps::Reg_t* m_statusReg; |
|||
icps::Reg_t* m_currentVelocityReg; |
|||
icps::Reg_t* m_currentPosReg; |
|||
icps::Reg_t* m_exceptionReg; |
|||
|
|||
RunToHomeContext m_runToHomeContext; |
|||
|
|||
StepActionScheduler m_step_scheduler; |
|||
|
|||
ZGPIO::onirq_t m_gpioirqprocesser; |
|||
|
|||
public: |
|||
icps::Reg_t* cfg_acc_reg; |
|||
icps::Reg_t* cfg_dec_reg; |
|||
icps::Reg_t* cfg_velocity_reg; |
|||
icps::Reg_t* cfg_zero_shift_reg; |
|||
icps::Reg_t* cfg_runhome_velocity_reg; |
|||
icps::Reg_t* cfg_runtohome_dec_reg; |
|||
icps::Reg_t* cfg_runtohome_max_distance_reg; |
|||
icps::Reg_t* cfg_runtohome_leave_zero_point_distance_reg; |
|||
icps::Reg_t* cfg_min_pos_reg; |
|||
icps::Reg_t* cfg_max_pos_reg; |
|||
icps::Reg_t* cfg_runtohome_keep_move_distance_reg; |
|||
|
|||
public: |
|||
SingleAxisMotorControler(){}; |
|||
virtual ~SingleAxisMotorControler(){}; |
|||
|
|||
void initialize(const char* name, IflytopCanProtocolStackProcesser* processer, int regaddoff, //
|
|||
ZGPIO* homegpio, //
|
|||
ZGPIO* mingpio, //
|
|||
ZGPIO* rightgpio, //
|
|||
IStepperMotor* motor); |
|||
|
|||
public: |
|||
virtual icps::error_t onHostRegisterWriteEvent(icps::WriteEvent* event); |
|||
|
|||
icps::error_t rotate(int32_t velocity); |
|||
icps::error_t moveTo(int32_t pos); |
|||
icps::error_t moveBy(int32_t pos); |
|||
icps::error_t runToHome(); |
|||
icps::error_t stop(); |
|||
icps::error_t clearException(); |
|||
|
|||
private: |
|||
void motor_moveTo(int32_t pos, int32_t velocity, int32_t acc, int32_t dec); |
|||
void motor_moveBy(int32_t pos, int32_t velocity, int32_t acc, int32_t dec); |
|||
void motor_rotate(int32_t velocity, int32_t acc, int32_t dec); |
|||
|
|||
void periodJob(ZHALCORE::Context* context); |
|||
void changeToState(state_t state); |
|||
|
|||
private: |
|||
bool motorIsStop(StepActionContext* context); |
|||
void regGpioIrqProcesser(ZGPIO::onirq_t irqprocesser) { m_gpioirqprocesser = irqprocesser; } |
|||
void clearGpioIrqProcesser() { m_gpioirqprocesser = NULL; } |
|||
}; |
|||
|
|||
} // namespace iflytop
|
|||
#endif
|
|||
@ -0,0 +1,117 @@ |
|||
#include "step_scheduler.hpp"
|
|||
|
|||
#include <stdint.h>
|
|||
#include <stdio.h>
|
|||
#include <string.h>
|
|||
|
|||
#include "sdk/hal/zhal.hpp"
|
|||
|
|||
using namespace iflytop; |
|||
using namespace std; |
|||
|
|||
void StepActionScheduler::initialize(int actionCacheSize, int call_period_ms) { |
|||
this->m_actionCacheSize = actionCacheSize; |
|||
m_actionCache = (StepAction *)calloc(1, sizeof(StepAction) * actionCacheSize); |
|||
m_actionNum = 0; |
|||
m_nowActionIndex = 0; |
|||
m_workingFlag = false; |
|||
|
|||
ZHALCORE::getInstance()->regPeriodJob([this](ZHALCORE::Context &con) { this->periodcall(con); }, call_period_ms); |
|||
} |
|||
|
|||
void StepActionScheduler::regDoneCallback(function<void()> callback) { done_callback = callback; } |
|||
void StepActionScheduler::regBreakCallback(function<void()> callback) { break_callback = callback; } |
|||
|
|||
void StepActionScheduler::pushAction(function<void(StepActionContext *)> action, function<bool(StepActionContext *)> isDone) { |
|||
ZASSERT(m_actionNum < m_actionCacheSize); |
|||
this->m_actionCache[m_actionNum].action = action; |
|||
this->m_actionCache[m_actionNum].isDone = isDone; |
|||
m_actionNum++; |
|||
} |
|||
|
|||
void StepActionScheduler::pushActionAction(function<void(StepActionContext *)> action) { |
|||
ZASSERT(m_actionNum < m_actionCacheSize); |
|||
this->m_actionCache[m_actionNum].action = action; |
|||
m_actionNum++; |
|||
} |
|||
void StepActionScheduler::pushActionIsDone(function<bool(StepActionContext *)> isDone) { |
|||
ZASSERT(m_actionNum > 0); |
|||
this->m_actionCache[m_actionNum - 1].isDone = isDone; |
|||
} |
|||
void StepActionScheduler::pushActionDone(function<void(StepActionContext *)> done) { |
|||
ZASSERT(m_actionNum > 0); |
|||
this->m_actionCache[m_actionNum - 1].done = done; |
|||
} |
|||
|
|||
void StepActionScheduler::startSchedule() { |
|||
m_nowActionIndex = 0; |
|||
for (int i = 0; i < m_actionNum; i++) { |
|||
m_actionCache[i].isDoActionFlag = false; |
|||
} |
|||
m_workingFlag = true; |
|||
} |
|||
|
|||
void StepActionScheduler::reset() { |
|||
m_nowActionIndex = 0; |
|||
m_actionNum = 0; |
|||
done_callback = nullptr; |
|||
break_callback = nullptr; |
|||
memset(m_actionCache, 0, sizeof(StepAction) * m_actionCacheSize); |
|||
} |
|||
|
|||
void StepActionScheduler::stop() { |
|||
if (m_workingFlag) { |
|||
if (done_callback) done_callback(); |
|||
} |
|||
m_workingFlag = false; |
|||
} |
|||
|
|||
void StepActionScheduler::breakSchedule() { |
|||
if (m_workingFlag) { |
|||
if (break_callback) break_callback(); |
|||
} |
|||
m_workingFlag = false; |
|||
} |
|||
|
|||
bool StepActionScheduler::isDone() { |
|||
if (m_workingFlag == false) { |
|||
return true; |
|||
} |
|||
if (m_nowActionIndex >= m_actionNum) { |
|||
return true; |
|||
} else { |
|||
return false; |
|||
} |
|||
} |
|||
|
|||
void StepActionScheduler::periodcall(ZHALCORE::Context &halcore_con) { |
|||
if (m_workingFlag == false) { |
|||
return; |
|||
} |
|||
|
|||
if (m_nowActionIndex >= m_actionNum) { |
|||
return; |
|||
} |
|||
|
|||
StepActionContext context = {0}; |
|||
context.coreContext = &halcore_con; |
|||
|
|||
if (m_actionCache[m_nowActionIndex].isDoActionFlag == false) { |
|||
m_actionCache[m_nowActionIndex].action(&context); |
|||
m_actionCache[m_nowActionIndex].isDoActionFlag = true; |
|||
} else { |
|||
if (m_actionCache[m_nowActionIndex].isDone(&context)) { |
|||
if (m_actionCache[m_nowActionIndex].done) { |
|||
m_actionCache[m_nowActionIndex].done(&context); |
|||
} |
|||
m_nowActionIndex++; |
|||
} |
|||
} |
|||
if (context.breakflag) { |
|||
if (break_callback) break_callback(); |
|||
m_workingFlag = false; |
|||
} else if (m_nowActionIndex >= m_actionNum) { |
|||
if (done_callback) done_callback(); |
|||
m_workingFlag = false; |
|||
} |
|||
} |
|||
@ -0,0 +1,57 @@ |
|||
#pragma once
|
|||
#include <stdlib.h>
|
|||
|
|||
#include <functional>
|
|||
|
|||
#include "sdk/hal/zhal.hpp"
|
|||
|
|||
namespace iflytop { |
|||
using namespace std; |
|||
class StepActionContext { |
|||
public: |
|||
bool breakflag = false; |
|||
ZHALCORE::Context *coreContext; |
|||
}; |
|||
|
|||
class StepAction { |
|||
public: |
|||
function<void(StepActionContext *)> action = {nullptr}; |
|||
function<bool(StepActionContext *)> isDone = {nullptr}; |
|||
function<void(StepActionContext *)> done = {nullptr}; |
|||
bool isDoActionFlag = false; |
|||
}; |
|||
|
|||
class StepActionScheduler { |
|||
public: |
|||
StepAction *m_actionCache = NULL; |
|||
int m_actionCacheSize = 0; |
|||
|
|||
int m_nowActionIndex = 0; |
|||
int m_actionNum = 0; |
|||
bool m_workingFlag = false; |
|||
|
|||
function<void()> done_callback; |
|||
function<void()> break_callback; |
|||
|
|||
public: |
|||
void initialize(int actionCacheSize, int call_period_ms); |
|||
|
|||
void pushAction(function<void(StepActionContext *)> action, function<bool(StepActionContext *)> isDone); |
|||
|
|||
void pushActionAction(function<void(StepActionContext *)> action); |
|||
void pushActionIsDone(function<bool(StepActionContext *)> isDone); |
|||
void pushActionDone(function<void(StepActionContext *)> done); |
|||
|
|||
void regDoneCallback(function<void()> callback); // 自然结束，或者调用stop结束
|
|||
void regBreakCallback(function<void()> callback); |
|||
|
|||
void startSchedule(); |
|||
void reset(); |
|||
void stop(); |
|||
void breakSchedule(); |
|||
|
|||
private: |
|||
bool isDone(); |
|||
void periodcall(ZHALCORE::Context &con); |
|||
}; |
|||
} // namespace iflytop
|
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue