update

arduino_lib/devices/m5stack_servo_module.h

@@ -0,0 +1,38 @@
+#pragma once
+
+#include <Arduino.h>
+#include <M5Stack.h>
+#include <Wire.h>
+
+#define SERVO_ADDR 0x53
+
+void init_servo_module(bool init_wire = true) {
+  if (init_wire) {
+    Wire.begin(21, 22, 100000UL);
+  }
+}
+
+// addr 0x01 mean control the number 1 servo by us
+void Servo_write_us(uint8_t number, uint16_t us) {
+  Wire.beginTransmission(SERVO_ADDR);
+  Wire.write(0x00 | number);
+  Wire.write(us & 0x00ff);
+  Wire.write(us >> 8 & 0x00ff);
+  Wire.endTransmission();
+}
+
+// addr 0x11 mean control the number 1 servo by angle
+void Servo_write_angle(uint8_t number, float servo_angle) {
+  uint8_t angle;
+  if (servo_angle < 0) {
+    angle = 0;
+  } else if (servo_angle > 180) {
+    angle = 180;
+  } else {
+    angle = (uint8_t)servo_angle;
+  }
+  Wire.beginTransmission(SERVO_ADDR);
+  Wire.write(0x10 | number);
+  Wire.write(angle);
+  Wire.endTransmission();
+}

arduino_lib/devices/m5stack_tof_unit.h

@@ -117,9 +117,9 @@ std::optional<std::tuple<uint16_t, uint16_t, uint16_t, byte>> get_m5stack_tof_un
     cnt++;
   }
   if (val & 0x01) {
-    Serial.println("ready");
+    // Serial.println("ready");
   } else {
-    Serial.println("not ready");
+    // Serial.println("not ready");
     return std::nullopt;
   }
   _read_block_data_at(0x14, 12);

sketchbooks/sdp_elevator_internal_panel/.gitignore

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

sketchbooks/sdp_elevator_internal_panel/data/config.json

@@ -0,0 +1,54 @@
+{
+    "device_name": "Elev. Internal panel",
+    "uwb_id": 0,
+    "panel_config": [
+        {
+            "floor": 0,
+            "servo_id": 0,
+            "default_angle": 0.0,
+            "pressed_angle": 90.0
+        },
+        {
+            "floor": 1,
+            "servo_id": 1,
+            "default_angle": 0.0,
+            "pressed_angle": 90.0
+        },
+        {
+            "floor": 2,
+            "servo_id": 2,
+            "default_angle": 0.0,
+            "pressed_angle": 90.0
+        },
+        {
+            "floor": 3,
+            "servo_id": 3,
+            "default_angle": 0.0,
+            "pressed_angle": 90.0
+        },
+        {
+            "floor": 4,
+            "servo_id": 4,
+            "default_angle": 0.0,
+            "pressed_angle": 90.0
+        },
+        {
+            "floor": 5,
+            "servo_id": 5,
+            "default_angle": 0.0,
+            "pressed_angle": 90.0
+        },
+        {
+            "floor": 6,
+            "servo_id": 6,
+            "default_angle": 0.0,
+            "pressed_angle": 90.0
+        },
+        {
+            "floor": 7,
+            "servo_id": 7,
+            "default_angle": 0.0,
+            "pressed_angle": 90.0
+        }
+    ]
+}

sketchbooks/sdp_elevator_internal_panel/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_elevator_internal_panel/include/lcd.h

@@ -0,0 +1,3 @@
+#pragma once
+
+void init_lcd();

sketchbooks/sdp_elevator_internal_panel/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_elevator_internal_panel/lib/arduino_lib

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

sketchbooks/sdp_elevator_internal_panel/lib/ros_lib

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

sketchbooks/sdp_elevator_internal_panel/platformio.ini

@@ -0,0 +1,28 @@
+; 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:m5stack-fire]
+platform = espressif32
+board = m5stack-fire
+framework = arduino
+lib_ldf_mode = deep
+lib_deps = 
+	m5stack/M5Stack@^0.4.3
+	bblanchon/ArduinoJson @ ^6.21.3
+    lovyan03/LovyanGFX @ ^1.1.5
+build_flags =
+    -std=gnu++17
+    -DBOARD_HAS_PSRAM=0
+build_unflags =
+    -std=gnu++11
+    -DBOARD_HAS_PSRAM
+    ; -mfix-esp32-psram-cache-issue
+    ; -mfix-esp32-psram-cache-strategy=memw
+monitor_speed = 115200

sketchbooks/sdp_elevator_internal_panel/src/lcd.cpp

@@ -0,0 +1,31 @@
+#include "lcd.h"
+
+#if 1
+#include <M5Stack.h>
+
+#include "m5stack_utils/m5stack.h"
+#endif
+
+#include <LGFX_AUTODETECT.hpp>
+#include <LovyanGFX.hpp>
+
+extern LGFX lcd;
+extern LGFX_Sprite sprite_header;
+extern LGFX_Sprite sprite_status;
+
+void init_lcd() {
+  lcd.init();
+  lcd.setRotation(1);
+  lcd.setBrightness(255);
+  lcd.setColorDepth(16);
+  lcd.fillScreen(TFT_WHITE);
+
+  sprite_header.createSprite(lcd.width(), lcd.height() / 3);  // Pos: 0, 0
+  sprite_status.createSprite(lcd.width(), lcd.height() / 3);  // Pos: 0, lcd.height() / 3
+
+  sprite_header.fillScreen(TFT_WHITE);
+  sprite_header.setTextColor(TFT_BLACK);
+  sprite_header.setTextSize(1.5, 1.5);
+  sprite_status.fillScreen(TFT_WHITE);
+  sprite_status.setTextColor(TFT_BLACK);
+}