You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

320 lines
12 KiB

2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
2 years ago
  1. #include "main.hpp"
  2. #include <stddef.h>
  3. #include <stdio.h>
  4. #include "main.h"
  5. #include "project.hpp"
  6. //
  7. #include "one_dimensional_code_laser_scanner.hpp"
  8. // #include "sdk/components/single_axis_motor_control_v2/single_axis_motor_control_v2.hpp"
  9. #include "sdk/components/iflytop_can_slave_modules/idcard_reader_service.hpp"
  10. #include "sdk/components/single_axis_motor_control/single_axis_motor_control.hpp"
  11. #include "sdk/hal/zhal.hpp"
  12. #include "sdk\components\iflytop_can_slave_modules\io_control_service.hpp"
  13. #include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp"
  14. #include "sdk\components\m3078\m3078_code_scaner.hpp"
  15. #include "sdk\components\tmc\ic\ztmc4361A.hpp"
  16. #include "sdk\components\tmc\ic\ztmc5130.hpp"
  17. //
  18. #include "sdk\components\huacheng_sensor\dp600_pressure_sensor.hpp"
  19. #include "sdk\components\zcan_module\huacheng_pressure_sensor.hpp"
  20. #include "sdk\components\zcan_module\zcan_basic_order_module.hpp"
  21. #include "sdk\components\zcan_module\zcan_pump_ctrl_module.hpp"
  22. #include "sdk\components\zcan_module\zcan_trigle_warning_light_ctl_module.hpp"
  23. //
  24. #include <stdint.h>
  25. #include "sdk\components\cmdscheduler\cmd_scheduler_v2.hpp"
  26. #include "sdk\hal\zuart.hpp"
  27. #define TAG "main"
  28. namespace iflytop {
  29. Main gmain;
  30. };
  31. using namespace iflytop;
  32. IflytopCanProtocolStackProcesser m_protocolStack;
  33. TMC5130 m_motor1;
  34. TMC5130 m_motor2;
  35. ZGPIO debuglight;
  36. ZGPIO triLight_R;
  37. ZGPIO triLight_G;
  38. ZGPIO triLight_B;
  39. ZGPIO triLight_BEEP;
  40. ZGPIO m_input1;
  41. ZGPIO m_input2;
  42. ZGPIO m_input3;
  43. ZGPIO m_input4;
  44. ZGPIO m_input5;
  45. ZGPIO output1;
  46. ZGPIO output2;
  47. ZCanReceiver m_canReceiver;
  48. ZCanBasicOrderModule m_basicOrderModule;
  49. ZCanPumpCtrlModule m_pumpCtrlModule;
  50. ZCanTrigleWarningLightCtlModule m_warningLightCtlModule;
  51. HuachengPressureSensor m_huachengPressureSensor;
  52. void setmotor(TMC5130 *motor, int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power) {
  53. int32_t ppm = rpm / 60.0 * 51200;
  54. int32_t acc = acc_rpm2 / 60.0 * 51200;
  55. int16_t _idlepower = 1;
  56. int16_t _power = 31;
  57. if (idlepower > 0 && idlepower < 31) {
  58. _idlepower = idlepower;
  59. }
  60. if (power > 0 && power < 31) {
  61. _power = power;
  62. }
  63. motor->setIHOLD_IRUN(_idlepower, _power, 10); // 5W
  64. motor->setAcceleration(acc);
  65. motor->setDeceleration(acc);
  66. motor->rotate(ppm);
  67. }
  68. void Main::onRceivePacket(CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) {
  69. ZLOGI(TAG, "onRceivePacket from %d %d", rxbuf->id, len);
  70. for (size_t i = 0; i < len; i++) {
  71. printf("%02X ", packet[i]);
  72. }
  73. printf("\n");
  74. }
  75. void Main::run() {
  76. ZHALCORE::cfg_t oscfg = {
  77. .delayhtim = &DELAY_US_TIMER,
  78. .debuguart = &DEBUG_UART,
  79. };
  80. ZHALCORE::getInstance()->initialize(oscfg);
  81. ZLOGI(TAG, "pipeline_disinfection_liquid_path_control:%s", VERSION);
  82. printf("int32_t %d int %d longint %d\n", sizeof(int32_t), sizeof(int), sizeof(long int));
  83. debuglight.initAsOutput(DEBUG_LIGHT_GPIO, ZGPIO::kMode_nopull, false, false);
  84. ZHAL_CORE_REG(200, { debuglight.toggleState(); });
  85. ZCanReceiver::CFG *cfg = m_canReceiver.createCFG(DEVICE_ID);
  86. m_canReceiver.init(cfg);
  87. m_canReceiver.registerListener(this);
  88. /**
  89. * @brief
  90. */
  91. m_input1.initAsInput(PD11, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/);
  92. m_input2.initAsInput(PC5, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/);
  93. m_input3.initAsInput(PD12, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/);
  94. m_input4.initAsInput(PD13, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/);
  95. m_input5.initAsInput(PC6, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true /*mirror*/);
  96. m_basicOrderModule.initialize(&m_canReceiver);
  97. m_basicOrderModule.regInputCtl([this](uint8_t id, bool &val) {
  98. if (id == 1) {
  99. val = m_input1.getState();
  100. return true;
  101. }
  102. if (id == 2) {
  103. val = m_input2.getState();
  104. return true;
  105. }
  106. if (id == 3) {
  107. val = m_input3.getState();
  108. return true;
  109. }
  110. if (id == 4) {
  111. val = m_input4.getState();
  112. return true;
  113. }
  114. if (id == 5) {
  115. val = m_input5.getState();
  116. return true;
  117. }
  118. return false;
  119. });
  120. // output1.reg
  121. // output2
  122. m_basicOrderModule.regOutCtl([this](uint8_t id, bool val) { return false; });
  123. ZHAL_CORE_REG(3000, {
  124. // ZLOGI(TAG, "IO1:%d IO2:%d", m_input1.getState(), m_input2.getState());
  125. // ZLOGI(TAG, "IO1:%d IO2:%d IO3:%d IO4:%d IO5:%d", m_input1.getState(), m_input2.getState(), m_input3.getState(),
  126. // m_input4.getState(), m_input5.getState());
  127. });
  128. /*******************************************************************************
  129. * *
  130. *******************************************************************************/
  131. {
  132. TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR1_ENN, .csn_pin = MOTOR1_CSN};
  133. m_motor1.initialize(&cfg);
  134. int32_t chipv = m_motor1.readChipVERSION();
  135. ZLOGI(TAG, "m_motor1:%lx", chipv);
  136. m_motor1.setIHOLD_IRUN(1, 20, 0);
  137. m_motor1.setMotorShaft(true);
  138. m_motor1.setAcceleration(300000);
  139. m_motor1.setDeceleration(300000);
  140. // m_motor1.rotate(1000000);
  141. }
  142. {
  143. TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR2_ENN, .csn_pin = MOTOR2_CSN};
  144. m_motor2.initialize(&cfg);
  145. int32_t chipv = m_motor2.readChipVERSION();
  146. ZLOGI(TAG, "m_motor2:%lx", chipv);
  147. m_motor2.setIHOLD_IRUN(1, 20, 0); // 5W
  148. m_motor2.setMotorShaft(true);
  149. m_motor2.setAcceleration(300000);
  150. m_motor2.setDeceleration(300000);
  151. // m_motor1.rotate(1000000);
  152. }
  153. m_pumpCtrlModule.initialize(&m_canReceiver);
  154. m_pumpCtrlModule.regSubmodule(1, [&](int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power) {
  155. ZLOGI(TAG, "pump1 acc_rpm2:%d rpm:%d", acc_rpm2, rpm);
  156. setmotor(&m_motor1, acc_rpm2, rpm, idlepower, power);
  157. });
  158. m_pumpCtrlModule.regSubmodule(2, [&](int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power) {
  159. ZLOGI(TAG, "pump2 acc:%d rpm:%d", acc_rpm2, rpm);
  160. setmotor(&m_motor2, acc_rpm2, rpm, idlepower, power);
  161. });
  162. /*******************************************************************************
  163. * *
  164. *******************************************************************************/
  165. {
  166. triLight_R.initAsOutput(PD8, ZGPIO::kMode_nopull, false, false);
  167. triLight_G.initAsOutput(PD7, ZGPIO::kMode_nopull, false, false);
  168. triLight_B.initAsOutput(PD9, ZGPIO::kMode_nopull, false, false);
  169. triLight_BEEP.initAsOutput(PD10, ZGPIO::kMode_nopull, false, false);
  170. m_warningLightCtlModule.initialize(&m_canReceiver);
  171. m_warningLightCtlModule.regSubmodule(1, [&](uint8_t r, uint8_t g, uint8_t b, uint8_t beep) {
  172. ZLOGI(TAG, "warningLightCtlModule r:%d g:%d b:%d beep:%d", r, g, b, beep);
  173. triLight_R.setState(r != 0);
  174. triLight_G.setState(g != 0);
  175. triLight_B.setState(b != 0);
  176. triLight_BEEP.setState(beep != 0);
  177. });
  178. }
  179. /*******************************************************************************
  180. * *
  181. *******************************************************************************/
  182. // while (true) {
  183. // static ModbusBlockHost modbusBlockHost;
  184. // modbusBlockHost.initialize(&huart3);
  185. // int16_t val[1] = {0};
  186. // bool suc = modbusBlockHost.readReg03Muti(1, 0x00, (uint16_t *)val, 1, 50);
  187. // printf("suc:%d val:%d\n", suc, val[0]);
  188. // chip_delay_ms(1000);
  189. // }
  190. {
  191. m_huachengPressureSensor.initialize(&m_canReceiver);
  192. m_huachengPressureSensor.regSubmodule(1, [this](DP600PressureSensor::sensor_data_t *data) { //
  193. static ModbusBlockHost modbusBlockHost;
  194. modbusBlockHost.initialize(&huart3);
  195. int16_t val[1] = {0};
  196. bool suc = modbusBlockHost.readReg03Muti(1, 0x00, (uint16_t *)val, 1, 50);
  197. if (!suc) {
  198. return false;
  199. }
  200. data->precision = 3;
  201. data->pressure_unit = 1;
  202. data->value = val[0];
  203. data->zero_point = 0;
  204. data->range_full_point = 0;
  205. return true;
  206. });
  207. m_huachengPressureSensor.regSubmodule(2, &huart3, 2);
  208. m_huachengPressureSensor.regSubmodule(3, &huart3, 3);
  209. m_huachengPressureSensor.regSubmodule(4, &huart3, 4);
  210. }
  211. /*******************************************************************************
  212. * *
  213. *******************************************************************************/
  214. static ZUART uartreceiver;
  215. static CmdSchedulerV2 cmdScheduler;
  216. static ZUART::cfg_t uartreceiver_cfg = {
  217. .name = "uartreceiver",
  218. .huart = &DEBUG_UART,
  219. .rxbuffersize = 512,
  220. .rxovertime_ms = 30,
  221. };
  222. uartreceiver.initialize(&uartreceiver_cfg);
  223. cmdScheduler.initialize(&uartreceiver);
  224. cmdScheduler.regCMD("setmotor1", "(int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power)", 4, //
  225. [](int32_t paramN, const char **paraV, ICmdParserACK *ack) {
  226. int16_t acc_rpm2 = atoi(paraV[0]);
  227. int16_t rpm = atoi(paraV[1]);
  228. int16_t idlepower = atoi(paraV[2]);
  229. int16_t power = atoi(paraV[3]);
  230. setmotor(&m_motor1, acc_rpm2, rpm, idlepower, power);
  231. ack->setNoneAck(0);
  232. });
  233. cmdScheduler.regCMD("setmotor2", "(int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power)", 4, //
  234. [](int32_t paramN, const char **paraV, ICmdParserACK *ack) {
  235. int16_t acc_rpm2 = atoi(paraV[0]);
  236. int16_t rpm = atoi(paraV[1]);
  237. int16_t idlepower = atoi(paraV[2]);
  238. int16_t power = atoi(paraV[3]);
  239. setmotor(&m_motor2, acc_rpm2, rpm, idlepower, power);
  240. ack->setNoneAck(0);
  241. });
  242. cmdScheduler.regCMD("setlight", "(uint8_t r, uint8_t g, uint8_t b, uint8_t beep)", 4, //
  243. [](int32_t paramN, const char **paraV, ICmdParserACK *ack) {
  244. uint8_t r = atoi(paraV[0]);
  245. uint8_t g = atoi(paraV[1]);
  246. uint8_t b = atoi(paraV[2]);
  247. uint8_t beep = atoi(paraV[3]);
  248. triLight_R.setState(r != 0);
  249. triLight_G.setState(g != 0);
  250. triLight_B.setState(b != 0);
  251. triLight_BEEP.setState(beep != 0);
  252. ack->setNoneAck(0);
  253. });
  254. cmdScheduler.regCMD("readio", "()", 0, //
  255. [](int32_t paramN, const char **paraV, ICmdParserACK *ack) {
  256. uint8_t sensorid = atoi(paraV[0]);
  257. ZLOGI(TAG, "IO1:%d IO2:%d IO3:%d IO4:%d IO5:%d", //
  258. m_input1.getState(), m_input2.getState(), m_input3.getState(), m_input4.getState(),
  259. m_input5.getState());
  260. ack->setNoneAck(0);
  261. });
  262. cmdScheduler.regCMD("pressure_sensor_read", "(uint8_t sensorid)", 1, //
  263. [](int32_t paramN, const char **paraV, ICmdParserACK *ack) {
  264. uint8_t sensorid = atoi(paraV[0]);
  265. DP600PressureSensor::sensor_data_t data = m_huachengPressureSensor.readsensordata(sensorid);
  266. ZLOGI(TAG, "precision:%d", data.precision);
  267. ZLOGI(TAG, "pressure_unit:%d", data.pressure_unit);
  268. ZLOGI(TAG, "value:%d", data.value);
  269. ZLOGI(TAG, "zero_point:%d", data.zero_point);
  270. ZLOGI(TAG, "range_full_point:%d", data.range_full_point);
  271. ack->setNoneAck(0);
  272. });
  273. ZLOGI(TAG, "init done");
  274. while (1) {
  275. cmdScheduler.schedule();
  276. ZHALCORE::getInstance()->loop();
  277. }
  278. }