暂存

2024-05-19 16:04:33 +08:00
parent f15fcd4b10
commit f9a87063d8
5 changed files with 440 additions and 287 deletions
--- a/app/by_servo.c
+++ b/app/by_servo.c
@@ -0,0 +1,142 @@
+#include "by_servo.h"
+
+#include "at32f415.h"
+#include "lwprintf.h"
+
+/**
+ * @brief 设置云台摇臂
+ *
+ * @param angle 角度 (0-180)
+ */
+void by_servo_set_claw_arm(int16_t angle)
+{
+#define PULSE_WIDTH_MIN 0.5f
+#define PULSE_WIDTH_MAX 2.5f
+
+  uint32_t ch_val = 0;
+
+  if (angle < 0) {
+    angle = 0;
+  } else if (angle > 180) {
+    angle = 180;
+  }
+
+  ch_val = (uint32_t)(((float)angle / 180.0f * (PULSE_WIDTH_MAX - PULSE_WIDTH_MIN) + PULSE_WIDTH_MIN) / 20.0f * 9999.0f);
+
+  tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, ch_val);
+
+#undef PULSE_WIDTH_MIN
+#undef PULSE_WIDTH_MAX
+}
+
+/**
+ * @brief 设置云台摇臂
+ *
+ * @param angle 角度 (0-180)
+ */
+void by_servo_set_claw(int16_t angle)
+{
+#define PULSE_WIDTH_MIN 1.5f
+#define PULSE_WIDTH_MAX 2.5f
+
+  uint32_t ch_val = 0;
+
+  if (angle < 0) {
+    angle = 0;
+  } else if (angle > 180) {
+    angle = 180;
+  }
+
+  ch_val = (uint32_t)(((float)angle / 180.0f * (PULSE_WIDTH_MAX - PULSE_WIDTH_MIN) + PULSE_WIDTH_MIN) / 20.0f * 9999.0f);
+
+  tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, ch_val);
+
+#undef PULSE_WIDTH_MIN
+#undef PULSE_WIDTH_MAX
+}
+
+/**
+ * @brief 设置云台摇臂
+ *
+ * @param angle 角度 (0-180)
+ */
+void by_servo_set_camera(int16_t angle)
+{
+#define PULSE_WIDTH_MIN 1.5f
+#define PULSE_WIDTH_MAX 2.5f
+
+  uint32_t ch_val = 0;
+
+  if (angle < 0) {
+    angle = 0;
+  } else if (angle > 180) {
+    angle = 180;
+  }
+
+  ch_val = (uint32_t)(((float)angle / 180.0f * (PULSE_WIDTH_MAX - PULSE_WIDTH_MIN) + PULSE_WIDTH_MIN) / 20.0f * 9999.0f);
+
+  tmr_channel_value_set(TMR3, TMR_SELECT_CHANNEL_4, ch_val);
+
+#undef PULSE_WIDTH_MIN
+#undef PULSE_WIDTH_MAX
+}
+
+/**
+ * @brief 设置云台摇臂
+ *
+ * @param angle 角度 (0-180)
+ */
+void by_servo_set_scoop(int16_t angle)
+{
+#define PULSE_WIDTH_MIN 1.5f
+#define PULSE_WIDTH_MAX 2.5f
+
+  uint32_t ch_val = 0;
+
+  if (angle < 0) {
+    angle = 0;
+  } else if (angle > 180) {
+    angle = 180;
+  }
+
+  ch_val = (uint32_t)(((float)angle / 180.0f * (PULSE_WIDTH_MAX - PULSE_WIDTH_MIN) + PULSE_WIDTH_MIN) / 20.0f * 9999.0f);
+
+  tmr_channel_value_set(TMR3, TMR_SELECT_CHANNEL_3, ch_val);
+
+#undef PULSE_WIDTH_MIN
+#undef PULSE_WIDTH_MAX
+}
+
+void by_servo_init(void)
+{
+  // 后续要不全初始化为空值，由主控按顺序初始化
+  by_servo_set_claw_arm(0);
+  by_servo_set_claw(0);
+  by_servo_set_camera(0);
+  by_servo_set_scoop(0);
+}
+
+void by_servo_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len)
+{
+
+  int16_t angle = 0;
+
+  angle = (int16_t)(data[0] | (data[1] << 8));
+
+  switch (stdd_id) {
+    case 0x009:
+      by_servo_set_claw_arm(angle);
+      break;
+    case 0x00A:
+      by_servo_set_claw(angle);
+      break;
+    case 0x00B:
+      by_servo_set_camera(angle);
+      break;
+    case 0x00C:
+      by_servo_set_scoop(angle);
+      break;
+    default:
+      break;
+  }
+}