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