p_Intelligent_winding_robot_v2
/
Intelligent_winding_robot_main_board
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

master
zhaohe 2 years ago
parent
bab30dca57
commit
301a470e75
9 changed files with 506 additions and 380 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. 6
      .cproject
  2. 2
      .settings/language.settings.xml
  3. 2
      sdk
  4. 193
      usrc/app_zmodule_device_manager.cpp
  5. 78
      usrc/app_zmodule_device_manager.hpp
  6. 448
      usrc/intelligent_winding_robot_ctrl.cpp
  7. 151
      usrc/intelligent_winding_robot_ctrl.hpp
  8. 5
      usrc/main.cpp
  9. 1
      usrc/project_configs.h

6
.cproject
View File

@ -27,6 +27,7 @@
<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.2099520072" name="Cpu clock frequence" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="144" valueType="string"/>
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat.412061075" name="Use float with printf from newlib-nano (-u _printf_float)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat" useByScannerDiscovery="false" value="true" valueType="boolean"/>
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoscanffloat.1033151657" name="Use float with scanf from newlib-nano (-u _scanf_float)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoscanffloat" useByScannerDiscovery="false" value="true" valueType="boolean"/>
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp.584576254" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.runtimelibrary_cpp.value.standard_c_standard_cpp" valueType="enumerated"/>
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.488425717" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
<builder buildPath="${workspace_loc:/Intelligent_winding_robot_main_board}/Debug" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.1708127142" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.2063489662" name="MCU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler">
@ -79,7 +80,10 @@
<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS"/>
<listOptionValue builtIn="false" value="../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F"/>
</option>
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.nortti.853156576" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.nortti" useByScannerDiscovery="false" value="false" valueType="boolean"/>
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.nortti.853156576" name="Disable generation of information about every class with virtual functions (-fno-rtti)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.nortti" useByScannerDiscovery="false" value="false" valueType="boolean"/>
<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.otherflags.556924015" name="Other flags" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.otherflags" useByScannerDiscovery="true" valueType="stringList">
<listOptionValue builtIn="false" value="-fexceptions"/>
</option>
<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp.1241273646" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.input.cpp"/>
</tool>
<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.52815412" name="MCU GCC Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker"/>

2
.settings/language.settings.xml
View File

@ -5,7 +5,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1057391452571803630" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="978131512185940093" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>

2
sdk

@ -1 +1 @@
Subproject commit 74b48f099c773a5cd6d4f402a671746792e57851
Subproject commit d4c0048415ea76a2baf4cf731b7ae7d97298b22d

193
usrc/app_zmodule_device_manager.cpp
View File

@ -0,0 +1,193 @@
#include "app_zmodule_device_manager.hpp"
#include "sdk/os/zos.hpp"
#define TAG "APPDM"
#define DM ZModuleDeviceManager
using namespace iflytop;
void APPDM::initialize(IZcanCmderMaster *m_cancmder) { //
DM::initialize(m_cancmder);
}
void APPDM::registerModule(ZIModule *module) { DM::registerModule(module); }
/*******************************************************************************
* ZIModule *
*******************************************************************************/
#define DOCMD(exptr) \
{ \
int32_t errcode = exptr; \
if (errcode != 0) { \
ZLOGE(TAG, "do %s fail,errcode %s(%d)", #exptr, err::error2str(errcode), errcode); \
} \
return errcode; \
}
int32_t APPDM::module_stop(uint16_t id) { //
ZLOGI(TAG, "module_stop %d", id);
DOCMD(DM::module_stop(id));
}
int32_t APPDM::module_break(uint16_t id) { //
ZLOGI(TAG, "module_break %d", id);
DOCMD(DM::module_break(id));
}
int32_t APPDM::module_get_last_exec_status(uint16_t id, int32_t *status) { //
DOCMD(DM::module_get_last_exec_status(id, status));
}
int32_t APPDM::module_get_status(uint16_t id, int32_t *status) { //
DOCMD(DM::module_get_status(id, status));
}
int32_t APPDM::module_set_param(uint16_t id, int32_t param_id, int32_t param_value) { //
ZLOGI(TAG, "module_set_param %d %d", id, param_value);
DOCMD(DM::module_set_param(id, param_id, param_value));
}
int32_t APPDM::module_get_param(uint16_t id, int32_t param_id, int32_t *param_value) { //
DOCMD(DM::module_get_param(id, param_id, param_value));
}
int32_t APPDM::module_readio(uint16_t id, int32_t *io) { //
DOCMD(DM::module_readio(id, io));
}
int32_t APPDM::module_writeio(uint16_t id, int32_t io) { //
ZLOGI(TAG, "module_writeio %d %d", id, io);
DOCMD(DM::module_writeio(id, io));
}
int32_t APPDM::module_read_adc(uint16_t id, int32_t adcindex, int32_t *adc) { //
DOCMD(DM::module_read_adc(id, adcindex, adc));
}
int32_t APPDM::module_get_error(uint16_t id, int32_t *iserror) { //
DOCMD(DM::module_get_error(id, iserror));
}
int32_t APPDM::module_clear_error(uint16_t id) { //
ZLOGI(TAG, "module_clear_error %d", id);
DOCMD(DM::module_clear_error(id));
}
int32_t APPDM::module_set_inited_flag(uint16_t id, int32_t flag) { //
ZLOGI(TAG, "module_set_inited_flag %d %d", id, flag);
DOCMD(DM::module_set_inited_flag(id, flag));
}
int32_t APPDM::module_get_inited_flag(uint16_t id, int32_t *flag) { //
DOCMD(DM::module_get_inited_flag(id, flag));
}
int32_t APPDM::module_factory_reset(uint16_t id) { //
ZLOGI(TAG, "module_factory_reset %d", id);
DOCMD(DM::module_factory_reset(id));
}
int32_t APPDM::module_flush_cfg(uint16_t id) { //
ZLOGI(TAG, "module_flush_cfg %d", id);
DOCMD(DM::module_flush_cfg(id));
}
int32_t APPDM::module_active_cfg(uint16_t id) { //
ZLOGI(TAG, "module_active_cfg %d", id);
DOCMD(DM::module_active_cfg(id));
}
int32_t APPDM::module_set_state(uint16_t id, int32_t state_id, int32_t state_value) { //
ZLOGI(TAG, "module_set_state %d %d %d", id, state_id, state_value);
DOCMD(DM::module_set_state(id, state_id, state_value));
}
int32_t APPDM::module_get_state(uint16_t id, int32_t state_id, int32_t *state_value) { //
DOCMD(DM::module_get_state(id, state_id, state_value));
}
/*******************************************************************************
* ZIMotor *
*******************************************************************************/
int32_t APPDM::motor_enable(uint16_t id, int32_t enable) { //
ZLOGI(TAG, "motor_enable %d %d", id, enable);
DOCMD(DM::motor_enable(id, enable));
}
int32_t APPDM::motor_rotate(uint16_t id, int32_t direction, int32_t motor_velocity, int32_t acc) { //
ZLOGI(TAG, "motor_rotate %d %d %d %d", id, direction, motor_velocity, acc);
DOCMD(DM::motor_rotate(id, direction, motor_velocity, acc));
}
int32_t APPDM::motor_move_by(uint16_t id, int32_t distance, int32_t motor_velocity, int32_t acc) { //
ZLOGI(TAG, "motor_move_by %d %d %d %d", id, distance, motor_velocity, acc);
DOCMD(DM::motor_move_by(id, distance, motor_velocity, acc));
}
int32_t APPDM::motor_move_to(uint16_t id, int32_t position, int32_t motor_velocity, int32_t acc) { //
ZLOGI(TAG, "motor_move_to %d %d %d %d", id, position, motor_velocity, acc);
DOCMD(DM::motor_move_to(id, position, motor_velocity, acc));
}
int32_t APPDM::motor_rotate_acctime(uint16_t id, int32_t direction, int32_t motor_velocity, int32_t acctime) { //
ZLOGI(TAG, "motor_rotate_acctime %d %d %d %d", id, direction, motor_velocity, acctime);
DOCMD(DM::motor_rotate_acctime(id, direction, motor_velocity, acctime));
}
int32_t APPDM::motor_move_by_acctime(uint16_t id, int32_t distance, int32_t motor_velocity, int32_t acctime) { //
ZLOGI(TAG, "motor_move_by_acctime %d %d %d %d", id, distance, motor_velocity, acctime);
DOCMD(DM::motor_move_by_acctime(id, distance, motor_velocity, acctime));
}
int32_t APPDM::motor_move_to_acctime(uint16_t id, int32_t position, int32_t motor_velocity, int32_t acctime) { //
ZLOGI(TAG, "motor_move_to_acctime %d %d %d %d", id, position, motor_velocity, acctime);
DOCMD(DM::motor_move_to_acctime(id, position, motor_velocity, acctime));
}
int32_t APPDM::motor_move_to_zero_forward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { //
ZLOGI(TAG, "motor_move_to_zero_forward %d %d %d %d %d", id, findzerospeed, findzeroedge_speed, acc, overtime);
DOCMD(DM::motor_move_to_zero_forward(id, findzerospeed, findzeroedge_speed, acc, overtime));
}
int32_t APPDM::motor_move_to_zero_backward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { //
ZLOGI(TAG, "motor_move_to_zero_backward %d %d %d %d %d", id, findzerospeed, findzeroedge_speed, acc, overtime);
DOCMD(DM::motor_move_to_zero_backward(id, findzerospeed, findzeroedge_speed, acc, overtime));
}
int32_t APPDM::motor_move_to_with_torque(uint16_t id, int32_t direction, int32_t torque) { //
ZLOGI(TAG, "motor_move_to_with_torque %d %d %d", id, direction, torque);
DOCMD(DM::motor_move_to_with_torque(id, direction, torque));
}
int32_t APPDM::motor_read_pos(uint16_t id, int32_t *pos) { //
DOCMD(DM::motor_read_pos(id, pos));
}
int32_t APPDM::motor_set_current_pos_by_change_shift(uint16_t id, int32_t pos) { //
ZLOGI(TAG, "motor_set_current_pos_by_change_shift %d %d", id, pos);
DOCMD(DM::motor_set_current_pos_by_change_shift(id, pos));
}
int32_t APPDM::motor_move_to_zero_forward_and_calculated_shift(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { //
ZLOGI(TAG, "motor_move_to_zero_forward_and_calculated_shift %d %d %d %d %d", id, findzerospeed, findzeroedge_speed, acc, overtime);
DOCMD(DM::motor_move_to_zero_forward_and_calculated_shift(id, findzerospeed, findzeroedge_speed, acc, overtime));
}
int32_t APPDM::motor_move_to_zero_backward_and_calculated_shift(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { //
ZLOGI(TAG, "motor_move_to_zero_backward_and_calculated_shift %d %d %d %d %d", id, findzerospeed, findzeroedge_speed, acc, overtime);
DOCMD(DM::motor_move_to_zero_backward_and_calculated_shift(id, findzerospeed, findzeroedge_speed, acc, overtime));
}
/*******************************************************************************
* ZIXYMotor *
*******************************************************************************/
int32_t APPDM::xymotor_enable(uint16_t id, int32_t enable) { //
ZLOGI(TAG, "xymotor_enable %d %d", id, enable);
DOCMD(DM::xymotor_enable(id, enable));
}
int32_t APPDM::xymotor_move_by(uint16_t id, int32_t dx, int32_t dy, int32_t motor_velocity) { //
ZLOGI(TAG, "xymotor_move_by %d %d %d %d", id, dx, dy, motor_velocity);
DOCMD(DM::xymotor_move_by(id, dx, dy, motor_velocity));
}
int32_t APPDM::xymotor_move_to(uint16_t id, int32_t x, int32_t y, int32_t motor_velocity) { //
ZLOGI(TAG, "xymotor_move_to %d %d %d %d", id, x, y, motor_velocity);
DOCMD(DM::xymotor_move_to(id, x, y, motor_velocity));
}
int32_t APPDM::xymotor_move_to_zero(uint16_t id) { //
ZLOGI(TAG, "xymotor_move_to_zero %d", id);
DOCMD(DM::xymotor_move_to_zero(id));
}
int32_t APPDM::xymotor_move_to_zero_and_calculated_shift(uint16_t id) { //
ZLOGI(TAG, "xymotor_move_to_zero_and_calculated_shift %d", id);
DOCMD(DM::xymotor_move_to_zero_and_calculated_shift(id));
}
int32_t APPDM::xymotor_read_pos(uint16_t id, int32_t *x, int32_t *y) { //
DOCMD(DM::xymotor_read_pos(id, x, y));
}
int32_t APPDM::xymotor_calculated_pos_by_move_to_zero(uint16_t id) { //
ZLOGI(TAG, "xymotor_calculated_pos_by_move_to_zero %d", id);
DOCMD(DM::xymotor_calculated_pos_by_move_to_zero(id));
}

78
usrc/app_zmodule_device_manager.hpp
View File

@ -0,0 +1,78 @@
#pragma once
#include <map>
#include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_manager.hpp"
namespace iflytop {
class APPDM : public ZModuleDeviceManager {
private:
public:
void initialize(IZcanCmderMaster *m_cancmder);
void registerModule(ZIModule *module);
/*******************************************************************************
* ZIModule *
*******************************************************************************/
virtual int32_t module_stop(uint16_t id) override;
virtual int32_t module_break(uint16_t id) override;
virtual int32_t module_get_last_exec_status(uint16_t id, int32_t *status) override;
virtual int32_t module_get_status(uint16_t id, int32_t *status) override;
virtual int32_t module_set_param(uint16_t id, int32_t param_id, int32_t param_value) override;
virtual int32_t module_get_param(uint16_t id, int32_t param_id, int32_t *param_value) override;
virtual int32_t module_readio(uint16_t id, int32_t *io) override;
virtual int32_t module_writeio(uint16_t id, int32_t io) override;
virtual int32_t module_read_adc(uint16_t id, int32_t adcindex, int32_t *adc) override;
virtual int32_t module_get_error(uint16_t id, int32_t *iserror) override;
virtual int32_t module_clear_error(uint16_t id) override;
virtual int32_t module_set_inited_flag(uint16_t id, int32_t flag) override;
virtual int32_t module_get_inited_flag(uint16_t id, int32_t *flag) override;
virtual int32_t module_factory_reset(uint16_t id) override;
virtual int32_t module_flush_cfg(uint16_t id) override;
virtual int32_t module_active_cfg(uint16_t id) override;
virtual int32_t module_set_state(uint16_t id, int32_t state_id, int32_t state_value) override;
virtual int32_t module_get_state(uint16_t id, int32_t state_id, int32_t *state_value) override;
/*******************************************************************************
* ZIMotor *
*******************************************************************************/
virtual int32_t motor_enable(uint16_t id, int32_t enable) override;
virtual int32_t motor_rotate(uint16_t id, int32_t direction, int32_t motor_velocity, int32_t acc) override;
virtual int32_t motor_move_by(uint16_t id, int32_t distance, int32_t motor_velocity, int32_t acc) override;
virtual int32_t motor_move_to(uint16_t id, int32_t position, int32_t motor_velocity, int32_t acc) override;
virtual int32_t motor_rotate_acctime(uint16_t id, int32_t direction, int32_t motor_velocity, int32_t acctime) override;
virtual int32_t motor_move_by_acctime(uint16_t id, int32_t distance, int32_t motor_velocity, int32_t acctime) override;
virtual int32_t motor_move_to_acctime(uint16_t id, int32_t position, int32_t motor_velocity, int32_t acctime) override;
virtual int32_t motor_move_to_zero_forward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
virtual int32_t motor_move_to_zero_backward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
virtual int32_t motor_move_to_with_torque(uint16_t id, int32_t direction, int32_t torque) override;
virtual int32_t motor_read_pos(uint16_t id, int32_t *pos) override;
virtual int32_t motor_set_current_pos_by_change_shift(uint16_t id, int32_t pos) override;
virtual int32_t motor_move_to_zero_forward_and_calculated_shift(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
virtual int32_t motor_move_to_zero_backward_and_calculated_shift(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
/*******************************************************************************
* ZIXYMotor *
*******************************************************************************/
virtual int32_t xymotor_enable(uint16_t id, int32_t enable) override;
virtual int32_t xymotor_move_by(uint16_t id, int32_t dx, int32_t dy, int32_t motor_velocity) override;
virtual int32_t xymotor_move_to(uint16_t id, int32_t x, int32_t y, int32_t motor_velocity) override;
virtual int32_t xymotor_move_to_zero(uint16_t id) override;
virtual int32_t xymotor_move_to_zero_and_calculated_shift(uint16_t id) override;
virtual int32_t xymotor_read_pos(uint16_t id, int32_t *x, int32_t *y) override;
virtual int32_t xymotor_calculated_pos_by_move_to_zero(uint16_t id) override;
private:
};
} // namespace iflytop

448
usrc/intelligent_winding_robot_ctrl.cpp
View File

@ -1,11 +1,14 @@
#include "intelligent_winding_robot_ctrl.hpp"
#include <stdarg.h>
#include <stdlib.h>
#include <string.h>
#include <exception>
using namespace std;
using namespace iflytop;
#define TAG "IntelligentWindingRobotCtrl"
#define TAG "APPDM"
#define DO(exptr) \
{ \
@ -15,8 +18,97 @@ using namespace iflytop;
} \
}
#define PARAM int32_t paramN, const char **paraV, ICmdParserACK *ack
void IntelligentWindingRobotCtrl::processError(int32_t err) { ZLOGE(TAG, "processError: %d", err); }
class myexception : public exception {
virtual const char* what() const throw() { return "My exception happened"; }
};
int32_t IntelligentWindingRobotCtrl::initialize(APPDM* dm, ICmdParser* cmdparse) {
m_dm = dm;
m_cmdparse = cmdparse;
initialize_device();
regcb();
return 0;
}
int32_t IntelligentWindingRobotCtrl::initialize_device() {
cfg.xy_platform_takeline_clip00_pos_x = 1736;
cfg.xy_platform_takeline_clip00_pos_y = 16853;
cfg.xy_platform_takeline_clipXX_pos_x = 52307;
cfg.xy_platform_takeline_clipXX_pos_y = 31993;
cfg.clip_line = 12;
cfg.clip_each_line_num = 5;
cfg.m11_arm_jiaxian_reset_pos = 2619;
cfg.m11_arm_jiaxian_clamp_torque = 330;
cfg.m11_arm_jiaxian_clamp_direction = -1;
cfg.m12_jiaxian_reset_pos = 2600;
cfg.m12_jiaxian_clamp_direction = -1;
cfg.m12_jiaxian_clamp_torque = 330;
cfg.m13_yaxian_forward_reset_pos = 1015;
cfg.m13_yaxian_backward_reset_pos = 2885;
cfg.m13_jiaxian_clamp_direction = -1;
cfg.m13_jiaxian_clamp_torque = 200;
cfg.m14_raoxiantance_reset_pos = 2047;
cfg.m14_raoxiantance_tance_zero_pos = 2517;
cfg.m15_paifei_reset_pos = 2046;
cfg.m15_paifei_press_direction = 1;
cfg.m15_paifei_press_torque = 330;
cfg.m16_xianlajin_reset_pos = 2047;
cfg.m16_xianlajin_tight_line_pos = 1966;
cfg.m16_xianlajin_winding_low_pos = 1900;
cfg.m16_xianlajin_winding_up_pos = 1865;
cfg.m16_xianlajin_line_entry_pos = 1833;
cfg.m21_arm_hook_claws_full_pos = 2558;
cfg.m21_arm_hook_claws_half_pos = 2294;
cfg.z_axis_take_clip_pos = 6924;
return 0;
}
void IntelligentWindingRobotCtrl::regcb() {
// device_reset
m_cmdparse->regCMD("device_reset", "()", 0, [this](PARAM) { device_reset(); });
m_cmdparse->regCMD("xy_run_to_clip_pos_test", "()", 1, [this](PARAM) { return xy_run_to_clip_pos_test(atoi(paraV[0])); });
m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); });
m_cmdparse->regCMD("app_m11_arm_jiaxian_move_to_reset_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_move_to_reset_pos(); });
m_cmdparse->regCMD("app_m11_arm_jiaxian_move_to_clamp_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_move_to_clamp_pos(); });
m_cmdparse->regCMD("app_m12_jiaxian_move_to_reset_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_reset_pos(); });
m_cmdparse->regCMD("app_m12_jiaxian_move_to_clamp_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_clamp_pos(); });
m_cmdparse->regCMD("app_m13_yaxian_move_to_reset_forward", "()", 0, [this](PARAM) { return m13_yaxian_move_to_reset_forward(); });
m_cmdparse->regCMD("app_m13_yaxian_move_to_reset_backward", "()", 0, [this](PARAM) { return m13_yaxian_move_to_reset_backward(); });
m_cmdparse->regCMD("app_m13_yaxian_press_clip", "()", 0, [this](PARAM) { return m13_yaxian_press_clip(); });
m_cmdparse->regCMD("app_m14_raoxiantance_move_to_reset", "()", 0, [this](PARAM) { return m14_raoxiantance_move_to_reset(); });
m_cmdparse->regCMD("app_m15_paifei_moveto_reset", "()", 0, [this](PARAM) { return m15_paifei_moveto_reset(); });
m_cmdparse->regCMD("app_m15_paifei_moveto_press", "()", 0, [this](PARAM) { return m15_paifei_moveto_press(); });
m_cmdparse->regCMD("app_m16_xianlajin_move_to_reset", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_reset(); });
m_cmdparse->regCMD("app_m16_xianlajin_move_to_tight_line_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_tight_line_pos(); });
m_cmdparse->regCMD("app_m16_xianlajin_move_to_winding_low_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_winding_low_pos(); });
m_cmdparse->regCMD("app_m16_xianlajin_move_to_winding_up_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_winding_up_pos(); });
m_cmdparse->regCMD("app_m16_xianlajin_move_to_line_entry_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_line_entry_pos(); });
m_cmdparse->regCMD("app_m21_arm_hook_claws_reset", "()", 0, [this](PARAM) { return m21_arm_hook_claws_reset(); });
m_cmdparse->regCMD("app_m21_arm_hook_claws_move_to_half_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_half_pos(); });
m_cmdparse->regCMD("app_m21_arm_hook_claws_move_to_full_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_full_pos(); });
m_cmdparse->regCMD("app_m22_scissors_move_reset_pos", "()", 0, [this](PARAM) { return m22_scissors_move_reset_pos(); });
m_cmdparse->regCMD("app_m22_scissors_cut", "()", 0, [this](PARAM) { return m22_scissors_cut(); });
m_cmdparse->regCMD("app_m23_laxian_motor_move_to_reset_pos", "()", 0, [this](PARAM) { return m23_laxian_motor_move_to_reset_pos(); });
m_cmdparse->regCMD("app_m23_laxian_motor_move_to_tight_line_pos", "()", 0, [this](PARAM) { return m23_laxian_motor_move_to_tight_line_pos(); });
}
void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) {
zos_delay(100);
int i = 0;
@ -59,160 +151,33 @@ int32_t IntelligentWindingRobotCtrl::device_reset() {
// Z轴复位
zreset();
m21_arm_hook_claws_reset();
m11_arm_jiaxian_duoji_move_to_reset_pos();
m11_arm_jiaxian_move_to_reset_pos();
xy_reset();
ZLOGI(TAG, "device_reset finished....");
}
int32_t IntelligentWindingRobotCtrl::initialize_device() { return 0; }
// 排废舵机
int32_t IntelligentWindingRobotCtrl::m15_paifei_duoji_moveto_reset() {
ZLOGI(TAG, "m15_paifei_duoji_moveto_reset %d %d %d", 15, cfg.paifei_duoji_reset_pos, 330);
DO(m_dm->motor_move_to(15, cfg.paifei_duoji_reset_pos, 2000, 0));
wait_module_idle(15);
return 0;
}
int32_t IntelligentWindingRobotCtrl::m15_paifei_duoji_moveto_press() {
ZLOGI(TAG, "m15_paifei_duoji_moveto_press %d %d %d", 15, cfg.paifei_duoji_press_pos, cfg.paifei_duoji_press_torque);
DO(m_dm->motor_move_to_with_torque(15, cfg.paifei_duoji_press_pos, cfg.paifei_duoji_press_torque));
wait_module_idle(15);
}
/**
* @brief 绕线探测舵机
*/
int32_t IntelligentWindingRobotCtrl::m14_raoxiantance_duoji_move_to_reset() {
ZLOGI(TAG, "m14_raoxiantance_duoji_move_to_reset %d %d %d", 14, cfg.raoxiantance_duoji_reset_pos, 330);
DO(m_dm->motor_move_to(14, cfg.raoxiantance_duoji_reset_pos, 2000, 0));
wait_module_idle(14);
return 0;
}
int32_t IntelligentWindingRobotCtrl::m14_raoxiantance_duoji_move_to_get_thickness(int32_t* thickness) {
ZLOGI(TAG, "m14_raoxiantance_duoji_move_to_get_thickness %d %d", 14, cfg.raoxiantance_duoji_tance_zero_pos);
DO(m_dm->motor_move_to_with_torque(14, cfg.raoxiantance_duoji_tance_zero_pos, 200));
zos_delay(2000);
int32_t nowpos = 0;
DO(m_dm->motor_read_pos(14, &nowpos));
*thickness = cfg.raoxiantance_duoji_tance_zero_pos - nowpos;
DO(m14_raoxiantance_duoji_move_to_reset());
return 0;
}
/**
* @brief 压线舵机
*/
int32_t IntelligentWindingRobotCtrl::m13_yaxian_duoji_move_to_reset() {
ZLOGI(TAG, "m13_yaxian_duoji_move_to_reset %d %d %d", 13, cfg.yaxian_duoji_reset_pos, 330);
DO(m_dm->motor_move_to(13, cfg.yaxian_duoji_reset_pos, 2000, 0));
wait_module_idle(13);
return 0;
}
int32_t IntelligentWindingRobotCtrl::m13_yaxian_duoji_move_to_press_pos() {
ZLOGI(TAG, "m13_yaxian_duoji_move_to_press_pos %d %d %d", 13, cfg.yaxian_duoji_press_pos, cfg.yaxian_duoji_press_torque);
DO(m_dm->motor_move_to_with_torque(13, cfg.yaxian_duoji_press_pos, cfg.yaxian_duoji_press_torque));
wait_module_idle(13);
return 0;
}
int32_t IntelligentWindingRobotCtrl::m13_yaxian_duoji_move_to_wait_for_press_pos() {
ZLOGI(TAG, "m13_yaxian_duoji_move_to_wait_for_press_pos %d %d %d", 13, cfg.yaxian_duoji_wait_for_press_pos, cfg.yaxian_duoji_press_torque);
DO(m_dm->motor_move_to_with_torque(13, cfg.yaxian_duoji_wait_for_press_pos, cfg.yaxian_duoji_press_torque));
wait_module_idle(13);
return 0;
}
/**
* @brief 线拉紧舵机
*/
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_duoji_move_to_reset() {
ZLOGI(TAG, "m16_xianlajin_duoji_move_to_reset %d %d %d", 12, cfg.xianlajin_duoji_reset_pos, 330);
DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_reset_pos, 2000, 0));
wait_module_idle(16);
return 0;
} // 零位
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_duoji_move_to_line_entry_pos() {
ZLOGI(TAG, "m16_xianlajin_duoji_move_to_line_entry_pos %d %d %d", 16, cfg.xianlajin_duoji_line_entry_pos, 330);
DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_line_entry_pos, 2000, 0));
wait_module_idle(16);
return 0;
} // 入线位
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_duoji_move_to_tight_line_pos() {
ZLOGI(TAG, "m16_xianlajin_duoji_move_to_line_entry_pos %d %d %d", 16, cfg.xianlajin_duoji_tight_line_pos, 330);
DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_tight_line_pos, 2000, 0));
wait_module_idle(16);
return 0;
} // 拉线位置
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_duoji_move_to_loose_line_pos() {
ZLOGI(TAG, "m16_xianlajin_duoji_move_to_loose_line_pos %d %d %d", 16, cfg.xianlajin_duoji_loose_line_pos, 330);
DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_loose_line_pos, 2000, 0));
wait_module_idle(16);
return 0;
} // 放线位置
/**
* @brief 夹线舵机12
*/
int32_t IntelligentWindingRobotCtrl::m12_jiaxian_duoji_move_to_reset_pos() {
ZLOGI(TAG, "m12_jiaxian_duoji_move_to_reset_pos %d %d %d", 12, cfg.jiaxian_duoji_reset_pos, 330);
DO(m_dm->motor_move_to(12, cfg.jiaxian_duoji_reset_pos, 2000, 0));
wait_module_idle(12);
return 0;
}
int32_t IntelligentWindingRobotCtrl::m12_jiaxian_duoji_move_to_clamp_pos() {
ZLOGI(TAG, "m12_jiaxian_duoji_move_to_clamp_pos %d %d %d", 12, cfg.jiaxian_duoji_clamp_pos, cfg.jiaxian_duoji_clamp_torque);
DO(m_dm->motor_move_to_with_torque(12, cfg.jiaxian_duoji_clamp_pos, cfg.jiaxian_duoji_clamp_torque));
wait_module_idle(12);
return 0;
}
/**
* @brief 剪刀
*/
int32_t IntelligentWindingRobotCtrl::m22_scissors_move_reset_pos() {
// ZLOGI(TAG, "m22_scissors_move_reset_pos %d", 11);
ZLOGI(TAG, "m22_scissors_move_reset_pos");
return 0;
} // block
int32_t IntelligentWindingRobotCtrl::m22_scissors_cut() {
ZLOGI(TAG, "m22_scissors_cut %d", 22);
DO(m_dm->motor_move_by(22, 4095, 0, 0));
wait_module_idle(22);
return 0;
} // block
/**
* @brief 机械臂夹线舵机
*/
int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_duoji_move_to_reset_pos() {
ZLOGI(TAG, "m11_arm_jiaxian_duoji_move_to_reset_pos");
DO(m_dm->motor_move_to(11, cfg.arm_jiaxian_duoji_reset_pos, 2000, 0));
wait_module_idle(11);
return 0;
}
int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_duoji_move_to_clamp_pos() {
ZLOGI(TAG, "m11_arm_jiaxian_duoji_move_to_clamp_pos");
DO(m_dm->motor_move_to_with_torque(11, cfg.arm_jiaxian_duoji_clamp_direction, cfg.arm_jiaxian_duoji_clamp_torque));
wait_module_idle(11);
return 0;
}
/**
* @brief 机械臂钩爪
*/
int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_reset() {
ZLOGI(TAG, "m21_arm_hook_claws_reset");
DO(m_dm->motor_move_to_zero_backward(21, 0, 0, 0, 0));
wait_module_idle(21);
return 0;
}
int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_move_to_half_pos() {
ZLOGI(TAG, "m21_arm_hook_claws_move_to_half_pos");
DO(m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_half_pos, 0, 0));
wait_module_idle(21);
return 0;
}
int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_move_to_full_pos() {
ZLOGI(TAG, "m21_arm_hook_claws_move_to_full_pos");
DO(m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_full_pos, 0, 0));
wait_module_idle(21);
return 0;
}
int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_move_to_reset_pos() { return m_dm->motor_move_to(11, cfg.m11_arm_jiaxian_reset_pos, 2000, 0); }
int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_move_to_clamp_pos() { return m_dm->motor_move_to_with_torque(11, cfg.m11_arm_jiaxian_clamp_direction, cfg.m11_arm_jiaxian_clamp_torque); }
int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_reset_pos() { return m_dm->motor_move_to(12, cfg.m12_jiaxian_reset_pos, 2000, 0); }
int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_clamp_pos() { return m_dm->motor_move_to_with_torque(12, cfg.m12_jiaxian_clamp_direction, cfg.m12_jiaxian_clamp_torque); }
int32_t IntelligentWindingRobotCtrl::m13_yaxian_move_to_reset_forward() { return m_dm->motor_move_to(13, cfg.m13_yaxian_forward_reset_pos, 2000, 0); }
int32_t IntelligentWindingRobotCtrl::m13_yaxian_move_to_reset_backward() { return m_dm->motor_move_to(13, cfg.m13_yaxian_backward_reset_pos, 2000, 0); }
int32_t IntelligentWindingRobotCtrl::m13_yaxian_press_clip() { return m_dm->motor_move_to_with_torque(13, cfg.m13_jiaxian_clamp_direction, cfg.m13_jiaxian_clamp_torque); }
int32_t IntelligentWindingRobotCtrl::m14_raoxiantance_move_to_reset() { return m_dm->motor_move_to(14, cfg.m14_raoxiantance_reset_pos, 2000, 0); }
int32_t IntelligentWindingRobotCtrl::m15_paifei_moveto_reset() { return m_dm->motor_move_to(15, cfg.m15_paifei_reset_pos, 2000, 0); }
int32_t IntelligentWindingRobotCtrl::m15_paifei_moveto_press() { return m_dm->motor_move_to_with_torque(15, cfg.m15_paifei_press_direction, cfg.m15_paifei_press_torque); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_reset() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_reset_pos, 300, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_tight_line_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_tight_line_pos, 100, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_winding_low_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_winding_low_pos, 100, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_winding_up_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_winding_up_pos, 100, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_line_entry_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_line_entry_pos, 100, 0); }
int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_reset() { return m_dm->motor_move_to_zero_backward(21, 0, 0, 0, 0); }
int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_move_to_half_pos() { return m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_half_pos, 0, 0); }
int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_move_to_full_pos() { return m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_full_pos, 0, 0); }
int32_t IntelligentWindingRobotCtrl::m22_scissors_move_reset_pos() { return 0; }
int32_t IntelligentWindingRobotCtrl::m22_scissors_cut() { return m_dm->motor_move_to_with_torque(22, 1, 4095); }
int32_t IntelligentWindingRobotCtrl::m23_laxian_motor_move_to_reset_pos() { return m_dm->motor_move_to_zero_backward(23, 0, 0, 0, 0); }
int32_t IntelligentWindingRobotCtrl::m23_laxian_motor_move_to_tight_line_pos() { return m_dm->motor_move_to(23, 500, 0, 0); }
int32_t IntelligentWindingRobotCtrl::main_shaft_run() {
ZLOGI(TAG, "main_shaft_run");
@ -324,7 +289,7 @@ int32_t IntelligentWindingRobotCtrl::xy_run_to_clip_pos_test(int32_t clip_index)
return err::kparam_out_of_range;
}
zreset();
m11_arm_jiaxian_duoji_move_to_reset_pos();
m11_arm_jiaxian_move_to_reset_pos();
m21_arm_hook_claws_reset();
int32_t x = 0;
@ -342,44 +307,44 @@ int32_t IntelligentWindingRobotCtrl::xy_run_to_clip_pos_test(int32_t clip_index)
int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalue) {
#if 0
if (strcmp(cfgname, "paifei_duoji_reset_pos") == 0)
cfg.paifei_duoji_reset_pos = cfgvalue;
else if (strcmp(cfgname, "paifei_duoji_press_pos") == 0)
cfg.paifei_duoji_press_pos = cfgvalue;
else if (strcmp(cfgname, "paifei_duoji_press_torque") == 0)
cfg.paifei_duoji_press_torque = cfgvalue;
else if (strcmp(cfgname, "raoxiantance_duoji_reset_pos") == 0)
cfg.raoxiantance_duoji_reset_pos = cfgvalue;
else if (strcmp(cfgname, "raoxiantance_duoji_tance_zero_pos") == 0)
cfg.raoxiantance_duoji_tance_zero_pos = cfgvalue;
else if (strcmp(cfgname, "yaxian_duoji_reset_pos") == 0)
cfg.yaxian_duoji_reset_pos = cfgvalue;
else if (strcmp(cfgname, "yaxian_duoji_press_pos") == 0)
cfg.yaxian_duoji_press_pos = cfgvalue;
else if (strcmp(cfgname, "yaxian_duoji_press_torque") == 0)
cfg.yaxian_duoji_press_torque = cfgvalue;
else if (strcmp(cfgname, "yaxian_duoji_wait_for_press_pos") == 0)
cfg.yaxian_duoji_wait_for_press_pos = cfgvalue;
else if (strcmp(cfgname, "xianlajin_duoji_reset_pos") == 0)
cfg.xianlajin_duoji_reset_pos = cfgvalue;
else if (strcmp(cfgname, "xianlajin_duoji_line_entry_pos") == 0)
cfg.xianlajin_duoji_line_entry_pos = cfgvalue;
else if (strcmp(cfgname, "xianlajin_duoji_tight_line_pos") == 0)
cfg.xianlajin_duoji_tight_line_pos = cfgvalue;
else if (strcmp(cfgname, "xianlajin_duoji_loose_line_pos") == 0)
cfg.xianlajin_duoji_loose_line_pos = cfgvalue;
else if (strcmp(cfgname, "jiaxian_duoji_reset_pos") == 0)
cfg.jiaxian_duoji_reset_pos = cfgvalue;
else if (strcmp(cfgname, "jiaxian_duoji_clamp_pos") == 0)
cfg.jiaxian_duoji_clamp_pos = cfgvalue;
else if (strcmp(cfgname, "jiaxian_duoji_clamp_torque") == 0)
cfg.jiaxian_duoji_clamp_torque = cfgvalue;
else if (strcmp(cfgname, "arm_jiaxian_duoji_reset_pos") == 0)
cfg.arm_jiaxian_duoji_reset_pos = cfgvalue;
else if (strcmp(cfgname, "arm_jiaxian_duoji_clamp_pos") == 0)
cfg.arm_jiaxian_duoji_clamp_pos = cfgvalue;
else if (strcmp(cfgname, "arm_jiaxian_duoji_clamp_torque") == 0)
cfg.arm_jiaxian_duoji_clamp_torque = cfgvalue;
if (strcmp(cfgname, "paifei_reset_pos") == 0)
cfg.paifei_reset_pos = cfgvalue;
else if (strcmp(cfgname, "paifei_press_pos") == 0)
cfg.paifei_press_pos = cfgvalue;
else if (strcmp(cfgname, "paifei_press_torque") == 0)
cfg.paifei_press_torque = cfgvalue;
else if (strcmp(cfgname, "raoxiantance_reset_pos") == 0)
cfg.raoxiantance_reset_pos = cfgvalue;
else if (strcmp(cfgname, "raoxiantance_tance_zero_pos") == 0)
cfg.raoxiantance_tance_zero_pos = cfgvalue;
else if (strcmp(cfgname, "yaxian_reset_pos") == 0)
cfg.yaxian_reset_pos = cfgvalue;
else if (strcmp(cfgname, "yaxian_press_pos") == 0)
cfg.yaxian_press_pos = cfgvalue;
else if (strcmp(cfgname, "yaxian_press_torque") == 0)
cfg.yaxian_press_torque = cfgvalue;
else if (strcmp(cfgname, "yaxian_wait_for_press_pos") == 0)
cfg.yaxian_wait_for_press_pos = cfgvalue;
else if (strcmp(cfgname, "xianlajin_reset_pos") == 0)
cfg.xianlajin_reset_pos = cfgvalue;
else if (strcmp(cfgname, "xianlajin_line_entry_pos") == 0)
cfg.xianlajin_line_entry_pos = cfgvalue;
else if (strcmp(cfgname, "xianlajin_tight_line_pos") == 0)
cfg.xianlajin_tight_line_pos = cfgvalue;
else if (strcmp(cfgname, "xianlajin_loose_line_pos") == 0)
cfg.xianlajin_loose_line_pos = cfgvalue;
else if (strcmp(cfgname, "jiaxian_reset_pos") == 0)
cfg.jiaxian_reset_pos = cfgvalue;
else if (strcmp(cfgname, "jiaxian_clamp_pos") == 0)
cfg.jiaxian_clamp_pos = cfgvalue;
else if (strcmp(cfgname, "jiaxian_clamp_torque") == 0)
cfg.jiaxian_clamp_torque = cfgvalue;
else if (strcmp(cfgname, "arm_jiaxian_reset_pos") == 0)
cfg.arm_jiaxian_reset_pos = cfgvalue;
else if (strcmp(cfgname, "arm_jiaxian_clamp_pos") == 0)
cfg.arm_jiaxian_clamp_pos = cfgvalue;
else if (strcmp(cfgname, "arm_jiaxian_clamp_torque") == 0)
cfg.arm_jiaxian_clamp_torque = cfgvalue;
else if (strcmp(cfgname, "scissors_reset_pos") == 0)
cfg.scissors_reset_pos = cfgvalue;
else if (strcmp(cfgname, "m22_scissors_cut_pos") == 0)
@ -392,109 +357,8 @@ int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalu
#endif
return 0;
}
int32_t IntelligentWindingRobotCtrl::dumpcfg() {
#if 0
ZLOGI(TAG, "paifei_duoji_reset_pos %d", cfg.paifei_duoji_reset_pos);
ZLOGI(TAG, "paifei_duoji_press_pos %d", cfg.paifei_duoji_press_pos);
ZLOGI(TAG, "paifei_duoji_press_torque %d", cfg.paifei_duoji_press_torque);
ZLOGI(TAG, "raoxiantance_duoji_reset_pos %d", cfg.raoxiantance_duoji_reset_pos);
ZLOGI(TAG, "raoxiantance_duoji_tance_zero_pos %d", cfg.raoxiantance_duoji_tance_zero_pos);
ZLOGI(TAG, "yaxian_duoji_reset_pos %d", cfg.yaxian_duoji_reset_pos);
ZLOGI(TAG, "yaxian_duoji_press_pos %d", cfg.yaxian_duoji_press_pos);
ZLOGI(TAG, "yaxian_duoji_press_torque %d", cfg.yaxian_duoji_press_torque);
ZLOGI(TAG, "yaxian_duoji_wait_for_press_pos %d", cfg.yaxian_duoji_wait_for_press_pos);
ZLOGI(TAG, "xianlajin_duoji_reset_pos %d", cfg.xianlajin_duoji_reset_pos);
ZLOGI(TAG, "xianlajin_duoji_line_entry_pos %d", cfg.xianlajin_duoji_line_entry_pos);
ZLOGI(TAG, "xianlajin_duoji_tight_line_pos %d", cfg.xianlajin_duoji_tight_line_pos);
ZLOGI(TAG, "xianlajin_duoji_loose_line_pos %d", cfg.xianlajin_duoji_loose_line_pos);
ZLOGI(TAG, "jiaxian_duoji_reset_pos %d", cfg.jiaxian_duoji_reset_pos);
ZLOGI(TAG, "jiaxian_duoji_clamp_pos %d", cfg.jiaxian_duoji_clamp_pos);
ZLOGI(TAG, "jiaxian_duoji_clamp_torque %d", cfg.jiaxian_duoji_clamp_torque);
ZLOGI(TAG, "arm_jiaxian_duoji_reset_pos %d", cfg.arm_jiaxian_duoji_reset_pos);
ZLOGI(TAG, "arm_jiaxian_duoji_clamp_pos %d", cfg.arm_jiaxian_duoji_clamp_pos);
ZLOGI(TAG, "arm_jiaxian_duoji_clamp_torque %d", cfg.arm_jiaxian_duoji_clamp_torque);
ZLOGI(TAG, "scissors_reset_pos %d", cfg.scissors_reset_pos);
ZLOGI(TAG, "m22_scissors_cut_pos %d", cfg.m22_scissors_cut_pos);
ZLOGI(TAG, "m21_arm_hook_claws_half_pos %d", cfg.m21_arm_hook_claws_half_pos);
ZLOGI(TAG, "m21_arm_hook_claws_full_pos %d", cfg.m21_arm_hook_claws_full_pos);
#endif
return 0;
}
int32_t IntelligentWindingRobotCtrl::dumpcfg() { return 0; }
#if 1
int32_t IntelligentWindingRobotCtrl::initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse) {
m_dm = dm;
m_cmdparse = cmdparse;
cfg.xy_platform_takeline_clip00_pos_x = 1736;
cfg.xy_platform_takeline_clip00_pos_y = 16853;
cfg.xy_platform_takeline_clipXX_pos_x = 52307;
cfg.xy_platform_takeline_clipXX_pos_y = 31993;
cfg.clip_line = 12;
cfg.clip_each_line_num = 5;
cfg.arm_jiaxian_duoji_reset_pos = 2619;
cfg.arm_jiaxian_duoji_clamp_torque = 330;
cfg.arm_jiaxian_duoji_clamp_direction = -1;
cfg.z_axis_take_clip_pos = 6924;
cfg.m21_arm_hook_claws_full_pos = 2458;
cfg.m21_arm_hook_claws_half_pos = 2294;
regcb();
return 0;
}
#endif
#define PARAM int32_t paramN, const char **paraV, ICmdParserACK *ack
void IntelligentWindingRobotCtrl::regcb() {
// device_reset
m_cmdparse->regCMD("device_reset", "()", 0, [this](PARAM) { return device_reset(); });
m_cmdparse->regCMD("xy_run_to_clip_pos_test", "()", 1, [this](PARAM) { return xy_run_to_clip_pos_test(atoi(paraV[0])); });
m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); });
#if 0
m_cmdparse->regCMD("app_m15_paifei_duoji_moveto_reset", "()", 0, [this](PARAM) { return m15_paifei_duoji_moveto_reset(); });
m_cmdparse->regCMD("app_m15_paifei_duoji_moveto_press", "()", 0, [this](PARAM) { return m15_paifei_duoji_moveto_press(); });
m_cmdparse->regCMD("app_m14_raoxiantance_duoji_move_to_reset", "()", 0, [this](PARAM) { return m14_raoxiantance_duoji_move_to_reset(); });
m_cmdparse->regCMD("app_m14_raoxiantance_duoji_move_to_get_thickness", "()", 0, [this](PARAM) {
int32_t thickness = 0;
int32_t err = m14_raoxiantance_duoji_move_to_get_thickness(ack->getAck(1));
ack->acktype = ICmdParserACK::kAckType_int32;
ack->rawlen = sizeof(int32_t);
});
m_cmdparse->regCMD("app_m13_yaxian_duoji_move_to_reset", "()", 0, [this](PARAM) { return m13_yaxian_duoji_move_to_reset(); });
m_cmdparse->regCMD("app_m13_yaxian_duoji_move_to_press_pos", "()", 0, [this](PARAM) { return m13_yaxian_duoji_move_to_press_pos(); });
m_cmdparse->regCMD("app_m13_yaxian_duoji_move_to_wait_for_press_pos", "()", 0, [this](PARAM) { return m13_yaxian_duoji_move_to_wait_for_press_pos(); });
m_cmdparse->regCMD("app_m16_xianlajin_duoji_move_to_reset", "()", 0, [this](PARAM) { return m16_xianlajin_duoji_move_to_reset(); });
m_cmdparse->regCMD("app_m16_xianlajin_duoji_move_to_line_entry_pos", "()", 0, [this](PARAM) { return m16_xianlajin_duoji_move_to_line_entry_pos(); });
m_cmdparse->regCMD("app_m16_xianlajin_duoji_move_to_line_entry_pos", "()", 0, [this](PARAM) { return m16_xianlajin_duoji_move_to_line_entry_pos(); });
m_cmdparse->regCMD("app_m16_xianlajin_duoji_move_to_loose_line_pos", "()", 0, [this](PARAM) { return m16_xianlajin_duoji_move_to_loose_line_pos(); });
m_cmdparse->regCMD("app_m12_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](PARAM) { return m12_jiaxian_duoji_move_to_reset_pos(); });
m_cmdparse->regCMD("app_m12_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](PARAM) { return m12_jiaxian_duoji_move_to_clamp_pos(); });
m_cmdparse->regCMD("app_m22_scissors_move_reset_pos", "()", 0, [this](PARAM) { return m22_scissors_move_reset_pos(); });
m_cmdparse->regCMD("app_m22_scissors_cut", "()", 0, [this](PARAM) { return m22_scissors_cut(); });
m_cmdparse->regCMD("app_m11_arm_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_duoji_move_to_reset_pos(); });
m_cmdparse->regCMD("app_m11_arm_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_duoji_move_to_clamp_pos(); });
m_cmdparse->regCMD("app_m21_arm_hook_claws_reset", "()", 0, [this](PARAM) { return m21_arm_hook_claws_reset(); });
m_cmdparse->regCMD("app_m21_arm_hook_claws_move_to_half_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_half_pos(); });
m_cmdparse->regCMD("app_m21_arm_hook_claws_move_to_full_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_full_pos(); });
m_cmdparse->regCMD("app_main_shaft_run", "()", 0, [this](PARAM) { return main_shaft_run(); });
m_cmdparse->regCMD("app_main_shaft_stop", "()", 0, [this](PARAM) { return main_shaft_stop(); });
m_cmdparse->regCMD("app_xy_platform_reset", "()", 0, [this](PARAM) { return xy_platform_reset(); });
m_cmdparse->regCMD("app_z_axis_reset", "()", 0, [this](PARAM) { return z_axis_reset(); });
m_cmdparse->regCMD("app_z_axis_move_to", "(int32_t pos)", 1, [this](PARAM) { return z_axis_move_to(atoi(paraV[0])); });
m_cmdparse->regCMD("app_xy_reset", "()", 0, [this](PARAM) { return xy_reset(); });
m_cmdparse->regCMD("app_xy_move_to_zero", "()", 0, [this](PARAM) { return xy_move_to_zero(); });
m_cmdparse->regCMD("app_xy_take_clip", "(int32_t index)", 1, [this](PARAM) { return xy_take_clip(atoi(paraV[0])); });
m_cmdparse->regCMD("app_xy_take_line", "()", 0, [this](PARAM) { return xy_take_line(); });
m_cmdparse->regCMD("app_xy_take_back_clip", "()", 0, [this](PARAM) { return xy_take_back_clip(); });
m_cmdparse->regCMD("app_xy_remove_line", "()", 0, [this](PARAM) { return xy_remove_line(); });
m_cmdparse->regCMD("app_start_winding", "()", 0, [this](PARAM) { return start_winding(); });
m_cmdparse->regCMD("app_stop_winding", "()", 0, [this](PARAM) { return stop_winding(); });
m_cmdparse->regCMD("app_reset_and_check_device", "()", 0, [this](PARAM) { return reset_and_check_device(); });
m_cmdparse->regCMD("app_setcfg", "(const char* cfgname, int32_t cfgvalue)", 2, [this](PARAM) { return setcfg(paraV[0], atoi(paraV[1])); });
m_cmdparse->regCMD("app_dumpcfg", "()", 0, [this](PARAM) { return dumpcfg(); });
#endif
}

151
usrc/intelligent_winding_robot_ctrl.hpp
View File

@ -3,6 +3,9 @@
#include "sdk\components\zprotocols\zcancmder_v2\api\api.hpp"
#include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_manager.hpp"
//
#include "app_zmodule_device_manager.hpp"
#define ZMOTOR_ID 4
#define XYRobot_ID 3
#define XYRobot_HOOK_ID 21
@ -14,59 +17,69 @@ class IntelligentWindingRobotCtrl {
public:
typedef struct {
/**
* @brief 排废舵机
* @brief 机械臂夹线舵机
*/
int32_t paifei_duoji_reset_pos;
int32_t paifei_duoji_press_pos;
int32_t paifei_duoji_press_torque;
int32_t m11_arm_jiaxian_reset_pos;
int32_t m11_arm_jiaxian_clamp_direction;
int32_t m11_arm_jiaxian_clamp_torque;
/**
* @brief 绕线探测舵机
* @brief 线夹紧舵机
*/
int32_t raoxiantance_duoji_reset_pos;
int32_t raoxiantance_duoji_tance_zero_pos;
int32_t m12_jiaxian_reset_pos;
int32_t m12_jiaxian_clamp_direction;
int32_t m12_jiaxian_clamp_torque;
/**
* @brief 压线舵机
*/
int32_t yaxian_duoji_reset_pos;
int32_t yaxian_duoji_press_pos;
int32_t yaxian_duoji_press_torque;
int32_t yaxian_duoji_wait_for_press_pos;
/**
* @brief 线拉紧舵机
* 高度:
* 1. 没有弹夹 1015
* 2. 有弹夹 1055
* 3. 弹夹有壳 1110
*
*/
int32_t xianlajin_duoji_reset_pos;
int32_t xianlajin_duoji_line_entry_pos;
int32_t xianlajin_duoji_tight_line_pos;
int32_t xianlajin_duoji_loose_line_pos;
int32_t m13_yaxian_forward_reset_pos; // 压死待机位
int32_t m13_yaxian_backward_reset_pos; // 反向待机位
int32_t m13_jiaxian_clamp_direction; // 压线方向
int32_t m13_jiaxian_clamp_torque; // 压线扭矩
/**
* @brief 线夹紧舵机
* @brief 绕线探测舵机
*/
int32_t jiaxian_duoji_reset_pos;
int32_t jiaxian_duoji_clamp_pos;
int32_t jiaxian_duoji_clamp_torque;
int32_t m14_raoxiantance_reset_pos;
int32_t m14_raoxiantance_tance_zero_pos;
/**
* @brief 机械臂夹线舵机
* @brief 排废舵机
*/
int32_t arm_jiaxian_duoji_reset_pos;
int32_t arm_jiaxian_duoji_clamp_direction;
int32_t arm_jiaxian_duoji_clamp_torque;
int32_t m15_paifei_reset_pos;
int32_t m15_paifei_press_direction;
int32_t m15_paifei_press_torque;
/**
* @brief 剪刀
* @brief 线拉紧舵机
*/
int32_t scissors_reset_pos;
int32_t m22_scissors_cut_pos;
int32_t m16_xianlajin_reset_pos;
int32_t m16_xianlajin_tight_line_pos;
int32_t m16_xianlajin_winding_low_pos;
int32_t m16_xianlajin_winding_up_pos;
int32_t m16_xianlajin_line_entry_pos;
/**
* @brief 钩爪
*/
int32_t m21_arm_hook_claws_half_pos;
int32_t m21_arm_hook_claws_full_pos;
/**
* @brief 剪刀
*/
// int32_t m22_scissors_cut_pos;
/**
* @brief 拉线
*/
// int32_t m23_laxian_move_to_tight_pos;
/**
* @brief Z轴定位
@ -95,69 +108,43 @@ class IntelligentWindingRobotCtrl {
} config_t;
ZModuleDeviceManager* m_dm;
ICmdParser* m_cmdparse;
config_t cfg;
APPDM* m_dm;
ICmdParser* m_cmdparse;
config_t cfg;
private:
void wait_module_idle(int32_t moduleid);
void wait_modules_idle(...);
public:
int32_t initialize(APPDM* dm, ICmdParser* cmdparse);
/**
* @brief 机械臂夹线舵机
*/
int32_t m11_arm_jiaxian_duoji_move_to_reset_pos();
int32_t m11_arm_jiaxian_duoji_move_to_clamp_pos();
/**
* @brief 夹线舵机
*/
int32_t m12_jiaxian_duoji_move_to_reset_pos();
int32_t m12_jiaxian_duoji_move_to_clamp_pos();
/**
* @brief 压线舵机
*/
int32_t m13_yaxian_duoji_move_to_reset();
int32_t m13_yaxian_duoji_move_to_press_pos();
int32_t m13_yaxian_duoji_move_to_wait_for_press_pos();
/**
* @brief 绕线探测舵机
*/
int32_t m14_raoxiantance_duoji_move_to_reset();
int32_t m14_raoxiantance_duoji_move_to_get_thickness(int32_t* thickness);
// 排废舵机
int32_t m15_paifei_duoji_moveto_reset();
int32_t m15_paifei_duoji_moveto_press();
/**
* @brief 线拉紧舵机
*/
int32_t m16_xianlajin_duoji_move_to_reset(); // 零位
int32_t m16_xianlajin_duoji_move_to_line_entry_pos(); // 入线位
int32_t m16_xianlajin_duoji_move_to_tight_line_pos(); // 拉线位置
int32_t m16_xianlajin_duoji_move_to_loose_line_pos(); // 放线位置
/**
* @brief 机械臂钩爪
*/
public:
void wait_module_idle(int32_t moduleid);
void wait_modules_idle(...);
int32_t m11_arm_jiaxian_move_to_reset_pos();
int32_t m11_arm_jiaxian_move_to_clamp_pos();
int32_t m12_jiaxian_move_to_reset_pos();
int32_t m12_jiaxian_move_to_clamp_pos();
int32_t m13_yaxian_move_to_reset_forward();
int32_t m13_yaxian_move_to_reset_backward();
int32_t m13_yaxian_press_clip();
int32_t m14_raoxiantance_move_to_reset();
int32_t m15_paifei_moveto_reset();
int32_t m15_paifei_moveto_press();
int32_t m16_xianlajin_move_to_reset();
int32_t m16_xianlajin_move_to_tight_line_pos();
int32_t m16_xianlajin_move_to_winding_low_pos();
int32_t m16_xianlajin_move_to_winding_up_pos();
int32_t m16_xianlajin_move_to_line_entry_pos();
int32_t m21_arm_hook_claws_reset();
int32_t m21_arm_hook_claws_move_to_half_pos();
int32_t m21_arm_hook_claws_move_to_full_pos();
/**
* @brief 剪刀
*/
int32_t m22_scissors_move_reset_pos(); // block
int32_t m22_scissors_cut(); // block
/**
* @brief 拉线电机
*/
int32_t m22_scissors_move_reset_pos(); // block
int32_t m22_scissors_cut(); // block
int32_t m23_laxian_motor_move_to_reset_pos(); // block
int32_t m23_laxian_motor_move_to_tight_line_pos(); // block
int32_t device_reset();
int32_t disable_all_motor();
int32_t xy_get_point(int32_t clip_index, int32_t& x, int32_t& y); // 取弹夹
int32_t xy_run_to(int32_t x, int32_t y, int32_t zpos = 0, bool jiaxian_duoji_reset = true); // 取弹夹
int32_t xy_get_point(int32_t clip_index, int32_t& x, int32_t& y); // 取弹夹
int32_t xy_run_to(int32_t x, int32_t y, int32_t zpos = 0, bool jiaxian_reset = true); // 取弹夹
int32_t xy_run_to_clip_pos_test(int32_t clip_index);
int32_t xy_reset();
@ -165,8 +152,6 @@ class IntelligentWindingRobotCtrl {
int32_t zreset();
int32_t xymove_to(int32_t x, int32_t y);
public:
int32_t initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse);
void regcb();
int32_t initialize_device();

5
usrc/main.cpp
View File

@ -32,6 +32,8 @@
#include "sdk\components\zprotocols\zcancmder_v2\protocol_proxy.hpp"
#include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_manager.hpp"
#include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_script_cmder_paser.hpp"
//
#include "app_zmodule_device_manager.hpp"
#define TAG "main"
namespace iflytop {
Main gmain;
@ -146,7 +148,7 @@ namespace iflytop {
ZCanCommnaderMaster m_zcanCommnaderMaster; // can总线
ModbusBlockHost g_modbusblockhost; // modbus总线
FeiTeServoMotor g_feiteservomotor_bus; // 飞特舵机总线
ZModuleDeviceManager g_zmodule_device_manager; // 用于管理所有的设备
APPDM g_zmodule_device_manager; // 用于管理所有的设备
StepMotor45Scheduler step_motor45_scheduler; // 45步进电机调度器
CmdSchedulerV2 g_cmdScheduler; // 串口字符串指令总线
@ -303,4 +305,3 @@ void Main::run() {
osDelay(1);
}
}

1
usrc/project_configs.h
View File

@ -15,4 +15,5 @@
#define IFLYTOP_ENABLE_OS 1
#define IFLYTOP_PREEMPTPRIORITY_DEFAULT 5
#define IFLYTOP_ENABLE_EXCEPTION 1
Write Preview
Loading…
Cancel
Save