diff --git a/arduino_lib/devices/m5stack_servo_module.h b/arduino_lib/devices/m5stack_servo_module.h new file mode 100644 index 00000000..352a0c53 --- /dev/null +++ b/arduino_lib/devices/m5stack_servo_module.h @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include + +#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(); +} diff --git a/arduino_lib/devices/m5stack_tof_unit.h b/arduino_lib/devices/m5stack_tof_unit.h index 45b2e853..4616ec7f 100644 --- a/arduino_lib/devices/m5stack_tof_unit.h +++ b/arduino_lib/devices/m5stack_tof_unit.h @@ -117,9 +117,9 @@ std::optional> 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); diff --git a/sketchbooks/sdp_elevator_internal_panel/.gitignore b/sketchbooks/sdp_elevator_internal_panel/.gitignore new file mode 100644 index 00000000..03f4a3c1 --- /dev/null +++ b/sketchbooks/sdp_elevator_internal_panel/.gitignore @@ -0,0 +1 @@ +.pio diff --git a/sketchbooks/sdp_elevator_internal_panel/data/config.json b/sketchbooks/sdp_elevator_internal_panel/data/config.json new file mode 100644 index 00000000..0494f1c1 --- /dev/null +++ b/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 + } + ] +} \ No newline at end of file diff --git a/sketchbooks/sdp_elevator_internal_panel/include/README b/sketchbooks/sdp_elevator_internal_panel/include/README new file mode 100644 index 00000000..194dcd43 --- /dev/null +++ b/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 diff --git a/sketchbooks/sdp_elevator_internal_panel/include/lcd.h b/sketchbooks/sdp_elevator_internal_panel/include/lcd.h new file mode 100644 index 00000000..42ff451e --- /dev/null +++ b/sketchbooks/sdp_elevator_internal_panel/include/lcd.h @@ -0,0 +1,3 @@ +#pragma once + +void init_lcd(); \ No newline at end of file diff --git a/sketchbooks/sdp_elevator_internal_panel/lib/README b/sketchbooks/sdp_elevator_internal_panel/lib/README new file mode 100644 index 00000000..2593a33f --- /dev/null +++ b/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 +#include + +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 diff --git a/sketchbooks/sdp_elevator_internal_panel/lib/arduino_lib b/sketchbooks/sdp_elevator_internal_panel/lib/arduino_lib new file mode 120000 index 00000000..881b0713 --- /dev/null +++ b/sketchbooks/sdp_elevator_internal_panel/lib/arduino_lib @@ -0,0 +1 @@ +../../../arduino_lib \ No newline at end of file diff --git a/sketchbooks/sdp_elevator_internal_panel/lib/ros_lib b/sketchbooks/sdp_elevator_internal_panel/lib/ros_lib new file mode 120000 index 00000000..507fc2ea --- /dev/null +++ b/sketchbooks/sdp_elevator_internal_panel/lib/ros_lib @@ -0,0 +1 @@ +../../../ros_lib/ros_lib \ No newline at end of file diff --git a/sketchbooks/sdp_elevator_internal_panel/platformio.ini b/sketchbooks/sdp_elevator_internal_panel/platformio.ini new file mode 100644 index 00000000..dc7a0b6a --- /dev/null +++ b/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 \ No newline at end of file diff --git a/sketchbooks/sdp_elevator_internal_panel/src/lcd.cpp b/sketchbooks/sdp_elevator_internal_panel/src/lcd.cpp new file mode 100644 index 00000000..72a10fc4 --- /dev/null +++ b/sketchbooks/sdp_elevator_internal_panel/src/lcd.cpp @@ -0,0 +1,31 @@ +#include "lcd.h" + +#if 1 +#include + +#include "m5stack_utils/m5stack.h" +#endif + +#include +#include + +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); +} \ No newline at end of file diff --git a/sketchbooks/sdp_elevator_internal_panel/src/main.cpp b/sketchbooks/sdp_elevator_internal_panel/src/main.cpp new file mode 100644 index 00000000..362b1a32 --- /dev/null +++ b/sketchbooks/sdp_elevator_internal_panel/src/main.cpp @@ -0,0 +1,214 @@ +#include + +#if 1 +#include + +#include "m5stack_utils/m5stack.h" +#endif + +#include +#include +#define LGFX_USE_V1 +#define LGFX_AUTODETECT +#include +#include +#include +#include +#include + +#include +#include + +#include "devices/m5stack_servo_module.h" +#include "devices/uwb_module_util.h" +#include "esp32-hal-ledc.h" +#include "lcd.h" +#include "sdp/sdp.h" +#include "utils/config_loader.h" + +M5StackSerialPortInfo port_info_uwb = M5StackSerialPortInfoList[PORT_C]; + +// LovyanGFX +LGFX lcd; +LGFX_Sprite sprite_header(&lcd); +LGFX_Sprite sprite_status(&lcd); + +// Device Name +String device_name; + +// ESP-NOW +uint8_t mac_address[6] = {0}; + +// Interface +std::string packet_description_control = "Target floor"; +std::string serialization_format_control = "i"; +SDPInterfaceDescription interface_description_control = + std::make_tuple(packet_description_control, serialization_format_control); + +// UWB +int uwb_id = -1; +std::string packet_description_uwb = "UWB Station"; +std::string serialization_format_uwb = "i"; +std::vector body_uwb; + +// Config floor num -> (servo id, default angle, pressed angle) +std::map> floor_to_servo; + +// Other +std::vector 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") or + not doc.containsKey("panel_config")) { + Serial.println("Invalid config file"); + return false; + } + + device_name = doc["device_name"].as(); + uwb_id = doc["uwb_id"].as(); + JsonArray panel_config = doc["panel_config"].as(); + for (auto panel : panel_config) { + if (not panel.containsKey("floor") or + not panel.containsKey("servo_id") or + not panel.containsKey("default_angle") or + not panel.containsKey("pressed_angle")) { + Serial.println("Invalid panel config"); + return false; + } + int floor = panel["floor"].as(); + int servo_id = panel["servo_id"].as(); + float default_angle = panel["default_angle"].as(); + float pressed_angle = panel["pressed_angle"].as(); + floor_to_servo[floor] = std::make_tuple(servo_id, default_angle, pressed_angle); + } + return true; +} + +void callback_for_servo_control(const uint8_t* mac_address, const std::vector& body) { + int32_t target_floor = std::get(body[0]); + Serial.printf("Target floor: %d\n", target_floor); + sprite_status.fillScreen(TFT_WHITE); + sprite_status.setCursor(0, 0); + sprite_status.printf("Target floor: %d\n", target_floor); + sprite_status.pushSprite(0, lcd.height() / 3); + if (floor_to_servo.find(target_floor) != floor_to_servo.end()) { + int servo_id = std::get<0>(floor_to_servo[target_floor]); + float default_angle = std::get<1>(floor_to_servo[target_floor]); + float pressed_angle = std::get<2>(floor_to_servo[target_floor]); + sprite_status.printf("Servo ID: %d\n", servo_id); + sprite_status.printf("Default angle: %f\n", default_angle); + sprite_status.printf("Pressed angle: %f\n", pressed_angle); + sprite_status.pushSprite(0, lcd.height() / 3); + Servo_write_angle(servo_id, pressed_angle); + sprite_status.println("Pressed"); + sprite_status.pushSprite(0, lcd.height() / 3); + delay(500); + Servo_write_angle(servo_id, default_angle); + sprite_status.println("Done"); + sprite_status.pushSprite(0, lcd.height() / 3); + } else { + Serial.printf("Invalid target floor: %d\n", target_floor); + sprite_status.printf("Invalid target floor: %d\n", target_floor); + sprite_status.pushSprite(0, lcd.height() / 3); + } +} + +void setup() { + // put your setup code here, to run once: + M5.begin(true, true, true, true); + Serial.begin(115200); + Serial1.begin(115200, SERIAL_8N1, port_info_uwb.rx, port_info_uwb.tx); + init_servo_module(); + init_lcd(); + + sprite_header.println("SDP Elevator Internal Panel"); + sprite_header.pushSprite(0, 0); + + // Load config + SD.begin(); + SPIFFS.begin(); + if (not load_config_from_FS(SD, "/config.json")) { + if (not load_config_from_FS(SPIFFS, "/config.json")) { + Serial.println("Failed to load config file"); + sprite_status.printf("Failed to load config file\n"); + sprite_status.pushSprite(0, lcd.height() / 3); + while (true) { + delay(1000); + } + } + } + + // Display device name + sprite_header.printf("Device Name: %s\n", device_name.c_str()); + sprite_header.pushSprite(0, 0); + + // Initialization of SDP + if (not init_sdp(mac_address, device_name.c_str())) { + Serial.println("Failed to initialize SDP"); + sprite_status.printf("Failed to initialize SDP\n"); + sprite_status.pushSprite(0, lcd.height() / 3); + while (true) { + delay(1000); + } + } + register_sdp_interface_callback(interface_description_control, callback_for_servo_control); + Serial.println("SDP Initialized!"); + + sprite_header.printf("ADDR: %02X:%02X:%02X:%02X:%02X:%02X\n", + mac_address[0], mac_address[1], mac_address[2], + mac_address[3], mac_address[4], mac_address[5]); + sprite_header.pushSprite(0, 0); + + // 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) { + sprite_header.printf("UWB ID: %d\n", uwb_id); + } else { + uwb_id = -1; + sprite_header.printf("UWB ID: Failed to initialize\n"); + } + } else { + bool result = resetUWB(Serial1); + sprite_header.printf("UWB ID: Not initialized\n"); + } + sprite_header.pushSprite(0, 0); + + // Move servo to default position + for (auto [floor, servo_info] : floor_to_servo) { + sprite_status.fillScreen(TFT_WHITE); + sprite_status.setCursor(0, 0); + int servo_id = std::get<0>(servo_info); + float default_angle = std::get<1>(servo_info); + float pressed_angle = std::get<2>(servo_info); + sprite_status.printf("Testing servo %d\n", servo_id); + sprite_status.printf("Move to %.2f deg\n", default_angle); + sprite_status.pushSprite(0, lcd.height() / 3); + Servo_write_angle(servo_id, default_angle); + delay(500); + sprite_status.printf("Move to %.2f deg\n", pressed_angle); + sprite_status.pushSprite(0, lcd.height() / 3); + Servo_write_angle(servo_id, pressed_angle); + delay(500); + } +} + +void loop() { + delay(100); + // Send SDP Data + 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()); + } + } +} \ No newline at end of file diff --git a/sketchbooks/sdp_elevator_internal_panel/test/README b/sketchbooks/sdp_elevator_internal_panel/test/README new file mode 100644 index 00000000..9b1e87bc --- /dev/null +++ b/sketchbooks/sdp_elevator_internal_panel/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 diff --git a/sketchbooks/sdp_m5stickcplus_elevator_internal_panel/data/config.json b/sketchbooks/sdp_m5stickcplus_elevator_internal_panel/data/config.json index 8b4564fc..446ca626 100644 --- a/sketchbooks/sdp_m5stickcplus_elevator_internal_panel/data/config.json +++ b/sketchbooks/sdp_m5stickcplus_elevator_internal_panel/data/config.json @@ -1,4 +1,4 @@ { "device_name": "Elev. Internal panel", - "uwb_id": -1 + "uwb_id": 0 } \ No newline at end of file diff --git a/sketchbooks/sdp_sesami_host/src/main.cpp b/sketchbooks/sdp_sesami_host/src/main.cpp index bf6ebf3c..480033d2 100644 --- a/sketchbooks/sdp_sesami_host/src/main.cpp +++ b/sketchbooks/sdp_sesami_host/src/main.cpp @@ -154,8 +154,8 @@ void setup() { M5.begin(true, true, true, false); Serial.begin(115200); - Serial1.begin(115200, SERIAL_8N1, PORT_A_SERIAL_RX, PORT_A_SERIAL_TX); - Serial2.begin(115200, SERIAL_8N1, PORT_C_SERIAL_RX, PORT_C_SERIAL_TX); + Serial1.begin(115200, SERIAL_8N1, PORT_C_SERIAL_RX, PORT_C_SERIAL_TX); + Serial2.begin(115200, SERIAL_8N1, PORT_A_SERIAL_RX, PORT_A_SERIAL_TX); // Sesami client uses serial2 M5.Lcd.printf("SDP SESAMI HOST DEVICE\n"); diff --git a/sketchbooks/sdp_switchbot_elevator_button/src/main.cpp b/sketchbooks/sdp_switchbot_elevator_button/src/main.cpp index b3532825..e0bafed1 100644 --- a/sketchbooks/sdp_switchbot_elevator_button/src/main.cpp +++ b/sketchbooks/sdp_switchbot_elevator_button/src/main.cpp @@ -29,8 +29,6 @@ LGFX lcd; LGFX_Sprite sprite_header(&lcd); LGFX_Sprite sprite_status(&lcd); -LGFX_Sprite sprite_info(&lcd); -LGFX_Sprite sprite_plot(&lcd); // Device Name String device_name; @@ -261,9 +259,7 @@ void setup() { } void loop() { - delay(100); - - Serial.println("Loop"); + delay(500); auto result = get_m5stack_tof_unit_data(); if (not result.has_value()) { @@ -274,7 +270,7 @@ void loop() { sprite_status.pushSprite(0, lcd.height() / 3); } else { auto [acnt, scnt, dist, status] = result.value(); - Serial.printf("acnt: %d, scnt: %d, dist: %d, status: %d\n", acnt, scnt, dist, status); + // Serial.printf("acnt: %d, scnt: %d, dist: %d, status: %d\n", acnt, scnt, dist, status); sprite_status.fillScreen(0xFFFFFF); sprite_status.setCursor(0, 0); sprite_status.printf("ambient count: %d\n", acnt); @@ -282,13 +278,13 @@ void loop() { sprite_status.printf("distance: %d\n", dist); sprite_status.printf("status: %d\n", status); if (dist > door_closed_distance + door_closed_clearance) { - Serial.println("Door is open"); + // Serial.println("Door is open"); sprite_status.printf("Door is open\n"); } else if (status == 11) { - Serial.println("Door is closed"); + // Serial.println("Door is closed"); sprite_status.printf("Door is closed\n"); } else { - Serial.println("Door status is unknown"); + // Serial.println("Door status is unknown"); sprite_status.printf("Door status is unknown\n"); } body_door_open.clear();