add sdp_m5stickcpplus_servohat

sketchbooks/sdp_m5stickcplus_servohat/.gitignore

@@ -0,0 +1 @@
+.pio

sketchbooks/sdp_m5stickcplus_servohat/data/config.json

@@ -0,0 +1,4 @@
+{
+    "device_name": "SDP Servo",
+    "uwb_id": -1
+}

sketchbooks/sdp_m5stickcplus_servohat/include/README

@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

sketchbooks/sdp_m5stickcplus_servohat/lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

sketchbooks/sdp_m5stickcplus_servohat/lib/arduino_lib

@@ -0,0 +1 @@
+../../../arduino_lib

sketchbooks/sdp_m5stickcplus_servohat/lib/ros_lib

@@ -0,0 +1 @@
+../../../ros_lib/ros_lib

sketchbooks/sdp_m5stickcplus_servohat/platformio.ini

@@ -0,0 +1,24 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:m5stick-cplus]
+platform = espressif32
+board = m5stick-c
+framework = arduino
+lib_ldf_mode = deep
+lib_deps = 
+	m5stack/M5StickCPlus @ ^0.1.0
+  	bblanchon/ArduinoJson @ ^6.21.3
+	lovyan03/LovyanGFX@ ^1.1.2
+    tanakamasayuki/EspEasyUtils
+build_flags = 
+	-DUSE_DISPLAY
+	-std=gnu++17
+build_unflags = -std=gnu++11

sketchbooks/sdp_m5stickcplus_servohat/src/main.cpp

@@ -0,0 +1,198 @@
+#include <Arduino.h>
+#include <M5StickCPlus.h>
+
+#include <variant>
+#include <vector>
+#define LGFX_USE_V1
+#define LGFX_AUTODETECT
+#include <ArduinoJson.h>
+#include <FS.h>
+#include <SPIFFS.h>
+#include <WiFi.h>
+#include <smart_device_protocol/Packet.h>
+
+#include <LGFX_AUTODETECT.hpp>
+#include <LovyanGFX.hpp>
+
+#include "EspEasyServo.h"
+#include "devices/uwb_module_util.h"
+#include "esp32-hal-ledc.h"
+#include "m5stack_utils/m5stickcplus.h"
+#include "sdp/sdp.h"
+#include "utils/config_loader.h"
+
+const int COUNT_LOW = 1500;
+const int COUNT_HIGH = 8500;
+const int TIMER_WIDTH = 16;
+const int LEDC_CHANNEL = 1;
+const int LEDC_FREQ = 50;
+const int LEDC_PIN = 26;
+
+const int SERVO_MAX_ANGLE = 180;
+const int SERVO_MIN_ANGLE = 0;
+const int SERVO_MAXIMUM_STEP = 5;
+
+M5StackSerialPortInfo port_info_uwb = M5StackSerialPortInfoList[PORT_A];
+
+// Device Name
+String device_name;
+
+// ESP-NOW
+uint8_t mac_address[6] = {0};
+
+// Interface
+std::string packet_description_control = "Servo control";
+std::string serialization_format_control = "i";
+SDPInterfaceDescription interface_description_control =
+    std::make_tuple(packet_description_control, serialization_format_control);
+
+// Light Status
+std::string packet_description_status = "Servo status (cur, min, max)";
+std::string serialization_format_status = "iii";
+std::vector<SDPData> body_status;
+
+// UWB
+int uwb_id = -1;
+std::string packet_description_uwb = "UWB Station";
+std::string serialization_format_uwb = "i";
+std::vector<SDPData> body_uwb;
+
+//
+int current_target = 0;
+int current_angle = 0;
+
+// Other
+std::vector<SDPData> data;
+StaticJsonDocument<1024> result_json;
+
+bool load_config_from_FS(fs::FS& fs, String filename = "/config.json") {
+  StaticJsonDocument<1024> doc;
+  if (not load_json_from_FS<1024>(fs, filename, doc)) {
+    return false;
+  }
+
+  if (not doc.containsKey("device_name") or not doc.containsKey("uwb_id")) {
+    return false;
+  }
+
+  device_name = doc["device_name"].as<String>();
+  uwb_id = doc["uwb_id"].as<int>();
+  return true;
+}
+
+void move_servo(int angle) {
+  int contained_angle = constrain(angle, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE);
+  uint32_t count = (uint32_t)(COUNT_LOW + (COUNT_HIGH - COUNT_LOW) * (contained_angle - SERVO_MIN_ANGLE) / (SERVO_MAX_ANGLE - SERVO_MIN_ANGLE));
+  ledcWrite(LEDC_CHANNEL, count);
+}
+
+void init_servo() {
+  ledcSetup(LEDC_CHANNEL, LEDC_FREQ, TIMER_WIDTH);
+  ledcAttachPin(LEDC_PIN, LEDC_CHANNEL);
+}
+
+void callback_for_servo_control(const uint8_t* mac_address, const std::vector<SDPData>& body) {
+  Serial.printf("Length of body: %d\n", body.size());
+  current_target = std::get<int32_t>(body[0]);
+  Serial.printf("Target: %d\n", current_target);
+}
+
+// void setup() {
+//   M5.begin(true, true, true);
+//   init_servo();
+// }
+
+// void loop() {
+//   for (int i = SERVO_MIN_ANGLE; i <= SERVO_MAX_ANGLE; i = i + 10) {
+//     move_servo(i);
+//     delay(1000);
+//   }
+// }
+
+void setup() {
+  // put your setup code here, to run once:
+  M5.begin(true, true, true);
+  Serial.begin(115200);
+  Serial1.begin(115200, SERIAL_8N1, port_info_uwb.rx, port_info_uwb.tx);
+
+  SPIFFS.begin();
+  if (not load_config_from_FS(SPIFFS, "/config.json")) {
+    Serial.println("Failed to load config file");
+    M5.lcd.printf("Failed to load config file\n");
+    while (true) {
+      delay(1000);
+    }
+  }
+
+  // Initialization of SDP
+  if (not init_sdp(mac_address, device_name.c_str())) {
+    Serial.println("Failed to initialize SDP");
+    M5.Lcd.printf("Failed to initialize SDP\n");
+    while (true) {
+      delay(1000);
+    }
+  }
+  register_sdp_interface_callback(interface_description_control, callback_for_servo_control);
+  Serial.println("SDP Initialized!");
+
+  // UWB module
+  if (uwb_id >= 0) {
+    bool result = initUWB(false, uwb_id, Serial1);
+    body_uwb.clear();
+    body_uwb.push_back(SDPData(uwb_id));
+    if (result) {
+      M5.Lcd.printf("UWB ID: %d\n", uwb_id);
+    } else {
+      uwb_id = -1;
+      M5.Lcd.printf("UWB ID: Failed to initialize\n");
+    }
+  } else {
+    bool result = resetUWB(Serial1);
+    M5.Lcd.printf("UWB ID: Not initialized\n");
+  }
+
+  // Init Servo
+  init_servo();
+  move_servo(current_target);
+  M5.Lcd.printf("Servo initialized\n");
+}
+
+void loop() {
+  delay(100);
+  // current angle update
+  if (current_target < SERVO_MIN_ANGLE) {
+    current_target = SERVO_MIN_ANGLE;
+  } else if (current_target > SERVO_MAX_ANGLE) {
+    current_target = SERVO_MAX_ANGLE;
+  }
+  if (current_angle < current_target) {
+    if (current_target - current_angle > SERVO_MAXIMUM_STEP) {
+      current_angle = current_angle + SERVO_MAXIMUM_STEP;
+    } else {
+      current_angle = current_target;
+    }
+  } else if (current_angle > current_target) {
+    if (current_angle - current_target > SERVO_MAXIMUM_STEP) {
+      current_angle = current_angle - SERVO_MAXIMUM_STEP;
+    } else {
+      current_angle = current_target;
+    }
+  }
+  move_servo(current_angle);
+  Serial.printf("Current target: %d, angle: %d\n", current_target, current_angle);
+  // Send SDP Data
+  body_status.clear();
+  body_status.push_back(SDPData((int32_t)current_angle));
+  body_status.push_back(SDPData((int32_t)SERVO_MIN_ANGLE));
+  body_status.push_back(SDPData((int32_t)SERVO_MAX_ANGLE));
+  if (not send_sdp_data_packet(packet_description_status, body_status)) {
+    Serial.printf("Failed to send SDP data packet\n");
+    Serial.printf("packet description is %s\n", packet_description_status.c_str());
+  }
+  if (uwb_id >= 0) {
+    if (not send_sdp_data_packet(packet_description_uwb, body_uwb)) {
+      Serial.printf("Failed to send SDP data packet\n");
+      Serial.printf("packet description is %s\n", packet_description_uwb.c_str());
+    }
+  }
+}

sketchbooks/sdp_m5stickcplus_servohat/test/README

@@ -0,0 +1,11 @@
+
+This directory is intended for PlatformIO Test Runner and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PlatformIO Unit Testing:
+- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html