|
20 | 20 | #include "network_config_mode_mp3.h" |
21 | 21 | #include "network_connected_mp3.h" |
22 | 22 | #include "notification_0_mp3.h" |
| 23 | +#include "servo.h" |
23 | 24 |
|
24 | 25 | #ifndef ARDUINO_ESP32S3_DEV |
25 | 26 | #error "This example only supports ESP32S3-Dev board." |
@@ -90,21 +91,18 @@ std::unique_ptr<Display> g_display; |
90 | 91 | auto g_observer = std::make_shared<ai_vox::Observer>(); |
91 | 92 | button_handle_t g_button_boot_handle = nullptr; |
92 | 93 |
|
93 | | -constexpr uint8_t kServoFrequency = 50; |
94 | | -constexpr uint8_t kServoResolution = 12; |
95 | 94 | constexpr uint32_t kMinPulse = 500; |
96 | 95 | constexpr uint32_t kMaxPulse = 2500; |
97 | 96 | constexpr uint16_t kMaxServoAngle = 180; |
98 | 97 |
|
99 | | -constexpr uint32_t kMaxPwmDuty = pow(2, kServoResolution) - 1; |
100 | | -constexpr uint32_t kPwmFactor = 1000 * 1000 / kServoFrequency; |
101 | | - |
102 | 98 | constexpr uint8_t kLcdBacklightChannel = 7; |
103 | 99 | constexpr uint32_t kDisplayBacklightFrequency = 1000; |
104 | 100 | constexpr uint8_t kDisplayBacklightResolution = 8; |
105 | 101 |
|
106 | 102 | std::vector<uint16_t> g_servo_angles(kServoPins.size(), 90); |
107 | 103 |
|
| 104 | +std::vector<std::unique_ptr<em::Servo>> g_servos; |
| 105 | + |
108 | 106 | uint8_t g_display_brightness = 255; |
109 | 107 |
|
110 | 108 | DHT g_dht11(kDht11Pin, DHT11); |
@@ -534,47 +532,22 @@ void InitMcpTools() { |
534 | 532 | ); |
535 | 533 | } |
536 | 534 |
|
537 | | -uint32_t CalculateDuty(const uint16_t angle) { |
538 | | - // Map the angle to the pulse width(500-2500μs) |
539 | | - const uint32_t pulse_width = map(angle, 0, kMaxServoAngle, kMinPulse, kMaxPulse); |
540 | | - return static_cast<uint32_t>((pulse_width * kMaxPwmDuty) / kPwmFactor); |
541 | | -} |
542 | | - |
543 | 535 | void InitServos() { |
544 | 536 | printf("init servos\n"); |
545 | 537 |
|
| 538 | + g_servos.clear(); |
546 | 539 | for (uint8_t i = 0; i < kServoPins.size(); i++) { |
547 | | - if (!ledcAttach(kServoPins[i], kServoFrequency, kServoResolution)) { |
548 | | - printf("Error: Failed to attach servo %" PRIu8 " on pin%" PRIu8 " .\n", i, kServoPins[i]); |
549 | | - continue; |
550 | | - } |
| 540 | + auto servo = std::make_unique<em::Servo>(kServoPins[i], 0, kMaxServoAngle, kMinPulse, kMaxPulse); |
551 | 541 |
|
552 | | - if (!ledcWrite(kServoPins[i], CalculateDuty(g_servo_angles[i]))) { |
553 | | - printf("Error: Failed to set initial duty for servo %" PRIu8 " .\n", i); |
| 542 | + if (!servo->Init()) { |
| 543 | + printf("Error: Failed to init servo %" PRIu8 " on pin %" PRIu16 "\n", i, kServoPins[i]); |
554 | 544 | continue; |
555 | 545 | } |
556 | | - } |
557 | | -} |
558 | 546 |
|
559 | | -bool SetServoAngle(const uint8_t servo_index, const uint16_t angle) { |
560 | | - if (servo_index < 1 || servo_index > kServoPins.size()) { |
561 | | - printf("Error: Invalid servo index: %" PRIu8 ", valid range: 1-%" PRIu8 " .\n", servo_index, kServoPins.size()); |
562 | | - return false; |
563 | | - } |
564 | | - |
565 | | - if (angle > kMaxServoAngle) { |
566 | | - printf("Error: Invalid servo angle: %" PRIu16 " for index: %" PRIu8 " .\n", angle, servo_index); |
567 | | - return false; |
568 | | - } |
| 547 | + servo->Write(g_servo_angles[i]); |
569 | 548 |
|
570 | | - if (!ledcWrite(kServoPins[servo_index - 1], CalculateDuty(angle))) { |
571 | | - printf("Error: Failed to write duty for servo %" PRIu8 " .\n", servo_index); |
572 | | - return false; |
| 549 | + g_servos.push_back(std::move(servo)); |
573 | 550 | } |
574 | | - |
575 | | - g_servo_angles[servo_index - 1] = angle; |
576 | | - |
577 | | - return true; |
578 | 551 | } |
579 | 552 |
|
580 | 553 | } // namespace |
@@ -771,11 +744,10 @@ void loop() { |
771 | 744 | } |
772 | 745 |
|
773 | 746 | printf("on mcp tool call: self.servo.set_one_servo, index: %" PRId64 ", angle: %" PRId64 "\n", *index_ptr, *angle_ptr); |
774 | | - if (SetServoAngle(static_cast<uint8_t>(*index_ptr), static_cast<uint16_t>(*angle_ptr))) { |
775 | | - engine.SendMcpCallResponse(mcp_tool_call_event->id, true); |
776 | | - } else { |
777 | | - engine.SendMcpCallError(mcp_tool_call_event->id, "Failed to set servo angle for index: " + std::to_string(*index_ptr)); |
778 | | - } |
| 747 | + g_servos[*index_ptr - 1]->Write(static_cast<uint16_t>(*angle_ptr)); |
| 748 | + g_servo_angles[*index_ptr - 1] = *angle_ptr; |
| 749 | + |
| 750 | + engine.SendMcpCallResponse(mcp_tool_call_event->id, true); |
779 | 751 |
|
780 | 752 | } else if ("self.servo.set_range_servos" == mcp_tool_call_event->name) { |
781 | 753 | const auto start_index_ptr = mcp_tool_call_event->param<int64_t>("start_index"); |
@@ -820,16 +792,11 @@ void loop() { |
820 | 792 | *end_index_ptr, |
821 | 793 | *angle_ptr); |
822 | 794 |
|
823 | | - uint8_t index = 0; |
824 | | - for (index = *start_index_ptr; index <= *end_index_ptr; index++) { |
825 | | - if (!SetServoAngle(index, static_cast<uint16_t>(*angle_ptr))) { |
826 | | - engine.SendMcpCallError(mcp_tool_call_event->id, "Failed to set servo angle for index: " + std::to_string(index)); |
827 | | - break; |
828 | | - } |
829 | | - } |
830 | | - if (index > *end_index_ptr) { |
831 | | - engine.SendMcpCallResponse(mcp_tool_call_event->id, true); |
| 795 | + for (uint8_t index = *start_index_ptr - 1; index < *end_index_ptr; index++) { |
| 796 | + g_servos[index]->Write(static_cast<uint16_t>(*angle_ptr)); |
| 797 | + g_servo_angles[index - 1] = *angle_ptr; |
832 | 798 | } |
| 799 | + engine.SendMcpCallResponse(mcp_tool_call_event->id, true); |
833 | 800 |
|
834 | 801 | } else if ("self.servo.get_index_servo_angle" == mcp_tool_call_event->name) { |
835 | 802 | const auto index_ptr = mcp_tool_call_event->param<int64_t>("index"); |
|
0 commit comments