From 5afdc774767bcd1d1529bac88f3d2ebe8fd485ad Mon Sep 17 00:00:00 2001
From: bmy <2583236812@qq.com>
Date: Fri, 14 Jun 2024 17:43:32 +0800
Subject: [PATCH] =?UTF-8?q?feat:=20=E5=A2=9E=E5=8A=A0=E8=BD=AC=E5=A1=94?=
 =?UTF-8?q?=E7=94=B5=E6=9C=BA=E6=8E=A5=E5=8F=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/by_servo.c | 27 +++++++++++++++++++++++++++
 1 file changed, 27 insertions(+)

diff --git a/app/by_servo.c b/app/by_servo.c
index 9bcab9e..db23b69 100644
--- a/app/by_servo.c
+++ b/app/by_servo.c
@@ -134,6 +134,29 @@ void by_servo_set_storage(int16_t angle)
 #undef PULSE_WIDTH_MAX
 }
 
+void by_servo_set_zhuan(int16_t angle)
+{
+#define PULSE_WIDTH_MIN 0.5f
+#define PULSE_WIDTH_MAX 2.5f
+
+  uint32_t ch_val = 0;
+
+  if (angle < -10) {
+    angle = -10;
+  } else if (angle > 10) {
+    angle = 10;
+  }
+
+  angle += 10;
+
+  ch_val = (uint32_t)(((float)angle / 20.0f * (PULSE_WIDTH_MAX - PULSE_WIDTH_MIN) + PULSE_WIDTH_MIN) / 20.0f * 9999.0f);
+
+  tmr_channel_value_set(TMR3, TMR_SELECT_CHANNEL_1, ch_val);
+
+#undef PULSE_WIDTH_MIN
+#undef PULSE_WIDTH_MAX
+}
+
 void by_servo_init(void)
 {
   // 后续要不全初始化为空值，由主控按顺序初始化
@@ -143,6 +166,7 @@ void by_servo_init(void)
   by_servo_set_storage(0);
   delay_sec(4);
   by_servo_set_claw_arm(220); // 36-220
+  by_servo_set_zhuan(0);      //-10-10
 }
 
 void by_servo_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len)
@@ -168,6 +192,9 @@ void by_servo_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len)
     case 0x00D:
       by_servo_set_storage(angle);
       break;
+    case 0x00E:
+      by_servo_set_zhuan(angle);
+      break;
     default:
       break;
   }