project_boditech_vidas_a8000_v3
/
a8000_subboard_sdk
2
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

master
zhaohe 2 years ago
parent
998c09f13e
commit
0f5a291b03
4 changed files with 145 additions and 20 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. 121
      components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
  2. 40
      components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
  3. 2
      components/zprotocols/zcancmder
  4. 2
      components/zprotocols/zcancmder_v2

121
components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
View File

@ -7,16 +7,17 @@
using namespace iflytop;
using namespace std;
#define TAG "XYRobotCtrlModule"
void XYRobotCtrlModule::initialize(IStepperMotor* stepM1, //
IStepperMotor* stepM2, //
ZGPIO* gpiotable, //
void XYRobotCtrlModule::initialize(int32_t id, IStepperMotor* stepM1, //
IStepperMotor* stepM2, //
ZGPIO* Xgpio, //
int ngpio, const char* flashmark) {
m_stepM1 = stepM1;
m_stepM2 = stepM2;
m_Xgpio = &gpiotable[0];
m_Ygpio = &gpiotable[1];
m_gpiotable = gpiotable;
m_Xgpio = &Xgpio[0];
m_Ygpio = &Xgpio[1];
m_gpiotable = Xgpio;
m_ngpio = ngpio;
m_moduleId = id;
ZASSERT(m_stepM1);
ZASSERT(m_stepM2);
ZASSERT(m_Xgpio);
@ -35,7 +36,7 @@ void XYRobotCtrlModule::initialize(IStepperMotor* stepM1, //
m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal);
}
int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, int speed, action_cb_status_t status_cb) {
int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, int32_t speed, action_cb_status_t status_cb) {
zlock_guard lock(m_lock);
ZLOGI(TAG, "move_to x:%d y:%d", x, y);
int runspeed = speed;
@ -75,7 +76,7 @@ int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, int speed, action_cb_st
return 0;
}
int32_t XYRobotCtrlModule::move_by_no_limit(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) {
int32_t XYRobotCtrlModule::move_by_no_limit(int32_t dx, int32_t dy, int32_t speed, action_cb_status_t status_cb) {
zlock_guard lock(m_lock);
ZLOGI(TAG, "move_by_no_limit dx:%d dy:%d", dx, dy);
@ -107,7 +108,7 @@ int32_t XYRobotCtrlModule::move_by_no_limit(int32_t dx, int32_t dy, int speed, a
return 0;
};
int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) {
int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, int32_t speed, action_cb_status_t status_cb) {
zlock_guard lock(m_lock);
ZLOGI(TAG, "move_by dx:%d dy:%d %d", dx, dy, speed);
@ -526,7 +527,7 @@ void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t
}
x += m_basecfg.shift_x;
y += m_basecfg.shift_y;
if (m_basecfg.x_shaft) x = -x;
if (m_basecfg.y_shaft) y = -y;
}
@ -594,4 +595,102 @@ int32_t XYRobotCtrlModule::factory_reset() { //
}
active_cfg();
return 0;
};
};
int32_t XYRobotCtrlModule::getid(int32_t* id) {
*id = m_moduleId;
return 0;
}
int32_t XYRobotCtrlModule::module_stop() {
ZLOGI(TAG, "module_stop");
m_thread.stop();
_motor_stop();
return 0;
}
int32_t XYRobotCtrlModule::module_break() {
ZLOGI(TAG, "module_break");
m_thread.stop();
_motor_stop(m_basecfg.breakdec);
return 0;
}
int32_t XYRobotCtrlModule::module_get_last_exec_status(int32_t* status) {
*status = 0;
return 0;
}
int32_t XYRobotCtrlModule::module_get_status(int32_t* status) {
*status = m_thread.isworking() ? 0x01 : 0x00;
return 0;
}
int32_t XYRobotCtrlModule::module_get_error(int32_t* iserror) {
*iserror = 0;
return 0;
}
int32_t XYRobotCtrlModule::module_clear_error() { return 0; }
int32_t XYRobotCtrlModule::module_set_param(int32_t param_id, int32_t param_value) { return _module_set_or_get_param(true, param_id, param_value); }
int32_t XYRobotCtrlModule::module_get_param(int32_t param_id, int32_t* param_value) { return _module_set_or_get_param(false, param_id, *param_value); }
#define SET_CONFIG(param_id, configval, param_value) \
case param_id: \
if (set) { \
configval = param_value; \
} else { \
param_value = configval; \
} \
break;
int32_t XYRobotCtrlModule::_module_set_or_get_param(bool set, int32_t param_id, int32_t& param_value) {
switch (param_id) {
SET_CONFIG(kcfg_motor_x_shift, m_basecfg.shift_x, param_value);
SET_CONFIG(kcfg_motor_y_shift, m_basecfg.shift_y, param_value);
SET_CONFIG(kcfg_motor_x_shaft, m_basecfg.x_shaft, param_value);
SET_CONFIG(kcfg_motor_y_shaft, m_basecfg.y_shaft, param_value);
SET_CONFIG(kcfg_motor_x_one_circle_pulse, m_basecfg.distance_scale, param_value);
SET_CONFIG(kcfg_motor_y_one_circle_pulse, m_basecfg.distance_scale, param_value);
SET_CONFIG(kcfg_motor_default_velocity, m_basecfg.maxspeed, param_value);
SET_CONFIG(kcfg_motor_default_acc, m_basecfg.acc, param_value);
SET_CONFIG(kcfg_motor_default_dec, m_basecfg.dec, param_value);
SET_CONFIG(kcfg_motor_default_break_dec, m_basecfg.breakdec, param_value);
SET_CONFIG(kcfg_motor_run_to_zero_max_x_d, m_basecfg.run_to_zero_max_d, param_value);
SET_CONFIG(kcfg_motor_run_to_zero_max_y_d, m_basecfg.run_to_zero_max_d, param_value);
SET_CONFIG(kcfg_motor_look_zero_edge_max_x_d, m_basecfg.look_zero_edge_max_d, param_value);
SET_CONFIG(kcfg_motor_look_zero_edge_max_y_d, m_basecfg.look_zero_edge_max_d, param_value);
SET_CONFIG(kcfg_motor_run_to_zero_speed, m_basecfg.run_to_zero_speed, param_value);
SET_CONFIG(kcfg_motor_run_to_zero_dec, m_basecfg.run_to_zero_dec, param_value);
SET_CONFIG(kcfg_motor_look_zero_edge_speed, m_basecfg.look_zero_edge_speed, param_value);
SET_CONFIG(kcfg_motor_look_zero_edge_dec, m_basecfg.look_zero_edge_dec, param_value);
SET_CONFIG(k_cfg_stepmotor_ihold, m_basecfg.ihold, param_value);
SET_CONFIG(k_cfg_stepmotor_irun, m_basecfg.irun, param_value);
SET_CONFIG(k_cfg_stepmotor_iholddelay, m_basecfg.iholddelay, param_value);
SET_CONFIG(k_cfg_xyrobot_robot_type, m_basecfg.robot_type, param_value);
default:
return err::kmodule_not_find_config_index;
}
return 0;
}
int32_t XYRobotCtrlModule::module_readio(int32_t* io) {
*io = 0;
for (int i = 0; i < m_ngpio; i++) {
if (i > 32) break;
if (m_gpiotable[i].getState()) *io |= (1 << i);
}
return 0;
}
int32_t XYRobotCtrlModule::module_writeio(int32_t io) { return 0; }
int32_t XYRobotCtrlModule::module_read_adc(int32_t adcindex, int32_t* adc) {
*adc = 0;
return 0;
}
int32_t XYRobotCtrlModule::module_factory_reset() { return factory_reset(); }
int32_t XYRobotCtrlModule::module_flush_cfg() { return flush(); }
int32_t XYRobotCtrlModule::module_active_cfg() {
active_cfg();
return 0;
}
int32_t XYRobotCtrlModule::xymotor_enable(int32_t varenable) {
enable(varenable);
return 0;
}
int32_t XYRobotCtrlModule::xymotor_move_by(int32_t dx, int32_t dy, int32_t motor_velocity) { return move_by(dx, dy, motor_velocity, nullptr); }
int32_t XYRobotCtrlModule::xymotor_move_to(int32_t x, int32_t y, int32_t motor_velocity) { return move_to(x, y, motor_velocity, nullptr); }
int32_t XYRobotCtrlModule::xymotor_move_to_zero() { return move_to_zero(nullptr); }

40
components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
View File

@ -4,9 +4,10 @@
#include "sdk\components\tmc\basic\tmc_ic_interface.hpp"
#include "sdk\components\zcancmder\zcanreceiver.hpp"
#include "sdk\components\zprotocols\zcancmder\api\i_xyrobot_ctrl_module.hpp"
#include "sdk\components\zprotocols\zcancmder_v2\api\api.hpp"
namespace iflytop {
class XYRobotCtrlModule : public I_XYRobotCtrlModule {
class XYRobotCtrlModule : public I_XYRobotCtrlModule, public ZIXYMotor, public ZIModule {
public:
private:
IStepperMotor* m_stepM1;
@ -20,6 +21,8 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule {
ZThread m_thread;
int32_t m_moduleId;
int m_x = 0;
int m_y = 0;
@ -38,14 +41,14 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule {
// zmutex m_statu_lock;
public:
void initialize(IStepperMotor* stepM1, //
IStepperMotor* stepM2, //
ZGPIO* Xgpio, //
void initialize(int32_t id, IStepperMotor* stepM1, //
IStepperMotor* stepM2, //
ZGPIO* Xgpio, //
int ngpio, const char* flashmark);
virtual int32_t move_to(int32_t x, int32_t y, int speed, action_cb_status_t status_cb) override;
virtual int32_t move_by(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) override;
virtual int32_t move_by_no_limit(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) override;
virtual int32_t move_to(int32_t x, int32_t y, int32_t speed, action_cb_status_t status_cb) override;
virtual int32_t move_by(int32_t dx, int32_t dy, int32_t speed, action_cb_status_t status_cb) override;
virtual int32_t move_by_no_limit(int32_t dx, int32_t dy, int32_t speed, action_cb_status_t status_cb) override;
virtual int32_t move_to_zero(action_cb_status_t status_cb) override;
virtual int32_t move_to_zero_with_calibrate(int32_t x, int32_t y, action_cb_status_t status_cb) override;
@ -67,7 +70,30 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule {
void loop();
void dumpcfg();
virtual int32_t getid(int32_t* id);
virtual int32_t module_stop();
virtual int32_t module_break();
virtual int32_t module_get_last_exec_status(int32_t* status);
virtual int32_t module_get_status(int32_t* status);
virtual int32_t module_get_error(int32_t* iserror);
virtual int32_t module_clear_error();
virtual int32_t module_set_param(int32_t param_id, int32_t param_value);
virtual int32_t module_get_param(int32_t param_id, int32_t* param_value);
virtual int32_t module_readio(int32_t* io);
virtual int32_t module_writeio(int32_t io);
virtual int32_t module_read_adc(int32_t adcindex, int32_t* adc);
virtual int32_t module_factory_reset();
virtual int32_t module_flush_cfg();
virtual int32_t module_active_cfg();
virtual int32_t xymotor_enable(int32_t enable);
virtual int32_t xymotor_move_by(int32_t dx, int32_t dy, int32_t motor_velocity);
virtual int32_t xymotor_move_to(int32_t x, int32_t y, int32_t motor_velocity);
virtual int32_t xymotor_move_to_zero();
private:
virtual int32_t _module_set_or_get_param(bool set, int32_t param_id, int32_t& param_value);
void active_cfg();
void computeTargetMotorPos();
void getnowpos(int32_t& x, int32_t& y);

2
components/zprotocols/zcancmder

@ -1 +1 @@
Subproject commit 676b588a68b809d8b7c7db9ea377f52cef377885
Subproject commit 7a3d9800ce4e710385e513f2c430c267dabefc7b

2
components/zprotocols/zcancmder_v2

@ -1 +1 @@
Subproject commit 87d1a547ef95f5739c1177063dfb8b3db6b6df81
Subproject commit 850402ceaf70cdcabc1773af1ef4bc7019cada36
Write Preview
Loading…
Cancel
Save