5 changed files with 241 additions and 384 deletions
-
378components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
-
36components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
-
165components/step_motor_ctrl_module/step_motor_speed_ctrl_module.cpp
-
44components/step_motor_ctrl_module/step_motor_speed_ctrl_module.hpp
-
2components/zprotocols/zcancmder
@ -1,165 +0,0 @@ |
|||||
#include "step_motor_speed_ctrl_module.hpp"
|
|
||||
|
|
||||
#include "sdk/components/errorcode/errorcode.hpp"
|
|
||||
using namespace iflytop; |
|
||||
using namespace iflytop::zcr; |
|
||||
#define TAG "StepMotorSpeedCtrlModule"
|
|
||||
void StepMotorSpeedCtrlModule::initialize(int id, IStepperMotor *stepM1) { |
|
||||
m_id = id; |
|
||||
m_stepM1 = stepM1; |
|
||||
|
|
||||
m_lock.init(); |
|
||||
m_thread.init("StepMotorSpeedCtrlModule", 1024, osPriorityNormal); |
|
||||
} |
|
||||
|
|
||||
int32_t StepMotorSpeedCtrlModule::set_acc(uint8_t act, int32_t &acc) { |
|
||||
zlock_guard l(m_lock); |
|
||||
ZLOGI(TAG, "set_acc act:%d acc:%d", act, acc); |
|
||||
if (act == 0) { // read
|
|
||||
acc = m_cfg_acc; |
|
||||
} else if (act == 1) { // set
|
|
||||
m_cfg_acc = acc; |
|
||||
} else { |
|
||||
return err::kcommon_error_operation_not_support; |
|
||||
} |
|
||||
return 0; |
|
||||
} |
|
||||
|
|
||||
int32_t StepMotorSpeedCtrlModule::set_dec(uint8_t act, int32_t &dec) { |
|
||||
zlock_guard l(m_lock); |
|
||||
ZLOGI(TAG, "set_dec act:%d dec:%d", act, dec); |
|
||||
if (act == 0) { // read
|
|
||||
dec = m_cfg_dec; |
|
||||
} else if (act == 1) { // set
|
|
||||
m_cfg_dec = dec; |
|
||||
} else { |
|
||||
return err::kcommon_error_operation_not_support; |
|
||||
} |
|
||||
return 0; |
|
||||
} |
|
||||
int32_t StepMotorSpeedCtrlModule::set_break_dec(uint8_t act, int32_t &dec) { |
|
||||
zlock_guard l(m_lock); |
|
||||
ZLOGI(TAG, "set_break_dec act:%d dec:%d", act, dec); |
|
||||
if (act == 0) { // read
|
|
||||
dec = m_cfg_break_dec; |
|
||||
} else if (act == 1) { // set
|
|
||||
m_cfg_break_dec = dec; |
|
||||
} else { |
|
||||
return err::kcommon_error_operation_not_support; |
|
||||
} |
|
||||
return 0; |
|
||||
} |
|
||||
int32_t StepMotorSpeedCtrlModule::set_max_speed(uint8_t act, int32_t &speed) { |
|
||||
zlock_guard l(m_lock); |
|
||||
ZLOGI(TAG, "set_max_speed act:%d speed:%d", act, speed); |
|
||||
if (act == 0) { // read
|
|
||||
speed = m_cfg_max_speed; |
|
||||
} else if (act == 1) { // set
|
|
||||
m_cfg_max_speed = abs(speed); |
|
||||
} else { |
|
||||
return err::kcommon_error_operation_not_support; |
|
||||
} |
|
||||
return 0; |
|
||||
} |
|
||||
int32_t StepMotorSpeedCtrlModule::set_distance_scale(uint8_t act, float &distance_scale) { |
|
||||
zlock_guard l(m_lock); |
|
||||
ZLOGI(TAG, "set_distance_scale act:%d distance_scale:%f", act, distance_scale); |
|
||||
if (act == 0) { // read
|
|
||||
distance_scale = m_cfg_distance_scale; |
|
||||
} else if (act == 1) { // set
|
|
||||
m_cfg_distance_scale = distance_scale; |
|
||||
} else { |
|
||||
return err::kcommon_error_operation_not_support; |
|
||||
} |
|
||||
return 0; |
|
||||
} |
|
||||
int32_t StepMotorSpeedCtrlModule::set_shaft(uint8_t act, bool &shaft) { |
|
||||
zlock_guard l(m_lock); |
|
||||
ZLOGI(TAG, "set_shaft act:%d shaft:%d", act, shaft); |
|
||||
if (act == 0) { // read
|
|
||||
shaft = m_x_shaft; |
|
||||
} else if (act == 1) { // set
|
|
||||
m_x_shaft = shaft; |
|
||||
m_stepM1->setMotorShaft(shaft); |
|
||||
} else { |
|
||||
return err::kcommon_error_operation_not_support; |
|
||||
} |
|
||||
return 0; |
|
||||
} |
|
||||
|
|
||||
int32_t StepMotorSpeedCtrlModule::enable(bool venable) { |
|
||||
zlock_guard l(m_lock); |
|
||||
ZLOGI(TAG, "enable venable:%d", venable); |
|
||||
m_stepM1->enable(venable); |
|
||||
return 0; |
|
||||
} |
|
||||
int32_t StepMotorSpeedCtrlModule::stop() { |
|
||||
zlock_guard l(m_lock); |
|
||||
ZLOGI(TAG, "stop"); |
|
||||
m_thread.stop(); |
|
||||
m_stepM1->stop(); |
|
||||
return 0; |
|
||||
} |
|
||||
int32_t StepMotorSpeedCtrlModule::brake() { |
|
||||
zlock_guard l(m_lock); |
|
||||
ZLOGI(TAG, "brake"); |
|
||||
m_thread.stop(); |
|
||||
m_stepM1->setDeceleration(m_cfg_break_dec); |
|
||||
m_stepM1->rotate(0); |
|
||||
return 0; |
|
||||
} |
|
||||
int32_t StepMotorSpeedCtrlModule::rotate(int32_t velocity, int32_t acc, int32_t dec) { |
|
||||
zlock_guard l(m_lock); |
|
||||
ZLOGI(TAG, "rotate velocity:%d acc:%d dec:%d", velocity, acc, dec); |
|
||||
if (acc <= 0) acc = m_cfg_acc; |
|
||||
if (dec <= 0) dec = m_cfg_dec; |
|
||||
if (abs(velocity) > m_cfg_max_speed) { |
|
||||
ZLOGW(TAG, "velocity:%d > m_cfg_max_speed:%d", velocity, m_cfg_max_speed); |
|
||||
velocity = m_cfg_max_speed; |
|
||||
} |
|
||||
|
|
||||
m_stepM1->setAcceleration(acc); |
|
||||
m_stepM1->setDeceleration(dec); |
|
||||
m_stepM1->rotate(velocity); |
|
||||
return 0; |
|
||||
} |
|
||||
int32_t StepMotorSpeedCtrlModule::rotate_auto_stop(int32_t velocity, int32_t duration, int32_t acc, int32_t dec, |
|
||||
function<void(bool reachtime, int32_t has_rotate_duration)> callback) { |
|
||||
zlock_guard l(m_lock); |
|
||||
ZLOGI(TAG, "rotate_auto_stop velocity:%d duration:%d acc:%d dec:%d", velocity, duration, acc, dec); |
|
||||
|
|
||||
if (duration < 0) { |
|
||||
ZLOGW(TAG, "duration:%d < 0", duration); |
|
||||
return err::kcommon_error_param_out_of_range; |
|
||||
} |
|
||||
|
|
||||
if (acc <= 0) acc = m_cfg_acc; |
|
||||
if (dec <= 0) dec = m_cfg_dec; |
|
||||
if (abs(velocity) > m_cfg_max_speed) { |
|
||||
ZLOGW(TAG, "velocity:%d > m_cfg_max_speed:%d", velocity, m_cfg_max_speed); |
|
||||
velocity = m_cfg_max_speed; |
|
||||
} |
|
||||
|
|
||||
m_thread.stop(); |
|
||||
m_thread.start([this, duration, acc, dec, velocity, callback]() { |
|
||||
m_stepM1->setAcceleration(acc); |
|
||||
m_stepM1->setDeceleration(dec); |
|
||||
m_stepM1->rotate(velocity); |
|
||||
|
|
||||
int32_t startticket = zos_get_tick(); |
|
||||
bool reachtime = false; |
|
||||
|
|
||||
while (!m_thread.getExitFlag()) { |
|
||||
if (zos_haspassedms(startticket) > duration) { |
|
||||
reachtime = true; |
|
||||
m_stepM1->stop(); |
|
||||
break; |
|
||||
} |
|
||||
osDelay(1); |
|
||||
} |
|
||||
if (callback) callback(reachtime, zos_haspassedms(startticket)); |
|
||||
m_stepM1->stop(); |
|
||||
return; |
|
||||
}); |
|
||||
return 0; |
|
||||
} |
|
||||
@ -1,44 +0,0 @@ |
|||||
#pragma once
|
|
||||
//
|
|
||||
#include "sdk/os/zos.hpp"
|
|
||||
#include "sdk\components\tmc\basic\tmc_ic_interface.hpp"
|
|
||||
#include "sdk\components\zcancmder\zcanreceiver.hpp"
|
|
||||
|
|
||||
namespace iflytop { |
|
||||
class StepMotorSpeedCtrlModule { |
|
||||
public: |
|
||||
private: |
|
||||
IStepperMotor *m_stepM1; |
|
||||
int m_id = 0; |
|
||||
|
|
||||
ZThread m_thread; |
|
||||
|
|
||||
zmutex m_lock; |
|
||||
|
|
||||
int32_t m_cfg_acc = 350; |
|
||||
int32_t m_cfg_dec = 350; |
|
||||
int32_t m_cfg_break_dec = 350; |
|
||||
int32_t m_cfg_max_speed = 1000; |
|
||||
|
|
||||
float m_cfg_distance_scale = 853.3333f; // ²½½øµç»ú step/s ת»»³É rpm
|
|
||||
|
|
||||
bool m_x_shaft = false; |
|
||||
|
|
||||
public: |
|
||||
void initialize(int id, IStepperMotor *stepM1); |
|
||||
|
|
||||
int32_t set_acc(uint8_t act, int32_t &acc); |
|
||||
int32_t set_dec(uint8_t act, int32_t &dec); |
|
||||
int32_t set_break_dec(uint8_t act, int32_t &dec); |
|
||||
int32_t set_max_speed(uint8_t act, int32_t &speed); |
|
||||
int32_t set_distance_scale(uint8_t act, float &distance_scale); |
|
||||
int32_t set_shaft(uint8_t act, bool &shaft); |
|
||||
|
|
||||
int32_t enable(bool venable); |
|
||||
int32_t stop(); |
|
||||
int32_t brake(); |
|
||||
int32_t rotate(int32_t velocity, int32_t acc, int32_t dec); |
|
||||
int32_t rotate_auto_stop(int32_t velocity, int32_t duration, int32_t acc, int32_t dec, |
|
||||
function<void(bool reachtime, int32_t has_rotate_duration)> callback); |
|
||||
}; |
|
||||
} // namespace iflytop
|
|
||||
@ -1 +1 @@ |
|||||
Subproject commit f051569d8988645cdb1ffd5429a3b72629df6020 |
|
||||
|
Subproject commit ba0b50468b776c0bf4182300d80d5d973ebea058 |
||||
Write
Preview
Loading…
Cancel
Save
Reference in new issue