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.

290 lines
9.4 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
  1. #include "hardware.hpp"
  2. #include <stddef.h>
  3. #include <stdio.h>
  4. #include "main.h"
  5. #include "project.hpp"
  6. //
  7. // #include "sdk/components/single_axis_motor_control_v2/single_axis_motor_control_v2.hpp"
  8. #include "sdk/components/iflytop_can_slave_modules/idcard_reader_service.hpp"
  9. #include "sdk/components/single_axis_motor_control/single_axis_motor_control.hpp"
  10. #include "sdk/hal/zhal.hpp"
  11. #include "sdk\components\iflytop_can_slave_modules\io_control_service.hpp"
  12. #include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp"
  13. #include "sdk\components\m3078\m3078_code_scaner.hpp"
  14. #include "sdk\components\tmc\ic\ztmc4361A.hpp"
  15. #include "sdk\components\tmc\ic\ztmc5130.hpp"
  16. //
  17. #include "sdk\components\huacheng_sensor\dp600_pressure_sensor.hpp"
  18. #include "sdk\components\zcan_module\huacheng_pressure_sensor.hpp"
  19. #include "sdk\components\zcan_module\zcan_basic_order_module.hpp"
  20. #include "sdk\components\zcan_module\zcan_pump_ctrl_module.hpp"
  21. #include "sdk\components\zcan_module\zcan_trigle_warning_light_ctl_module.hpp"
  22. using namespace iflytop;
  23. #define TAG "main"
  24. TMC5130 m_motor1;
  25. TMC5130 m_motor2;
  26. ZGPIO triLight_R;
  27. ZGPIO triLight_G;
  28. ZGPIO triLight_Y;
  29. ZGPIO triLight_BEEP;
  30. ZCanBasicOrderModule m_basicOrderModule;
  31. ZCanPumpCtrlModule m_pumpCtrlModule;
  32. ZCanTrigleWarningLightCtlModule m_warningLightCtlModule;
  33. HuachengPressureSensor m_huachengPressureSensor;
  34. DP600PressureSensor m_dp600PressureSensor2;
  35. DP600PressureSensor m_dp600PressureSensor3;
  36. DP600PressureSensor m_dp600PressureSensor4;
  37. ZGPIO IO_PD13_IN;
  38. ZGPIO IO_PC7_IN;
  39. #define PROCESS_CMD(cmd, _subcmdid, id) \
  40. if ((cmdheader->cmdid == (uint16_t)cmd) && (cmdheader->subcmdid == _subcmdid) && cmdheader->data[0] == id) { \
  41. matching = true; \
  42. } \
  43. if ((cmdheader->cmdid == (uint16_t)cmd) && (cmdheader->subcmdid == _subcmdid) && cmdheader->data[0] == id)
  44. void setmotor(TMC5130 *motor, int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power) {
  45. int32_t ppm = rpm / 60.0 * 51200;
  46. int32_t acc = acc_rpm2 / 60.0 * 51200;
  47. int16_t _idlepower = 1;
  48. int16_t _power = 31;
  49. if (idlepower > 0 && idlepower < 31) {
  50. _idlepower = idlepower;
  51. }
  52. if (power > 0 && power < 31) {
  53. _power = power;
  54. }
  55. motor->setIHOLD_IRUN(_idlepower, _power, 10); // 5W
  56. motor->setAcceleration(acc);
  57. motor->setDeceleration(acc);
  58. motor->rotate(ppm);
  59. }
  60. void Hardware::initialize(int deviceId) {
  61. m_device_id = deviceId;
  62. IO_PD13_IN.initAsInput(PD13, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);
  63. IO_PC7_IN.initAsInput(PC7, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/);
  64. {
  65. TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR1_ENN, .csn_pin = MOTOR1_CSN};
  66. m_motor1.initialize(&cfg);
  67. int32_t chipv = m_motor1.readChipVERSION();
  68. ZLOGI(TAG, "m_motor1:%lx", chipv);
  69. m_motor1.setIHOLD_IRUN(1, 20, 0);
  70. m_motor1.setMotorShaft(true);
  71. m_motor1.setAcceleration(300000);
  72. m_motor1.setDeceleration(300000);
  73. // m_motor1.rotate(1000000);
  74. }
  75. {
  76. TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR2_ENN, .csn_pin = MOTOR2_CSN};
  77. m_motor2.initialize(&cfg);
  78. int32_t chipv = m_motor2.readChipVERSION();
  79. ZLOGI(TAG, "m_motor2:%lx", chipv);
  80. m_motor2.setIHOLD_IRUN(1, 20, 0); // 5W
  81. m_motor2.setMotorShaft(true);
  82. m_motor2.setAcceleration(300000);
  83. m_motor2.setDeceleration(300000);
  84. // m_motor2.rotate(1000000);
  85. }
  86. triLight_R.initAsOutput(PD8, ZGPIO::kMode_nopull, false, false);
  87. triLight_G.initAsOutput(PD7, ZGPIO::kMode_nopull, false, false);
  88. triLight_Y.initAsOutput(PD9, ZGPIO::kMode_nopull, false, false);
  89. triLight_BEEP.initAsOutput(PD10, ZGPIO::kMode_nopull, false, false);
  90. m_dp600PressureSensor2.initialize(&huart3, 2);
  91. m_dp600PressureSensor3.initialize(&huart3, 3);
  92. m_dp600PressureSensor4.initialize(&huart3, 4);
  93. }
  94. void dumpdp600data(DP600PressureSensor::sensor_data_t *data) {
  95. ZLOGI(TAG, "value:%d", data->value);
  96. ZLOGI(TAG, "zero_point:%d", data->zero_point);
  97. ZLOGI(TAG, "range_full_point:%d", data->range_full_point);
  98. ZLOGI(TAG, "precision:%d", data->precision);
  99. ZLOGI(TAG, "pressure_unit:%d", data->pressure_unit);
  100. }
  101. void packet_kcmd_read_huacheng_pressure_sensor_data(int id, DP600PressureSensor::sensor_data_t *dp600data, uint8_t *receipt, int32_t &receiptsize) {
  102. receipt[0] = id;
  103. receipt[1] = 0;
  104. receipt[2] = dp600data->precision;
  105. receipt[3] = dp600data->pressure_unit;
  106. memcpy(receipt + 4, &dp600data->value, 2);
  107. memcpy(receipt + 6, &dp600data->zero_point, 2);
  108. memcpy(receipt + 8, &dp600data->range_full_point, 2);
  109. receiptsize = 10;
  110. }
  111. int32_t Hardware::process_rx_packet(from_where_t fromwhere, uint8_t *packet, int32_t len, uint8_t *receipt, int32_t &receiptsize, bool &matching) {
  112. Cmdheader_t *cmdheader = (Cmdheader_t *)packet;
  113. printf("rx: cmdid:%d subcmdid:%d id:%d\n", cmdheader->cmdid, cmdheader->subcmdid, cmdheader->data[0]);
  114. /**
  115. * @brief Ping
  116. */
  117. PROCESS_CMD(kcmd_ping, 0, m_device_id) {
  118. receipt[0] = cmdheader->data[0];
  119. receiptsize = 1;
  120. return 0;
  121. }
  122. /**
  123. * @brief
  124. */
  125. PROCESS_CMD(kcmd_proportional_read_water_immersion_sensor, 0, 0) {
  126. ((int32_t *)receipt)[0] = !IO_PC7_IN.getState();
  127. receiptsize = 4;
  128. return 0;
  129. }
  130. /**
  131. * @brief
  132. */
  133. PROCESS_CMD(kcmd_proportional_read_water_immersion_sensor, 0, 1) {
  134. ((int32_t *)receipt)[0] = !IO_PD13_IN.getState();
  135. receiptsize = 4;
  136. return 0;
  137. }
  138. /**
  139. * @brief
  140. */
  141. PROCESS_CMD(kcmd_peristaltic_pump_ctl, 0, 1) {
  142. int16_t acc = *(int16_t *)(&cmdheader->data[2]);
  143. int16_t rpm = *(int16_t *)(&cmdheader->data[4]);
  144. int16_t idlepower = cmdheader->data[6];
  145. int16_t power = cmdheader->data[7];
  146. printf("kcmd_peristaltic_pump_ctl 1 acc:%d rpm:%d idlepower:%d power:%d\n", acc, rpm, idlepower, power);
  147. setmotor(&m_motor1, acc, rpm, idlepower, power);
  148. receipt[0] = cmdheader->data[0];
  149. receiptsize = 1;
  150. return 0;
  151. }
  152. /**
  153. * @brief -
  154. */
  155. PROCESS_CMD(kcmd_peristaltic_pump_ctl, 0, 2) {
  156. int16_t acc = *(int16_t *)(&cmdheader->data[2]);
  157. int16_t rpm = *(int16_t *)(&cmdheader->data[4]);
  158. int16_t idlepower = cmdheader->data[6];
  159. int16_t power = cmdheader->data[7];
  160. printf("kcmd_peristaltic_pump_ctl 2 acc:%d rpm:%d idlepower:%d power:%d\n", acc, rpm, idlepower, power);
  161. setmotor(&m_motor2, acc, rpm, idlepower, power);
  162. receipt[0] = cmdheader->data[0];
  163. receiptsize = 1;
  164. return 0;
  165. }
  166. /**
  167. * @brief
  168. */
  169. PROCESS_CMD(kcmd_triple_warning_light_ctl, 0, 1) {
  170. /**
  171. * @brief 0:
  172. * cmd:
  173. * [0]:SENSORID
  174. * [2]:
  175. * [3]:
  176. * [4]:绿
  177. * [5]:
  178. * ack : b0:id
  179. * ack_datalen : 1
  180. */
  181. uint8_t id = cmdheader->data[0];
  182. uint8_t r = cmdheader->data[2];
  183. uint8_t g = cmdheader->data[3];
  184. uint8_t b = cmdheader->data[4];
  185. uint8_t beep = cmdheader->data[5];
  186. triLight_R.setState(r != 0);
  187. triLight_G.setState(g != 0);
  188. triLight_Y.setState(b != 0);
  189. triLight_BEEP.setState(beep != 0);
  190. receipt[0] = cmdheader->data[0];
  191. receiptsize = 1;
  192. }
  193. static DP600PressureSensor::sensor_data_t dp600data;
  194. /**
  195. * @brief
  196. */
  197. PROCESS_CMD(kcmd_read_huacheng_pressure_sensor, 0, 1) {
  198. static ModbusBlockHost modbusBlockHost;
  199. modbusBlockHost.initialize(&huart3);
  200. int16_t val[1] = {0};
  201. bool suc = modbusBlockHost.readReg03Muti(1, 0x00, (uint16_t *)val, 1, 50);
  202. if (!suc) return 1002;
  203. dp600data.precision = 3;
  204. dp600data.pressure_unit = 1;
  205. dp600data.value = val[0];
  206. dp600data.zero_point = 0;
  207. dp600data.range_full_point = 0;
  208. packet_kcmd_read_huacheng_pressure_sensor_data(cmdheader->data[0], &dp600data, receipt, receiptsize);
  209. if (fromwhere == kuart) {
  210. dumpdp600data(&dp600data);
  211. }
  212. return 0;
  213. }
  214. /**
  215. * @brief
  216. */
  217. PROCESS_CMD(kcmd_read_huacheng_pressure_sensor, 0, 2) {
  218. bool suc = m_dp600PressureSensor2.readVal(&dp600data);
  219. if (!suc) return 1002;
  220. packet_kcmd_read_huacheng_pressure_sensor_data(cmdheader->data[0], &dp600data, receipt, receiptsize);
  221. if (fromwhere == kuart) {
  222. dumpdp600data(&dp600data);
  223. }
  224. return 0;
  225. }
  226. /**
  227. * @brief
  228. */
  229. PROCESS_CMD(kcmd_read_huacheng_pressure_sensor, 0, 3) {
  230. bool suc = m_dp600PressureSensor3.readVal(&dp600data);
  231. if (!suc) return 1002;
  232. packet_kcmd_read_huacheng_pressure_sensor_data(cmdheader->data[0], &dp600data, receipt, receiptsize);
  233. if (fromwhere == kuart) {
  234. dumpdp600data(&dp600data);
  235. }
  236. return 0;
  237. }
  238. /**
  239. * @brief
  240. */
  241. PROCESS_CMD(kcmd_read_huacheng_pressure_sensor, 0, 4) {
  242. bool suc = m_dp600PressureSensor4.readVal(&dp600data);
  243. if (!suc) return 1002;
  244. packet_kcmd_read_huacheng_pressure_sensor_data(cmdheader->data[0], &dp600data, receipt, receiptsize);
  245. if (fromwhere == kuart) {
  246. dumpdp600data(&dp600data);
  247. }
  248. return 0;
  249. }
  250. return 0;
  251. }
  252. void Hardware::loop() {}