├── docs ├── images │ ├── pullup.jpg │ ├── ESP32_pin_guide.png │ ├── Meridian_logo.png │ ├── Meridian_unity.png │ ├── Merimote_illust.jpg │ ├── roid1_direction.png │ ├── GettingStarted_1.png │ ├── GettingStarted_10.png │ ├── GettingStarted_11.png │ ├── GettingStarted_2.png │ ├── GettingStarted_3.png │ ├── GettingStarted_4.png │ ├── GettingStarted_5.png │ ├── GettingStarted_6.png │ ├── GettingStarted_7.png │ ├── GettingStarted_8.png │ ├── GettingStarted_9.png │ ├── Meridian_TWIN_Board.png │ ├── Meridian_console_py.png │ ├── circulation_illust.png │ └── GettingStarted_Download.png ├── Meridian90_220720.pdf ├── TypeK_pinassign.png ├── Meridian90_220720.xlsx ├── Meridian_TypeK_schema.pdf ├── Meridian_flowsystem_twin.png ├── Meridian_TWIN_flowchart_20240504.png └── Meridian_TWIN_module_diagram_20240506.png ├── Meridian_TWIN_ESP32 ├── .gitignore ├── lib │ ├── ESP32Wiimote_remocon_led1_on.png │ └── ESP32Wiimote │ │ ├── src │ │ ├── library.properties │ │ ├── TinyWiimote.h │ │ ├── ESP32Wiimote.h │ │ └── ESP32Wiimote.cpp │ │ └── README.md ├── platformio.ini ├── src │ ├── keys.h │ ├── mrd_wifi.h │ ├── mrd_util.h │ ├── mrd_disp.h │ ├── mrd_bt_pad.h │ ├── main.h │ ├── config.h │ ├── mrd_eeprom.h │ └── main.cpp ├── Meridian_TWIN_for_ESP32.code-workspace └── .clang-format ├── Meridian_TWIN_Tsy40 ├── .gitignore ├── lib │ ├── IcsClass_V210 │ │ ├── readme.md │ │ ├── library.properties │ │ ├── MIT_Licence.txt │ │ ├── keywords.txt │ │ ├── src │ │ │ ├── IcsHardSerialClass.h │ │ │ ├── IcsHardSerialClass.cpp │ │ │ └── IcsBaseClass.h │ │ └── examples │ │ │ └── KrrButton │ │ │ └── KrrButton.ino │ └── gs2d │ │ ├── crc16.h │ │ ├── gs2d_serial.h │ │ ├── gs2d_type.h │ │ ├── gs2d_command.h │ │ └── gs2d_driver.h ├── src │ ├── mrd_module │ │ ├── mv_firstIK.h │ │ ├── sv_ftc.h │ │ ├── sv_dxl2.h │ │ ├── sv_xbus.h │ │ ├── mv_motionplay.h │ │ ├── sv_ftbrx.h │ │ └── sv_ics.h │ ├── mrd_move.h │ ├── mrd_hwtimer.h │ ├── mrd_wire1.h │ ├── mrd_sd.h │ ├── mrd_servo.h │ ├── mrd_util.h │ ├── mrd_pad.h │ ├── mrd_wire0.h │ ├── mrd_disp.h │ ├── mrd_command.h │ ├── main.h │ └── mrd_eeprom.h ├── platformio.ini ├── Meridian_TWIN_for_Tsy40.code-workspace └── .clang-format ├── .gitignore └── LICENSE /docs/images/pullup.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/pullup.jpg -------------------------------------------------------------------------------- /docs/Meridian90_220720.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/Meridian90_220720.pdf -------------------------------------------------------------------------------- /docs/TypeK_pinassign.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/TypeK_pinassign.png -------------------------------------------------------------------------------- /docs/Meridian90_220720.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/Meridian90_220720.xlsx -------------------------------------------------------------------------------- /docs/Meridian_TypeK_schema.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/Meridian_TypeK_schema.pdf -------------------------------------------------------------------------------- /docs/images/ESP32_pin_guide.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/ESP32_pin_guide.png -------------------------------------------------------------------------------- /docs/images/Meridian_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/Meridian_logo.png -------------------------------------------------------------------------------- /docs/images/Meridian_unity.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/Meridian_unity.png -------------------------------------------------------------------------------- /docs/images/Merimote_illust.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/Merimote_illust.jpg -------------------------------------------------------------------------------- /docs/images/roid1_direction.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/roid1_direction.png -------------------------------------------------------------------------------- /docs/Meridian_flowsystem_twin.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/Meridian_flowsystem_twin.png -------------------------------------------------------------------------------- /docs/images/GettingStarted_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/GettingStarted_1.png -------------------------------------------------------------------------------- /docs/images/GettingStarted_10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/GettingStarted_10.png -------------------------------------------------------------------------------- /docs/images/GettingStarted_11.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/GettingStarted_11.png -------------------------------------------------------------------------------- /docs/images/GettingStarted_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/GettingStarted_2.png -------------------------------------------------------------------------------- /docs/images/GettingStarted_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/GettingStarted_3.png -------------------------------------------------------------------------------- /docs/images/GettingStarted_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/GettingStarted_4.png -------------------------------------------------------------------------------- /docs/images/GettingStarted_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/GettingStarted_5.png -------------------------------------------------------------------------------- /docs/images/GettingStarted_6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/GettingStarted_6.png -------------------------------------------------------------------------------- /docs/images/GettingStarted_7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/GettingStarted_7.png -------------------------------------------------------------------------------- /docs/images/GettingStarted_8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/GettingStarted_8.png -------------------------------------------------------------------------------- /docs/images/GettingStarted_9.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/GettingStarted_9.png -------------------------------------------------------------------------------- /docs/images/Meridian_TWIN_Board.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/Meridian_TWIN_Board.png -------------------------------------------------------------------------------- /docs/images/Meridian_console_py.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/Meridian_console_py.png -------------------------------------------------------------------------------- /docs/images/circulation_illust.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/circulation_illust.png -------------------------------------------------------------------------------- /docs/Meridian_TWIN_flowchart_20240504.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/Meridian_TWIN_flowchart_20240504.png -------------------------------------------------------------------------------- /docs/images/GettingStarted_Download.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/images/GettingStarted_Download.png -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/.browse.c_cpp.db* 3 | .vscode/c_cpp_properties.json 4 | .vscode/launch.json 5 | .vscode/ipch 6 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/.browse.c_cpp.db* 3 | .vscode/c_cpp_properties.json 4 | .vscode/launch.json 5 | .vscode/ipch 6 | -------------------------------------------------------------------------------- /docs/Meridian_TWIN_module_diagram_20240506.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/docs/Meridian_TWIN_module_diagram_20240506.png -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/lib/IcsClass_V210/readme.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/Meridian_TWIN_Tsy40/lib/IcsClass_V210/readme.md -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/lib/ESP32Wiimote_remocon_led1_on.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ninagawa123/Meridian_TWIN/HEAD/Meridian_TWIN_ESP32/lib/ESP32Wiimote_remocon_led1_on.png -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/lib/IcsClass_V210/library.properties: -------------------------------------------------------------------------------- 1 | name=IcsClass_V210 2 | version=2.1 3 | author=T.Nobuhara 4 | maintainer=Kondo Kagaku co.,ltd. 5 | sentence=ICS device control 6 | paragraph=This library where ICS device(servo motor etc...) is controlled using Serial(HardwareSerial) in Arduino. 7 | category=Device Control 8 | url=http://kondo-robot.com/ 9 | architectures=* 10 | -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/lib/ESP32Wiimote/src/library.properties: -------------------------------------------------------------------------------- 1 | name=ESP32Wiimote 2 | version=0.1.3 3 | author=Daiki 4 | maintainer=Daiki 5 | sentence=Wiimote Library for ESP32 6 | paragraph=library that connects with a Wii remote. Supported chip ESP32. Included with some examples for real application. 7 | category=Communication 8 | url=https://github.com/bigw00d/ESP32Wiimote 9 | architectures=* 10 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/lib/gs2d/crc16.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace crc16 4 | { 5 | unsigned short calculate(unsigned char* data, unsigned short length) 6 | { 7 | unsigned short crc = 0; 8 | for (unsigned short i = 0; i < length; i++) { 9 | crc ^= (data[i] << 8); 10 | for (unsigned char j = 0; j < 8; j++) { 11 | if (crc & 0x8000) { 12 | crc = (crc << 1) ^ 0x8005; 13 | } 14 | else { 15 | crc <<= 1; 16 | } 17 | } 18 | } 19 | 20 | return crc; 21 | } 22 | } 23 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_module/mv_firstIK.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_MOVEMENT_FIRST_IK_H__ 2 | #define __MERIDIAN_MOVEMENT_FIRST_IK_H__ 3 | 4 | #include "main.h" 5 | #include "config.h" 6 | 7 | //================================================================================================================ 8 | // 逆運動学関連の処理 9 | //================================================================================================================ 10 | 11 | void mrd_ik_x() 12 | { 13 | // 14 | } 15 | 16 | #endif // __MERIDIAN_MOVEMENT_FIRST_IK_H__ 17 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_module/sv_ftc.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_SERVO_FEETECH_H__ 2 | #define __MERIDIAN_SERVO_FEETECH_H__ 3 | 4 | #include "main.h" 5 | #include "config.h" 6 | 7 | //================================================================================================================ 8 | // FEETECH TTLサーボ関連の処理 9 | //================================================================================================================ 10 | 11 | void mrd_servo_ftc_x() 12 | { 13 | // 14 | } 15 | 16 | #endif // __MERIDIAN_SERVO_FEETECH_H__ 17 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_module/sv_dxl2.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_SERVO_DYNAMIXEL_H__ 2 | #define __MERIDIAN_SERVO_DYNAMIXEL_H__ 3 | 4 | #include "main.h" 5 | #include "config.h" 6 | 7 | //================================================================================================================ 8 | // DYNOMIXELサーボ関連の処理 9 | //================================================================================================================ 10 | 11 | void mrd_servo_dxl20_x() 12 | { 13 | // 14 | } 15 | 16 | #endif // __MERIDIAN_SERVO_DYNAMIXEL_H__ 17 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_module/sv_xbus.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_SERVO_JRPROPO_XBUS_H__ 2 | #define __MERIDIAN_SERVO_JRPROPO_XBUS_H__ 3 | 4 | #include "main.h" 5 | #include "config.h" 6 | 7 | //================================================================================================================ 8 | // JR PROPO XBUSサーボ関連の処理 9 | //================================================================================================================ 10 | 11 | void mrd_servo_xbus_x() 12 | { 13 | // 14 | } 15 | 16 | #endif // __MERIDIAN_SERVO_JRPROPO_XBUS_H__ 17 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_module/mv_motionplay.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_MOVEMENT_MOTIONPLAY_H__ 2 | #define __MERIDIAN_MOVEMENT_MOTIONPLAY_H__ 3 | 4 | #include "main.h" 5 | #include "config.h" 6 | 7 | //================================================================================================================ 8 | // モーション再生関連の処理 9 | //================================================================================================================ 10 | 11 | void mrd_mv_motionplay_x() 12 | { 13 | // 14 | } 15 | 16 | #endif // __MERIDIAN_MOVEMENT_MOTIONPLAY_H__ 17 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_module/sv_ftbrx.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_SERVO_FUTABA_RSxTTL_H__ 2 | #define __MERIDIAN_SERVO_FUTABA_RSxTTL_H__ 3 | 4 | #include "main.h" 5 | #include "config.h" 6 | 7 | //================================================================================================================ 8 | // FUTABA RSxTTLサーボ関連の処理 9 | //================================================================================================================ 10 | 11 | void mrd_servo_ftbrs_x() 12 | { 13 | // 14 | } 15 | 16 | #endif // __MERIDIAN_SERVO_FUTABA_RSxTTL_H__ 17 | -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | [env:esp32dev] 12 | platform = espressif32 13 | board = esp32dev 14 | framework = arduino 15 | build_flags = -DCORE_DEBUG_LEVEL=0 16 | monitor_speed = 1000000 17 | lib_deps = 18 | ninagawa123/Meridian@^0.1.0 19 | hideakitai/ESP32DMASPI@0.3.0 20 | board_build.partitions = no_ota.csv 21 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # General 2 | .DS_Store 3 | .AppleDouble 4 | .LSOverride 5 | ._* 6 | ~$* 7 | 8 | # Prerequisites 9 | *.d 10 | 11 | # Compiled Object files 12 | *.slo 13 | *.lo 14 | *.o 15 | *.obj 16 | 17 | # Precompiled Headers 18 | *.gch 19 | *.pch 20 | 21 | # Compiled Dynamic libraries 22 | *.so 23 | *.dylib 24 | *.dll 25 | 26 | # Fortran module files 27 | *.mod 28 | *.smod 29 | 30 | # Compiled Static libraries 31 | *.lai 32 | *.la 33 | *.a 34 | *.lib 35 | 36 | # Executables 37 | *.exe 38 | *.out 39 | *.app 40 | 41 | # Platform files 42 | **/.pio 43 | 44 | # Vscode files 45 | **/.vscode/.browse.c_cpp.db* 46 | **/.vscode/c_cpp_properties.json 47 | **/.vscode/launch.json 48 | **/.vscode/ipch 49 | 50 | # Doxygen files 51 | doxygen 52 | 53 | # My Project 54 | keys.h -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/platformio.ini: -------------------------------------------------------------------------------- 1 | 2 | ; PlatformIO Project Configuration File 3 | ; 4 | ; Build options: build flags, source filter 5 | ; Upload options: custom upload port, speed and extra flags 6 | ; Library options: dependencies, extra library storages 7 | ; Advanced options: extra scripting 8 | ; 9 | ; Please visit documentation for the other options and examples 10 | ; https://docs.platformio.org/page/projectconf.html 11 | 12 | [env:teensy40] 13 | platform = teensy 14 | board = teensy40 15 | framework = arduino 16 | monitor_speed = 6000000 17 | lib_deps = 18 | ninagawa123/Meridian@^0.1.0 19 | hideakitai/TsyDMASPI@^0.1.3 20 | electroniccats/MPU6050@^0.5.0 21 | adafruit/Adafruit MPU6050@^2.2.1 22 | adafruit/Adafruit BusIO@^1.15.0 23 | adafruit/Adafruit Unified Sensor@^1.1.14 24 | adafruit/Adafruit BNO055@^1.6.3 25 | -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/src/keys.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_KEYS_H__ 2 | #define __MERIDIAN_KEYS_H__ 3 | 4 | // Wifiアクセスポイントの設定 5 | #define WIFI_AP_SSID "xxxxxxxx" // アクセスポイントのWIFI_AP_SSID 6 | #define WIFI_AP_PASS "xxxxxxxx" // アクセスポイントのパスワード 7 | #define WIFI_SEND_IP "192.168.1.xx" // 送り先のPCのIPアドレス（PCのIPアドレスを調べておく） 8 | #define UDP_SEND_PORT 22222 // 送り先のポート番号 9 | #define UDP_RECV_PORT 22224 // このESP32のポート番号 10 | 11 | // ESP32のIPアドレスを固定する場合は下記の4項目を設定 12 | #define MODE_FIXED_IP 0 // IPアドレスを固定するか（0:NO, 1:YES） 13 | #define FIXED_IP_ADDR "192. 168. 1. xx" // ESP32のIPアドレスを固定する場合のESPのIPアドレス 14 | #define FIXED_IP_GATEWAY "192. 168. 1. xx" // ESP32のIPアドレスを固定する場合のルーターのゲートウェイ 15 | #define FIXED_IP_SUBNET "255. 255. 255. 0" // ESP32のIPアドレスを固定する場合のサブネット 16 | 17 | #endif // __MERIDIAN_KEYS_H__ 18 | -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/Meridian_TWIN_for_ESP32.code-workspace: -------------------------------------------------------------------------------- 1 | { 2 | "folders": [ 3 | { 4 | "path": "." 5 | } 6 | ], 7 | "settings": { 8 | "workbench.colorCustomizations": { 9 | "activityBar.activeBackground": "#ab307e", 10 | "activityBar.background": "#ab307e", 11 | "activityBar.foreground": "#e7e7e7", 12 | "activityBar.inactiveForeground": "#e7e7e799", 13 | "activityBarBadge.background": "#25320e", 14 | "activityBarBadge.foreground": "#e7e7e7", 15 | "commandCenter.border": "#e7e7e799", 16 | "sash.hoverBorder": "#ab307e", 17 | "statusBar.background": "#832561", 18 | "statusBar.foreground": "#e7e7e7", 19 | "statusBarItem.hoverBackground": "#ab307e", 20 | "statusBarItem.remoteBackground": "#832561", 21 | "statusBarItem.remoteForeground": "#e7e7e7", 22 | "titleBar.activeBackground": "#832561", 23 | "titleBar.activeForeground": "#e7e7e7", 24 | "titleBar.inactiveBackground": "#83256199", 25 | "titleBar.inactiveForeground": "#e7e7e799" 26 | }, 27 | "peacock.color": "#832561" 28 | } 29 | } -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/Meridian_TWIN_for_Tsy40.code-workspace: -------------------------------------------------------------------------------- 1 | { 2 | "folders": [ 3 | { 4 | "path": "." 5 | } 6 | ], 7 | "settings": { 8 | "workbench.colorCustomizations": { 9 | "activityBar.activeBackground": "#3399ff", 10 | "activityBar.background": "#3399ff", 11 | "activityBar.foreground": "#15202b", 12 | "activityBar.inactiveForeground": "#15202b99", 13 | "activityBarBadge.background": "#bf0060", 14 | "activityBarBadge.foreground": "#e7e7e7", 15 | "commandCenter.border": "#e7e7e799", 16 | "sash.hoverBorder": "#3399ff", 17 | "statusBar.background": "#007fff", 18 | "statusBar.foreground": "#e7e7e7", 19 | "statusBarItem.hoverBackground": "#3399ff", 20 | "statusBarItem.remoteBackground": "#007fff", 21 | "statusBarItem.remoteForeground": "#e7e7e7", 22 | "titleBar.activeBackground": "#007fff", 23 | "titleBar.activeForeground": "#e7e7e7", 24 | "titleBar.inactiveBackground": "#007fff99", 25 | "titleBar.inactiveForeground": "#e7e7e799" 26 | }, 27 | "peacock.color": "#007fff" 28 | } 29 | } -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_move.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_MOVEMENT_H__ 2 | #define __MERIDIAN_MOVEMENT_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "main.h" 7 | 8 | //================================================================================================================ 9 | // 動作計算関連の処理 10 | //================================================================================================================ 11 | 12 | //------------------------------------------------------------------------------------ 13 | // 各動作計算モジュールへの分岐 14 | //------------------------------------------------------------------------------------ 15 | 16 | /// @brief モーション初期化のためのスタブ関数. 現在は何も行わず, 常にfalseを返す. 17 | /// @param mrd_s_meridim モーションデータを保持する構造体. 18 | /// @return 常にfalseを返す. 19 | bool mrd_move_init(Meridim90Union mrd_s_meridim) 20 | { 21 | return false; 22 | } 23 | 24 | //------------------------------------------------------------------------------------ 25 | // 各種オペレーション 26 | //------------------------------------------------------------------------------------ 27 | 28 | #endif // __MERIDIAN_MOVEMENT_H__ 29 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Ninagawa123 (GitHub) 4 | and Project Meridian Team 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/lib/IcsClass_V210/MIT_Licence.txt: -------------------------------------------------------------------------------- 1 | 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2016 Kondo Kagaku co.,ltd. 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in 14 | all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | THE SOFTWARE. -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_hwtimer.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_HARDWARE_TIMER_H__ 2 | #define __MERIDIAN_HARDWARE_TIMER_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "main.h" 7 | 8 | //================================================================================================================ 9 | // HardwareTimer関連の処理 10 | //================================================================================================================ 11 | // グローバル変数の宣言 12 | volatile unsigned long hwtimer_counter = 0; // ハードウェアタイマーのフレーム管理用カウント 13 | 14 | // ハードウェアタイマーの定義 15 | IntervalTimer hwtimer; 16 | 17 | /// @brief hwtimer_counterを保護しつつ1ずつインクリメント 18 | inline void hwtimer_tick() { 19 | __disable_irq(); // 割り込みを無効化 20 | hwtimer_counter++; 21 | __enable_irq(); // 割り込みを再度有効化 22 | } 23 | 24 | /// @brief 指定msでカウントアップするハードウェアタイマーを開始 25 | /// @param a_frame_duration ハードウェアタイマーをカウントアップする間隔(ms) 26 | inline void mrd_hwtimer_start_tsy(int a_frame_duration) { 27 | hwtimer.begin(hwtimer_tick, a_frame_duration * 1000); // タイマー周期をマイクロ秒で指定 28 | } 29 | 30 | /// @brief count_timerの安全な読み取り関数 31 | inline unsigned long mrd_hwtimer_read_tsy() { 32 | unsigned long timer_tmp = 0; 33 | __disable_irq(); // 割り込みを無効化 34 | timer_tmp = hwtimer_counter; 35 | __enable_irq(); // 割り込みを再度有効化 36 | return timer_tmp; 37 | } 38 | 39 | #endif // __MERIDIAN_HARDWARE_TIMER_H__ 40 | -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/lib/ESP32Wiimote/src/TinyWiimote.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Daiki Yasuda 2 | // 3 | // This is licensed under 4 | // - Creative Commons Attribution-NonCommercial 3.0 Unported 5 | // - https://creativecommons.org/licenses/by-nc/3.0/ 6 | // - Or see LICENSE.md 7 | // 8 | // The short of it is... 9 | // You are free to: 10 | // Share — copy and redistribute the material in any medium or format 11 | // Adapt — remix, transform, and build upon the material 12 | // Under the following terms: 13 | // NonCommercial — You may not use the material for commercial purposes. 14 | 15 | #ifndef _TINY_WIIMOTE_H_ 16 | #define _TINY_WIIMOTE_H_ 17 | 18 | #define RECIEVED_DATA_MAX_LEN (50) 19 | struct TinyWiimoteData { 20 | uint8_t number; 21 | uint8_t data[RECIEVED_DATA_MAX_LEN]; 22 | uint8_t len; 23 | }; 24 | //#define TWII_OFFSET_BTNS1 (2) 25 | //#define TWII_OFFSET_BTNS2 (3) 26 | //#define TWII_OFFSET_EXTCTRL (4) // Offset for Extension Controllers data 27 | 28 | typedef struct tinywii_device_callback { 29 | void (*hci_send_packet)(uint8_t *data, size_t len); 30 | } TwHciInterface; 31 | 32 | void TinyWiimoteInit(TwHciInterface hciInterface); 33 | int TinyWiimoteAvailable(void); 34 | TinyWiimoteData TinyWiimoteRead(void); 35 | 36 | void TinyWiimoteResetDevice(void); 37 | bool TinyWiimoteDeviceIsInited(void); 38 | 39 | void TinyWiimoteReqAccelerometer(bool use); 40 | 41 | void handleHciData(uint8_t* data, size_t len); 42 | 43 | char* format2Hex(uint8_t* data, uint16_t len); 44 | 45 | #endif // _TINY_WIIMOTE_H_ 46 | -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/lib/ESP32Wiimote/README.md: -------------------------------------------------------------------------------- 1 | # ESP32Wiimote 2 | 3 | ESP32Wiimote is an Arduino library for ESP32 devices, that connects over Bluetooth with a Wii remote (Wiimote), and its optional connected Nunchuk. 4 | 5 | This fork has the following improvements: 6 | - better output in example 7 | - optional accelerometer read-out of Wiimote itself 8 | 9 | On the ESP32, it reports easily at 100Hz: 10 | - all regular button presses (A/B/C/Z/1/2/-/Home/+/D-Pad) 11 | - the 3-dimensional acceleration/orientation of both Wiimote and Nunchuk 12 | - the analog joystick of the Nunchuk 13 | 14 | ## Requirement 15 | 16 | - ESP32 board (any) 17 | - Arduino IDE (Version >= 1.8.5) 18 | - Wii Remote (RVL-CNT-01) 19 | - Wii Nunchuk (optional) 20 | 21 | ## Installation 22 | 1. Download the zip file. 23 | 2. Move the zip file to your libraries directory. 24 | 3. In the Arduino IDE, navigate to Sketch > Include Library > Add .ZIP Library. 25 | 4. Select the zip file. 26 | 27 | ## Examples 28 | 29 | A full example can be found at [ESP32WiimoteDemo.ino](./examples/ESP32WiimoteDemo/ESP32WiimoteDemo.ino) 30 | 31 | - Caution: the accelerometers report a lot of data 32 | - This can get filtered/prevented by using 'add filter(ACTION_IGNORE,...)' 33 | - The reports from the analog joystick of the Nunchuk can als be configured for larger minimal steps 34 | 35 | ## Usage 36 | No need to pair the controller over Bluetooth. Just do: 37 | 1. To connect, press the 1 and 2 buttons on Wii Remote 38 | 2. The LED1 will be on when they have finished connecting 39 | 40 | 41 | ## Licence 42 | 43 | see [LICENSE.md](./LICENSE.md) 44 | -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: LLVM 2 | ColumnLimit: 0 3 | AlignTrailingComments: true 4 | AlignConsecutiveMacros: true 5 | 6 | # コーディングスタイルはおおむねLLVMガイドラインに準拠する. 7 | 8 | # 9 | # 1. 変数名はスネークケースとする 例) a_meridim 10 | # 2. 関数名はスネークケースとする(ROS風) 11 | # 3. クラス名・構造体名・列挙型名はパスカルケースとする 12 | # 4. 関数宣言時の引数は a_ をつける 例) a_meridim, メンバ変数は m_ 13 | # 5. 関数内の補助変数は _tmp をつける 例) int count_tmp 14 | # 6. モジュールとなるファイル名は mrd_ をつける 例) mrd_wifi.h 15 | 16 | # <スネークケースの使い方> 17 | # 1. 大カテゴリ(名詞) > 中カテゴリ(名詞) > 内容(名詞・動詞) > 動詞 > 補足 18 | # の優先順とする. カテゴリ順を守りつつ, 後半は重要な単語が早めに現れるようにする. 19 | # 例) mrd_wire0_init, MCMD_SENSOR_YAW_CALIB 20 | # 2. meridianオリジナルの関数は先頭に mrd_ をつける. 21 | # 3. 関数をライブラリやクラスに昇格する際は, _の区切りでカテゴライズする. 22 | # 例) mrd_wire0_init → mrd_wire0.init 23 | 24 | # <ドキュメンテーションコメント> 25 | # 関数などの解説には下記のように///を使用する. 26 | # 27 | # /// @brief 解説. 28 | # /// @param 引数. 29 | # /// @return 戻り値. 30 | # 31 | # /** */はコードの改造時にブロックコメントアウトとして使用できるよう, 32 | # ドキュメンテーションコメントとしては使用しないこと. 33 | 34 | # <変数等のコメント> 35 | # 変数などの解説は同じ行もしくは一つ上の行に// でコメントをつける. 36 | # 37 | # こうすることで, VSCODE上で関数や変数にカーソルを当てた時に説明が表示されるようになる. 38 | 39 | # <主な用語> 40 | # Meridian : デバイスやソフトを中間プロトコルでつなぐリアルタイムシステム 41 | # Meridim : Meridianのコアとなる配列 42 | # Meridim90 : 基準となる簡易的な固定配列(180バイト) 43 | 44 | # <主な定数の命名ルール> 45 | # MODE_ 動作モードのconfig 46 | # EEPROM_ EEPROM関連 47 | # CHECK_ 起動時の動作チェック関連 48 | # MONITOR_ シリアルモニタリング 49 | # MOUNT_ ハードウェアのマウント設定 50 | # PAD_ ジョイパッド・リモコン関連 51 | # SERVO_ サーボの通信設定関連 52 | # MCMD_ Meridimのマスターコマンド 53 | # PIN_ ピンアサイン 54 | # IXR_ L系統のサーボパラメータ 55 | # IXL_ R系統のサーボパラメータ 56 | # IXC_ C系統のサーボパラメータ 57 | # MRD_ Meridim90の配列キー(例外ありconfig.h参照) 58 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: LLVM 2 | ColumnLimit: 0 3 | AlignTrailingComments: true 4 | AlignConsecutiveMacros: true 5 | 6 | # コーディングスタイルはおおむねLLVMガイドラインに準拠する. 7 | 8 | # 9 | # 1. 変数名はスネークケースとする 例) a_meridim 10 | # 2. 関数名はスネークケースとする(ROS風) 11 | # 3. クラス名・構造体名・列挙型名はパスカルケースとする 12 | # 4. 関数宣言時の引数は a_ をつける 例) a_meridim, メンバ変数は m_ 13 | # 5. 関数内の補助変数は _tmp をつける 例) int count_tmp 14 | # 6. モジュールとなるファイル名は mrd_ をつける 例) mrd_wifi.h 15 | 16 | # <スネークケースの使い方> 17 | # 1. 大カテゴリ(名詞) > 中カテゴリ(名詞) > 内容(名詞・動詞) > 動詞 > 補足 18 | # の優先順とする. カテゴリ順を守りつつ, 後半は重要な単語が早めに現れるようにする. 19 | # 例) mrd_wire0_init, MCMD_SENSOR_YAW_CALIB 20 | # 2. meridianオリジナルの関数は先頭に mrd_ をつける. 21 | # 3. 関数をライブラリやクラスに昇格する際は, _の区切りでカテゴライズする. 22 | # 例) mrd_wire0_init → mrd_wire0.init 23 | 24 | # <ドキュメンテーションコメント> 25 | # 関数などの解説には下記のように///を使用する. 26 | # 27 | # /// @brief 解説. 28 | # /// @param 引数. 29 | # /// @return 戻り値. 30 | # 31 | # /** */はコードの改造時にブロックコメントアウトとして使用できるよう, 32 | # ドキュメンテーションコメントとしては使用しないこと. 33 | 34 | # <変数等のコメント> 35 | # 変数などの解説は同じ行もしくは一つ上の行に// でコメントをつける. 36 | # 37 | # こうすることで, VSCODE上で関数や変数にカーソルを当てた時に説明が表示されるようになる. 38 | 39 | # <主な用語> 40 | # Meridian : デバイスやソフトを中間プロトコルでつなぐリアルタイムシステム 41 | # Meridim : Meridianのコアとなる配列 42 | # Meridim90 : 基準となる簡易的な固定配列(180バイト) 43 | 44 | # <主な定数の命名ルール> 45 | # MODE_ 動作モードのconfig 46 | # EEPROM_ EEPROM関連 47 | # CHECK_ 起動時の動作チェック関連 48 | # MONITOR_ シリアルモニタリング 49 | # MOUNT_ ハードウェアのマウント設定 50 | # PAD_ ジョイパッド・リモコン関連 51 | # SERVO_ サーボの通信設定関連 52 | # MCMD_ Meridimのマスターコマンド 53 | # PIN_ ピンアサイン 54 | # IXR_ L系統のサーボパラメータ 55 | # IXL_ R系統のサーボパラメータ 56 | # IXC_ C系統のサーボパラメータ 57 | # MRD_ Meridim90の配列キー(例外ありconfig.h参照) 58 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/lib/IcsClass_V210/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map IcsClass 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | IcsBaseClass KEYWORD1 10 | IcsHardSerialClass KEYWORD1 11 | KRR_BUTTON KEYWORD1 12 | 13 | ####################################### 14 | # Methods and Functions (KEYWORD2) 15 | ####################################### 16 | 17 | begin KEYWORD2 18 | synchronize KEYWORD2 19 | 20 | setPos KEYWORD2 21 | setFree KEYWORD2 22 | 23 | setStrc KEYWORD2 24 | setSpd KEYWORD2 25 | setCur KEYWORD2 26 | setTmp KEYWORD2 27 | 28 | getStrc KEYWORD2 29 | getSpd KEYWORD2 30 | getCur KEYWORD2 31 | getTmp KEYWORD2 32 | getPos KEYWORD2 33 | 34 | setID KEYWORD2 35 | getID KEYWORD2 36 | 37 | getKrrButton KEYWORD2 38 | getKrrAnalog KEYWORD2 39 | getKrrAllData KEYWORD2 40 | 41 | degPos KEYWORD2 42 | posDeg KEYWORD2 43 | degPos100 KEYWORD2 44 | posDeg100 KEYWORD2 45 | 46 | 47 | ####################################### 48 | # Constants (LITERAL1) 49 | ####################################### 50 | MAX_POS LITERAL1 51 | MIN_POS LITERAL1 52 | 53 | ICS_FALSE LITERAL1 54 | 55 | KRR_BUTTON_NONE LITERAL1 56 | KRR_BUTTON_UP LITERAL1 57 | KRR_BUTTON_DOWN LITERAL1 58 | KRR_BUTTON_RIGHT LITERAL1 59 | KRR_BUTTON_LEFT LITERAL1 60 | KRR_BUTTON_TRIANGLE LITERAL1 61 | KRR_BUTTON_CROSS LITERAL1 62 | KRR_BUTTON_CIRCLE LITERAL1 63 | KRR_BUTTON_SQUARE LITERAL1 64 | KRR_BUTTON_S1 LITERAL1 65 | KRR_BUTTON_S2 LITERAL1 66 | KRR_BUTTON_S3 LITERAL1 67 | KRR_BUTTON_S4 LITERAL1 68 | KRR_BUTTON_FALSE LITERAL1 69 | 70 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/lib/gs2d/gs2d_serial.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @file gs2d_serial.h 3 | * @author 4 | * @date 2021/01/26 5 | * @brief Serial Base Class 6 | */ 7 | #pragma once 8 | 9 | /* Includes ------------------------------------------------------------------*/ 10 | /* USER INCLUDE CODE START */ 11 | 12 | // TODO : シリアル送受信に必要なファイルをインクルードする 13 | 14 | /* USER INCLUDE CODE END */ 15 | 16 | /* Variables -----------------------------------------------------------------*/ 17 | class GS2DSerial 18 | { 19 | public: 20 | GS2DSerial() {} 21 | ~GS2DSerial() {} 22 | 23 | int open(void) 24 | { 25 | /* USER OPEN CODE START */ 26 | 27 | // TODO : シリアルポート初期化処理 28 | // RS485の場合はTXENのIOも初期化 29 | 30 | /* USER OPEN CODE END */ 31 | return 0; 32 | } 33 | 34 | void close(void) 35 | { 36 | /* USER CLOSE CODE START */ 37 | 38 | // TODO : シリアルポートを閉じる処理 39 | 40 | /* USER CLOSE CODE END */ 41 | return; 42 | } 43 | 44 | int isConnected(void) 45 | { 46 | /* USER IS_CONNECTED CODE START */ 47 | 48 | // TODO : シリアルポートの接続状態を返す関数 49 | // 開いている時に１，閉じている時に0を返す 50 | 51 | /* USER IS_CONNECTED CODE END */ 52 | return 0; 53 | } 54 | 55 | int read(void) 56 | { 57 | /* USER READ CODE START */ 58 | 59 | // TODO : データがバッファにあればデータを、無ければ -1 を返す 60 | 61 | /* USER READ CODE END */ 62 | 63 | return -1; 64 | } 65 | 66 | int write(unsigned char* data, unsigned char size) 67 | { 68 | /* USER WRITE CODE START */ 69 | 70 | // TODO : データ送信関数 71 | // RS485のTXENもここで操作 72 | 73 | /* USER WRITE CODE END */ 74 | return 0; 75 | } 76 | 77 | unsigned long long int time(void) 78 | { 79 | /* USER TIME CODE START */ 80 | 81 | // TODO : ms単位で開始からの経過時間を返す 82 | 83 | /* USER TIME CODE END */ 84 | return 0; 85 | } 86 | }; 87 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/lib/IcsClass_V210/src/IcsHardSerialClass.h: -------------------------------------------------------------------------------- 1 | 2 | /** 3 | * @file IcsHardSerialClass.h 4 | * @brief ICS3.5/3.6 arduino library use HardwareSeria header file 5 | * @author Kondo Kagaku Co.,Ltd. 6 | * @date 2017/12/27 7 | * @version 2.0.0 8 | * @copyright Kondo Kagaku Co.,Ltd. 2017 9 | 10 | **/ 11 | 12 | #ifndef _ics_HardSerial_Servo_h_ 13 | #define _ics_HardSerial_Servo_h_ 14 | 15 | #include 16 | #include 17 | 18 | //IcsHardSerialClassクラス/////////////////////////////////////////////////// 19 | /** 20 | * @class IcsHardSerialClass 21 | * @brief 近藤科学のKRSサーボをArduinoのHardwareSerialからアクセスできるようにしたクラス 22 | * @brief IcsBaseClassからの派生して作られている 23 | **/ 24 | class IcsHardSerialClass : public IcsBaseClass 25 | { 26 | //クラス内の型定義 27 | public: 28 | 29 | //コンストラクタ、デストラクタ 30 | public: 31 | //コンストラクタ(construncor) 32 | 33 | IcsHardSerialClass(); 34 | 35 | IcsHardSerialClass(HardwareSerial* icsSerial,byte enpin); 36 | IcsHardSerialClass(HardwareSerial* icsSerial,byte enpin,long baudrate,int timeout); 37 | 38 | 39 | 40 | //デストラクタ(destruntor) 41 | ~IcsHardSerialClass(); 42 | 43 | //変数 44 | public: 45 | 46 | 47 | 48 | protected: 49 | 50 | HardwareSerial *icsHardSerial; /// 13 | 14 | const byte EN_PIN = 2; 15 | const long BAUDRATE = 115200; 16 | const int TIMEOUT = 100; 17 | 18 | IcsHardSerialClass krs(&Serial,EN_PIN,BAUDRATE,TIMEOUT); //インスタンス＋ENピン(2番ピン)およびUARTの指定 19 | 20 | 21 | void setup() { 22 | // put your setup code here, to run once: 23 | krs.begin(); //サーボモータの通信初期設定 24 | 25 | 26 | } 27 | 28 | void loop() { 29 | 30 | unsigned short buttonData; 31 | 32 | buttonData = krs.getKrrButton(); 33 | if(buttonData != KRR_BUTTON_FALSE) //ボタンデータが受信できていたら 34 | { 35 | switch(buttonData) 36 | { 37 | case KRR_BUTTON_UP : //↑ボタンだった時 38 | { 39 | krs.setPos(0,7500); 40 | break; 41 | } 42 | case KRR_BUTTON_DOWN : //↓ボタンだった時 43 | { 44 | krs.setFree(0); //移動できないのでフリー 45 | break; 46 | } 47 | case KRR_BUTTON_RIGHT : //→ボタンだった時 48 | { 49 | krs.setPos(0,krs.degPos100(9000)); 50 | break; 51 | } 52 | case KRR_BUTTON_LEFT : //←ボタンだった時 53 | { 54 | krs.setPos(0,krs.degPos100(-9000)); 55 | break; 56 | } 57 | case (KRR_BUTTON_UP | KRR_BUTTON_RIGHT) : //↑→ボタンだった時 58 | { 59 | krs.setPos(0,krs.degPos100(4500)); 60 | break; 61 | } 62 | case (KRR_BUTTON_UP |KRR_BUTTON_LEFT) : //←↑ボタンだった時 63 | { 64 | krs.setPos(0,krs.degPos100(-4500)); 65 | break; 66 | } 67 | case (KRR_BUTTON_DOWN |KRR_BUTTON_RIGHT) : //↓→ボタンだった時 68 | { 69 | krs.setPos(0,11500); 70 | break; 71 | } 72 | case (KRR_BUTTON_DOWN | KRR_BUTTON_LEFT) : //←↓ボタンだった時 73 | { 74 | krs.setPos(0,3500); 75 | break; 76 | } 77 | default: 78 | { 79 | //なにもしない 80 | break; 81 | } 82 | 83 | } 84 | } 85 | else //失敗していた時の処理 86 | { 87 | delay(500); 88 | } 89 | 90 | 91 | delay(10); //KRR5は10ms以下の応答にはついていけないので待つ 92 | } 93 | -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/src/mrd_wifi.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_WIFI_H__ 2 | #define __MERIDIAN_WIFI_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "keys.h" 7 | #include "main.h" 8 | 9 | // ライブラリ導入 10 | #include 11 | #include 12 | WiFiUDP udp; // wifi設定 13 | 14 | //================================================================================================================ 15 | // Wifi 関連の処理 16 | //================================================================================================================ 17 | 18 | /// @brief wifiを初期化する. 19 | /// @param a_ssid WifiアクセスポイントのSSID. 20 | /// @param a_pass Wifiアクセスポイントのパスワード. 21 | /// @param a_serial 出力先シリアルの指定. 22 | /// @return 初期化に成功した場合はtrueを, 失敗した場合はfalseを返す. 23 | bool mrd_wifi_init(WiFiUDP &a_udp, const char *a_ssid, const char *a_pass, 24 | HardwareSerial &a_serial) { 25 | WiFi.disconnect(true, true); // WiFi接続をリセット 26 | delay(100); 27 | WiFi.begin(a_ssid, a_pass); // Wifiに接続 28 | int i = 0; 29 | while (WiFi.status() != 30 | WL_CONNECTED) { // https://www.arduino.cc/en/Reference/WiFiStatus 戻り値一覧 31 | i++; 32 | delay(50); // 接続が完了するまでループで待つ 33 | if (i > 200) { // 10秒でタイムアウト 34 | a_serial.println("Wifi init TIMEOUT."); 35 | return false; 36 | } 37 | } 38 | a_udp.begin(UDP_RECV_PORT); 39 | return true; 40 | } 41 | 42 | /// @brief 第一引数のMeridim配列にUDP経由でデータを受信, 格納する. 43 | /// @param a_meridim_bval バイト型のMeridim配列 44 | /// @param a_len バイト型のMeridim配列の長さ 45 | /// @param a_udp 使用するWiFiUDPのインスタンス 46 | /// @return 受信した場合はtrueを, 受信しなかった場合はfalseを返す. 47 | bool mrd_wifi_udp_receive(byte *a_meridim_bval, int a_len, WiFiUDP &a_udp) { 48 | if (a_udp.parsePacket() >= a_len) // データの受信バッファ確認 49 | { 50 | a_udp.read(a_meridim_bval, a_len); // データの受信 51 | return true; 52 | } 53 | return false; 54 | } 55 | 56 | /// @brief 第一引数のMeridim配列のデータをUDP経由でWIFI_SEND_IP, UDP_SEND_PORTに送信する. 57 | /// @param a_meridim_bval バイト型のMeridim配列 58 | /// @param a_len バイト型のMeridim配列の長さ 59 | /// @param a_udp 使用するWiFiUDPのインスタンス 60 | /// @return 送信完了時にtrueを返す. 61 | /// ※WIFI_SEND_IP, UDP_SEND_PORTを関数内で使用. 62 | bool mrd_wifi_udp_send(byte *a_meridim_bval, int a_len, WiFiUDP &a_udp) { 63 | a_udp.beginPacket(WIFI_SEND_IP, UDP_SEND_PORT); // UDPパケットの開始 64 | a_udp.write(a_meridim_bval, a_len); 65 | a_udp.endPacket(); // UDPパケットの終了 66 | return true; 67 | } 68 | 69 | #endif // __MERIDIAN_WIFI_H__ 70 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_module/sv_ics.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_SERVO_KONDO_ICS_H__ 2 | #define __MERIDIAN_SERVO_KONDO_ICS_H__ 3 | 4 | #include "config.h" 5 | #include "gs2d_krs.h" 6 | #include "main.h" 7 | #include "mrd_disp.h" 8 | 9 | //================================================================================================================ 10 | // KONDO ICSサーボ関連の処理 11 | //================================================================================================================ 12 | 13 | /// @brief ICSサーボの実行処理を行う関数 14 | /// @param a_servo_id サーボのインデックス番号 15 | /// @param a_cmnd サーボのコマンド 16 | /// @param a_tgt サーボの目標位置 17 | /// @param a_tgt_past 前回のサーボの目標位置 18 | /// @param a_tgt_trim サーボの補正値 19 | /// @param a_cw サーボの回転方向補正値 20 | /// @param a_err_cnt サーボのエラーカウント 21 | /// @param a_stat サーボのステータス 22 | /// @param ics サーボクラスのインスタンス 23 | float mrd_servo_process_ics(int a_servo_id, int a_cmnd, float a_tgt, float a_tgt_past, int a_trim, 24 | int a_cw, int &a_err_cnt, uint16_t &a_stat, IcsHardSerialClass &ics) { 25 | int val_tmp = 0; 26 | if (a_cmnd == 1) { // コマンドが1ならPos指定 27 | val_tmp = ics.setPos(a_servo_id, mrd.Deg2Krs(a_tgt, a_trim, a_cw)); 28 | } else { // コマンドが0等なら脱力して値を取得 29 | val_tmp = ics.setFree(a_servo_id); 30 | } 31 | 32 | if (val_tmp == -1) { // サーボからの返信信号を受け取れなかった場合 33 | val_tmp = mrd.Deg2Krs(a_tgt_past, a_trim, a_cw); 34 | a_err_cnt++; 35 | if (a_err_cnt >= SERVO_LOST_ERR_WAIT) { // 一定以上の連続エラーで通信不能とみなす 36 | a_err_cnt = SERVO_LOST_ERR_WAIT; 37 | a_stat = 1; 38 | } 39 | } else { 40 | a_err_cnt = 0; 41 | a_stat = 0; 42 | } 43 | 44 | return mrd.Krs2Deg(val_tmp, a_trim, a_cw); 45 | } 46 | 47 | /// @brief ICSサーボを駆動する関数 48 | /// @param a_meridim Meridimデータの参照 49 | /// @param a_sv サーボパラメータの配列 50 | void mrd_servos_drive_ics_double(Meridim90Union &a_meridim, ServoParam &a_sv) { 51 | for (int i = 0; i < a_sv.num_max; i++) { 52 | // L系統サーボの処理 53 | if (a_sv.ixl_mount[i]) { 54 | a_sv.ixl_tgt[i] = mrd_servo_process_ics( 55 | a_sv.ixl_id[i], a_meridim.sval[(i * 2) + 20], a_sv.ixl_tgt[i], a_sv.ixl_tgt_past[i], 56 | a_sv.ixl_trim[i], a_sv.ixl_cw[i], a_sv.ixl_err[i], a_sv.ixl_stat[i], ics_L); 57 | } 58 | // R系統サーボの処理 59 | if (a_sv.ixr_mount[i]) { 60 | a_sv.ixr_tgt[i] = mrd_servo_process_ics( 61 | a_sv.ixr_id[i], a_meridim.sval[(i * 2) + 50], a_sv.ixr_tgt[i], a_sv.ixr_tgt_past[i], 62 | a_sv.ixr_trim[i], a_sv.ixr_cw[i], a_sv.ixr_err[i], a_sv.ixr_stat[i], ics_R); 63 | } 64 | delayMicroseconds(1); // Teensyの場合には必要かも 65 | } 66 | } 67 | 68 | #endif // __MERIDIAN_SERVO_KONDO_ICS_H__ -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/lib/ESP32Wiimote/src/ESP32Wiimote.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Daiki Yasuda 2 | // 3 | // This is licensed under 4 | // - Creative Commons Attribution-NonCommercial 3.0 Unported 5 | // - https://creativecommons.org/licenses/by-nc/3.0/ 6 | // - Or see LICENSE.md 7 | // 8 | // The short of it is... 9 | // You are free to: 10 | // Share — copy and redistribute the material in any medium or format 11 | // Adapt — remix, transform, and build upon the material 12 | // Under the following terms: 13 | // NonCommercial — You may not use the material for commercial purposes. 14 | 15 | #ifndef __ESP32_WIIMOTE_H__ 16 | #define __ESP32_WIIMOTE_H__ 17 | 18 | #include "esp_bt.h" 19 | #include "TinyWiimote.h" 20 | 21 | typedef struct { 22 | uint8_t xAxis; 23 | uint8_t yAxis; 24 | uint8_t zAxis; 25 | } AccelState; 26 | 27 | typedef enum { 28 | BUTTON_Z = 0x00020000, // nunchuk 29 | BUTTON_C = 0x00010000, // nunchuk 30 | BUTTON_PLUS = 0x00001000, 31 | BUTTON_UP = 0x00000800, // vertical orientation 32 | BUTTON_DOWN = 0x00000400, 33 | BUTTON_RIGHT = 0x00000200, 34 | BUTTON_LEFT = 0x00000100, 35 | BUTTON_HOME = 0x00000080, 36 | BUTTON_MINUS = 0x00000010, 37 | BUTTON_A = 0x00000008, 38 | BUTTON_B = 0x00000004, 39 | BUTTON_ONE = 0x00000002, 40 | BUTTON_TWO = 0x00000001, 41 | NO_BUTTON = 0x00000000 42 | } ButtonState; 43 | 44 | typedef struct { 45 | uint8_t xStick; 46 | uint8_t yStick; 47 | uint8_t xAxis; 48 | uint8_t yAxis; 49 | uint8_t zAxis; 50 | // moved to ButtonState 51 | // uint8_t cBtn; 52 | // uint8_t zBtn; 53 | } NunchukState; 54 | 55 | enum 56 | { 57 | FILTER_NONE = 0x0000, 58 | FILTER_BUTTON = 0x0001, 59 | //FILTER_NUNCHUK_BUTTON = 0x0002, 60 | FILTER_NUNCHUK_STICK = 0x0004, 61 | FILTER_ACCEL = 0x0008, 62 | }; 63 | 64 | enum 65 | { 66 | ACTION_IGNORE, 67 | }; 68 | 69 | class ESP32Wiimote 70 | { 71 | public: 72 | ESP32Wiimote(int NUNCHUK_STICK_THRESHOLD = 1); // was 2 73 | 74 | void init(void); 75 | void task(void); 76 | int available(void); 77 | ButtonState getButtonState(void); 78 | AccelState getAccelState(void); 79 | NunchukState getNunchukState(void); 80 | void addFilter(int action, int filter); 81 | 82 | private: 83 | 84 | typedef struct { 85 | size_t len; 86 | uint8_t data[]; 87 | } queuedata_t; 88 | 89 | ButtonState _buttonState; 90 | ButtonState _oldButtonState; 91 | 92 | AccelState _accelState; 93 | AccelState _oldAccelState; 94 | 95 | NunchukState _nunchukState; 96 | NunchukState _oldNunchukState; 97 | 98 | int _nunStickThreshold; 99 | 100 | int _filter; 101 | 102 | static const TwHciInterface tinywii_hci_interface; 103 | static esp_vhci_host_callback_t vhci_callback; 104 | static xQueueHandle txQueue; 105 | static xQueueHandle rxQueue; 106 | 107 | static void createQueue(void); 108 | static void handleTxQueue(void); 109 | static void handleRxQueue(void); 110 | static esp_err_t sendQueueData(xQueueHandle queue, uint8_t *data, size_t len); 111 | static void notifyHostSendAvailable(void); 112 | static int notifyHostRecv(uint8_t *data, uint16_t len); 113 | static void hciHostSendPacket(uint8_t *data, size_t len); 114 | 115 | }; 116 | 117 | #endif // __ESP32_WIIMOTE_H__ 118 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_wire1.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_WIRE1_H__ 2 | #define __MERIDIAN_WIRE1_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "main.h" 7 | 8 | IntervalTimer wireTimer1; 9 | 10 | //================================================================================================================ 11 | // I2C wire0 関連の処理 12 | //================================================================================================================ 13 | 14 | //------------------------------------------------------------------------------------ 15 | // リモコンのデータ取得処理 16 | //------------------------------------------------------------------------------------ 17 | 18 | /// @brief MerimoteのI2C通信を通じてジョイパッドデータを読み取る. 19 | /// @param a_pad_array 処理後のジョイパッドデータを格納する構造体. 20 | /// @param a_pad_i2c I2C通信から読み取った生データを格納する構造体. 21 | /// @param a_len a_pad_i2cの長さ. デフォルトは5. 22 | /// @return 正常にデータが読み取れた場合はtrueを, そうでない場合はfalseを返す. 23 | bool mrd_wire1_read_merimote_i2c(PadUnion &a_pad_array, PadUnion &a_pad_i2c, int a_len) { 24 | int i_tmp = 0; 25 | Wire1.requestFrom(I2C1_MERIMOTE_ADDR, a_len * 2); 26 | while (Wire1.available()) { // バッファにデータがある間 27 | a_pad_i2c.bval[i_tmp] = Wire1.read(); // バッファから1バイト読み取り 28 | i_tmp++; 29 | if (i_tmp > a_len * 2) { 30 | break; 31 | } 32 | } 33 | 34 | if (mrd.cksm_val(a_pad_i2c.sval, a_len)) { 35 | for (int i = 0; i < a_len; i++) { 36 | a_pad_array.sval[i] = a_pad_i2c.sval[i]; 37 | } 38 | return true; 39 | } 40 | return false; 41 | } 42 | 43 | //------------------------------------------------------------------------------------ 44 | // 初期設定 45 | //------------------------------------------------------------------------------------ 46 | 47 | /// @brief 指定されたジョイパッドタイプに基づいてWire1 I2C通信を設定する. 48 | /// @param a_type ジョイパッドのタイプを示す列挙型. Merimoteがサポートされている. 49 | /// @param a_i2c_bps I2C通信のクロック速. 50 | /// @param a_serial メッセージ表示用のハードウェアシリアル. 51 | /// @return ジョイパッドが正しく初期化され, データの読み取りに成功した場合はtrueを, 52 | /// それ以外の場合はfalseを返す. 53 | bool mrd_wire1_setup(PadType a_type, int a_i2c_bps, Stream &a_serial) { 54 | if (a_type == MERIMOTE) // Merimoteを搭載 55 | { 56 | // Serial.print("Initializing wire1 I2C..."); 57 | Wire1.setSDA(17); 58 | Wire1.setSCL(16); 59 | Wire1.begin(); 60 | Wire1.setClock(a_i2c_bps); 61 | } else { 62 | return false; 63 | } 64 | bool rslt = mrd_wire1_read_merimote_i2c(pad_array, pad_i2c, PAD_LEN); 65 | if (rslt) { 66 | a_serial.print("Merimote OK."); 67 | } 68 | return rslt; 69 | } 70 | 71 | //------------------------------------------------------------------------------------ 72 | // IntervalTimerTimerのスタート 73 | //------------------------------------------------------------------------------------ 74 | 75 | /// @brief MerimoteのデータをI2C経由で読み取る関数を実行する. 76 | void mrd_wire1_run() // ※IntervalTimer用の関数のためvoidのみ可 77 | { 78 | mrd_wire1_read_merimote_i2c(pad_array, pad_i2c, PAD_LEN); 79 | } 80 | 81 | /// @brief インターバルタイマーを開始して, 定期的にMerimoteデータの読み取りを行う. 82 | /// @param a_wire1_init 初期化の合否. 83 | /// @return タイマーの初期化と開始が成功した場合はtrue, それ以外の場合はfalseを返す. 84 | bool mrd_wire1_startIntervalTimer(bool a_wire1_init) { 85 | if (a_wire1_init) { 86 | wireTimer1.begin(mrd_wire1_run, 10000); // インターバルはマイクロ秒指定 87 | // wireTimer.priority(180); 88 | return true; 89 | } 90 | return false; 91 | } 92 | 93 | //------------------------------------------------------------------------------------ 94 | // 各種オペレーション 95 | //------------------------------------------------------------------------------------ 96 | 97 | #endif // __MERIDIAN_WIRE1_H__ 98 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/lib/gs2d/gs2d_type.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @file gs2d_type.h 3 | * @author 4 | * @date 2021/01/26 5 | * @brief 6 | * 7 | * マルチスレッド処理を行った際、データ入出力にエラーが起こる場合は 8 | * 以下のクラスを環境向けにスレッドセーフに組み直す必要があるかも知れません。 9 | * CircularBuffer, ThreadSafeType 10 | * 11 | */ 12 | #pragma once 13 | 14 | /* Includes ------------------------------------------------------------------*/ 15 | #include 16 | 17 | namespace gs2d 18 | { 19 | // gs2d用エラーコード 20 | enum { 21 | TimeoutError = 0x01, 22 | ResponseError = 0x02, 23 | ProtocolError = 0x04, 24 | NotSupportError = 0x00, 25 | BadInputError = 0x08, 26 | SystemError = 0x10 27 | }; 28 | 29 | /* Classes -------------------------------------------------------------------*/ 30 | // gs2dで扱う浮動小数点の型 31 | using gFloat = float; 32 | 33 | // Int, Floatの戻り地に対応するためのクラス 34 | class EventDataType 35 | { 36 | private: 37 | int32_t intData; 38 | gFloat floatData; 39 | bool flag; 40 | 41 | public: 42 | EventDataType(gFloat data) { intData = (int32_t)data; floatData = data; flag = true; } 43 | EventDataType(int32_t data) { intData = data; floatData = (gFloat)data; flag = false; } 44 | EventDataType() { intData = 0; floatData = 0; flag = false; } 45 | ~EventDataType() {} 46 | 47 | explicit operator int32_t(void) { return flag ? floatData : intData; } 48 | explicit operator gFloat(void) { return flag ? floatData : intData; } 49 | 50 | void set(int32_t data) { intData = data; floatData = (gFloat)data; flag = false; } 51 | void set(gFloat data) { intData = (int32_t)data; floatData = data; flag = true; } 52 | }; 53 | 54 | // コールバックの引数用クラス 55 | class CallbackEventArgs 56 | { 57 | public: 58 | // ID, エラー状態、データ(int or gFloat) 59 | uint8_t id; 60 | uint8_t status; 61 | EventDataType data; 62 | 63 | // 各種コンストラクタ 64 | CallbackEventArgs(uint8_t id, uint8_t status, gFloat data) :id(id), status(status), data(data) {} 65 | CallbackEventArgs(uint8_t id, uint8_t status, int32_t data) : id(id), status(status), data(data) {} 66 | CallbackEventArgs(uint8_t id, uint8_t status, EventDataType data) : id(id), status(status) { this->data = data; } 67 | CallbackEventArgs(uint8_t status) : id(0), status(status), data((int32_t)0) {} 68 | ~CallbackEventArgs() {} 69 | }; 70 | 71 | // gs2d用サーキュラバッファクラス 72 | template 73 | class CircularBuffer 74 | { 75 | private: 76 | T buffer[bufferSize]; 77 | volatile uint8_t writePos = 0; 78 | volatile uint8_t readPos = 0; 79 | uint8_t size; 80 | 81 | public: 82 | CircularBuffer() : size(bufferSize) {} 83 | virtual ~CircularBuffer() {} 84 | 85 | inline bool push(T data) 86 | { 87 | uint8_t tempPos = (uint8_t)((writePos + 1) % size); 88 | if (tempPos == readPos) return false; 89 | buffer[writePos++] = data; 90 | writePos %= size; 91 | return true; 92 | } 93 | 94 | inline T pop(void) 95 | { 96 | uint8_t tmpPos = readPos; 97 | if (writePos != readPos) { 98 | readPos++; readPos %= size; 99 | } 100 | return buffer[tmpPos]; 101 | } 102 | 103 | bool isEmpty() { return (writePos == readPos); } 104 | }; 105 | 106 | // スレッドセーフにする必要があるかもしれないので、念の為 107 | template 108 | class Gs2dType 109 | { 110 | private: 111 | volatile T variable; 112 | 113 | public: 114 | inline void set(T data) { variable = data; } 115 | inline T get(void) { return variable; } 116 | 117 | Gs2dType(T data) { variable = data; } 118 | ~Gs2dType() {} 119 | }; 120 | 121 | // 受信データ処理関数の型 122 | using ResponseProcess = EventDataType(*)(int32_t); 123 | 124 | // コールバック関数の型 125 | using CallbackType = void(*)(CallbackEventArgs); 126 | } 127 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_sd.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_SD_H__ 2 | #define __MERIDIAN_SD_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "main.h" 7 | 8 | // ライブラリ導入 9 | #include // SDカード用 10 | File sdfile; // SDカード用 11 | 12 | //================================================================================================================ 13 | // SDメモリ 関連の処理 14 | //================================================================================================================ 15 | 16 | class MrdSdHandler { 17 | private: 18 | Stream &m_serial; // シリアルオブジェクトの参照を保持 19 | 20 | public: 21 | // コンストラクタでStreamオブジェクトを受け取り, メンバーに保存 22 | MrdSdHandler(Stream &a_serial) : m_serial(a_serial) {} 23 | 24 | //------------------------------------------------------------------------------------ 25 | // 初期化処理 26 | //------------------------------------------------------------------------------------ 27 | 28 | /// @brief SDカードの初期化を試みる. 29 | /// @param a_sd_mount SDカードがマウントされているかどうかのブール値. 30 | /// @param a_chipselect SDカードのチップ選択ピン番号. 31 | /// @return SDカードの初期化が成功した場合はtrue, 失敗またはマウントされていない場合はfalse. 32 | bool init(bool a_sd_mount, int a_chipselect) { 33 | if (!a_sd_mount) { 34 | m_serial.println("SD not mounted."); 35 | delay(100); 36 | return false; 37 | } 38 | 39 | const int max_attempts_tmp = 5; // 最大再試行回数 40 | m_serial.print("Initializing SD card."); 41 | 42 | for (int attempt = 0; attempt < max_attempts_tmp; attempt++) { 43 | m_serial.print("."); 44 | delay(100); 45 | 46 | if (SD.begin(a_chipselect)) { 47 | m_serial.println(" OK."); 48 | return true; 49 | } else { 50 | delay(100); // 再試行までの待機時間 51 | } 52 | } 53 | 54 | m_serial.println(" failed."); 55 | return false; 56 | } 57 | 58 | //------------------------------------------------------------------------------------ 59 | // SDリードライトテスト 60 | //------------------------------------------------------------------------------------ 61 | 62 | /// @brief SDカードの読み書き機能をテストする. 63 | /// @param a_sd_mount SDカードがマウントされているかどうかのブール値. 64 | /// @param a_chipselect SDカードのチップ選択ピン番号. 65 | /// @param a_flg_rw SDカードの読み書きをチェックするかどうかのブール値. 66 | /// @return SDカードの読み書きが成功した場合はtrueを, 失敗した場合はfalseを返す. 67 | bool check_rw(bool a_sd_mount, int a_chipselect, bool a_flg_rw) { 68 | if (a_sd_mount && a_flg_rw) { 69 | sdfile = SD.open("/test.txt", FILE_WRITE); 70 | delay(1); // SPI安定化検証用 71 | 72 | if (sdfile) { 73 | m_serial.print("Checking SD card r/w..."); 74 | // SD書き込みテスト用のランダムな4桁の数字を生成 75 | randomSeed(long(analogRead(A0) + analogRead(A1) * 1000 + 76 | analogRead(A2) * 1000000)); // 未接続ピンのノイズを利用 77 | int randNumber = random(1000, 9999); 78 | 79 | m_serial.print(" write code "); 80 | m_serial.print(randNumber); 81 | // ファイルへの書き込みを実行 82 | sdfile.println(randNumber); 83 | delayMicroseconds(1); // SPI安定化検証用 84 | sdfile.close(); 85 | m_serial.print(" and"); 86 | delayMicroseconds(10); // SPI安定化検証用 87 | // ファイルからの読み込みを実行 88 | sdfile = SD.open("/test.txt"); 89 | if (sdfile) { 90 | m_serial.print(" read code "); 91 | while (sdfile.available()) { 92 | m_serial.write(sdfile.read()); 93 | } 94 | sdfile.close(); 95 | } 96 | SD.remove("/test.txt"); 97 | delay(10); 98 | return true; 99 | } else { 100 | m_serial.println("Could not open SD test.txt file."); 101 | } 102 | } 103 | return false; 104 | } 105 | 106 | //------------------------------------------------------------------------------------ 107 | // 各種オペレーション 108 | //------------------------------------------------------------------------------------ 109 | }; 110 | 111 | #endif // __MERIDIAN_SD_H__ -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/src/mrd_util.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_UTILITY_H__ 2 | #define __MERIDIAN_UTILITY_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "main.h" 7 | 8 | //================================================================================================================ 9 | // Utility ごく小規模な汎用関数 10 | //================================================================================================================ 11 | 12 | /// @brief 配列の中で0以外が入っている最大のIndexを求める. 13 | /// @param a_arr 配列 14 | /// @param a_size 配列の長さ 15 | /// @return 0以外が入っている最大のIndex. すべて0の場合は1を反す. 16 | int mrd_max_used_index(const int a_arr[], int a_size) { 17 | int max_index_tmp = 0; 18 | for (int i = 0; i < a_size; ++i) { 19 | if (a_arr[i] != 0) { 20 | max_index_tmp = i; 21 | } 22 | } 23 | return max_index_tmp + 1; 24 | } 25 | 26 | /// @brief 指定された位置のビットをセットする(16ビット変数用). 27 | /// @param a_byte ビットをセットする16ビットの変数.参照渡し. 28 | /// @param a_bit_pos セットするビットの位置(0から15). 29 | /// @return なし. 30 | inline void mrd_set_bit16(uint16_t &a_byte, uint16_t a_bit_pos) { a_byte |= (1 << a_bit_pos); } 31 | 32 | /// @brief 指定された位置のビットをクリアする(16ビット変数用). 33 | /// @param a_byte ビットをクリアする16ビットの変数.参照渡し. 34 | /// @param a_bit_pos クリアするビットの位置(0から15). 35 | /// @return なし. 36 | inline void mrd_clear_bit16(uint16_t &a_byte, uint16_t a_bit_pos) { a_byte &= ~(1 << a_bit_pos); } 37 | 38 | /// @brief 指定された位置のビットをセットする(8ビット変数用). 39 | /// @param value ビットをセットする8ビットの変数.参照渡し. 40 | /// @param a_bit_pos セットするビットの位置(0から7). 41 | /// @return なし. 42 | inline void mrd_setBit8(uint8_t &value, uint8_t a_bit_pos) { value |= (1 << a_bit_pos); } 43 | 44 | /// @brief 指定された位置のビットをクリアする(8ビット変数用). 45 | /// @param value ビットをクリアする8ビットの変数.参照渡し. 46 | /// @param a_bit_pos クリアするビットの位置(0から7). 47 | /// @return なし. 48 | inline void mrd_clear_bit8(uint8_t &value, uint8_t a_bit_pos) { value &= ~(1 << a_bit_pos); } 49 | 50 | /// @brief 列挙型(L,R,C)から文字列を取得する関数. 51 | /// @param a_line 列挙型 enum UartLine 52 | /// @return 列挙型の内容に応じて文字列"L","R","C"返す. 53 | const char *mrd_get_line_name(UartLine a_line) { 54 | switch (a_line) { 55 | case L: 56 | return "L"; 57 | case R: 58 | return "R"; 59 | case C: 60 | return "C"; 61 | default: 62 | return "Unknown"; 63 | } 64 | } 65 | 66 | //------------------------------------------------------------------------------------ 67 | // タイムアウト監視用タイマー 68 | //------------------------------------------------------------------------------------ 69 | // タイマーの開始状態を保持するための静的変数 70 | static unsigned long timeout_start = 0; 71 | static bool flg_timer_started = false; // タイマーが開始されたかどうかのフラグ 72 | 73 | /// @brief 指定されたミリ秒のタイムアウトを監視する. mrd_timeout_resetとセットで使う. 74 | /// @param a_limit タイムアウトまでの時間(ms) 75 | /// @return タイムアウトでtrueを返す. 76 | bool mrd_timeout_check(unsigned long a_limit) { 77 | // タイマーが開始されていない場合は現在の時間を記録してタイマーを開始 78 | if (!flg_timer_started) { 79 | timeout_start = millis(); 80 | flg_timer_started = true; // タイムアウト監視開始フラグをアゲる 81 | } 82 | 83 | unsigned long current_time = millis(); // 現在の時間を取得 84 | 85 | if (current_time - timeout_start >= a_limit) { // 指定された時間が経過しているかチェック 86 | flg_timer_started = false; // タイムアウト監視開始フラグをサゲる 87 | return true; // 指定された時間が経過していれば true を返す 88 | } 89 | 90 | return false; // まだ時間が経過していなければ false を返す 91 | } 92 | 93 | /// @brief タイムアウト監視開始フラグをリセットする. mrd_timeout_checkとセットで使う. 94 | void mrd_timeout_reset() { 95 | flg_timer_started = false; // タイマーをリセットして次回の呼び出しに備える 96 | } 97 | 98 | //------------------------------------------------------------------------------------ 99 | // meriput / meridimへのデータ書き込み 100 | //------------------------------------------------------------------------------------ 101 | 102 | /// @brief meridim配列のチェックサムを算出して[len-1]に書き込む. 103 | /// @param a_meridim Meridim配列の共用体. 参照渡し. 104 | /// @return 常にtrueを返す. 105 | bool mrd_meriput90_cksm(Meridim90Union &a_meridim, int len = 90) { 106 | int a_cksm = 0; 107 | for (int i = 0; i < len - 1; i++) { 108 | a_cksm += int(a_meridim.sval[i]); 109 | } 110 | a_meridim.sval[len - 1] = short(~a_cksm); 111 | return true; 112 | } 113 | 114 | #endif //__MERIDIAN_UTILITY_H__ 115 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/lib/gs2d/gs2d_command.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @file gs2d_command.h 3 | * @author 4 | * @date 2021/01/26 5 | * @brief 6 | */ 7 | #pragma once 8 | 9 | /* Includes ------------------------------------------------------------------*/ 10 | #include "gs2d_type.h" 11 | #include 12 | 13 | namespace gs2d 14 | { 15 | /* Variables -----------------------------------------------------------------*/ 16 | // コマンドバッファの型 17 | template 18 | class CommandBufferType 19 | { 20 | public: 21 | uint8_t data[commandSize]; 22 | uint8_t length = 0; 23 | uint8_t count = 0; 24 | ResponseProcess responseProcess; 25 | CallbackType callback; 26 | 27 | CommandBufferType(uint8_t* data, uint8_t length, uint8_t count = 1, ResponseProcess response = 0, CallbackType callback = 0) 28 | { 29 | memcpy(this->data, data, length); 30 | this->length = length; 31 | this->count = count; 32 | responseProcess = response; 33 | this->callback = callback; 34 | } 35 | CommandBufferType() {} 36 | ~CommandBufferType() {} 37 | }; 38 | 39 | template 40 | class CommandHandler 41 | { 42 | protected: 43 | // 受信タイムアウト 44 | uint16_t receiveTimeout = 2000; // msec 45 | 46 | // シリアルポート 47 | SerialClass serialPort; 48 | 49 | // コマンドバッファ 50 | CircularBuffer, bufferSize> commandStack; 51 | CommandBufferType currentCommand; 52 | 53 | // 受信データ 54 | uint8_t response[100]; 55 | uint8_t responsePos = 0; 56 | 57 | // 継承先の関数 58 | virtual bool isComplete(uint8_t* data, uint8_t length) = 0; 59 | virtual void dataReceivedEvent(uint8_t* data, uint8_t length, uint8_t status) = 0; 60 | 61 | // 処理ステータス 62 | Gs2dType isTrafficFree; 63 | 64 | // タイマ 65 | uint64_t startTime = 0; 66 | 67 | // 68 | CommandHandler() : isTrafficFree(true), currentCommand() { serialPort.open(); } 69 | virtual ~CommandHandler() { serialPort.close(); } 70 | 71 | // コマンド追加関数 72 | void addCommand(uint8_t* data, uint8_t length, ResponseProcess response, CallbackType callback, uint8_t count) 73 | { 74 | CommandBufferType command(data, length, count, response, callback); 75 | commandStack.push(command); 76 | if (isTrafficFree.get()) sendCommand(); 77 | } 78 | 79 | // コマンド送信 80 | void sendCommand() 81 | { 82 | // コマンドを取り出して送信 83 | currentCommand = commandStack.pop(); 84 | serialPort.write(currentCommand.data, currentCommand.length); 85 | 86 | // リスナ初期化 87 | // 受信個数が0個でも無視 88 | if (currentCommand.count == 0) { 89 | // コマンドがたまっている場合は送信 90 | if (!commandStack.isEmpty()) sendCommand(); 91 | else isTrafficFree.set(true); 92 | } 93 | else { 94 | responsePos = 0; 95 | isTrafficFree.set(false); 96 | startTime = serialPort.time(); 97 | } 98 | } 99 | 100 | // リスナー関数 101 | void listener(void) 102 | { 103 | // 無意味なデータを無視 104 | if (isTrafficFree.get()) return; 105 | 106 | if (serialPort.isConnected()) 107 | { 108 | // タイムアウト確認 109 | if (serialPort.time() > startTime + receiveTimeout) 110 | { 111 | // タイムアウトを通知 112 | dataReceivedEvent(0, 0, TimeoutError); 113 | 114 | // コマンドがたまっている場合は送信 115 | if (!commandStack.isEmpty()) sendCommand(); 116 | else isTrafficFree.set(true); 117 | 118 | return; 119 | } 120 | 121 | // データが無ければ終了 122 | int data = serialPort.read(); 123 | if (data == -1) return; 124 | 125 | // 受信データ保存 126 | response[responsePos++] = data; 127 | 128 | // TODO:エラー処理 129 | if (responsePos >= 100) responsePos = 0; 130 | 131 | // 受信完了チェック 132 | if (isComplete(response, responsePos)) 133 | { 134 | dataReceivedEvent(response, responsePos, 0); 135 | 136 | // 全サーボ受信完了チェック 137 | currentCommand.count--; 138 | if (currentCommand.count == 0) { 139 | // コマンドがたまっている場合は送信 140 | if (!commandStack.isEmpty()) sendCommand(); 141 | else isTrafficFree.set(true); 142 | } 143 | else { 144 | responsePos = 0; 145 | } 146 | } 147 | } 148 | } 149 | }; 150 | } 151 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/lib/IcsClass_V210/src/IcsHardSerialClass.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file IcsHardSerialClass.cpp 3 | * @brief ICS3.5/3.6 arduino library use HardwareSerial 4 | * @author Kondo Kagaku Co.,Ltd. 5 | * @date 2017/12/27 6 | * @version 2.0.0 7 | * @copyright © Kondo Kagaku Co.,Ltd. 2017 8 | **/ 9 | 10 | 11 | #include 12 | #include "IcsHardSerialClass.h" 13 | 14 | /** 15 | * @brief コンストラクタ 16 | **/ 17 | IcsHardSerialClass::IcsHardSerialClass() 18 | { 19 | 20 | } 21 | 22 | 23 | 24 | 25 | /** 26 | * @brief コンストラクタ 27 | * @param[in] *icsSerial ICSに設定するUART(HardwareSerial型のポインタ) 28 | * @param[in] enpin 送受信切替えピンのピン番号 29 | **/ 30 | IcsHardSerialClass::IcsHardSerialClass(HardwareSerial *icsSerial,byte enpin) 31 | { 32 | icsHardSerial = icsSerial; 33 | enPin = enpin; 34 | } 35 | 36 | /** 37 | * @brief コンストラクタ 38 | * @param[in] *hardSerial ICSに設定するUART(HardwareSerial型のポインタ) 39 | * @param[in] enpin 送受信切替えピンのピン番号 40 | * @param[in] baudrate サーボの通信速度 41 | * @param[in] timeout 受信タイムアウト(ms) 42 | 43 | **/ 44 | IcsHardSerialClass::IcsHardSerialClass(HardwareSerial *hardSerial,byte enpin, long baudrate, int timeout) 45 | { 46 | icsHardSerial = hardSerial; 47 | enPin = enpin; 48 | baudRate = baudrate; 49 | timeOut = timeout; 50 | } 51 | 52 | 53 | /** 54 | * @brief デストラクタ 55 | * @post 使用していたのicsHardSerialを開放します 56 | **/ 57 | IcsHardSerialClass::~IcsHardSerialClass() 58 | { 59 | if (icsHardSerial) //定義してあったらSerialを解放する 60 | { 61 | icsHardSerial->end(); 62 | } 63 | } 64 | 65 | 66 | 67 | //初期設定 　void setup()へ　/////////////////////////////////////////////////////////////////////////////// 68 | /** 69 | * @brief 通信の初期設定 70 | * @retval true 通信設定完了 71 | * @retval false 通信設定失敗 72 | * @attention Serialの設定、通信速度、タイムアウトは事前に設定してある事 73 | **/ 74 | bool IcsHardSerialClass::begin() 75 | { 76 | if (icsHardSerial == nullptr) 77 | { 78 | return false; 79 | } 80 | 81 | icsHardSerial->begin(baudRate,SERIAL_8E1); 82 | icsHardSerial->setTimeout(timeOut); 83 | pinMode(enPin, OUTPUT); 84 | enLow(); 85 | 86 | 87 | return true; 88 | } 89 | 90 | /** 91 | * @brief 通信の初期設定 92 | * @param[in] baudrate ICSの通信速度(115200,625000,1250000(1.25M)bps) 93 | * @param[in] timeout 受信タイムアウト(ms) 94 | * @retval true 通信設定完了 95 | * @retval false 通信設定失敗 96 | * @attention UARTピンおよび送受信切替えピンは設定してある事 97 | **/ 98 | bool IcsHardSerialClass::begin(long baudrate,int timeout) 99 | { 100 | baudRate = baudrate; 101 | timeOut = timeout; 102 | return begin(); 103 | } 104 | 105 | /** 106 | * @brief 通信の初期設定 107 | * @param[in] *serial ICSに設定するUART(HardwareSerial型のポインタ) 108 | * @param[in] enpin 送受信切替えピンのピン番号 109 | * @param[in] baudrate ICSの通信速度(115200,625000,1250000(1.25M)bps) 110 | * @param[in] timeout 受信タイムアウト(ms) 111 | * @retval true 通信設定完了 112 | * @retval false 通信設定失敗 113 | **/ 114 | bool IcsHardSerialClass::begin(HardwareSerial *serial,int enpin,long baudrate,int timeout) 115 | { 116 | icsHardSerial = serial; 117 | enPin = enpin; 118 | baudRate = baudrate; 119 | timeOut = timeout; 120 | return begin(); 121 | } 122 | 123 | 124 | 125 | //データ送受信 ///////////////////////////////////////////////////////////////////////////////////////////// 126 | /** 127 | * @brief ICS通信の送受信 128 | * @param[in,out] *txBuf 129 | * @param[in] txLen 130 | * @param[out] *rxBuf 受信格納バッファ 131 | * @param[in] rxLen 受信データ数 132 | * @retval true 通信成功 133 | * @retval false 通信失敗 134 | * @attention 送信データ数、受信データ数はコマンドによって違うので注意する 135 | * @date 2020/02/18 protectedからpublicに変更 136 | **/ 137 | bool IcsHardSerialClass::synchronize(byte *txBuf, byte txLen, byte *rxBuf, byte rxLen) 138 | { 139 | int rxSize; //受信数 140 | 141 | //シリアル初期化確認 142 | 143 | if(icsHardSerial == nullptr ) 144 | { 145 | return false; 146 | } 147 | 148 | icsHardSerial->flush(); //待つ 149 | enHigh(); //送信切替 150 | icsHardSerial->write(txBuf, txLen); 151 | icsHardSerial->flush(); //待つ 152 | 153 | while (icsHardSerial->available() > 0) //受信バッファを消す 154 | { 155 | // buff = icsSerial->read(); //空読み 156 | icsHardSerial->read(); //空読み 157 | } 158 | 159 | enLow(); //受信切替 160 | 161 | 162 | rxSize = icsHardSerial->readBytes(rxBuf, rxLen); 163 | 164 | if (rxSize != rxLen) //受信数確認 165 | { 166 | return false; 167 | } 168 | return true; 169 | 170 | 171 | } 172 | 173 | 174 | 175 | 176 | 177 | 178 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_servo.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_SERVO_DISTRIBUTOR_H__ 2 | #define __MERIDIAN_SERVO_DISTRIBUTOR_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "main.h" 7 | #include "mrd_module/sv_dxl2.h" 8 | #include "mrd_module/sv_ftbrx.h" 9 | #include "mrd_module/sv_ftc.h" 10 | #include "mrd_module/sv_ics.h" 11 | #include "mrd_module/sv_xbus.h" 12 | 13 | //================================================================================================================ 14 | // Servo 関連の処理 15 | //================================================================================================================ 16 | 17 | //------------------------------------------------------------------------------------ 18 | // 各UARTの開始 19 | //------------------------------------------------------------------------------------ 20 | 21 | /// @brief 指定されたUARTラインとサーボタイプに基づいてサーボの通信プロトコルを設定する. 22 | /// @param a_line UART通信ライン（L, R, またはC）. 23 | /// @param mrd_servo_type サーボのタイプを示す整数値. 24 | /// @return サーボがサポートされている場合はtrueを, サポートされていない場合はfalseを返す. 25 | bool mrd_servos_begin(UartLine a_line, int mrd_servo_type) { 26 | if (mrd_servo_type > 0) { 27 | switch (mrd_servo_type) { 28 | case 1: // single PWM 29 | break; 30 | case 11: // I2C_PCA9685 to PWM 31 | break; 32 | case 21: // RSxTTL (FUTABA) 33 | break; 34 | case 31: // DYNAMIXEL Protocol 1.0 35 | break; 36 | case 32: // DYNAMIXEL Protocol 2.0 37 | break; 38 | case 43: // ICS3.5/3.6(KONDO,KRS) 39 | if (a_line == L) 40 | ics_L.begin(); 41 | else if (a_line == R) 42 | ics_R.begin(); 43 | else if (a_line == C) 44 | ics_C.begin(); 45 | break; 46 | case 44: // PMX(KONDO) 47 | break; 48 | case 51: // XBUS(JR PROPO) 49 | break; 50 | case 61: // STS(FEETECH) 51 | break; 52 | case 62: // SCS(FEETECH) 53 | break; 54 | default: 55 | break; 56 | } 57 | return true; 58 | } 59 | return false; 60 | } 61 | 62 | //------------------------------------------------------------------------------------ 63 | // サーボ通信フォーメーションの分岐 64 | //------------------------------------------------------------------------------------ 65 | 66 | /// @brief 指定されたサーボにコマンドを分配する. 67 | /// @param a_meridim meridim配列. 68 | /// @return サーボの駆動が成功した場合はtrueを, 失敗した場合はfalseを返す. 69 | bool mrd_servos_drive(Meridim90Union a_meridim, int sv_L_type, int sv_R_type, int sv_C_type, ServoParam &a_sv) { 70 | if (sv_L_type == 43 && sv_R_type == 43) // ICSサーボがL系R系に設定されていた場合はLR均等送信を実行 71 | { 72 | mrd_servos_drive_ics_double(a_meridim, a_sv); 73 | return true; 74 | } else { 75 | return false; 76 | } 77 | } 78 | 79 | //------------------------------------------------------------------------------------ 80 | // EEPROM等から読み込んだサーボパラメータの適用 81 | //------------------------------------------------------------------------------------ 82 | /// @brief EEPROM等に格納されているデータを読み出し、各サーボの設定に割り当てる. 83 | /// @param a_eeprom_data EEPROMから読み出したデータを格納したUnionEEPROM配列. 84 | /// @param a_sv サーボパラメータの構造体. 85 | /// @return サーボの設定を反映したUnionEEPROM配列を返す. 86 | void mrd_servos_load_param_lite(UnionEEPROM a_eeprom_data, ServoParam &a_sv) { 87 | for (int i = 0; i < 15; i++) { 88 | // 左サーボの情報を読み出し 89 | a_sv.ixl_mount[i] = (a_eeprom_data.usaval[1][20 + i * 2] >> 15) & 0x01; 90 | a_sv.ixl_id[i] = (a_eeprom_data.usaval[1][20 + i * 2] >> 8) & 0x7F; 91 | a_sv.ixl_cw[i] = (a_eeprom_data.usaval[1][20 + i * 2] & (1 << 8)) ? -1 : 1; 92 | a_sv.ixl_trim[i] = mrd.HfShort2float(short(a_eeprom_data.saval[1][21 + i * 2])); 93 | 94 | // 右サーボの情報を読み出し 95 | a_sv.ixr_mount[i] = (a_eeprom_data.usaval[1][50 + i * 2] >> 15) & 0x01; 96 | a_sv.ixr_id[i] = (a_eeprom_data.usaval[1][50 + i * 2] >> 8) & 0x7F; 97 | a_sv.ixr_cw[i] = (a_eeprom_data.usaval[1][50 + i * 2] & (1 << 8)) ? -1 : 1; 98 | a_sv.ixr_trim[i] = mrd.HfShort2float(short(a_eeprom_data.saval[1][51 + i * 2])); 99 | } 100 | } 101 | 102 | //------------------------------------------------------------------------------------ 103 | // 各種オペレーション 104 | //------------------------------------------------------------------------------------ 105 | 106 | /// @brief サーボパラメータからエラーのあるサーボのインデックス番号を作る. 107 | /// @param a_sv サーボパラメータの構造体. 108 | /// @return uint8_tで番号を返す. 109 | /// 100-149(L系統 0-49),200-249(R系統 0-49),150-199(C系統 0-49) 110 | uint8_t mrd_servos_make_errcode_3lines(ServoParam a_sv) { 111 | uint8_t servo_ix_tmp = 0; 112 | for (int i = 0; i < 15; i++) { 113 | if (a_sv.ixl_stat[i]) { 114 | servo_ix_tmp = uint8_t(i + 100); 115 | } 116 | if (a_sv.ixr_stat[i]) { 117 | servo_ix_tmp = uint8_t(i + 200); 118 | } 119 | if (a_sv.ixc_stat[i]) { 120 | servo_ix_tmp = uint8_t(i + 150); 121 | } 122 | } 123 | return servo_ix_tmp; 124 | } 125 | 126 | /// @brief すべてのサーボモーターをオフ（フリー状態）に設定する. 127 | bool mrd_servos_all_off(Meridim90Union a_meridim) { 128 | for (int i = 0; i < 15; i++) { 129 | a_meridim.sval[i * 2 + 20] = 0; // サーボのコマンドをオフに設定 130 | a_meridim.sval[i * 2 + 50] = 0; // 131 | } 132 | Serial.println("All servos off."); 133 | return true; 134 | } 135 | 136 | #endif //__MERIDIAN_SERVO_DISTRIBUTOR_H__ 137 | -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/src/mrd_disp.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_DISPLAY_MESSAGE_H__ 2 | #define __MERIDIAN_DISPLAY_MESSAGE_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "keys.h" 7 | #include "main.h" 8 | 9 | // ライブラリ導入 10 | #include 11 | 12 | //================================================================================================================ 13 | // シリアルモニタリング出力関連の処理 14 | //================================================================================================================ 15 | 16 | class MrdMsgHandler { 17 | private: 18 | Stream &m_serial; // シリアルオブジェクトの参照を保持 19 | 20 | public: 21 | // コンストラクタでStreamオブジェクトを受け取り, メンバーに保存 22 | MrdMsgHandler(Stream &a_serial) : m_serial(a_serial) {} 23 | 24 | //------------------------------------------------------------------------------------ 25 | // 起動時メッセージ 26 | //------------------------------------------------------------------------------------ 27 | 28 | /// @brief 指定された秒数だけキャパシタの充電プロセスを示すメッセージを表示する. 29 | /// @param a_mill 充電プロセスの期間を秒単位で指定. 30 | void charging(int a_mill) { 31 | m_serial.print("Charging the capacitor."); 32 | for (int i = 0; i < a_mill; i++) { 33 | if (i % 100 == 0) { // 100msごとにピリオドを表示 34 | m_serial.print("."); 35 | } 36 | delay(1); 37 | } 38 | m_serial.println(); 39 | } 40 | 41 | /// @brief システムのバージョン情報と通信速度の設定を表示するためのメッセージを出力する. 42 | /// @param a_version バージョン情報. 43 | /// @param a_pc_speed PCとのUSBシリアル通信速度. 44 | /// @param a_spi_speed SPIの通信速度. 45 | void hello_twin_esp(String a_version, int a_pc_speed, int a_spi_speed) { 46 | m_serial.println(); 47 | m_serial.print("Hi, This is "); 48 | m_serial.println(a_version); 49 | m_serial.print("Set PC-USB "); 50 | m_serial.print(a_pc_speed); 51 | m_serial.println(" bps"); 52 | m_serial.print("Set SPI0 "); 53 | m_serial.print(a_spi_speed); 54 | m_serial.println(" bps"); 55 | } 56 | 57 | /// @brief wifiの接続開始メッセージを出力する. 58 | /// @param a_ssid 接続先のSSID. 59 | void esp_wifi(const char *a_ssid) { 60 | m_serial.println("WiFi connecting to => " + String(a_ssid)); // WiFi接続完了通知 61 | } 62 | 63 | /// @brief wifiの接続完了メッセージと各IPアドレスを出力する. 64 | /// @param a_flg_fixed_ip 固定IPかどうか. true:固定IP, false:動的IP. 65 | /// @param a_ssid 接続先のSSID. 66 | /// @param a_fixedip 固定IPの場合の値. 67 | void esp_ip(bool a_flg_fixed_ip, const char *a_ssid, const char *a_fixedip) { 68 | m_serial.println("WiFi successfully connected."); // WiFi接続完了通知 69 | m_serial.println("PC's IP address target => " + 70 | String(WIFI_SEND_IP)); // 送信先PCのIPアドレスの表示 71 | 72 | if (a_flg_fixed_ip) { 73 | m_serial.println("ESP32's IP address => " + String(FIXED_IP_ADDR) + 74 | " (*Fixed)"); // ESP32自身のIPアドレスの表示 75 | } else { 76 | m_serial.print("ESP32's IP address => "); // ESP32自身のIPアドレスの表示 77 | m_serial.println(WiFi.localIP().toString()); 78 | } 79 | } 80 | 81 | /// @brief システムに接続設定したジョイパッドのタイプを表示する. 82 | void mounted_pad() { 83 | m_serial.print("Pad Receiver mounted : "); 84 | 85 | if (MOUNT_PAD == MERIMOTE) { 86 | m_serial.println("Merimote."); 87 | } else if (MOUNT_PAD == BLUERETRO) { 88 | m_serial.println("BlueRetro."); 89 | } else if (MOUNT_PAD == SBDBT) { 90 | m_serial.println("SBDBT."); 91 | } else if (MOUNT_PAD == KRR5FH) { 92 | m_serial.println("KRC-5FH."); 93 | } else { 94 | m_serial.println("None (PC)."); 95 | } 96 | } 97 | 98 | /// @brief システムの動作開始を示すメッセージを出力する. 99 | void flow_start_twin_esp() { 100 | m_serial.println(); 101 | m_serial.println("-) Meridian TWIN system on side ESP32 now flows. (-"); 102 | } 103 | 104 | //------------------------------------------------------------------------------------ 105 | // イベントメッセージ 106 | //------------------------------------------------------------------------------------ 107 | 108 | /// @brief システム内の様々な通信エラーとスキップ数をモニタリングし, シリアルポートに出力する. 109 | /// @param mrd_disp_all_err モニタリング表示のオンオフ. 110 | /// @param a_err エラーデータの入った構造体. 111 | /// @return エラーメッセージを表示した場合はtrueを, 表示しなかった場合はfalseを返す. 112 | bool all_err(bool mrd_disp_all_err, MrdErr a_err) { 113 | if (mrd_disp_all_err) { 114 | m_serial.print("[ERR] es>pc:"); 115 | m_serial.print(a_err.esp_pc); 116 | m_serial.print(" pc>es:"); 117 | m_serial.print(a_err.pc_esp); 118 | m_serial.print(" es>ts:"); 119 | m_serial.print(a_err.esp_tsy); 120 | m_serial.print(" ts>es:"); 121 | m_serial.print(a_err.esp_tsy); 122 | m_serial.print(" tsSkp:"); 123 | m_serial.print(a_err.tsy_skip); 124 | m_serial.print(" esSkp:"); 125 | m_serial.print(a_err.esp_skip); 126 | m_serial.print(" pcSkp:"); 127 | m_serial.print(a_err.pc_skip); 128 | m_serial.println(); 129 | return true; 130 | } 131 | return false; 132 | } 133 | 134 | /// @brief 期待するシーケンス番号と実施に受信したシーケンス番号を表示する. 135 | /// @param a_seq_expect 期待するシーケンス番号. 136 | /// @param a_seq_rcvd 実際に受信したシーケンス番号. 137 | /// @param a_disp_seq_num 表示するかどうかのブール値. 138 | /// @return エラーメッセージを表示した場合はtrueを, 表示しなかった場合はfalseを返す. 139 | bool seq_number(uint16_t a_seq_expect, uint16_t a_seq_rcvd, bool a_disp) { 140 | if (a_disp) { 141 | m_serial.print("Seq ep/rv "); 142 | m_serial.print(a_seq_expect); 143 | m_serial.print("/"); 144 | m_serial.println(a_seq_rcvd); 145 | return true; 146 | } 147 | return false; 148 | } 149 | }; 150 | 151 | #endif // __MERIDIAN_DISPLAY_MESSAGE_H__ -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/lib/gs2d/gs2d_driver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @file gs2d_driver.h 3 | * @author 4 | * @date 2021/01/26 5 | * @brief 6 | */ 7 | #pragma once 8 | 9 | /* Includes ------------------------------------------------------------------*/ 10 | #include 11 | #include 12 | #include "gs2d_type.h" 13 | 14 | /* Variables -----------------------------------------------------------------*/ 15 | namespace gs2d 16 | { 17 | class Driver 18 | { 19 | protected: 20 | // エラーコード保存用（例外発生はしない） 21 | uint8_t errorBits = 0; 22 | void notSupport(void) { errorBits |= NotSupportError; } 23 | void badInput(void) { 24 | errorBits |= BadInputError; 25 | } 26 | 27 | // 同期/非同期モードのフラグ 28 | bool operatingMode = false; 29 | 30 | public: 31 | 32 | Driver() {} 33 | virtual ~Driver() {} 34 | 35 | // System 36 | // Multi Thread 37 | void changeOperatingMode(bool threading) { operatingMode = threading; } 38 | virtual void spin(void) = 0; 39 | 40 | // Error 41 | uint8_t getErrorCode(void) { return errorBits; } 42 | void clearErrorCode(void) { errorBits = 0; } 43 | 44 | // ------------------------------------------------------------------------------------------ 45 | // General 46 | virtual uint32_t readMemory(uint8_t id, uint16_t address, uint8_t length, CallbackType callback) = 0; 47 | virtual void writeMemory(uint8_t id, uint16_t address, uint32_t data, uint8_t length) = 0; 48 | 49 | // Ping 50 | virtual uint16_t ping(uint8_t id, CallbackType callback = 0) = 0; 51 | 52 | // Torque 53 | virtual uint8_t readTorqueEnable(uint8_t id, CallbackType callback = 0) = 0; 54 | virtual void writeTorqueEnable(uint8_t id, uint8_t torque) = 0; 55 | 56 | // Temperature 57 | virtual uint16_t readTemperature(uint8_t id, CallbackType callback = 0) = 0; 58 | 59 | // Current 60 | virtual int32_t readCurrent(uint8_t id, CallbackType callback = 0) = 0; 61 | 62 | // Voltage 63 | virtual gFloat readVoltage(uint8_t id, CallbackType callback = 0) = 0; 64 | 65 | // Target Position 66 | virtual gFloat readTargetPosition(uint8_t id, CallbackType callback = 0) = 0; 67 | virtual void writeTargetPosition(uint8_t id, gFloat position) = 0; 68 | 69 | // Current Position 70 | virtual gFloat readCurrentPosition(uint8_t id, CallbackType callback = 0) = 0; 71 | 72 | // Offset 73 | virtual gFloat readOffset(uint8_t id, CallbackType callback = 0) = 0; 74 | virtual void writeOffset(uint8_t id, gFloat offset) = 0; 75 | 76 | // Deadband 77 | virtual gFloat readDeadband(uint8_t id, CallbackType callback = 0) = 0; 78 | virtual void writeDeadband(uint8_t id, gFloat deadband) = 0; 79 | 80 | // Target Time 81 | virtual gFloat readTargetTime(uint8_t id, CallbackType callback = 0) = 0; 82 | virtual void writeTargetTime(uint8_t id, gFloat targetTime) = 0; 83 | 84 | // Accel Time 85 | virtual gFloat readAccelTime(uint8_t id, CallbackType callback = 0) = 0; 86 | virtual void writeAccelTime(uint8_t id, gFloat accelTime) = 0; 87 | 88 | // P Gain 89 | virtual uint32_t readPGain(uint8_t id, CallbackType callback = 0) = 0; 90 | virtual void writePGain(uint8_t id, uint32_t gain) = 0; 91 | 92 | // I Gain 93 | virtual uint32_t readIGain(uint8_t id, CallbackType callback = 0) = 0; 94 | virtual void writeIGain(uint8_t id, uint32_t gain) = 0; 95 | 96 | // D Gain 97 | virtual uint32_t readDGain(uint8_t id, CallbackType callback = 0) = 0; 98 | virtual void writeDGain(uint8_t id, uint32_t gain) = 0; 99 | 100 | // Max Torque 101 | virtual uint32_t readMaxTorque(uint8_t id, CallbackType callback = 0) = 0; 102 | virtual void writeMaxTorque(uint8_t id, uint32_t maxTorque) = 0; 103 | 104 | // Speed 105 | virtual gFloat readSpeed(uint8_t id, CallbackType callback = 0) = 0; 106 | virtual void writeSpeed(uint8_t id, gFloat speed) = 0; 107 | 108 | // ID 109 | virtual uint32_t readID(uint8_t id, CallbackType callback = 0) = 0; 110 | virtual void writeID(uint8_t id, uint32_t newid) = 0; 111 | 112 | // ROM 113 | virtual void saveRom(uint8_t id) = 0; 114 | virtual void loadRom(uint8_t id) = 0; 115 | virtual void resetMemory(uint8_t id) = 0; 116 | 117 | // Baudrate 118 | virtual uint32_t readBaudrate(uint8_t id, CallbackType callback = 0) = 0; 119 | virtual void writeBaudrate(uint8_t id, uint32_t baudrate) = 0; 120 | 121 | // CW Limit Position 122 | virtual gFloat readLimitCWPosition(uint8_t id, CallbackType callback = 0) = 0; 123 | virtual void writeLimitCWPosition(uint8_t id, gFloat limitPosition) = 0; 124 | 125 | // CCW Limit Position 126 | virtual gFloat readLimitCCWPosition(uint8_t id, CallbackType callback = 0) = 0; 127 | virtual void writeLimitCCWPosition(uint8_t id, gFloat limitPosition) = 0; 128 | 129 | // Temperature Limit 130 | virtual uint32_t readLimitTemperature(uint8_t id, CallbackType callback = 0) = 0; 131 | virtual void writeLimitTemperature(uint8_t id, uint32_t temperature) = 0; 132 | 133 | // Curent Limit 134 | virtual uint32_t readLimitCurrent(uint8_t id, CallbackType callback = 0) = 0; 135 | virtual void writeLimitCurrent(uint8_t id, uint32_t current) = 0; 136 | 137 | // Drive Mode 138 | virtual uint32_t readDriveMode(uint8_t id, CallbackType callback = 0) = 0; 139 | virtual void writeDriveMode(uint8_t id, uint32_t mode) = 0; 140 | 141 | // Burst Function 142 | virtual void burstReadMemory(uint8_t* idList, uint8_t count, uint16_t address, uint8_t length, CallbackType callback) = 0; 143 | virtual void burstWriteMemory(uint8_t* idList, uint32_t* dataList, uint8_t count, uint16_t address, uint8_t length) = 0; 144 | 145 | // Burst Function(Position) 146 | virtual void burstReadPositions(uint8_t* idList, uint8_t count, CallbackType callback) = 0; 147 | virtual void burstWriteTargetPositions(uint8_t* idList, gFloat* positionList, uint8_t count) = 0; 148 | }; 149 | } 150 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/lib/IcsClass_V210/src/IcsBaseClass.h: -------------------------------------------------------------------------------- 1 | 2 | /** 3 | * @file IcsBaseClass.h 4 | * @brief ICS3.5/3.6 bace library header file 5 | * @author Kondo Kagaku Co.,Ltd. 6 | * @date 2017/12/27 7 | * @version 2.1.0 8 | * @copyright Kondo Kagaku Co.,Ltd. 2020 9 | 10 | * @mainpage IcsClassの概要 11 | * このライブラリは近藤科学製ロボット用サーボ(KRSシリーズ)を動かすためのライブラリです。
12 | * ICS3.5およびICS3.6に対応しています。
13 | * 現行では、Arduino製品で使えるように設計しています。
14 | * 使い方および詳細は、下記弊社HPをご覧ください。
15 | * http://kondo-robot.com/
16 | * 不具合等ありましたら、弊社HPを参照にご連絡ください。
17 | 18 | *@par 変更履歴 19 | *@par 2020/02/20 Ver 2.1.0 20 | * - keywords.txtのgetStrをgetStrcに変更 21 | * - synchronizeがprotectedになっていたのをpublicに変更 //外部で使いたかったため 22 | * - IDコマンドの実装 23 | *@par 2017/12/27 ver 2.0.0 24 | * - ICSClassを通信部分を分離。IcsBaseClassを作成。 25 | * - Serialを使用する「IcsHardSerialClass」と 26 | *
SoftSerialを使用する「IcsSoftSerialClass」をIcsBaseClassから派生させる 27 | * - SoftSerialはパリティが使えずタイミングが合わなかったので、「KoCustomSoftSerial」を作成 28 | *@par 2016/12/27　ver 1.0.0 29 | * - 初回配布 30 | **/ 31 | 32 | 33 | 34 | #ifndef __ics_Base_Servo_h__ 35 | #define __ics_Base_Servo_h__ 36 | #include "Arduino.h" 37 | 38 | 39 | //KRR KRC-5FH ボタン定義//////// 40 | /** 41 | * @enum KRR_BUTTON 42 | * @brief KRRが受信するボタンデータの定義 43 | * @brief 同時押しの場合は各データの論理和をとります 44 | */ 45 | enum KRR_BUTTON : unsigned short 46 | { 47 | KRR_BUTTON_NONE = 0x0000, ///< 何も押されていない 48 | 49 | //左パッド 50 | KRR_BUTTON_UP = 0x0001, ///< ↑ 51 | KRR_BUTTON_DOWN = 0x0002, ///< ↓ 52 | KRR_BUTTON_RIGHT = 0x0004, ///< → 53 | KRR_BUTTON_LEFT = 0x0008, ///< ← 54 | 55 | //右パッド 56 | KRR_BUTTON_TRIANGLE = 0x0010, ///< △ 57 | KRR_BUTTON_CROSS = 0x0020, ///< × 58 | KRR_BUTTON_CIRCLE = 0x0040, ///< ○ 59 | KRR_BUTTON_SQUARE = 0x0100, ///< □ 60 | 61 | KRR_BUTTON_S1 = 0x0200, ///< シフト1 左手前 62 | KRR_BUTTON_S2 = 0x0400, ///< シフト2 左奥 63 | KRR_BUTTON_S3 = 0x0800, ///< シフト3 右手前 64 | KRR_BUTTON_S4 = 0x1000, ///< シフト4 右奥 65 | 66 | KRR_BUTTON_FALSE = 0xFFFF ///< エラー値(受信失敗等) 67 | }; 68 | 69 | 70 | 71 | //IcsBaseClassクラス/////////////////////////////////////////////////// 72 | /** 73 | * @class IcsBaseClass 74 | * @brief 近藤科学のICS 3.5/3.6 サーボモータをマイコン経由で動作させるための基クラス 75 | * @brief Arduino用にHardwareSerial,SoftwareSerialが使えるように基クラスを作成 76 | **/ 77 | class IcsBaseClass 78 | { 79 | //固定値(公開分) 80 | public: 81 | //サーボID範囲 //////////////////////////////////// 82 | static constexpr int MAX_ID = 31; ///< サーボIDの最大値 83 | static constexpr int MIN_ID = 0; ///< サーボIDの最小値 84 | 85 | //サーボ最大最小リミット値 86 | 87 | static constexpr int MAX_POS = 11500; ///<サーボのポジション最大値 88 | 89 | static constexpr int MIN_POS = 3500; ///<サーボのポジション最小値 90 | 91 | static constexpr int ICS_FALSE = -1; ///< ICS通信等々で失敗したときの値 92 | 93 | //固定値(非公開分) 94 | protected : 95 | 96 | static constexpr float ANGLE_F_FALSE = 9999.9; ///< 角度計算時、範囲内に入ってない場合は999.9にする(負側の場合はマイナスをつける) 97 | static constexpr int ANGLE_I_FALSE = 0x7FFF; ///< 角度計算時、範囲内に入ってない場合は0x7FFFにする(負側の場合はマイナスをつける) 98 | 99 | //各パラメータ設定範囲 100 | static constexpr int MAX_127 = 127; ///< パラメータの最大値 101 | 102 | static constexpr int MAX_63 = 63; ///< パラメータの最大値(電流値) 103 | 104 | static constexpr int MIN_1 = 1; ///< パラメータの最小値 105 | 106 | // static const float MAX_DEG = 135.0; 107 | static constexpr float MAX_DEG = 180.0; ///< 角度の最大値 108 | 109 | // static const float MIN_DEG = -135.0; 110 | static constexpr float MIN_DEG = -180.0; ///< 角度の最大値 111 | 112 | //static const int MAX_100DEG = 13500; 113 | static constexpr int MAX_100DEG = 18000; ///< 角度(x100)の最大値 114 | 115 | //static const int MIN_100DEG = -13500; 116 | static constexpr int MIN_100DEG = -18000; ///< 角度(x100)の最小値 117 | 118 | //クラス内の型定義 119 | public: 120 | 121 | //コンストラクタ、デストラクタ 122 | public: 123 | //コンストラクタ(construncor) 124 | 125 | //変数 126 | public: 127 | 128 | 129 | protected : 130 | 131 | //関数 132 | 133 | //データ送受信 134 | public: 135 | //データ送受信 ///////////////////////////////////////////////////////////////////////////////////////////// 136 | /** 137 | * @brief ICS通信の送受信 138 | * @param[in,out] *txBuf 139 | * @param[in] txLen 140 | * @param[out] *rxBuf 受信格納バッファ 141 | * @param[in] rxLen 受信データ数 142 | * @retval true 通信成功 143 | * @retval false 通信失敗 144 | * @attention 送信データ数、受信データ数はコマンドによって違うので注意する 145 | * @attention この関数は外部に書く事 146 | **/ 147 | virtual bool synchronize(byte *txBuf, byte txLen, byte *rxBuf, byte rxLen); 148 | 149 | //servo関連 150 | public: 151 | 152 | //サーボ位置決め設定 153 | int setPos(byte id,unsigned int pos); //目標値設定 154 | int setFree(byte id); //サーボ脱力＋現在値読込 155 | 156 | //各種パラメータ書込み 157 | int setStrc(byte id, unsigned int strc); //ストレッチ書込 1～127 1(弱） <=> 127(強） 158 | int setSpd(byte id, unsigned int spd); //スピード書込 1～127 1(遅い) <=> 127(速い) 159 | int setCur(byte id, unsigned int curlim); //電流制限値書込 1～63 1(低い) <=> 63 (高い) 160 | int setTmp(byte id, unsigned int tmplim); //温度上限書込 1～127 127(低温） <=> 1(高温) 161 | //各種パラメータ読込み 162 | int getStrc(byte id); //ストレッチ読込 1～127 1(弱） <=> 127(強） 163 | int getSpd(byte id); //スピード読込 1～127 1(遅い)<=> 127(速い) 164 | int getCur(byte id); //電流値読込 63←0 | 64→127 165 | int getTmp(byte id); //現在温度読込 127(低温）<=>　0(高温) 166 | int getPos(byte id); //現在位置読込　 ※ICS3.6以降で有効 167 | 168 | int getID(); 169 | int setID(byte id); 170 | 171 | protected : 172 | //サーボIDリミット 173 | byte idMax(byte id); 174 | 175 | ////サーボ可動範囲　パラメータ範囲　リミット設定 176 | bool maxMin(int maxPos, int minPos, int val); 177 | 178 | //角度関連 179 | public: 180 | //角度変換 POSから角度へ変換 181 | static int degPos(float deg); 182 | //角度変換 角度からPOSへ変換 183 | static float posDeg(int pos); 184 | 185 | //角度変換 x100 POSから角度へ変換 186 | static int degPos100(int deg); 187 | //角度変換 x100 角度からPOSへ変換 188 | static int posDeg100(int pos); 189 | 190 | //KRR関連 191 | public: 192 | //KRRからボタンデータ受信 193 | unsigned short getKrrButton(); 194 | 195 | //KRRからPAアナログデータ受信 196 | int getKrrAnalog(int paCh); 197 | 198 | //KRRから全データ受信 199 | bool getKrrAllData(unsigned short *button,int adData[4]); 200 | 201 | }; 202 | 203 | #endif 204 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_util.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_UTILITY_H__ 2 | #define __MERIDIAN_UTILITY_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "main.h" 7 | 8 | //================================================================================================================ 9 | // Utility ごく小規模な汎用関数 10 | //================================================================================================================ 11 | 12 | /// @brief 配列の中で0以外が入っている最大のIndexを求める. 13 | /// @param a_arr 配列 14 | /// @param a_size 配列の長さ 15 | /// @return 0以外が入っている最大のIndex. すべて0の場合は1を反す. 16 | int mrd_max_used_index(const int a_arr[], int a_size) { 17 | int max_index_tmp = 0; 18 | for (int i = 0; i < a_size; ++i) { 19 | if (a_arr[i] != 0) { 20 | max_index_tmp = i; 21 | } 22 | } 23 | return max_index_tmp + 1; 24 | } 25 | 26 | /// @brief 指定された位置のビットをセットする(16ビット変数用). 27 | /// @param a_byte ビットをセットする16ビットの変数.参照渡し. 28 | /// @param a_bit_pos セットするビットの位置(0から15). 29 | /// @return なし. 30 | inline void mrd_set_bit16(uint16_t &a_byte, uint16_t a_bit_pos) { a_byte |= (1 << a_bit_pos); } 31 | 32 | /// @brief 指定された位置のビットをクリアする(16ビット変数用). 33 | /// @param a_byte ビットをクリアする16ビットの変数.参照渡し. 34 | /// @param a_bit_pos クリアするビットの位置(0から15). 35 | /// @return なし. 36 | inline void mrd_clear_bit16(uint16_t &a_byte, uint16_t a_bit_pos) { a_byte &= ~(1 << a_bit_pos); } 37 | 38 | /// @brief 指定された位置のビットをセットする(8ビット変数用). 39 | /// @param value ビットをセットする8ビットの変数.参照渡し. 40 | /// @param a_bit_pos セットするビットの位置(0から7). 41 | /// @return なし. 42 | inline void mrd_set_bit8(uint8_t &value, uint8_t a_bit_pos) { value |= (1 << a_bit_pos); } 43 | 44 | /// @brief 指定された位置のビットをクリアする(8ビット変数用). 45 | /// @param value ビットをクリアする8ビットの変数.参照渡し. 46 | /// @param a_bit_pos クリアするビットの位置(0から7). 47 | /// @return なし. 48 | inline void mrd_clear_bit8(uint8_t &value, uint8_t a_bit_pos) { value &= ~(1 << a_bit_pos); } 49 | 50 | /// @brief 任意の整数値から、任意幅のビット列を取り出す汎用関数. 51 | /// @tparam T 型テンプレート.任意の整数型(符号付き・符号無しどちらでも可) 52 | /// @param value 抽出元となる値. 53 | /// @param pos 取り出し開始位置(LSB＝0, 右から数え, 最初は0番) 54 | /// @param len 取り出すビット幅 55 | /// @return unsigned 取り出したビット列(0〜 2^len−1 の範囲) 56 | template // 型テンプレート 57 | unsigned mrd_slice_bits(T value, unsigned pos, unsigned len) { 58 | return (static_cast(value) >> pos) & ((1u << len) - 1u); 59 | } 60 | 61 | /// @brief 任意の整数値の指定範囲のビット列に値を設定する汎用関数. 62 | /// @tparam T 型テンプレート.任意の整数型(符号付き・符号無しどちらでも可) 63 | /// @tparam U 設定する値の型(通常は整数型) 64 | /// @param value 設定先となる値. 65 | /// @param pos 設定開始位置(LSB＝0, 右から数え, 最初は0番) 66 | /// @param len 設定するビット幅 67 | /// @param new_value 設定したい値(0〜 2^len−1 の範囲) 68 | /// @return T 設定後の値 69 | template 70 | T mrd_set_bits(T value, unsigned pos, unsigned len, unsigned new_value) { 71 | unsigned mask = ((1u << len) - 1u); // マスクを作成.指定範囲のビットを1で埋める 72 | unsigned masked_new_value = new_value & mask; // マスクして範囲内に収める 73 | T cleared = value & ~(static_cast(mask) << pos); // 既存の値から指定範囲のビットをクリア 74 | return cleared | (static_cast(masked_new_value) << pos); // 新しい値を設定 75 | } 76 | 77 | /// @brief 列挙型(L,R,C)から文字列を取得する関数. 78 | /// @param a_line 列挙型 enum UartLine 79 | /// @return 列挙型の内容に応じて文字列"L","R","C"返す. 80 | const char *mrd_get_line_name(UartLine a_line) { 81 | switch (a_line) { 82 | case L: 83 | return "L"; 84 | case R: 85 | return "R"; 86 | case C: 87 | return "C"; 88 | default: 89 | return "Unknown"; 90 | } 91 | } 92 | 93 | /// @brief 数値をシリアルモニタ表示するときにパディングする. 94 | /// @param num 表示したい値. 95 | /// @param total_width 桁数. 96 | /// @param rac_Width 小数点以下の桁数(0ならば小数点以下非表示). 97 | /// @param show_plus +記号の有無. 98 | /// @return 整形されたストリング. 99 | String mrd_pddstr(float num, int total_width, int frac_width, bool show_plus = true) { 100 | char buf[30]; 101 | char sign = (num < 0) ? '-' : (show_plus ? '+' : '\0'); 102 | if (num < 0) 103 | num = -num; 104 | 105 | // 小数あり/なし 106 | if (frac_width) { 107 | if (sign) 108 | sprintf(buf, "%c%d.%0*d", sign, (int)num, frac_width, (int)((num - (int)num) * pow(10, frac_width) + 0.5)); 109 | else 110 | sprintf(buf, "%d.%0*d", (int)num, frac_width, (int)((num - (int)num) * pow(10, frac_width) + 0.5)); 111 | } else { 112 | if (sign) 113 | sprintf(buf, "%c%d", sign, (int)(num + 0.5)); 114 | else 115 | sprintf(buf, "%d", (int)(num + 0.5)); 116 | } 117 | 118 | // パディング 119 | String result = ""; 120 | int pad = total_width - strlen(buf); 121 | for (int i = 0; i < pad; i++) 122 | result += ' '; 123 | result += buf; 124 | return result; 125 | } 126 | 127 | //------------------------------------------------------------------------------------ 128 | // タイムアウト監視用タイマー 129 | //------------------------------------------------------------------------------------ 130 | // タイマーの開始状態を保持するための静的変数 131 | static unsigned long timeout_start = 0; 132 | static bool flg_timer_started = false; // タイマーが開始されたかどうかのフラグ 133 | 134 | /// @brief 指定されたミリ秒のタイムアウトを監視する. mrd_timeout_resetとセットで使う. 135 | /// @param a_limit タイムアウトまでの時間(ms) 136 | /// @return タイムアウトでtrueを返す. 137 | bool mrd_timeout_check(unsigned long a_limit) { 138 | // タイマーが開始されていない場合は現在の時間を記録してタイマーを開始 139 | if (!flg_timer_started) { 140 | timeout_start = millis(); 141 | flg_timer_started = true; // タイムアウト監視開始フラグをアゲる 142 | } 143 | 144 | unsigned long current_time = millis(); // 現在の時間を取得 145 | 146 | if (current_time - timeout_start >= a_limit) { // 指定された時間が経過しているかチェック 147 | flg_timer_started = false; // タイムアウト監視開始フラグをサゲる 148 | return true; // 指定された時間が経過していれば true を返す 149 | } 150 | 151 | return false; // まだ時間が経過していなければ false を返す 152 | } 153 | 154 | /// @brief タイムアウト監視開始フラグをリセットする. mrd_timeout_checkとセットで使う. 155 | void mrd_timeout_reset() { 156 | flg_timer_started = false; // タイマーをリセットして次回の呼び出しに備える 157 | } 158 | 159 | //------------------------------------------------------------------------------------ 160 | // meriput / meridimへのデータ書き込み 161 | //------------------------------------------------------------------------------------ 162 | 163 | /// @brief meridim配列のチェックサムを算出して[len-1]に書き込む. 164 | /// @param a_meridim Meridim配列の共用体. 参照渡し. 165 | /// @return 常にtrueを返す. 166 | bool mrd_meriput90_cksm(Meridim90Union &a_meridim, int len = 90) { 167 | int a_cksm = 0; 168 | for (int i = 0; i < len - 1; i++) { 169 | a_cksm += int(a_meridim.sval[i]); 170 | } 171 | a_meridim.sval[len - 1] = short(~a_cksm); 172 | return true; 173 | } 174 | 175 | #endif //__MERIDIAN_UTILITY_H__ 176 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_pad.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_JOYPAD_H__ 2 | #define __MERIDIAN_JOYPAD_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "main.h" 7 | 8 | //================================================================================================================ 9 | // JOYPAD 関連の処理 10 | //================================================================================================================ 11 | 12 | // リモコン受信ボタンデータの変換テーブル 13 | constexpr unsigned short PAD_TABLE_KRC5FH_TO_COMMON[16] = // 14 | {0, 64, 32, 128, 1, 4, 2, 8, // 15 | 1024, 4096, 512, 2048, 16, 64, 32, 256}; // 16 | 17 | //------------------------------------------------------------------------------------ 18 | // タイプ別のJOYPAD読み込み処理 19 | //------------------------------------------------------------------------------------ 20 | 21 | //---------------------------------------------------------------------- 22 | // KRC-5FHの読み込み 23 | //---------------------------------------------------------------------- 24 | 25 | /// @brief KRC-5FHジョイパッドからデータを読み取り, 指定された間隔でデータを更新する. 26 | /// @param a_interval 読み取り間隔（ミリ秒）. 27 | /// @return 更新されたジョイパッドの状態を64ビット整数で返す. 28 | uint64_t mrd_pad_read_krc(uint a_interval, IcsHardSerialClass &a_ics) { 29 | static uint64_t pre_val_tmp = 0; // 前回の値を保持する静的変数 30 | int8_t pad_analog_tmp[4] = {0}; // アナログ入力のデータ組み立て用 31 | //static int calib[4] = {0}; // アナログスティックのキャリブレーション値 32 | 33 | static unsigned long last_time_tmp = 0; // 最後に関数が呼ばれた時間を記録 34 | unsigned long current_time_tmp = millis(); 35 | 36 | if (current_time_tmp - last_time_tmp >= a_interval) { 37 | unsigned short krr_button_tmp; // krrからのボタン入力データ 38 | int krr_analog_tmp[4]; // krrからのアナログ入力データ 39 | unsigned short pad_common_tmp = 0; // PS準拠に変換後のボタンデータ 40 | bool rcvd_tmp; // 受信機がデータを受信成功したか 41 | rcvd_tmp = ics_R.getKrrAllData(&krr_button_tmp, krr_analog_tmp); 42 | delayMicroseconds(2); 43 | 44 | if (rcvd_tmp) // リモコンデータが受信できていたら 45 | { 46 | // ボタンデータの処理 47 | int button_tmp = krr_button_tmp; // 受信ボタンデータの読み込み用 48 | 49 | if (PAD_GENERALIZE) { // ボタンデータの一般化処理 50 | if ((button_tmp & 15) == 15) { // 左側十字ボタン全部押しなら select押下とみなす 51 | pad_common_tmp += 1; 52 | button_tmp &= 0b1111111111110000; // 左十字ボタンのクリア 53 | } 54 | 55 | if ((button_tmp & 368) == 368) { 56 | pad_common_tmp += 8; // 右側十字ボタン全部押しなら start押下とみなす 57 | button_tmp &= 0b1111111010001111; // 右十字ボタンのクリア 58 | } 59 | 60 | // ボタン値の変換(一般化) 61 | for (int i = 0; i < 16; i++) { 62 | uint16_t mask_tmp = 1 << i; 63 | if (PAD_TABLE_KRC5FH_TO_COMMON[i] & button_tmp) { 64 | pad_common_tmp |= mask_tmp; 65 | } 66 | } 67 | pad_common_tmp &= 0b11111111111111001; // 2と4のビットのクリア(謎のデータ調整) 68 | 69 | // アナログ入力データの処理 70 | if (krr_analog_tmp[0] + krr_analog_tmp[1] + krr_analog_tmp[2] + krr_analog_tmp[3]) { 71 | for (int i = 0; i < 4; i++) { 72 | pad_analog_tmp[i] = (krr_analog_tmp[i] - 62) << 2; 73 | pad_analog_tmp[i] = (pad_analog_tmp[i] < -127) ? -127 : pad_analog_tmp[i]; 74 | pad_analog_tmp[i] = (pad_analog_tmp[i] > 127) ? 127 : pad_analog_tmp[i]; 75 | } 76 | } else 77 | for (int i = 0; i < 4; i++) { 78 | pad_analog_tmp[i] = 0; 79 | } 80 | } else { 81 | pad_common_tmp = button_tmp; // ボタンの変換なし生値を使用 82 | } 83 | } 84 | 85 | // アナログスティックのキャリブレーション 86 | // [WIP] 87 | 88 | // データの組み立て 89 | uint64_t updated_val_tmp = 0; 90 | updated_val_tmp = static_cast(pad_common_tmp); 91 | updated_val_tmp |= ((uint64_t)pad_analog_tmp[0] & 0xFF) << 16; 92 | updated_val_tmp |= ((uint64_t)pad_analog_tmp[1] & 0xFF) << 24; 93 | updated_val_tmp |= ((uint64_t)pad_analog_tmp[2] & 0xFF) << 32; 94 | updated_val_tmp |= ((uint64_t)pad_analog_tmp[3] & 0xFF) << 40; 95 | 96 | last_time_tmp = current_time_tmp; // 最後の実行時間を更新 97 | pre_val_tmp = updated_val_tmp; 98 | return updated_val_tmp; 99 | } 100 | return pre_val_tmp; 101 | } 102 | 103 | //------------------------------------------------------------------------------------ 104 | // 各種パッド読み取りへの分岐 105 | //------------------------------------------------------------------------------------ 106 | 107 | /// @brief 指定されたジョイパッドタイプに応じてジョイパッドのデータを読み取る. 108 | /// @param a_type 使用するジョイパッドのタイプを示す列挙型（MERIMOTE, BLUERETRO, SBDBT, KRR5FH）. 109 | /// @param a_interval ジョイパッドのデータ読み取り間隔（ミリ秒）. 110 | /// @return PAD受信値を共用体(PadUnion)データで返す. 111 | /// @note 関数内で外部変数ics_Rを使用. 112 | PadUnion mrd_pad_reader(PadType a_type, uint a_interval) { 113 | PadUnion pad_array_tmp = {0}; 114 | if (a_type == KRR5FH) // KRC-5FH+KRR-5FH 115 | { 116 | pad_array_tmp.ui64val[0] = mrd_pad_read_krc(a_interval, ics_R); 117 | return pad_array_tmp; 118 | } else if (a_type == MERIMOTE) // merimote 119 | { 120 | for (int i = 0; i < 4; i++) { 121 | pad_array_tmp.sval[i] = pad_array.sval[i]; 122 | } 123 | } else if (a_type == BLUERETRO) // BLUERETRO(未実装) 124 | { 125 | return pad_array_tmp; 126 | } else if (a_type == SBDBT) // SBDBT(未実装) 127 | { 128 | return pad_array_tmp; 129 | } 130 | return pad_array_tmp; 131 | } 132 | 133 | //------------------------------------------------------------------------------------ 134 | // meridimへのデータ書き込み 135 | //------------------------------------------------------------------------------------ 136 | 137 | /// @brief meridim配列にPADデータを書き込む. 138 | /// @param a_type 使用するジョイパッドのタイプを示す列挙型（MERIMOTE, BLUERETRO, SBDBT, KRR5FH）. 139 | /// @param a_meridim Meridim配列の共用体. 参照渡し. 140 | /// @param a_pad_array PAD受信値の格納用配列. 141 | /// @param a_marge PADボタンデータをマージするかどうかのブール値. 142 | /// trueの場合は既存のデータにビット単位でOR演算を行い, falseの場合は新しいデータで上書きする. 143 | bool mrd_meriput90_pad(PadType a_type, Meridim90Union &a_meridim, PadUnion a_pad_array, bool a_marge) { 144 | 145 | if (a_type == PC) { 146 | return false; 147 | } 148 | 149 | // ボタンデータの処理 (マージ or 上書き) 150 | if (a_marge) { 151 | a_meridim.usval[MRD_PAD_BUTTONS] |= a_pad_array.usval[0]; 152 | } else { 153 | a_meridim.usval[MRD_PAD_BUTTONS] = a_pad_array.usval[0]; 154 | } 155 | 156 | // アナログ入力データの処理 (上書きのみ) 157 | for (int i = 1; i < 4; i++) { 158 | a_meridim.usval[MRD_PAD_BUTTONS + i] = a_pad_array.usval[i]; 159 | } 160 | return true; 161 | } 162 | 163 | #endif // __MERIDIAN_JOYPAD_H__ 164 | -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/src/mrd_bt_pad.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_BT_PAD_H__ 2 | #define __MERIDIAN_BT_PAD_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "main.h" 7 | 8 | // ライブラリ導入 9 | #include // Wiiコントローラー 10 | ESP32Wiimote wiimote; 11 | 12 | // リモコン受信ボタンデータの変換テーブル 13 | constexpr unsigned short PAD_TABLE_WIIMOTE_SOLO[16] = { 14 | 0x1000, 0x0080, 0x0000, 0x0010, 0x0200, 0x0400, 0x0100, 0x0800, 15 | 0x0000, 0x0000, 0x0000, 0x0000, 0x0008, 0x0001, 0x0002, 0x0004}; 16 | constexpr unsigned short PAD_TABLE_WIIMOTE_ORIG[16] = { 17 | 0x0100, 0x0200, 0x0400, 0x0800, 0x1000, 0x0000, 0x0000, 0x0000, 18 | 0x0001, 0x0002, 0x0004, 0x0008, 0x0010, 0x0000, 0x0000, 0x0080}; 19 | 20 | //================================================================================================================ 21 | // JOYPAD読み込み処理 22 | //================================================================================================================ 23 | 24 | //---------------------------------------------------------------------- 25 | // WIIMOTEの読み込み 26 | //---------------------------------------------------------------------- 27 | 28 | /// @brief Wiiリモコンからの入力データを受信し, 処理する. 29 | /// @return 更新されたジョイパッドの状態を64ビット整数で返す. 30 | /// @note ESP32Wiimoteインスタンス wiimote, 定数PAD_GENERALIZE を関数内で使用. 31 | uint64_t mrd_bt_read_wiimote() { 32 | static uint64_t pre_val_tmp = 0; // 前回の値を保持する静的変数 33 | static int calib_l1x = 0; 34 | static int calib_l1y = 0; 35 | 36 | // 受信データの問い合わせ 37 | wiimote.task(); 38 | ButtonState rcvd_button_tmp; 39 | NunchukState nunchuk_tmp; 40 | // AccelState accel_tmp; 41 | 42 | if (wiimote.available() > 0) { 43 | 44 | // リモコンデータの取得 45 | rcvd_button_tmp = wiimote.getButtonState(); 46 | nunchuk_tmp = wiimote.getNunchukState(); 47 | 48 | uint16_t new_pad_tmp[4] = {0}; // アナログ入力のデータ組み立て用 49 | 50 | // ボタン値の変換(一般化) 51 | for (int i = 0; i < 16; i++) { 52 | uint16_t mask_tmp = 1 << i; 53 | if ((PAD_GENERALIZE && (PAD_TABLE_WIIMOTE_SOLO[i] & rcvd_button_tmp)) || 54 | (!PAD_GENERALIZE && (PAD_TABLE_WIIMOTE_ORIG[i] & rcvd_button_tmp))) { 55 | new_pad_tmp[0] |= mask_tmp; 56 | } 57 | } 58 | 59 | if (rcvd_button_tmp & BUTTON_C) { // ヌンチャクCボタンの処理 60 | if (PAD_GENERALIZE) { 61 | new_pad_tmp[0] |= 1024; 62 | } else { 63 | new_pad_tmp[0] |= 8192; 64 | } 65 | } 66 | 67 | if (rcvd_button_tmp & BUTTON_Z) { // ヌンチャクZボタンの処理 68 | if (PAD_GENERALIZE) { 69 | new_pad_tmp[0] |= 2048; 70 | } else { 71 | new_pad_tmp[0] |= 16384; 72 | } 73 | } 74 | 75 | if (rcvd_button_tmp & BUTTON_HOME) { // ホームボタンでスティックのキャリブレーション 76 | calib_l1x = nunchuk_tmp.xStick - 127; 77 | calib_l1y = nunchuk_tmp.yStick - 127; 78 | } 79 | 80 | // ヌンチャクの値を組み入れ 81 | new_pad_tmp[1] = ((nunchuk_tmp.xStick - calib_l1x - 127) * 256 // 82 | + (nunchuk_tmp.yStick - calib_l1y - 127)); 83 | 84 | // データの組み立て 85 | uint64_t new_val_tmp = 0; // 戻り値格納用 86 | new_val_tmp = static_cast(new_pad_tmp[0]); 87 | new_val_tmp |= ((uint64_t)new_pad_tmp[1] << 16); 88 | // new_val_tmp |= ((uint64_t)new_analog_tmp[2]) << 32; 89 | // new_val_tmp |= ((uint64_t)new_analog_tmp[3]) << 40; 90 | 91 | pre_val_tmp = new_val_tmp; 92 | return new_val_tmp; 93 | } 94 | return pre_val_tmp; 95 | } 96 | 97 | 98 | //================================================================================================================ 99 | // 初期化と準備 100 | //================================================================================================================ 101 | 102 | //---------------------------------------------------------------------- 103 | // Bluetooth, WIIMOTEの初期化 104 | //---------------------------------------------------------------------- 105 | 106 | /// @brief Bluetoothの設定を行い, Wiiコントローラの接続を開始する. 107 | bool mrd_bt_settings(int a_mount_pad, int a_timeout, ESP32Wiimote &a_wiimote, int a_led, 108 | HardwareSerial &a_serial) { 109 | // Wiiコントローラの接続開始 110 | if (a_mount_pad == 5) { 111 | a_serial.println("Try to connect Wiimote..."); 112 | a_wiimote.init(); 113 | a_wiimote.addFilter(ACTION_IGNORE, FILTER_ACCEL); 114 | 115 | uint16_t count_tmp = 0; 116 | unsigned long start_time = millis(); 117 | while (!a_wiimote.available()) { 118 | 119 | // リモコンへの問い合わせ 120 | a_wiimote.task(); 121 | 122 | // タイムアウトチェック 123 | if (millis() - start_time >= a_timeout) { 124 | digitalWrite(a_led, LOW); 125 | a_serial.println("Wiimote connection timed out."); 126 | return false; 127 | } 128 | 129 | // LEDの点滅 130 | count_tmp++; 131 | if (count_tmp < 500) { 132 | digitalWrite(a_led, HIGH); 133 | } else { 134 | digitalWrite(a_led, LOW); 135 | } 136 | if (count_tmp > 1000) { 137 | a_serial.print("."); 138 | count_tmp = 0; 139 | } 140 | 141 | delay(1); // 1ms秒待機して再チェック 142 | } 143 | digitalWrite(a_led, HIGH); 144 | a_serial.println("Wiimote successfully connected. "); 145 | return true; 146 | } 147 | digitalWrite(a_led, LOW); 148 | return false; 149 | } 150 | 151 | 152 | //---------------------------------------------------------------------- 153 | // WIIMOTE用スレッド 154 | //---------------------------------------------------------------------- 155 | 156 | /// @brief サブCPU (Core0) で実行されるBluetooth通信用のルーチン. 157 | /// @param args この関数に渡される引数. 現在は不使用. 158 | /// @note PadUnion型の pad_array.ui64val, 定数PAD_INTERVAL, WIIMOTE を関数内で使用. 159 | void Core0_BT_r(void *args) { // サブCPU(Core0)で実行するプログラム 160 | while (true) { // Bluetooth待受用の無限ループ 161 | pad_array.ui64val = mrd_bt_read_wiimote(); 162 | vTaskDelay(PAD_INTERVAL); // 他のタスクにCPU時間を譲る 163 | } 164 | } 165 | 166 | 167 | //------------------------------------------------------------------------------------ 168 | // meridimへのデータ書き込み 169 | //------------------------------------------------------------------------------------ 170 | 171 | /// @brief meridim配列にPADデータを書き込む. 172 | /// @param a_meridim Meridim配列の共用体. 参照渡し. 173 | /// @param a_pad_array PAD受信値の格納用配列. 174 | /// @param a_marge PADボタンデータをマージするかどうかのブール値. 175 | /// trueの場合は既存のデータにビット単位でOR演算を行い, falseの場合は新しいデータで上書きする. 176 | bool mrd_meriput90_pad(Meridim90Union &a_meridim, PadUnion a_pad_array, bool a_marge) { 177 | 178 | // ボタンデータの処理 (マージ or 上書き) 179 | if (a_marge) { 180 | a_meridim.usval[MRD_PAD_BUTTONS] |= a_pad_array.usval[0]; 181 | } else { 182 | a_meridim.usval[MRD_PAD_BUTTONS] = a_pad_array.usval[0]; 183 | } 184 | 185 | // アナログ入力データの処理 (上書きのみ) 186 | for (int i = 1; i < 4; i++) { 187 | a_meridim.usval[MRD_PAD_BUTTONS + i] = a_pad_array.usval[i]; 188 | } 189 | return true; 190 | } 191 | 192 | #endif // __MERIDIAN_BT_PAD_H__ 193 | -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/src/main.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_MAIN_H__ 2 | #define __MERIDIAN_MAIN_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | 7 | // ライブラリ導入 8 | #include // Meridianのライブラリ導入 9 | MERIDIANFLOW::Meridian mrd; 10 | 11 | //------------------------------------------------------------------------------------ 12 | // 列挙型 13 | //------------------------------------------------------------------------------------ 14 | 15 | enum UartLine { // サーボ系統の列挙型(L,R,C) 16 | L, // Left 17 | R, // Right 18 | C // Center 19 | }; 20 | 21 | enum ImuAhrsType { // 6軸9軸センサ種の列挙型(NO_IMU, MPU6050_IMU, MPU9250_IMU, BNO055_AHRS) 22 | NO_IMU = 0, // IMU/AHRS なし. 23 | MPU6050_IMU = 1, // MPU6050 24 | MPU9250_IMU = 2, // MPU9250(未設定) 25 | BNO055_AHRS = 3 // BNO055 26 | }; 27 | 28 | enum PadType { // リモコン種の列挙型(NONE, PC, MERIMOTE, BLUERETRO, SBDBT, KRR5FH) 29 | NONE = 0, // リモコンなし 30 | PC = 0, // PCからのPD入力情報を使用 31 | MERIMOTE = 1, // MERIMOTE(未導入) 32 | BLUERETRO = 2, // BLUERETRO(未導入) 33 | SBDBT = 3, // SBDBT(未導入) 34 | KRR5FH = 4, // KRR5FH 35 | WIIMOTE = 5, // WIIMOTE / WIIMOTE + Nunchuk 36 | WIIMOTE_C = 6, // WIIMOTE+Classic 37 | }; 38 | 39 | enum BinHexDec { // 数値表示タイプの列挙型(Bin, Hex, Dec) 40 | Bin = 0, // BIN 41 | Hex = 1, // HEX 42 | Dec = 2, // DEC 43 | }; 44 | 45 | //------------------------------------------------------------------------------------ 46 | // 変数 47 | //------------------------------------------------------------------------------------ 48 | 49 | // システム用の変数 50 | const int MRDM_BYTE = MRDM_LEN * 2; // Meridim配列のバイト型の長さ 51 | const int MRD_ERR = MRDM_LEN - 2; // エラーフラグの格納場所 (配列の末尾から2つめ) 52 | const int MRD_ERR_u = MRD_ERR * 2 + 1; // エラーフラグの格納場所 (上位8ビット) 53 | const int MRD_ERR_l = MRD_ERR * 2; // エラーフラグの格納場所 (下位8ビット) 54 | const int MRD_CKSM = MRDM_LEN - 1; // チェックサムの格納場所 (配列の末尾) 55 | const int PAD_LEN = 5; // リモコン用配列の長さ 56 | TaskHandle_t thp[4]; // マルチスレッドのタスクハンドル格納用 57 | 58 | //------------------------------------------------------------------------------------ 59 | // クラス・構造体・共用体 60 | //------------------------------------------------------------------------------------ 61 | 62 | // Meridim配列用の共用体の設定 63 | typedef union { 64 | short sval[MRDM_LEN + 4]; // short型で90個の配列データを持つ 65 | unsigned short usval[MRDM_LEN + 2]; // 上記のunsigned short型 66 | uint8_t bval[+4]; // byte型で180個の配列データを持つ 67 | uint8_t ubval[MRDM_BYTE + 4]; // 上記のunsigned byte型 68 | } Meridim90Union; 69 | Meridim90Union s_udp_meridim; // Meridim配列データ送信用(short型, センサや角度は100倍値) 70 | Meridim90Union r_udp_meridim; // Meridim配列データ受信用 71 | Meridim90Union s_spi_meridim; // Meridim配列データ送信用 72 | Meridim90Union r_spi_meridim; // Meridim配列データ受信用 73 | Meridim90Union s_udp_meridim_dummy; // SPI送信ダミー用 74 | Meridim90Union tmp_meridim; // チェック用配列 75 | uint8_t *s_spi_meridim_dma; // DMA用 76 | uint8_t *r_spi_meridim_dma; // DMA用 77 | 78 | // フラグ用変数 79 | struct MrdFlags { 80 | bool imuahrs_available = true; // メインセンサ値を読み取る間, サブスレッドによる書き込みを待機 81 | bool udp_board_passive = false; // UDP通信の周期制御がボード主導(false) か, PC主導(true)か. 82 | bool frame_timer_reset = false; // フレーム管理時計をリセットする 83 | bool stop_board_during = false; // ボードの末端処理をmeridim[2]秒, meridim[3]ミリ秒だけ止める. 84 | bool eeprom_write_mode = false; // EEPROMへの書き込みモード. 85 | bool eeprom_read_mode = false; // EEPROMからの読み込みモード. 86 | bool eeprom_protect = EEPROM_PROTECT; // EEPROMの書き込みプロテクト. 87 | bool eeprom_load = EEPROM_LOAD; // 起動時にEEPROMの内容を読み込む 88 | bool eeprom_set = EEPROM_SET; // 起動時にEEPROMに規定値をセット 89 | bool sdcard_write_mode = false; // SDCARDへの書き込みモード. 90 | bool sdcard_read_mode = false; // SDCARDからの読み込みモード. 91 | bool wire0_init = false; // I2C 0系統の初期化合否 92 | bool wire1_init = false; // I2C 1系統の初期化合否 93 | bool udp_rcvd = false; // UDPが受信できたか. 94 | bool udp_busy = false; // UDPスレッドでの受信中フラグ (送信抑制) 95 | bool udp_receive_mode = MODE_UDP_RECEIVE; // PCからのデータ受信実施 (0:OFF, 1:ON, 通常は1) 96 | bool udp_send_mode = MODE_UDP_SEND; // PCへのデータ送信実施 (0:OFF, 1:ON, 通常は1) 97 | bool spi_rcvd = true; // SPIのデータ受信判定 98 | bool bt_busy = false; // Bluetoothの受信中フラグ (UDPコンフリクト回避用) 99 | bool meridim_rcvd = false; // Meridimが正しく受信できたか. 100 | bool test_value = false; // テスト用の仮設変数. 意味を持たない. 101 | bool fixed_ip = MODE_FIXED_IP; // 固定IPモード (0:OFF, 1:ON). 102 | }; 103 | MrdFlags flg; 104 | 105 | // シーケンス番号理用の変数 106 | struct MrdSq { 107 | uint16_t s_increment = 0; // フレーム毎に0-59999をカウントし, 送信 108 | uint16_t r_expect = 0; // フレーム毎に0-59999をカウントし, 受信値と比較 109 | uint16_t r_past = 0; // 前回のシーケンス番号をキープ 110 | }; 111 | MrdSq mrdsq; 112 | 113 | // タイマー管理用の変数 114 | struct MrdTimer { 115 | long frame_ms = FRAME_DURATION; // 1フレームあたりの単位時間(ms) 116 | int loop_count = 0; // サイン計算用の循環カウンタ 117 | int loop_count_dlt = 2; // サイン計算用の循環カウンタを1フレームにいくつ進めるか 118 | int loop_count_max = 359999; // 循環カウンタの最大値 119 | }; 120 | MrdTimer tmr; 121 | 122 | // エラーカウント用 123 | struct MrdErr { 124 | int esp_pc = 0; // PCの受信エラー (ESP32からのUDP) 125 | int pc_esp = 0; // ESP32の受信エラー (PCからのUDP) 126 | int esp_tsy = 0; // Teensyの受信エラー (ESP32からのSPI) 127 | int tsy_esp = 0; // ESP32の受信エラー (TeensyからのSPI) 128 | int esp_skip = 0; // UDP→ESP受信のカウントの連番スキップ回数 129 | int tsy_skip = 0; // ESP→Teensy受信のカウントの連番スキップ回数 130 | int pc_skip = 0; // PC受信のカウントの連番スキップ回数 131 | }; 132 | MrdErr err; 133 | 134 | typedef union // リモコン値格納用 135 | { 136 | short sval[PAD_LEN]; // short型で4個の配列データを持つ 137 | uint16_t usval[PAD_LEN]; // 上記のunsigned short型 138 | int8_t bval[PAD_LEN * 2]; // 上記のbyte型 139 | uint8_t ubval[PAD_LEN * 2]; // 上記のunsigned byte型 140 | uint64_t ui64val; // 上記のunsigned int16型 141 | // [0]button, [1]pad.stick_L_x:pad.stick_L_y, 142 | // [2]pad.stick_R_x:pad.stick_R_y, [3]pad.L2_val:pad.R2_val 143 | } PadUnion; 144 | PadUnion pad_array = {0}; // pad値の格納用配列 145 | PadUnion pad_i2c = {0}; // pad値のi2c送受信用配列 146 | 147 | struct PadValue // リモコンのアナログ入力データ 148 | { 149 | unsigned short btn = 0; 150 | unsigned short stick_R = 0; 151 | int stick_R_x = 0; 152 | int stick_R_y = 0; 153 | unsigned short stick_L = 0; 154 | int stick_L_x = 0; 155 | int stick_L_y = 0; 156 | unsigned short stick_L2R2V = 0; 157 | int R2_val = 0; 158 | int L2_val = 0; 159 | }; 160 | PadValue pad_analog; 161 | 162 | // モニタリング設定 163 | struct MrdMonitor { 164 | bool flow = MONITOR_FLOW; // フローを表示 165 | bool all_err = MONITOR_ERR_ALL; // 全経路の受信エラー率を表示 166 | bool seq_num = MONITOR_SEQ; // シーケンス番号チェックを表示 167 | bool pad = MONITOR_PAD; // リモコンのデータを表示 168 | }; 169 | MrdMonitor monitor; 170 | 171 | //================================================================================================================ 172 | // 関数各種 173 | //================================================================================================================ 174 | 175 | // 予約用 176 | bool execute_master_command_from_PC(Meridim90Union a_meridim, bool a_flg_exe); 177 | bool execute_master_command_from_Tsy(Meridim90Union a_meridim, bool a_flg_exe); 178 | 179 | #include "mrd_disp.h" 180 | MrdMsgHandler mrd_disp(Serial); 181 | 182 | #endif //__MERIDIAN_MAIN_H__ -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_wire0.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_WIRE0_H__ 2 | #define __MERIDIAN_WIRE0_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "main.h" 7 | 8 | IntervalTimer wireTimer0; 9 | 10 | //================================================================================================================ 11 | // I2C wire0 関連の処理 12 | //================================================================================================================ 13 | 14 | //------------------------------------------------------------------------------------ 15 | // センサデータの取得処理 16 | //------------------------------------------------------------------------------------ 17 | 18 | /// @brief AHRSセンサーからI2C経由でデータを読み取る関数. 19 | /// MPU6050, MPU9250, BNO055に対応予定だが, MPU9250とBNO055は現在実装されていない. 20 | /// 各データは`ahrs.read`配列に格納され, 利用可能な場合は`ahrs.result`にコピーされる. 21 | void mrd_wire0_read_ahrs_i2c() // ※wireTimer0.beginの引数のためvoid必須 22 | { 23 | if (MOUNT_IMUAHRS == MPU6050_IMU) // MPU6050 24 | { 25 | if (ahrs.mpu6050.dmpGetCurrentFIFOPacket(ahrs.fifoBuffer)) { // Get new data 26 | ahrs.mpu6050.dmpGetQuaternion(&ahrs.q, ahrs.fifoBuffer); 27 | ahrs.mpu6050.dmpGetGravity(&ahrs.gravity, &ahrs.q); 28 | ahrs.mpu6050.dmpGetYawPitchRoll(ahrs.ypr, &ahrs.q, &ahrs.gravity); 29 | 30 | // acceleration values 31 | ahrs.mpu6050.dmpGetAccel(&ahrs.aa, ahrs.fifoBuffer); 32 | ahrs.read[0] = (float)ahrs.aa.x; 33 | ahrs.read[1] = (float)ahrs.aa.y; 34 | ahrs.read[2] = (float)ahrs.aa.z; 35 | 36 | // gyro values 37 | ahrs.mpu6050.dmpGetGyro(&ahrs.gyro, ahrs.fifoBuffer); 38 | ahrs.read[3] = (float)ahrs.gyro.x; 39 | ahrs.read[4] = (float)ahrs.gyro.y; 40 | ahrs.read[5] = (float)ahrs.gyro.z; 41 | 42 | // magnetic field values 43 | ahrs.read[6] = (float)ahrs.mag.x; 44 | ahrs.read[7] = (float)ahrs.mag.y; 45 | ahrs.read[8] = (float)ahrs.mag.z; 46 | 47 | // Estimated gravity DMP value. 48 | ahrs.read[9] = ahrs.gravity.x; 49 | ahrs.read[10] = ahrs.gravity.y; 50 | ahrs.read[11] = ahrs.gravity.z; 51 | 52 | // Estimated heading value using DMP. 53 | ahrs.read[12] = ahrs.ypr[2] * 180 / M_PI; // Estimated DMP_ROLL 54 | ahrs.read[13] = ahrs.ypr[1] * 180 / M_PI; // Estimated DMP_PITCH 55 | ahrs.read[14] = (ahrs.ypr[0] * 180 / M_PI) - ahrs.yaw_origin; // Estimated DMP_YAW 56 | 57 | // Temperature 58 | ahrs.read[15] = 0; // Not implemented. 59 | 60 | if (flg.imuahrs_available) { 61 | memcpy(ahrs.result, ahrs.read, sizeof(float) * 16); 62 | } 63 | } 64 | } else if (MOUNT_IMUAHRS == MPU9250_IMU) // MPU9250 65 | { 66 | ; // 67 | } else if (MOUNT_IMUAHRS == BNO055_AHRS) // BNO055 68 | { 69 | ; // 70 | } 71 | } 72 | 73 | //------------------------------------------------------------------------------------ 74 | // 初期設定 75 | //------------------------------------------------------------------------------------ 76 | 77 | /// @brief Wire0 I2C通信を初期化し, 指定されたクロック速度で設定する. 78 | /// @param a_i2c_bps I2C通信のクロック速度. 79 | void mrd_wire0_init_common(int a_i2c_bps) { 80 | Serial.print("Initializing wire0 I2C..."); 81 | Wire.begin(); 82 | Wire.setClock(a_i2c_bps); 83 | } 84 | 85 | /// @brief MPU6050センサーのDMP（デジタルモーションプロセッサ）を初期化し, 86 | /// ジャイロスコープと加速度センサーのオフセットを設定する. 87 | /// @param a_ahrs AHRSの値を保持する構造体. 88 | /// @return DMPの初期化が成功した場合はtrue, 失敗した場合はfalseを返す. 89 | bool mrd_wire0_init_mpu6050_dmp(AhrsValue &a_ahrs) { 90 | a_ahrs.mpu6050.initialize(); 91 | a_ahrs.devStatus = a_ahrs.mpu6050.dmpInitialize(); 92 | 93 | // supply your own gyro offsets here, scaled for min sensitivity 94 | a_ahrs.mpu6050.setXAccelOffset(-1745); 95 | a_ahrs.mpu6050.setYAccelOffset(-1034); 96 | a_ahrs.mpu6050.setZAccelOffset(966); 97 | a_ahrs.mpu6050.setXGyroOffset(176); 98 | a_ahrs.mpu6050.setYGyroOffset(-6); 99 | a_ahrs.mpu6050.setZGyroOffset(-25); 100 | 101 | // make sure it worked (returns 0 if so) 102 | if (a_ahrs.devStatus == 0) { 103 | a_ahrs.mpu6050.CalibrateAccel(6); 104 | a_ahrs.mpu6050.CalibrateGyro(6); 105 | a_ahrs.mpu6050.setDMPEnabled(true); 106 | a_ahrs.packetSize = a_ahrs.mpu6050.dmpGetFIFOPacketSize(); 107 | Serial.println("MPU6050 OK."); 108 | return true; 109 | } 110 | Serial.println("IMU/AHRS DMP Initialization FAILED!"); 111 | return false; 112 | } 113 | 114 | /// @brief BNO055センサーの初期化を試みる. 115 | /// @param a_ahrs AHRSの値を保持する構造体. 116 | /// @return BNO055センサーの初期化が成功した場合はtrue, それ以外の場合はfalseを返す. 117 | /// 現在, この関数は常にfalseを返すように設定されている. 118 | bool mrd_wire0_init_bno055(AhrsValue &a_ahrs) { 119 | /* 120 | else if (mount_ahrs == 3) // BNO055の場合 121 | { 122 | // BNO055の初期化 123 | if (!bno.begin()) 124 | { 125 | Serial.println("No BNO055 detected ... Check your wiring or I2C ADDR!"); 126 | } 127 | else 128 | { 129 | Serial.println("BNO055 mounted."); 130 | delay(50); 131 | bno.setExtCrystalUse(false); 132 | delay(10); 133 | // _ahrsTimer.begin(IMU_ahrs_getYawPitchRoll, int(IMU_ahrs_POLLING * 1000)); // 134 | インターバルはマイクロ秒指定 135 | } 136 | // データの取得はセンサー用スレッドで実行? 137 | } 138 | */ 139 | return false; 140 | } 141 | 142 | /// @brief 指定されたIMU/AHRSタイプに応じて適切なセンサの初期化を行う. 143 | /// @param a_imuahrs_type 使用するセンサのタイプを示す列挙型（MPU6050, MPU9250, BNO055）. 144 | /// @param a_i2c_bps I2C通信のクロック速. 145 | /// @param a_ahrs AHRSの値を保持する構造体. 146 | /// @return センサが正しく初期化された場合はtrueを, そうでない場合はfalseを返す. 147 | bool mrd_wire0_setup(ImuAhrsType a_imuahrs_type, int a_i2c_bps, AhrsValue &a_ahrs) { 148 | if (a_imuahrs_type > 0) // 何らかのセンサを搭載 149 | { 150 | mrd_wire0_init_common(a_i2c_bps); 151 | } 152 | 153 | if (a_imuahrs_type == MPU6050_IMU) // MPU6050 154 | { 155 | return mrd_wire0_init_mpu6050_dmp(a_ahrs); 156 | } else if (a_imuahrs_type == MPU9250_IMU) // MPU9250の場合 157 | { 158 | // mrd_wire_init_mpu9250_dmp(a_ahrs) 159 | return false; 160 | } else if (a_imuahrs_type == BNO055_AHRS) // BNO055の場合 161 | { 162 | // mrd_wire0_init_bno055(a_ahrs) 163 | return false; 164 | } 165 | // Serial.println("No IMU/AHRS sensor mounted."); 166 | return false; 167 | } 168 | 169 | //------------------------------------------------------------------------------------ 170 | // IntervalTimerのスタート 171 | //------------------------------------------------------------------------------------ 172 | 173 | /// @brief AHRSセンサーからデータを読み取るための関数を実行する. 174 | void mrd_wire0_run() // ※IntervalTimer用の関数のためvoidのみ可 175 | { 176 | mrd_wire0_read_ahrs_i2c(); 177 | } 178 | 179 | /// @brief インターバルタイマーを設定し, 180 | /// 定期的にAHRSセンサーからのデータ読み取りを行う関数を実行する. 181 | /// @param a_wr_check タイマーを開始するかどうかの条件を示すブール値. 182 | /// @return タイマーが正しく開始された場合はtrue, それ以外の場合はfalseを返す. 183 | bool mrd_wire0_intervaltimer_start(bool a_wr_check) { 184 | if (a_wr_check) { 185 | bool rlst = 186 | wireTimer0.begin(mrd_wire0_run, IMUAHRS_INTERVAL * 1000); // インターバルはマイクロ秒指定 187 | // wireTimer.priority(90); 188 | return rlst; 189 | } 190 | return false; 191 | } 192 | 193 | //------------------------------------------------------------------------------------ 194 | // meridimへの書き込み処理 195 | //------------------------------------------------------------------------------------ 196 | 197 | /// @brief 指定されたIMU/AHRSタイプに基づいて, 計測したAHRSデータを読み込む. 198 | /// @param a_type 使用するセンサのタイプを示す列挙（MPU6050, MPU9250, BNO055）. 199 | /// @param a_ahrs_result AHRSから読み取った結果を格納した配列. 200 | /// @return データの書き込みが成功した場合はtrue, それ以外の場合はfalseを返す. 201 | bool mrd_meriput90_ahrs(Meridim90Union &a_meridim, float a_ahrs_result[]) { 202 | // if (a_type == MPU6050_IMU) { 203 | flg.imuahrs_available = false; 204 | a_meridim.sval[2] = mrd.float2HfShort(a_ahrs_result[0]); // IMU/AHRS_acc_x 205 | a_meridim.sval[3] = mrd.float2HfShort(a_ahrs_result[1]); // IMU/AHRS_acc_y 206 | a_meridim.sval[4] = mrd.float2HfShort(a_ahrs_result[2]); // IMU/AHRS_acc_z 207 | a_meridim.sval[5] = mrd.float2HfShort(a_ahrs_result[3]); // IMU/AHRS_gyro_x 208 | a_meridim.sval[6] = mrd.float2HfShort(a_ahrs_result[4]); // IMU/AHRS_gyro_y 209 | a_meridim.sval[7] = mrd.float2HfShort(a_ahrs_result[5]); // IMU/AHRS_gyro_z 210 | a_meridim.sval[8] = mrd.float2HfShort(a_ahrs_result[6]); // IMU/AHRS_mag_x 211 | a_meridim.sval[9] = mrd.float2HfShort(a_ahrs_result[7]); // IMU/AHRS_mag_y 212 | a_meridim.sval[10] = mrd.float2HfShort(a_ahrs_result[8]); // IMU/AHRS_mag_z 213 | a_meridim.sval[11] = mrd.float2HfShort(a_ahrs_result[15]); // temperature 214 | a_meridim.sval[12] = mrd.float2HfShort(a_ahrs_result[12]); // DMP_ROLL推定値 215 | a_meridim.sval[13] = mrd.float2HfShort(a_ahrs_result[13]); // DMP_PITCH推定値 216 | a_meridim.sval[14] = mrd.float2HfShort(a_ahrs_result[14]); // DMP_YAW推定値 217 | flg.imuahrs_available = true; 218 | return true; 219 | } 220 | 221 | //------------------------------------------------------------------------------------ 222 | // 各種オペレーション 223 | //------------------------------------------------------------------------------------ 224 | 225 | /// @brief 現在のヨー角を基準値として設定し, 適切なセンサの単位に変換する. 226 | /// @param yaw 現在のヨー角をラジアン単位で指定する. 227 | /// @return 変換されたヨー角を度単位で返す. 対応していないセンサの場合は0を返す. 228 | float mrd_wire0_setyaw(float yaw) { 229 | if (MOUNT_IMUAHRS == MPU6050_IMU) // MPU6050 230 | { 231 | return yaw * 180 / M_PI; 232 | } else if (MOUNT_IMUAHRS == MPU9250_IMU) // MPU9250 233 | { 234 | return 0; 235 | } else if (MOUNT_IMUAHRS == BNO055_AHRS) // BNO055 236 | return 0; 237 | { return 0; } 238 | } 239 | 240 | #endif // __MERIDIAN_WIRE0_H__ 241 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_disp.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_DISPLAY_MESSAGE_H__ 2 | #define __MERIDIAN_DISPLAY_MESSAGE_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "main.h" 7 | #include "mrd_util.h" 8 | 9 | //================================================================================================================ 10 | // シリアルモニタリング出力関連の処理 11 | //================================================================================================================ 12 | 13 | class MrdMsgHandler { 14 | private: 15 | Stream &m_serial; // シリアルオブジェクトの参照を保持 16 | 17 | public: 18 | // コンストラクタでStreamオブジェクトを受け取り, メンバーに保存 19 | MrdMsgHandler(Stream &a_serial) : m_serial(a_serial) {} 20 | 21 | //------------------------------------------------------------------------------------ 22 | // 起動時メッセージ 23 | //------------------------------------------------------------------------------------ 24 | 25 | /// @brief 指定された秒数だけキャパシタの充電プロセスを示すメッセージを表示する. 26 | void charging(int duration_ms) { 27 | m_serial.print("Charging the capacitor."); 28 | for (int i = 0; i < duration_ms; i++) { 29 | if (i % 100 == 0) { 30 | m_serial.print("."); 31 | } 32 | delay(1); 33 | } 34 | m_serial.println(); 35 | } 36 | 37 | /// @brief システムのバージョン情報と通信速度の設定をシリアルモニタに出力する. 38 | /// @param a_version システムのバージョンを示す文字列. 39 | /// @param a_pc PCとの有線シリアル接続速度(bps). 40 | /// @param a_spi0 SPI0の通信速度(Hz). 41 | /// @param a_i2c0 I2C0の通信速度(Hz). 42 | /// @param a_i2c1 I2C1の通信速度(Hz). 43 | void hello_twin_tsy(String a_version, int a_pc, int a_spi0, int a_i2c0, int a_i2c1) { 44 | m_serial.println(); 45 | m_serial.print("Hi, This is "); 46 | m_serial.println(a_version); 47 | m_serial.print("Set PC-USB speed: "); 48 | m_serial.println(a_pc); 49 | m_serial.print("Set SPI0 speed : "); 50 | m_serial.println(a_spi0); 51 | m_serial.print("Set i2c0 speed : "); 52 | m_serial.println(a_i2c0); 53 | m_serial.print("Set i2c1 speed : "); 54 | m_serial.println(a_i2c1); 55 | } 56 | 57 | /// @brief マウント設定したジョイパッドのタイプをシリアルモニタに出力する. 58 | /// @param a_mount_pad パッドの定義(PC,MERIMOTE,BLUERETRO,SBDBT,KRR5FH,WIIMOTE) 59 | void mounted_pad(int a_mount_pad) { 60 | m_serial.print("Pad Receiver mounted : "); 61 | if (a_mount_pad == MERIMOTE) { 62 | m_serial.println("Merimote."); 63 | } else if (a_mount_pad == BLUERETRO) { 64 | m_serial.println("BlueRetro."); 65 | } else if (a_mount_pad == SBDBT) { 66 | m_serial.println("SBDBT."); 67 | } else if (a_mount_pad == KRR5FH) { 68 | m_serial.println("KRC-5FH."); 69 | } else { 70 | m_serial.println("None (PC)."); 71 | } 72 | } 73 | 74 | /// @brief マウント設定したサーボのbpsをシリアルモニタに出力する. 75 | /// @param a_servo_l L系統のサーボbps. 76 | /// @param a_servo_r R系統のサーボbps. 77 | /// @param a_servo_c C系統のサーボbps. 78 | void servo_bps_3lines(int a_servo_l, int a_servo_r, int a_servo_c) { 79 | m_serial.print("Set UART_L "); 80 | m_serial.print(a_servo_l); 81 | m_serial.println(" bps"); 82 | m_serial.print("Set UART_R "); 83 | m_serial.print(a_servo_r); 84 | m_serial.println(" bps"); 85 | m_serial.print("Set UART_C "); 86 | m_serial.print(a_servo_c); 87 | m_serial.println(" bps"); 88 | } 89 | 90 | /// @brief 指定されたUARTラインのサーボIDを表示する. 91 | /// @param a_label UARTラインのラベル. 92 | /// @param a_max サーボの最大数. 93 | /// @param a_mount サーボのマウント状態を示す配列. 94 | /// @param a_id サーボIDの配列. 95 | void print_servo_ids(const char *a_label, int a_max, int *a_mount, const int *a_id) { 96 | m_serial.print(a_label); 97 | for (int i = 0; i <= a_max; i++) { 98 | if (a_mount[i] != 0) { 99 | if (a_id[i] < 10) { 100 | m_serial.print(" "); 101 | } 102 | m_serial.print(a_id[i]); 103 | } else { 104 | m_serial.print("__"); 105 | } 106 | m_serial.print(" "); 107 | } 108 | m_serial.println(); 109 | } 110 | 111 | /// @brief マウントされているサーボのIDを表示する. 112 | /// @param a_sv サーボパラメータの構造体. 113 | void servo_mounts_3lines(ServoParam a_sv) { 114 | print_servo_ids("UART_L Servos mounted: ", a_sv.num_max, a_sv.ixl_mount, a_sv.ixl_id); 115 | print_servo_ids("UART_R Servos mounted: ", a_sv.num_max, a_sv.ixr_mount, a_sv.ixr_id); 116 | print_servo_ids("UART_C Servos mounted: ", a_sv.num_max, a_sv.ixc_mount, a_sv.ixc_id); 117 | } 118 | 119 | /// @brief 指定されたUARTラインとサーボタイプに基づいてサーボの通信プロトコルを表示する. 120 | /// @param a_line UART通信ライン（L, R, C）. 121 | /// @param a_servo_type サーボのタイプを示す整数値. 122 | void servo_protcol(UartLine a_line, int a_servo_type) { 123 | if (a_servo_type > 0) { 124 | m_serial.print("Set UART_"); 125 | m_serial.print(mrd_get_line_name(a_line)); 126 | m_serial.print(" protocol : "); 127 | 128 | switch (a_servo_type) { 129 | case 1: 130 | m_serial.print("single PWM"); 131 | m_serial.println(" - Not supported yet."); 132 | break; 133 | case 11: 134 | m_serial.print("I2C_PCA9685 to PWM"); 135 | m_serial.println(" - Not supported yet."); 136 | break; 137 | case 21: 138 | m_serial.print("RSxTTL (FUTABA)"); 139 | m_serial.println(" - Not supported yet."); 140 | break; 141 | case 31: 142 | m_serial.print("DYNAMIXEL Protocol 1.0"); 143 | m_serial.println(" - Not supported yet."); 144 | break; 145 | case 32: 146 | m_serial.print("DYNAMIXEL Protocol 2.0"); 147 | m_serial.println(" - Not supported yet."); 148 | break; 149 | case 43: 150 | m_serial.println("ICS3.5/3.6(KONDO,KRS)"); 151 | break; 152 | case 44: 153 | m_serial.print("PMX(KONDO)"); 154 | m_serial.println(" - Not supported yet."); 155 | break; 156 | case 51: 157 | m_serial.print("XBUS(JR PROPO)"); 158 | m_serial.println(" - Not supported yet."); 159 | break; 160 | case 61: 161 | m_serial.print("STS(FEETECH)"); 162 | m_serial.println(" - Not supported yet."); 163 | break; 164 | case 62: 165 | m_serial.print("SCS(FEETECH)"); 166 | m_serial.println(" - Not supported yet."); 167 | break; 168 | default: 169 | m_serial.println(" Not defined. "); 170 | break; 171 | } 172 | } 173 | } 174 | 175 | /// @brief システム内の各経路の通信エラーとスキップ数をモニタリングし, シリアルポートに出力する. 176 | /// @param a_meridim Meridim配列の共用体. 177 | /// @param a_err エラーカウント管理用の構造体 178 | /// @param a_mrdsq シーケンス番号管理用の構造体. 179 | void err_monitor(const Meridim90Union &a_meridim, const MrdErr &a_err, const MrdSq &a_mrdsq) { 180 | m_serial.print("[ERRs] esp>pc:"); 181 | m_serial.print(a_err.esp_pc); 182 | m_serial.print(" pc>esp:"); 183 | m_serial.print(a_err.pc_esp); 184 | m_serial.print(" esp>tsy:"); 185 | m_serial.print(a_err.esp_tsy); 186 | m_serial.print(" tsy>esp:"); 187 | m_serial.print(a_err.esp_tsy); 188 | m_serial.print(" tsySkip:"); 189 | m_serial.print(a_err.tsy_skip); 190 | m_serial.print(" espSkip:"); 191 | m_serial.print(a_err.esp_skip); 192 | m_serial.print(" pcSkip:"); 193 | m_serial.print(a_err.pc_skip); 194 | m_serial.print(" seq:"); 195 | m_serial.print(int(a_mrdsq.r_expect)); 196 | m_serial.print(" [u]:"); 197 | m_serial.print(a_meridim.bval[MRD_ERR_u], BIN); 198 | m_serial.println(); 199 | } 200 | 201 | /// @brief システムに接続されているIMU/AHRSセンサーのタイプを表示する. 202 | /// @param a_type 6軸9軸センサ種の列挙型(NO_IMU, MPU6050_IMU, MPU9250_IMU, BNO055_AHRS) 203 | void imuahrs(ImuAhrsType a_type) { 204 | m_serial.print("IMU/AHRS Sensor mounted: "); 205 | switch (a_type) { 206 | case NO_IMU: 207 | m_serial.println("None."); 208 | break; 209 | case MPU6050_IMU: 210 | m_serial.println("MPU6050(GY-521)"); 211 | break; 212 | case MPU9250_IMU: 213 | m_serial.println("MPU9250(GY-6050/GY-9250)"); 214 | break; 215 | case BNO055_AHRS: 216 | m_serial.println("BNO055"); 217 | break; 218 | default: 219 | break; 220 | } 221 | } 222 | 223 | /// @brief システムの動作開始を示すメッセージを出力する. 224 | void flow_start_twin_tsy() { 225 | m_serial.println(); 226 | m_serial.println("-) Meridian TWIN system on side Teensy now flows. (-"); 227 | } 228 | 229 | //------------------------------------------------------------------------------------ 230 | // イベントメッセージ 231 | //------------------------------------------------------------------------------------ 232 | /// @brief 遅延時間をシリアルモニタに出力する. 233 | /// @param a_delay_time 表示する値. 234 | /// @param a_flg_disp 表示を実施するかどうか. 235 | void frame_delay(unsigned long a_delay_time, bool a_flg_disp) { 236 | if (a_flg_disp) { 237 | m_serial.print("deley: "); 238 | m_serial.print(a_delay_time); 239 | m_serial.println("ms"); 240 | } 241 | } 242 | 243 | /// @brief サーボモーターのエラーを検出した場合にエラーメッセージを表示する. 244 | /// @param a_line サーボモーターが接続されているUARTライン（L, R, C）. 245 | /// @param a_num エラーが発生しているサーボの番号. 246 | /// @param a_flg_disp エラーメッセージを表示するかどうかのブール値. 247 | /// @return エラーメッセージが表示された場合はtrueを, 表示されなかった場合はfalseを返す. 248 | bool servo_err(UartLine a_line, int a_num, bool a_flg_disp) { 249 | if (a_flg_disp) { 250 | m_serial.print("Found servo err "); 251 | if (a_line == L) { 252 | m_serial.print("L_"); 253 | m_serial.println(a_num); 254 | return true; 255 | } else if (a_line == R) { 256 | m_serial.print("R_"); 257 | m_serial.println(a_num); 258 | return true; 259 | } else if (a_line == C) { 260 | m_serial.print("C_"); 261 | m_serial.println(a_num); 262 | return true; 263 | } 264 | } 265 | return false; 266 | } 267 | }; 268 | 269 | #endif // __MERIDIAN_DISPLAY_MESSAGE_H__ -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/lib/ESP32Wiimote/src/ESP32Wiimote.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Daiki Yasuda 2 | // 3 | // This is licensed under 4 | // - Creative Commons Attribution-NonCommercial 3.0 Unported 5 | // - https://creativecommons.org/licenses/by-nc/3.0/ 6 | // - Or see LICENSE.md 7 | // 8 | // The short of it is... 9 | // You are free to: 10 | // Share — copy and redistribute the material in any medium or format 11 | // Adapt — remix, transform, and build upon the material 12 | // Under the following terms: 13 | // NonCommercial — You may not use the material for commercial purposes. 14 | 15 | //#define CONFIG_CLASSIC_BT_ENABLED 1 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include "nvs.h" 22 | #include "nvs_flash.h" 23 | #include "freertos/FreeRTOS.h" 24 | #include "freertos/task.h" 25 | #include "Arduino.h" 26 | #include "esp_bt.h" 27 | #include 28 | 29 | #include "time.h" 30 | #include "sys/time.h" 31 | 32 | #include "ESP32Wiimote.h" 33 | #include "TinyWiimote.h" 34 | 35 | #define WIIMOTE_VERBOSE 0 36 | 37 | #if WIIMOTE_VERBOSE 38 | #define VERBOSE_PRINT(...) Serial.printf(__VA_ARGS__) 39 | #define VERBOSE_PRINTLN(...) Serial.println(__VA_ARGS__) 40 | #else 41 | #define VERBOSE_PRINT(...) do {} while(0) 42 | #define VERBOSE_PRINTLN(...) do {} while(0) 43 | #endif 44 | 45 | //#define UNVERBOSE_PRINT(...) Serial.printf(__VA_ARGS__) 46 | #define UNVERBOSE_PRINT(...) do {} while(0) 47 | 48 | #define RX_QUEUE_SIZE 32 49 | #define TX_QUEUE_SIZE 32 50 | xQueueHandle ESP32Wiimote::rxQueue = NULL; 51 | xQueueHandle ESP32Wiimote::txQueue = NULL; 52 | 53 | const TwHciInterface ESP32Wiimote::tinywii_hci_interface = { 54 | ESP32Wiimote::hciHostSendPacket 55 | }; 56 | 57 | esp_vhci_host_callback_t ESP32Wiimote::vhci_callback; 58 | 59 | ESP32Wiimote::ESP32Wiimote(int NUNCHUK_STICK_THRESHOLD) 60 | { 61 | _nunStickThreshold = NUNCHUK_STICK_THRESHOLD; 62 | _filter = FILTER_NONE; 63 | } 64 | 65 | void ESP32Wiimote::notifyHostSendAvailable(void) { 66 | VERBOSE_PRINT("notifyHostSendAvailable\n"); 67 | if(!TinyWiimoteDeviceIsInited()){ 68 | TinyWiimoteResetDevice(); 69 | } 70 | } 71 | 72 | void ESP32Wiimote::createQueue(void) { 73 | txQueue = xQueueCreate(TX_QUEUE_SIZE, sizeof(queuedata_t*)); 74 | if (txQueue == NULL){ 75 | VERBOSE_PRINTLN("xQueueCreate(txQueue) failed"); 76 | return; 77 | } 78 | rxQueue = xQueueCreate(RX_QUEUE_SIZE, sizeof(queuedata_t*)); 79 | if (rxQueue == NULL){ 80 | VERBOSE_PRINTLN("xQueueCreate(rxQueue) failed"); 81 | return; 82 | } 83 | } 84 | 85 | void ESP32Wiimote::handleTxQueue(void) { 86 | if(uxQueueMessagesWaiting(txQueue)){ 87 | bool ok = esp_vhci_host_check_send_available(); 88 | VERBOSE_PRINT("esp_vhci_host_check_send_available=%d", ok); 89 | if(ok){ 90 | queuedata_t *queuedata = NULL; 91 | if(xQueueReceive(txQueue, &queuedata, 0) == pdTRUE){ 92 | esp_vhci_host_send_packet(queuedata->data, queuedata->len); 93 | UNVERBOSE_PRINT("SEND => %s\n", format2Hex(queuedata->data, queuedata->len)); 94 | free(queuedata); 95 | } 96 | } 97 | } 98 | } 99 | 100 | void ESP32Wiimote::handleRxQueue(void) { 101 | if(uxQueueMessagesWaiting(rxQueue)){ 102 | queuedata_t *queuedata = NULL; 103 | if(xQueueReceive(rxQueue, &queuedata, 0) == pdTRUE){ 104 | handleHciData(queuedata->data, queuedata->len); 105 | free(queuedata); 106 | } 107 | } 108 | } 109 | 110 | esp_err_t ESP32Wiimote::sendQueueData(xQueueHandle queue, uint8_t *data, size_t len) { 111 | VERBOSE_PRINTLN("sendQueueData"); 112 | if(!data || !len){ 113 | VERBOSE_PRINTLN("no data"); 114 | return ESP_OK; 115 | } 116 | queuedata_t * queuedata = (queuedata_t*)malloc(sizeof(queuedata_t) + len); 117 | if(!queuedata){ 118 | VERBOSE_PRINTLN("malloc failed"); 119 | return ESP_FAIL; 120 | } 121 | queuedata->len = len; 122 | memcpy(queuedata->data, data, len); 123 | UNVERBOSE_PRINT("RECV <= %s\n", format2Hex(queuedata->data, queuedata->len)); 124 | if (xQueueSend(queue, &queuedata, portMAX_DELAY) != pdPASS) { 125 | VERBOSE_PRINTLN("xQueueSend failed"); 126 | free(queuedata); 127 | return ESP_FAIL; 128 | } 129 | return ESP_OK; 130 | } 131 | 132 | void ESP32Wiimote::hciHostSendPacket(uint8_t *data, size_t len) { 133 | sendQueueData(txQueue, data, len); 134 | } 135 | 136 | int ESP32Wiimote::notifyHostRecv(uint8_t *data, uint16_t len) { 137 | VERBOSE_PRINT("notifyHostRecv:"); 138 | for (int i = 0; i < len; i++) 139 | { 140 | VERBOSE_PRINT(" %02x", data[i]); 141 | } 142 | VERBOSE_PRINTLN(""); 143 | 144 | if(ESP_OK == sendQueueData(rxQueue, data, len)){ 145 | return ESP_OK; 146 | }else{ 147 | return ESP_FAIL; 148 | } 149 | } 150 | 151 | void ESP32Wiimote::init(void) 152 | { 153 | TinyWiimoteInit(tinywii_hci_interface); 154 | createQueue(); 155 | vhci_callback.notify_host_recv = notifyHostRecv; 156 | vhci_callback.notify_host_send_available = notifyHostSendAvailable; 157 | 158 | esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT(); 159 | if (!btStart()) { 160 | Serial.printf( "btStart() failed\n"); 161 | return; 162 | } 163 | 164 | esp_err_t ret; 165 | if ((ret = esp_vhci_host_register_callback(&vhci_callback)) != ESP_OK) { 166 | return; 167 | } 168 | } 169 | 170 | void ESP32Wiimote::task(void) 171 | { 172 | if(!btStarted()){ 173 | return; 174 | } 175 | handleTxQueue(); 176 | handleRxQueue(); 177 | } 178 | 179 | int ESP32Wiimote::available(void) 180 | { 181 | int offs = 0; 182 | int buttonIsChanged = false; 183 | // int nunchukButtonIsChanged = false; 184 | int accelIsChanged = false; 185 | int nunchukStickIsChanged = false; 186 | uint8_t cBtn = 0; 187 | uint8_t zBtn = 0; 188 | 189 | if (! TinyWiimoteAvailable()) 190 | return 0; 191 | 192 | TinyWiimoteData rd = TinyWiimoteRead(); 193 | 194 | if (rd.len < 4) // 195 | return 0; 196 | if (rd.data[0] != 0xA1) // no data input 197 | return 0; 198 | 199 | // update old states 200 | _oldButtonState = _buttonState; 201 | _oldAccelState = _accelState; 202 | _oldNunchukState = _nunchukState; 203 | 204 | if ((rd.data[1] >= 0x30) && (rd.data[1] <= 0x37)) // data report with button data 205 | offs = 2; 206 | 207 | if (offs) // update button state 208 | _buttonState = (ButtonState)((rd.data[offs + 0] << 8) | rd.data[offs + 1]); 209 | 210 | // get accelerometer offset 211 | switch (rd.data[1]) 212 | { 213 | case 0x31: offs = 4; break; // Core Buttons and Accelerometer 214 | case 0x35: offs = 4; break; // Core Buttons and Accelerometer with 16 Extension Bytes 215 | default: offs = 0; 216 | } 217 | 218 | if (offs) // update accelerometer 219 | { 220 | _accelState.xAxis = rd.data[offs + 0]; 221 | _accelState.yAxis = rd.data[offs + 1]; 222 | _accelState.zAxis = rd.data[offs + 2]; 223 | 224 | // check accel change 225 | if (_filter & FILTER_ACCEL) { 226 | ; // ignore 227 | } 228 | else { 229 | accelIsChanged = true; 230 | } 231 | } 232 | else 233 | { 234 | _accelState.xAxis = 0; 235 | _accelState.yAxis = 0; 236 | _accelState.zAxis = 0; 237 | } 238 | 239 | // get extension offset 240 | switch (rd.data[1]) 241 | { 242 | case 0x32: offs = 4; break; // Core Buttons with 8 Extension bytes 243 | case 0x35: offs = 7; break; // Core Buttons and Accelerometer with 16 Extension Bytes 244 | default: offs = 0; 245 | } 246 | 247 | if (offs) // update nunchuk state 248 | { 249 | _nunchukState.xStick = rd.data[offs + 0]; 250 | _nunchukState.yStick = rd.data[offs + 1]; 251 | _nunchukState.xAxis = rd.data[offs + 2]; 252 | _nunchukState.yAxis = rd.data[offs + 3]; 253 | _nunchukState.zAxis = rd.data[offs + 4]; 254 | 255 | // update nunchuk buttons 256 | cBtn = ((rd.data[offs + 5] & 0x02) >> 1) ^ 0x01; 257 | zBtn = (rd.data[offs + 5] & 0x01) ^ 0x01; 258 | } 259 | else 260 | { 261 | _nunchukState.xStick = 0; 262 | _nunchukState.yStick = 0; 263 | _nunchukState.xAxis = 0; 264 | _nunchukState.yAxis = 0; 265 | _nunchukState.zAxis = 0; 266 | } 267 | 268 | 269 | // add nunchuk buttons 270 | if (cBtn) 271 | _buttonState = (ButtonState)((int)_buttonState | BUTTON_C); 272 | if (zBtn) 273 | _buttonState = (ButtonState)((int)_buttonState | BUTTON_Z); 274 | 275 | // check button change 276 | if (_filter & FILTER_BUTTON) { 277 | ; // ignore 278 | } 279 | else if (_buttonState != _oldButtonState) { 280 | buttonIsChanged = true; 281 | } 282 | 283 | // check nunchuk stick change 284 | if (offs) 285 | { 286 | int nunXStickDelta = (int)(_nunchukState.xStick) - _oldNunchukState.xStick; 287 | int nunYStickDelta = (int)(_nunchukState.yStick) - _oldNunchukState.yStick; 288 | int nunStickDelta = (nunXStickDelta*nunXStickDelta + nunYStickDelta*nunYStickDelta); 289 | if (_filter & FILTER_NUNCHUK_STICK) { 290 | ; // ignore 291 | } 292 | else if (nunStickDelta >= _nunStickThreshold) { 293 | nunchukStickIsChanged = true; 294 | } 295 | 296 | // // check nunchuk button change 297 | // if (_filter & FILTER_NUNCHUK_BUTTON) { 298 | // ; // ignore 299 | // } 300 | // else if ( 301 | // (_pNunchukState->cBtn != _pOldNunchukState->cBtn) 302 | // || (_pNunchukState->zBtn != _pOldNunchukState->zBtn) 303 | // ) { 304 | // nunchukButtonIsChanged = true; 305 | // } 306 | 307 | // check accel change 308 | if (_filter & FILTER_ACCEL) { 309 | ; // ignore 310 | } 311 | else { 312 | accelIsChanged = true; 313 | } 314 | } 315 | 316 | return 317 | ( buttonIsChanged 318 | | nunchukStickIsChanged 319 | // | nunchukButtonIsChanged 320 | | accelIsChanged 321 | ); 322 | } 323 | 324 | ButtonState ESP32Wiimote::getButtonState(void) 325 | { 326 | return _buttonState; 327 | } 328 | 329 | AccelState ESP32Wiimote::getAccelState(void) 330 | { 331 | return _accelState; 332 | } 333 | 334 | NunchukState ESP32Wiimote::getNunchukState(void) 335 | { 336 | return _nunchukState; 337 | } 338 | 339 | void ESP32Wiimote::addFilter(int action, int filter) { 340 | if (action == ACTION_IGNORE) { 341 | _filter = _filter | filter; 342 | 343 | if (filter & FILTER_ACCEL) 344 | TinyWiimoteReqAccelerometer(false); 345 | } 346 | } 347 | 348 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_command.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_COMMAND_H__ 2 | #define __MERIDIAN_COMMAND_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "main.h" 7 | 8 | // ライブラリ導入 9 | #include "mrd_eeprom.h" 10 | #include "mrd_servo.h" 11 | #include "mrd_wire0.h" 12 | 13 | //================================================================================================== 14 | // コマンド処理 15 | //================================================================================================== 16 | 17 | /// @brief Master Commandの第1群を実行する. 受信コマンドに基づき, 異なる処理を行う. 18 | /// @param a_meridim 実行したいコマンドの入ったMeridim配列.(参照渡し) 19 | /// @param a_flg_exe Meridimの受信成功判定フラグ. 20 | /// @param a_sv サーボパラメータの構造体.(参照渡し) 21 | /// @return コマンドを実行した場合はtrue, しなかった場合はfalseを返す. 22 | bool execute_master_command_1(Meridim90Union &a_meridim, bool a_flg_exe, ServoParam &a_sv, Stream &a_serial) { 23 | if (!a_flg_exe) { 24 | return false; 25 | } 26 | // コマンド[90]: 1~999は MeridimのLength. デフォルトは90 27 | 28 | // コマンド:MCMD_ERR_CLEAR_SERVO_ID (10004) 通信エラーサーボIDのクリア 29 | if (a_meridim.sval[MRD_MASTER] == MCMD_ERR_CLEAR_SERVO_ID) { 30 | a_meridim.bval[MRD_ERR_l] = 0; 31 | for (int i = 0; i < IXL_MAX; i++) { 32 | a_sv.ixl_err[i] = 0; 33 | } 34 | for (int i = 0; i < IXR_MAX; i++) { 35 | a_sv.ixr_err[i] = 0; 36 | } 37 | String msg_tmp = "cmd: reset servo error id.[" + String(MCMD_ERR_CLEAR_SERVO_ID) + "]"; 38 | Serial.println(msg_tmp); 39 | return true; 40 | } 41 | 42 | // コマンド:MCMD_BOARD_TRANSMIT_ACTIVE (10005) UDP受信の通信周期制御をボード側主導に(デフォルト) 43 | if (a_meridim.sval[MRD_MASTER] == MCMD_BOARD_TRANSMIT_ACTIVE) { 44 | flg.udp_board_passive = false; // UDP送信をアクティブモードに 45 | flg.count_frame_reset = true; // フレームの管理時計をリセットフラグをセット 46 | return true; 47 | } 48 | 49 | // コマンド:MCMD_EEPROM_ENTER_WRITE (10009) EEPROMの書き込みモードスタート 50 | if (a_meridim.sval[MRD_MASTER] == MCMD_EEPROM_ENTER_WRITE) { 51 | flg.eeprom_write_mode = true; // 書き込みモードのフラグをセット 52 | flg.count_frame_reset = true; // フレームの管理時計をリセットフラグをセット 53 | return true; 54 | } 55 | 56 | // コマンド:MCMD_EEPROM_SAVE_TRIM (10101) EEPROMに現在のサーボ値をTRIM値として書き込む 57 | if (a_meridim.sval[MRD_MASTER] == MCMD_EEPROM_SAVE_TRIM) { 58 | String msg_tmp = "cmd: set EEPROM data from current trim.[" + String(MCMD_EEPROM_SAVE_TRIM) + "]"; 59 | a_serial.println(msg_tmp); 60 | 61 | // 空のUnionEEPROM構造体を作成し初期化 62 | UnionEEPROM array_tmp = {0}; 63 | 64 | // サーボの設定情報をsaval[1]にコピー 65 | for (int i = 0; i < a_sv.num_max; i++) { 66 | array_tmp.saval[1][20 + i * 2] = a_meridim.sval[20 + i * 2]; 67 | array_tmp.saval[1][21 + i * 2] = a_meridim.sval[21 + i * 2]; 68 | array_tmp.saval[1][50 + i * 2] = a_meridim.sval[50 + i * 2]; // - int(sv.ixl_trim[i] * 100); 69 | array_tmp.saval[1][51 + i * 2] = a_meridim.sval[51 + i * 2]; // - int(sv.ixr_trim[i] * 100); 70 | } 71 | 72 | // 書き込みデータの作成と書き込み 73 | if (mrd_eeprom_write(array_tmp, EEPROM_PROTECT, a_serial)) { 74 | a_serial.println("write EEPROM succeed."); 75 | } else { 76 | a_serial.println("write EEPROM failed."); 77 | return false; 78 | } 79 | return true; 80 | } 81 | 82 | // コマンド:MCMD_EEPROM_LOAD_TRIM (10102) EEPROMからTRIM値を読み込んで設定 83 | if (a_meridim.sval[MRD_MASTER] == MCMD_EEPROM_LOAD_TRIM) { 84 | // EEPROMのデータを展開する 85 | mrd_eeprom_load_servosettings(a_sv, true, a_serial); 86 | 87 | // サーボをEEPROMのTRIM値で補正されたHOME(原点)に移動する 88 | for (int i = 0; i < MRD_SERVO_SLOTS; i++) { 89 | a_meridim.sval[MRD_L_ORIGIDX + 1 + i * 2] = 0; // L系統の目標値を原点に 90 | a_meridim.sval[MRD_R_ORIGIDX + 1 + i * 2] = 0; // R系統の目標値を原点に 91 | a_sv.ixl_tgt[i] = 0; // 92 | a_sv.ixr_tgt[i] = 0; 93 | } 94 | 95 | // サーボ動作を実行する 96 | if (!MODE_TSY_STANDALONE) { 97 | mrd_servos_drive(a_meridim, MOUNT_SERVO_TYPE_L, MOUNT_SERVO_TYPE_R, MOUNT_SERVO_TYPE_C, a_sv); 98 | } 99 | 100 | flg.count_frame_reset = true; // フレームの管理時計をリセットフラグをセット 101 | return true; 102 | } 103 | 104 | return false; 105 | } 106 | 107 | /// @brief Master Commandの第2群を実行する. 受信コマンドに基づき, 異なる処理を行う. 108 | /// @param a_meridim 実行したいコマンドの入ったMeridim配列.(参照渡し) 109 | /// @param a_flg_exe Meridimの受信成功判定フラグ. 110 | /// @param a_sv サーボパラメータの構造体.(参照渡し) 111 | /// @return コマンドを実行した場合はtrue, しなかった場合はfalseを返す. 112 | bool execute_master_command_2(Meridim90Union &a_meridim, bool a_flg_exe, ServoParam &a_sv, Stream &a_serial) { 113 | if (!a_flg_exe) { 114 | return false; 115 | } 116 | // コマンド[90]: 1~999は MeridimのLength. デフォルトは90 117 | 118 | // コマンド:[0] 全サーボ脱力 119 | if (a_meridim.sval[MRD_MASTER] == 0) { 120 | mrd_servos_all_off(a_meridim); 121 | return true; 122 | } 123 | 124 | // コマンド:[1] サーボオン 通常動作 125 | 126 | // コマンド:MCMD_SENSOR_YAW_CALIB(10002) IMU/AHRSのヨー軸リセット 127 | if (a_meridim.sval[MRD_MASTER] == MCMD_SENSOR_YAW_CALIB) { 128 | ahrs.yaw_origin = mrd_wire0_setyaw(ahrs.ypr[0]); 129 | String msg_tmp = "cmd: caliblate sensor's yaw.[" + String(MCMD_SENSOR_YAW_CALIB) + "]"; 130 | Serial.println(msg_tmp); 131 | return true; 132 | } 133 | 134 | // コマンド:MCMD_BOARD_TRANSMIT_PASSIVE (10006) UDP受信の通信周期制御をPC側主導に(SSH的な動作) 135 | if (a_meridim.sval[MRD_MASTER] == MCMD_BOARD_TRANSMIT_PASSIVE) { 136 | flg.udp_board_passive = true; // UDP送信をパッシブモードに 137 | flg.count_frame_reset = true; // フレームの管理時計をリセットフラグをセット 138 | String msg_tmp = "cmd: enter passive mode.[" + String(MCMD_BOARD_TRANSMIT_PASSIVE) + "]"; 139 | Serial.println(msg_tmp); 140 | return true; 141 | } 142 | 143 | // コマンド:MCMD_FRAMETIMER_RESET) (10007) フレームカウンタを現在時刻にリセット 144 | if (a_meridim.sval[MRD_MASTER] == MCMD_FRAMETIMER_RESET) { 145 | flg.count_frame_reset = true; // フレームの管理時計をリセットフラグをセット 146 | return true; 147 | } 148 | 149 | // コマンド:MCMD_BOARD_STOP_DURING (10008) ボードの末端処理を指定時間だけ止める. 150 | if (a_meridim.sval[MRD_MASTER] == MCMD_BOARD_STOP_DURING) { 151 | flg.stop_board_during = true; // ボードの処理停止フラグをセット 152 | // ボードの末端処理をmeridim[2]ミリ秒だけ止める. 153 | 154 | String msg_tmp = "cmd: stop ESP32's processing during " + String(int(a_meridim.sval[MRD_STOP_FRAMES])) + " ms.[" + String(MCMD_BOARD_TRANSMIT_PASSIVE) + "]"; 155 | Serial.println(msg_tmp); 156 | 157 | for (int i = 0; i < int(a_meridim.sval[MRD_STOP_FRAMES]); i++) { 158 | delay(1); 159 | } 160 | flg.stop_board_during = false; // ボードの処理停止フラグをクリア 161 | flg.count_frame_reset = true; // フレームの管理時計をリセットフラグをセット 162 | return true; 163 | } 164 | return false; 165 | } 166 | 167 | /// @brief Master Commandの第3群を実行する. 受信コマンドに基づき, 異なる処理を行う. 168 | /// @param a_meridim 実行したいコマンドの入ったMeridim配列.(参照渡し) 169 | /// @param a_flg_exe Meridimの受信成功判定フラグ. 170 | /// @param a_sv サーボパラメータの構造体.(参照渡し) 171 | /// @return コマンドを実行した場合はtrue, しなかった場合はfalseを返す. 172 | bool execute_master_command_3(Meridim90Union &a_meridim, bool a_flg_exe, ServoParam &a_sv, bool &a_flg_special, Stream &a_serial) { 173 | if (!a_flg_exe) { 174 | return false; 175 | } 176 | // コマンド[90]: 1~999は MeridimのLength. デフォルトは90 177 | 178 | // コマンド:[0] 全サーボ脱力 179 | if (a_meridim.sval[MRD_MASTER] == 0) { 180 | mrd_servos_all_off(a_meridim); 181 | return true; 182 | } 183 | 184 | // コマンド:[1] サーボオン 通常動作 185 | 186 | // コマンド:MCMD_START_TRIM_SETTING (10100) TRIM設定のスタート(Meridian_console連携) 187 | if (a_meridim.sval[MRD_MASTER] == MCMD_START_TRIM_SETTING) { 188 | 189 | // EEPROMのデータを展開する 190 | mrd_eeprom_load_servosettings(a_sv, true, a_serial); 191 | 192 | // サーボをEEPROMのTRIM値で補正されたHOME(原点)に移動する 193 | for (int i = 0; i < MRD_SERVO_SLOTS; i++) { 194 | a_meridim.sval[MRD_L_ORIGIDX + 1 + i * 2] = 0; // L系統の目標値を原点に 195 | a_meridim.sval[MRD_R_ORIGIDX + 1 + i * 2] = 0; // R系統の目標値を原点に 196 | a_sv.ixl_tgt_past[i] = a_sv.ixl_tgt[i]; // 前回のdegreeをキープ 197 | a_sv.ixr_tgt_past[i] = a_sv.ixr_tgt[i]; 198 | a_sv.ixl_tgt[i] = 0; // 199 | a_sv.ixr_tgt[i] = 0; 200 | } 201 | 202 | // サーボ動作を実行する 203 | if (!MODE_TSY_STANDALONE) { 204 | mrd_servos_drive(a_meridim, MOUNT_SERVO_TYPE_L, MOUNT_SERVO_TYPE_R, MOUNT_SERVO_TYPE_C, a_sv); 205 | } 206 | 207 | // サーボの目標値として現在のTRIM値をセットする 208 | for (int i = 0; i < MRD_SERVO_SLOTS; i++) { 209 | a_meridim.sval[MRD_L_ORIGIDX + 1 + i * 2] = a_sv.ixl_trim[i]; 210 | a_meridim.sval[MRD_R_ORIGIDX + 1 + i * 2] = a_sv.ixr_trim[i]; 211 | } 212 | 213 | // サーボのTRIM値をゼロリセットする 214 | for (int i = 0; i < MRD_SERVO_SLOTS; i++) { 215 | a_sv.ixl_trim[i] = 0; 216 | a_sv.ixr_trim[i] = 0; 217 | } 218 | 219 | // サーボ動作を実行する. サーボはTRIM値を0としつつ, tgtとしてこれまでのTRIM値の角度をキープする 220 | if (!MODE_TSY_STANDALONE) { 221 | mrd_servos_drive(a_meridim, MOUNT_SERVO_TYPE_L, MOUNT_SERVO_TYPE_R, MOUNT_SERVO_TYPE_C, a_sv); // サーボ動作を実行する 222 | } 223 | 224 | // サーボの設定値とTRIM値をPCに送信する 225 | UnionEEPROM array_tmp = mrd_eeprom_read(); 226 | for (int i = 0; i < MRDM_LEN; i++) { 227 | a_meridim.sval[i] = array_tmp.saval[1][i]; 228 | } 229 | a_meridim.sval[MRD_MASTER] = MCMD_EEPROM_BOARDTOPC_DATA1; 230 | 231 | a_serial.println("send:"); 232 | for (int i = 0; i < MRDM_LEN; i++) { 233 | a_serial.print(a_meridim.sval[i]); 234 | a_serial.print(","); 235 | } 236 | a_serial.println(); 237 | 238 | a_flg_special = true; 239 | 240 | String msg_tmp = "cmd: enter trim setting mode and send EEPROM[1][*] to PC.[" + String(MCMD_START_TRIM_SETTING) + "]"; 241 | Serial.println(msg_tmp); 242 | 243 | return true; 244 | } 245 | 246 | // コマンド:MCMD_EEPROM_BOARDTOPC_DATA2(10200) EEPROMの[0][*]をボードからPCにMeridimで送信する 247 | if (a_meridim.sval[MRD_MASTER] == MCMD_EEPROM_BOARDTOPC_DATA0) { 248 | // eepromをs_meridimに代入する 249 | UnionEEPROM array_tmp = mrd_eeprom_read(); 250 | for (int i = 0; i < MRDM_LEN; i++) { 251 | a_meridim.sval[i] = array_tmp.saval[0][i]; 252 | } 253 | a_meridim.sval[MRD_MASTER] = MCMD_EEPROM_BOARDTOPC_DATA0; 254 | 255 | String msg_tmp = "cmd: enter trim setting mode and send EEPROM[0][*] to PC.[" + String(MCMD_EEPROM_BOARDTOPC_DATA0) + "]"; 256 | Serial.println(msg_tmp); 257 | return true; 258 | } 259 | 260 | // コマンド:MCMD_EEPROM_BOARDTOPC_DATA2(10201) EEPROMの[1][*]をボードからPCにMeridimで送信する 261 | if (a_meridim.sval[MRD_MASTER] == MCMD_EEPROM_BOARDTOPC_DATA1) { 262 | // eepromをs_meridimに代入する 263 | UnionEEPROM array_tmp = mrd_eeprom_read(); 264 | for (int i = 0; i < MRDM_LEN; i++) { 265 | a_meridim.sval[i] = array_tmp.saval[1][i]; 266 | } 267 | a_meridim.sval[MRD_MASTER] = MCMD_EEPROM_BOARDTOPC_DATA1; 268 | 269 | String msg_tmp = "cmd: enter trim setting mode and send EEPROM[1][*] to PC.[" + String(MCMD_EEPROM_BOARDTOPC_DATA1) + "]"; 270 | Serial.println(msg_tmp); 271 | return true; 272 | } 273 | 274 | // コマンド:MCMD_EEPROM_BOARDTOPC_DATA2(10202) EEPROMの[2][*]をボードからPCにMeridimで送信する 275 | if (a_meridim.sval[MRD_MASTER] == MCMD_EEPROM_BOARDTOPC_DATA2) { 276 | // eepromをs_meridimに代入する 277 | UnionEEPROM array_tmp = mrd_eeprom_read(); 278 | for (int i = 0; i < MRDM_LEN; i++) { 279 | a_meridim.sval[i] = array_tmp.saval[2][i]; 280 | } 281 | a_meridim.sval[MRD_MASTER] = MCMD_EEPROM_BOARDTOPC_DATA2; 282 | 283 | String msg_tmp = "cmd: enter trim setting mode and send EEPROM[2][*] to PC.[" + String(MCMD_EEPROM_BOARDTOPC_DATA2) + "]"; 284 | Serial.println(msg_tmp); 285 | return true; 286 | } 287 | 288 | return false; 289 | } 290 | 291 | #endif // __MERIDIAN_COMMAND_H__ -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/src/config.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef __MERIDIAN_CONFIG_H__ 3 | #define __MERIDIAN_CONFIG_H__ 4 | 5 | //================================================================================================================ 6 | // MERIDIAN TWIN ESP32の配線 7 | //================================================================================================================ 8 | // ESP32devkitC - Teensy4.0 or PIN 9 | // [3.3v] -> x 10 | // [EN] -> x 11 | // [SENSOR_VP] -> x 12 | // [SENSOR_VN] -> x 13 | // [34] -> x 14 | // [35] -> x 15 | // [32] -> x 16 | // [33] -> x 17 | // [25] -> x 18 | // [26] -> x 19 | // [27] -> x 20 | // [14] -> SPI/SD_SCK (Teensy[13]) & SD_CLK (SD[5]) 21 | // [12] -> MISO (Teensy[12]) 22 | // [GND1] -> x 23 | // [13] -> MOSI (Teensy[11]) 24 | // [D2] -> x 25 | // [D3] -> x 26 | // [CMD] -> x 27 | // [5V] -> 5V 28 | 29 | // [GND3] -> GND 30 | // [23] -> x 31 | // [22] -> x 32 | // [TXD0] -> x 33 | // [RXD0] -> x 34 | // [21] -> x 35 | // [GND2] -> x 36 | // [19] -> x 37 | // [18] -> x 38 | // [05] -> x 39 | // [17] -> x 40 | // [16] -> x 41 | // [04] -> x 42 | // [00] -> x 43 | // [02] -> x 44 | // [15] -> SPI_CS (Teensy[10]) 45 | // [D1] -> x 46 | // [D0] -> x 47 | // [CLK] -> x 48 | 49 | //================================================================================================================ 50 | // Meridim90配列 一覧表 51 | //================================================================================================================ 52 | // [00] マスターコマンド デフォルトは90 で配列数も同時に示す 53 | // [01] シーケンス番号 54 | // [02]-[04] IMU/AHRS:acc＿x,acc＿y,acc＿z 加速度x,y,z 55 | // [05]-[07] IMU/AHRS:gyro＿x,gyro＿y,gyro＿z ジャイロx,y,z 56 | // [08]-[10] IMU/AHRS:mag＿x,mag＿y,mag＿z 磁気コンパスx,y,z 57 | // [11] IMU/AHRS:temp 温度 58 | // [12]-[14] IMU/AHRS:DMP ROLL,PITCH,YAW DMP推定値 ロール,ピッチ,ヨー 59 | // [15] ボタンデータ1 60 | // [16] ボタンアナログ1 (Stick Left) 61 | // [17] ボタンアナログ2 (Stick Right) 62 | // [18] ボタンアナログ3 (L2,R2 アナログ) 63 | // [19] モーション設定 (フレーム数) 64 | // [20] サーボID LO コマンド 65 | // [21] サーボID LO データ値 66 | // [...] ... 67 | // [48] サーボID L14 コマンド 68 | // [49] サーボID L14 データ値 69 | // [50] サーボID RO コマンド 70 | // [51] サーボID RO データ値 71 | // [...] ... 72 | // [78] サーボID R14 コマンド 73 | // [79] サーボID R14 データ値 74 | // [80]-[MRDM_LEN-3] free (Meridim90では[87]まで) 75 | // [MRDM_LEN-2] ERROR CODE 76 | // [MRDM_LEN-1] チェックサム 77 | 78 | // Meridimの基本設定 79 | #define MRDM_LEN 90 // Meridim配列の長さ設定 (デフォルトは90) 80 | #define FRAME_DURATION 10 // 1フレームあたりの単位時間 (単位ms) 81 | #define CHARGE_TIME 200 // 起動時のコンデンサチャージ待機時間 (単位ms) 82 | 83 | // 動作モード 84 | #define MODE_UDP_RECEIVE 1 // PCからのデータ受信 (0:OFF, 1:ON, 通常は1) 85 | #define MODE_UDP_SEND 1 // PCへのデータ送信 (0:OFF, 1:ON, 通常は1) 86 | 87 | // Wifiの設定(SSID,パスワード等は別途keys.hで指定) 88 | #define MODE_FIXED_IP 0 // IPアドレスを固定するか (0:NO, 1:YES) 89 | #define UDP_TIMEOUT 4 // UDPの待受タイムアウト (単位ms,推奨値0) 90 | 91 | // EEPROMの設定 92 | #define EEPROM_BYTE 540 // 使用するEEPROMのサイズ(バイト) 93 | #define EEPROM_SET 0 // 起動時にEEPROMにconfig.hの内容をセット(mrd_set_eeprom) 94 | #define EEPROM_PROTECT 0 // EEPROMの書き込み保護(0:保護しない, 1:書き込み禁止) 95 | #define EEPROM_LOAD 0 // 起動時にEEPROMの内容を諸設定にロードする(未導入) 96 | #define EEPROM_DUMP 1 // 起動時のEEPROM内容のダンプ表示 97 | #define EEPROM_STYLE Hex // 起動時のEEPROM内容のダンプ表示の書式(Bin,Hex,Dec) 98 | 99 | // 動作チェックモード 100 | #define CHECK_EEPROM_RW 0 // 起動時のEEPROMの動作チェック 101 | 102 | // シリアルモニタリング 103 | #define MONITOR_FLOW 0 // シリアルモニタでフローを表示 (0:OFF, 1:ON) 104 | #define MONITOR_ERR_ALL 0 // 全経路の受信エラー率を表示 105 | #define MONITOR_SEQ 0 // シリアルモニタでシーケンス番号チェックを表示 (0:OFF, 1:ON) 106 | #define MONITOR_PAD 0 // シリアルモニタでリモコンのデータを表示 (0:OFF, 1:ON) 107 | #define MONITOR_SUPPRESS_DURATION 20000 // 起動直後のタイムアウトメッセージ抑制時間(単位ms) 108 | 109 | // SPI設定 110 | #define SPI0_SPEED 6000000 // SPI通信の速度 (6000000kHz推奨) 111 | 112 | // PC接続関連設定 113 | #define SERIAL_PC_BPS 1000000 // PCとのシリアル速度 (モニタリング表示用) 114 | #define SERIAL_PC_TIMEOUT 2000 // PCとのシリアル接続確立タイムアウト(ms) 115 | 116 | // JOYPAD関連設定 117 | #define MOUNT_PAD NONE // ESP32へのジョイパッドの搭載 NONE:0, PC:0, WIIMOTE:5, WIIMOTE_C:6 118 | #define PAD_INIT_TIMEOUT 10000 // 起動時のJOYPADの接続確立のタイムアウト(ms) 119 | #define PAD_INTERVAL 10 // JOYPADのデータを読みに行くフレーム間隔 (※KRC-5FHでは4推奨) 120 | #define PAD_BUTTON_MARGE 0 // 0:JOYPADのボタンデータをMeridim受信値に論理積, 1:Meridim受信値に論理和 121 | #define PAD_GENERALIZE 1 // ジョイパッドの入力値をPS系に一般化する 122 | 123 | // ピンアサイン 124 | #define PIN_ERR_LED 25 // LED用 処理が時間内に収まっていない場合に点灯 125 | #define PIN_EN_L 33 // サーボL系統のENピン 126 | #define PIN_EN_R 4 // サーボR系統のENピン 127 | #define PIN_CHIPSELECT_SD 15 // SDカード用のCSピン 128 | #define PIN_I2C0_SDA 22 // I2CのSDAピン 129 | #define PIN_I2C0_SCL 21 // I2CのSCLピン 130 | #define PIN_LED_BT 23 // Bluetooth接続確認用ピン(点滅はペアリング,点灯でリンク確立) 131 | 132 | //------------------------------------------------------------------------- 133 | // 固定値, マスターコマンド定義 134 | //------------------------------------------------------------------------- 135 | #define MCMD_DUMMY_DATA -32768 // SPI送受信用のダミーデータ判定用 136 | #define MCMD_TEST_VALUE -32767 // テスト用の仮設変数 137 | #define MCMD_SENSOR_YAW_CALIB 10002 // センサの推定ヨー軸を現在値センターとしてリセット 138 | #define MCMD_SENSOR_ALL_CALIB 10003 // センサの3軸について現在値を原点としてリセット 139 | #define MCMD_ERR_CLEAR_SERVO_ID 10004 // 通信エラーのサーボのIDをクリア(MRD_ERR_l) 140 | #define MCMD_BOARD_TRANSMIT_ACTIVE 10005 // ボードが定刻で送信を行うモード (PC側が受信待ち) 141 | #define MCMD_BOARD_TRANSMIT_PASSIVE 10006 // ボードが受信を待ち返信するモード (PC側が定刻送信) 142 | #define MCMD_FRAMETIMER_RESET 10007 // フレームタイマーを現在時刻にリセット 143 | #define MCMD_BOARD_STOP_DURING 10008 // ボードの末端処理を[MRD_STOP_FRAMES]ミリ秒止める 144 | #define MCMD_EEPROM_ENTER_WRITE 10009 // EEPROM書き込みモードのスタート 145 | #define MCMD_EEPROM_EXIT_WRITE 10010 // EEPROM書き込みモードの終了 146 | #define MCMD_EEPROM_ENTER_READ 10011 // EEPROM読み出しモードのスタート 147 | #define MCMD_EEPROM_EXIT_READ 10012 // EEPROM読み出しモードの終了 148 | #define MCMD_SDCARD_ENTER_WRITE 10013 // SDCARD書き込みモードのスタート 149 | #define MCMD_SDCARD_EXIT_WRITE 10014 // SDCARD書き込みモードの終了 150 | #define MCMD_SDCARD_ENTER_READ 10015 // SDCARD読み出しモードのスタート 151 | #define MCMD_SDCARD_EXIT_READ 10016 // SDCARD読み出しモードの終了 152 | 153 | //------------------------------------------------------------------------- 154 | // Meridim90 配列アクセス対応キー 155 | //------------------------------------------------------------------------- 156 | #define MRD_MASTER 0 // マスターコマンド 157 | #define MRD_SEQ 1 // シーケンス番号 158 | #define MRD_ACC_X 2 // 加速度センサX値 159 | #define MRD_ACC_Y 3 // 加速度センサY値 160 | #define MRD_ACC_Z 4 // 加速度センサZ値 161 | #define MRD_GYRO_X 5 // ジャイロセンサX値 162 | #define MRD_GYRO_Y 6 // ジャイロセンサY値 163 | #define MRD_GYRO_Z 7 // ジャイロセンサZ値 164 | #define MRD_MAG_X 8 // 磁気コンパスX値 165 | #define MRD_MAG_Y 9 // 磁気コンパスY値 166 | #define MRD_MAG_Z 10 // 磁気コンパスZ値 167 | #define MRD_TEMP 11 // 温度センサ値 168 | #define MRD_DIR_ROLL 12 // DMP推定ロール方向値 169 | #define MRD_DIR_PITCH 13 // DMP推定ピッチ方向値 170 | #define MRD_DIR_YAW 14 // DMP推定ヨー方向値 171 | #define MRD_PAD_BUTTONS 15 // リモコンの基本ボタン値 172 | #define MRD_PAD_STICK_L 16 // リモコンの左スティックアナログ値 173 | #define MRD_PAD_STICK_R 17 // リモコンの右スティックアナログ値 174 | #define MRD_PAD_L2R2VAL 18 // リモコンのL2R2ボタンアナログ値 175 | #define MRD_MOTION_FRAMES 19 // モーション設定のフレーム数 176 | #define MRD_STOP_FRAMES 19 // ボード停止時のフレーム数(MCMD_STOP_BOARD_DURINGで指定) 177 | 178 | #define C_HEAD_Y_CMD 20 // 頭ヨーのコマンド 179 | #define C_HEAD_Y_VAL 21 // 頭ヨーの値 180 | #define L_SHOULDER_P_CMD 22 // 左肩ピッチのコマンド 181 | #define L_SHOULDER_P_VAL 23 // 左肩ピッチの値 182 | #define L_SHOULDER_R_CMD 24 // 左肩ロールのコマンド 183 | #define L_SHOULDER_R_VAL 25 // 左肩ロールの値 184 | #define L_ELBOW_Y_CMD 26 // 左肘ヨーのコマンド 185 | #define L_ELBOW_Y_VAL 27 // 左肘ヨーの値 186 | #define L_ELBOW_P_CMD 28 // 左肘ピッチのコマンド 187 | #define L_ELBOW_P_VAL 29 // 左肘ピッチの値 188 | #define L_HIPJOINT_Y_CMD 30 // 左股ヨーのコマンド 189 | #define L_HIPJOINT_Y_VAL 31 // 左股ヨーの値 190 | #define L_HIPJOINT_R_CMD 32 // 左股ロールのコマンド 191 | #define L_HIPJOINT_R_VAL 33 // 左股ロールの値 192 | #define L_HIPJOINT_P_CMD 34 // 左股ピッチのコマンド 193 | #define L_HIPJOINT_P_VAL 35 // 左股ピッチの値 194 | #define L_KNEE_P_CMD 36 // 左膝ピッチのコマンド 195 | #define L_KNEE_P_VAL 37 // 左膝ピッチの値 196 | #define L_ANKLE_P_CMD 38 // 左足首ピッチのコマンド 197 | #define L_ANKLE_P_VAL 39 // 左足首ピッチの値 198 | #define L_ANKLE_R_CMD 40 // 左足首ロールのコマンド 199 | #define L_ANKLE_R_VAL 41 // 左足首ロールの値 200 | #define L_SERVO_IX11_CMD 42 // 追加サーボ用のコマンド 201 | #define L_SERVO_IX11_VAL 43 // 追加サーボ用の値 202 | #define L_SERVO_IX12_CMD 44 // 追加サーボ用のコマンド 203 | #define L_SERVO_IX12_VAL 45 // 追加サーボ用の値 204 | #define L_SERVO_IX13_CMD 46 // 追加サーボ用のコマンド 205 | #define L_SERVO_IX13_VAL 47 // 追加サーボ用の値 206 | #define L_SERVO_IX14_CMD 48 // 追加サーボ用のコマンド 207 | #define L_SERVO_IX14_VAL 49 // 追加サーボ用の値 208 | 209 | #define C_WAIST_Y_CMD 50 // 腰ヨーのコマンド 210 | #define C_WAIST_Y_VAL 51 // 腰ヨーの値 211 | #define R_SHOULDER_P_CMD 52 // 右肩ピッチのコマンド 212 | #define R_SHOULDER_P_VAL 53 // 右肩ピッチの値 213 | #define R_SHOULDER_R_CMD 54 // 右肩ロールのコマンド 214 | #define R_SHOULDER_R_VAL 55 // 右肩ロールの値 215 | #define R_ELBOW_Y_CMD 56 // 右肘ヨーのコマンド 216 | #define R_ELBOW_Y_VAL 57 // 右肘ヨーの値 217 | #define R_ELBOW_P_CMD 58 // 右肘ピッチのコマンド 218 | #define R_ELBOW_P_VAL 59 // 右肘ピッチの値 219 | #define R_HIPJOINT_Y_CMD 60 // 右股ヨーのコマンド 220 | #define R_HIPJOINT_Y_VAL 61 // 右股ヨーの値 221 | #define R_HIPJOINT_R_CMD 62 // 右股ロールのコマンド 222 | #define R_HIPJOINT_R_VAL 63 // 右股ロールの値 223 | #define R_HIPJOINT_P_CMD 64 // 右股ピッチのコマンド 224 | #define R_HIPJOINT_P_VAL 65 // 右股ピッチの値 225 | #define R_KNEE_P_CMD 66 // 右膝ピッチのコマンド 226 | #define R_KNEE_P_VAL 67 // 右膝ピッチの値 227 | #define R_ANKLE_P_CMD 68 // 右足首ピッチのコマンド 228 | #define R_ANKLE_P_VAL 69 // 右足首ピッチの値 229 | #define R_ANKLE_R_CMD 70 // 右足首ロールのコマンド 230 | #define R_ANKLE_R_VAL 71 // 右足首ロールの値 231 | #define R_SERVO_IX11_CMD 72 // 追加テスト用のコマンド 232 | #define R_SERVO_IX11_VAL 73 // 追加テスト用の値 233 | #define R_SERVO_IX12_CMD 74 // 追加テスト用のコマンド 234 | #define R_SERVO_IX12_VAL 75 // 追加テスト用の値 235 | #define R_SERVO_IX13_CMD 76 // 追加テスト用のコマンド 236 | #define R_SERVO_IX13_VAL 77 // 追加テスト用の値 237 | #define R_SERVO_IX14_CMD 78 // 追加テスト用のコマンド 238 | #define R_SERVO_IX14_VAL 79 // 追加テスト用の値 239 | 240 | #define MRD_USERDATA_80 80 // ユーザー定義用 241 | #define MRD_USERDATA_81 81 // ユーザー定義用 242 | #define MRD_USERDATA_82 82 // ユーザー定義用 243 | #define MRD_USERDATA_83 83 // ユーザー定義用 244 | #define MRD_USERDATA_84 84 // ユーザー定義用 245 | #define MRD_USERDATA_85 85 // ユーザー定義用 246 | #define MRD_USERDATA_86 86 // ユーザー定義用 247 | #define MRD_USERDATA_87 87 // ユーザー定義用 248 | // #define MRD_ERR 88 // エラーコード (MRDM_LEN - 2) 249 | // #define MRD_CKSM 89 // チェックサム (MRDM_LEN - 1) 250 | 251 | // エラービット MRD_ERR_CODEの上位8bit分 252 | #define ERRBIT_15_ESP_PC 15 // ESP32 → PC のUDP受信エラー (0:エラーなし、1:エラー検出) 253 | #define ERRBIT_14_PC_ESP 14 // PC → ESP32 のUDP受信エラー 254 | #define ERRBIT_13_ESP_TSY 13 // ESP32 → TeensyのSPI受信エラー 255 | #define ERRBIT_12_TSY_ESP 12 // Teensy → ESP32 のSPI受信エラー 256 | #define ERRBIT_11_BOARD_DELAY 11 // Teensy or ESP32の処理ディレイ (末端で捕捉) 257 | #define ERRBIT_10_UDP_ESP_SKIP 10 // PC → ESP32 のUDPフレームスキップエラー 258 | #define ERRBIT_9_BOARD_SKIP 9 // PC → ESP32 → Teensy のフレームスキップエラー(末端で捕捉) 259 | #define ERRBIT_8_PC_SKIP 8 // Teensy → ESP32 → PC のフレームスキップエラー(末端で捕捉) 260 | 261 | #endif // __MERIDIAN_CONFIG_H__ 262 | -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/src/mrd_eeprom.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_EEPROM_H__ 2 | #define __MERIDIAN_EEPROM_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "main.h" 7 | 8 | // ライブラリ導入 9 | #include 10 | 11 | //================================================================================================================ 12 | // EEPROM関連の処理 13 | //================================================================================================================ 14 | 15 | /// @brief EEPROMの初期化 16 | /// @param a_eeprom_size EEPROMのバイト長 17 | /// @return 初期化が成功すればtrue, 失敗ならfalseを返す. 18 | bool mrd_eeprom_init(int a_eeprom_size) { 19 | Serial.print("Initializing EEPROM... "); 20 | if (EEPROM.begin(a_eeprom_size)) { 21 | Serial.println("OK."); 22 | return true; 23 | } else { 24 | Serial.println("Failed."); 25 | } 26 | return false; 27 | } 28 | 29 | // EEPROM読み書き用共用体 30 | typedef union { 31 | uint8_t bval[EEPROM_BYTE]; // 1バイト単位で540個のデータを持つ 32 | int16_t saval[6][int(EEPROM_BYTE / 6)]; // short型で6*90個の配列データを持つ 33 | uint16_t sauval[6][int(EEPROM_BYTE / 6)]; // ushort型で6*90個の配列データを持つ 34 | int16_t baval[6][int(EEPROM_BYTE / 3)]; // byte型で6*180個の配列データを持つ 35 | uint8_t bauval[6][int(EEPROM_BYTE / 3)]; // ubyte型で6*180個の配列データを持つ 36 | int16_t sval[int(EEPROM_BYTE / 2)]; // short型で270個のデータを持つ 37 | uint16_t usval[int(EEPROM_BYTE / 2)]; // short型で270個のデータを持つ 38 | } UnionEEPROM; 39 | UnionEEPROM eeprom_write_data; // EEPROM書き込み用 40 | UnionEEPROM eeprom_read_data; // EEPROM読み込み用 41 | 42 | //------------------------------------------------------------------------------------ 43 | // EEPROM 読み込み 44 | //------------------------------------------------------------------------------------ 45 | 46 | /// @brief EEPROMの内容を読み込んで返す. 47 | /// @param a_len_byte EEPROMの使用サイズ. 48 | /// @param a_serial メッセージ表示用のハードウェアシリアル. 49 | /// @return UnionEEPROM のフォーマットで配列を返す. 50 | UnionEEPROM mrd_eeprom_load(int a_len_byte, Stream &a_serial) { 51 | if (a_len_byte > EEPROM.length()) // EEPROMのサイズを超えないようチェック 52 | { 53 | a_len_byte = EEPROM.length(); 54 | a_serial.println("Error: EEPROM address out of range."); 55 | } 56 | 57 | UnionEEPROM data_tmp; 58 | for (int i = 0; i < a_len_byte; i++) // データを読み込む時はbyte型 59 | { 60 | data_tmp.bval[i] = EEPROM.read(i); 61 | } 62 | return data_tmp; 63 | } 64 | 65 | //------------------------------------------------------------------------------------ 66 | // EEPROM ダンプ出力 67 | //------------------------------------------------------------------------------------ 68 | 69 | /// @brief EEPROM格納用の配列データをシリアルにダンプ出力する. 70 | /// @param a_data EEPROM用の配列データ. 71 | /// @param a_len_byte EEPROMのサイズ(byte). 72 | /// @param a_bhd ダンプリストの表示形式.(0:Bin, 1:Hex, 2:Dec) 73 | /// @param a_serial メッセージ表示用のハードウェアシリアル. 74 | /// @return 終了時にtrueを返す. 75 | bool mrd_eeprom_dump_serial(UnionEEPROM a_data, int a_len_byte, int a_bhd, Stream &a_serial) { 76 | a_serial.print("EEPROM Length "); 77 | a_serial.print(EEPROM.length()); // EEPROMの長さ表示 78 | a_serial.print(", Used Length "); 79 | a_serial.print(a_len_byte); // EEPROMの使用サイズ表示(BYTE) 80 | a_serial.println("byte, 16bit Dump;"); 81 | int k = 0; 82 | char hex_tmp[5]; 83 | for (int i = 0; i < int(a_len_byte / 2); i++) // 読み込むデータはshort型で作成 84 | { 85 | k++; 86 | if (a_bhd == Bin) { 87 | // 16桁のビット列を表示 88 | for (int bit = 15; bit >= 0; bit--) { 89 | a_serial.print((a_data.usval[i] >> bit) & 1); 90 | } 91 | } else if (a_bhd == Hex) { 92 | sprintf(hex_tmp, "%04X", (uint16_t)a_data.sval[i]); // 符号なし16ビットにキャストして表示 93 | a_serial.print(hex_tmp); 94 | } else { 95 | a_serial.print(a_data.sval[i], DEC); 96 | } 97 | 98 | if (a_bhd == Bin) { 99 | if (k % 90 == 0) { 100 | a_serial.println(",\n"); 101 | k = 0; 102 | } else if (k % 20 == 0) { 103 | a_serial.println("/"); 104 | } else if (k % 5 == 0) { 105 | a_serial.println(); 106 | } else { 107 | a_serial.print(" "); 108 | } 109 | } else { 110 | if (k % 90 == 0) { 111 | a_serial.println(","); 112 | k = 0; 113 | } else if (k % 10 == 0) { 114 | a_serial.print((k % 20 == 0) ? "\n" : " "); 115 | } else { 116 | a_serial.print(" "); 117 | } 118 | } 119 | } 120 | return true; 121 | } 122 | 123 | //------------------------------------------------------------------------------------ 124 | // EEPROM データ作成 125 | //------------------------------------------------------------------------------------ 126 | 127 | /// @brief config.hにあるサーボの諸設定からEEPROM格納用の配列データを作成する. 128 | /// @return config.hから作成したEEPROM格納用の配列データを返す. 129 | UnionEEPROM mrd_eeprom_make_data_from_config() { 130 | UnionEEPROM array_tmp = {0}; 131 | for (int i = 0; i < 15; i++) { 132 | // // 左サーボの情報を格納 133 | // array_tmp.sauval[1][20 + i * 2] = 134 | // ((sv.ixl_mount[i] != 0) << 15) | // bit16: マウント有無 (0以外なら1) 135 | // ((sv.ixl_id[i] & 0x7F) << 8) | // bit15-9: ID (7ビット) 136 | // ((sv.ixl_cw[i] == -1 ? 1 : 0) << 8); // bit8: 正逆 (1ビット) 137 | // // 各サーボの直立デフォルト値 degree 138 | // array_tmp.saval[1][21 + i * 2] = short(mrd.float2HfShort(sv.ixl_trim[i])); 139 | 140 | // // 右サーボの情報を格納 141 | // array_tmp.sauval[1][50 + i * 2] = 142 | // ((sv.ixl_mount[i] != 0) << 15) | // bit16: マウント有無 (0以外なら1) 143 | // ((sv.ixl_id[i] & 0x7F) << 8) | // bit15-9: ID (7ビット) 144 | // ((sv.ixl_cw[i] == -1 ? 1 : 0) << 8); // bit8: 正逆 (1ビット) 145 | // // 各サーボの直立デフォルト値 degree 146 | // array_tmp.saval[1][51 + i * 2] = short(mrd.float2HfShort(sv.ixr_trim[i])); 147 | }; 148 | return array_tmp; 149 | } 150 | 151 | //------------------------------------------------------------------------------------ 152 | // EEPROM 書き込み 153 | //------------------------------------------------------------------------------------ 154 | 155 | /// @brief EEPROMを0でフォーマットする 156 | /// @param a_flg_protect EEPROMの書き込み許可があるかどうかのブール値. 157 | /// @param a_len_byte EEPROMの使用サイズ. 158 | /// @param a_serial メッセージ表示用のハードウェアシリアル. 159 | /// @return EEPROMの書き込みと読み込みが成功した場合はtrueを, 書き込まなかった場合はfalseを返す. 160 | bool mrd_eeprom_zero_format(bool a_flg_protect, int a_len_byte, Stream &a_serial) { 161 | a_serial.println("Try to EEPROM zero format..."); 162 | 163 | if (a_flg_protect) { // EEPROM書き込み実施フラグをチェック 164 | return false; 165 | } 166 | if (flg.eeprom_protect) // config.hのEEPROM書き込みプロテクトをチェック 167 | { 168 | a_serial.println("EEPROM is protected. To unprotect, please set 'EEPROM_PROTECT' to false."); 169 | return false; 170 | } 171 | 172 | // EEPROM書き込み 173 | for (int i = 0; i < EEPROM_BYTE; i++) // データを書き込む時はbyte型 174 | { 175 | if (i >= EEPROM.length()) // EEPROMのサイズを超えないようチェック 176 | { 177 | a_serial.println("Error: EEPROM address out of range."); 178 | return false; 179 | } 180 | // 書き込みデータがEEPROM内のデータと違う場合のみ書き込みをセット 181 | EEPROM.write(i, 0); 182 | } 183 | a_serial.println("EEPROM zero format finished."); 184 | return true; 185 | } 186 | 187 | /// @brief EEPROMにEEPROM格納用の配列データを書き込む. 188 | /// @param a_data EEPROM書き込み用の配列データ. 189 | /// @param a_flg_protect EEPROMの書き込み許可があるかどうかのブール値. 190 | /// @param a_serial メッセージ表示用のハードウェアシリアル. 191 | /// @return EEPROMの書き込みと読み込みが成功した場合はtrueを, 書き込まなかった場合はfalseを返す. 192 | bool mrd_eeprom_write(UnionEEPROM a_data, bool a_flg_protect, Stream &a_serial) { 193 | if (a_flg_protect) { // EEPROM書き込み実施フラグをチェック 194 | return false; 195 | } 196 | if (flg.eeprom_protect) // config.hのEEPROM書き込みプロテクトをチェック 197 | { 198 | a_serial.println("EEPROM is protected. To unprotect, please set 'EEPROM_PROTECT' to false."); 199 | return false; 200 | } 201 | 202 | // EEPROM書き込み 203 | byte old_value_tmp; // EEPROMにすでに書き込んであるデータ 204 | bool flg_renew_tmp = false; // 書き込みコミットを実施するかのフラグ 205 | for (int i = 0; i < EEPROM_BYTE; i++) // データを書き込む時はbyte型 206 | { 207 | if (i >= EEPROM.length()) // EEPROMのサイズを超えないようチェック 208 | { 209 | a_serial.println("Error: EEPROM address out of range."); 210 | return false; 211 | } 212 | old_value_tmp = EEPROM.read(i); 213 | // 書き込みデータがEEPROM内のデータと違う場合のみ書き込みをセット 214 | if (old_value_tmp != a_data.bval[i]) { 215 | EEPROM.write(i, a_data.bval[i]); 216 | flg_renew_tmp = true; 217 | } 218 | } 219 | if (flg_renew_tmp) // 変更箇所があれば書き込みを実施 220 | { 221 | EEPROM.commit(); // 書き込みを確定にcommitがESP32では必要.Teensyでは不要, 222 | a_serial.print("Value updated "); 223 | return true; 224 | } else { 225 | a_serial.print("Same value "); 226 | } 227 | return false; 228 | } 229 | 230 | //------------------------------------------------------------------------------------ 231 | // EEPROM データ作成→書き込み 232 | //------------------------------------------------------------------------------------ 233 | 234 | /// @brief EEPROMにサーボの設定とデフォルト位置を保存する. 235 | /// @param a_data EEPROM書き込み用の配列データ. 236 | /// @param a_len_byte EEPROMの使用サイズ. 237 | /// @return UnionEEPROM のフォーマットで配列を返す. 238 | bool mrd_eeprom_set(UnionEEPROM a_data, int a_len_byte) // 239 | { 240 | if (flg.eeprom_set) { 241 | // a_data = mrd_eeprom_make_data_from_config(); 242 | 243 | // EEPROM書き込み 244 | for (int i = 0; i < a_len_byte; i++) // データを書き込む時はbyte型 245 | { 246 | EEPROM.write(i, a_data.bval[i]); 247 | } 248 | // Serial.println(); 249 | return true; 250 | } 251 | return false; 252 | } 253 | 254 | /// @brief EEPROM格納用の配列データをシリアルにダンプ出力する.(起動時用) 255 | /// @param a_len_byte EEPROMのサイズ(byte). 256 | /// @param a_flg_do 実施するか否か. 257 | /// @param a_bhd ダンプリストの表示形式.(0:Bin, 1:Hex, 2:Dec) 258 | /// @return 終了時にtrueを返す. 259 | bool mrd_eeprom_dump_at_boot(int a_len_byte, bool a_flg_do, int a_bhd, Stream &a_serial) { 260 | if (a_flg_do) { 261 | mrd_eeprom_dump_serial(mrd_eeprom_load(a_len_byte, a_serial), a_len_byte, a_bhd, a_serial); 262 | return true; 263 | } 264 | return false; 265 | } 266 | 267 | /// @brief EEPROMに設定値を書き込み, その後で読み込んで内容を確認し, シリアルポートに出力する. 268 | /// @param a_write_data EEPROM書き込み用の配列データ. 269 | /// @param a_a_flg_do EEPROMの読み書きチェックを実施するかのブール値. 270 | /// @param a_protect EEPROMの書き込み許可があるかどうかのブール値. 271 | /// @param a_bhd ダンプリストの表示形式.(0:Bin, 1:Hex, 2:Dec) 272 | /// @return EEPROMの書き込みと読み込みが成功した場合はtrueを, それ以外はfalseを返す. 273 | bool mrd_eeprom_write_read_check(UnionEEPROM a_write_data, int a_len_byte, bool a_flg_do, 274 | bool a_protect, int a_bhd, Stream &a_serial) { 275 | if (!a_flg_do) // EEPROMの読み書きチェックを実施するか 276 | { 277 | return false; 278 | } 279 | 280 | // EEPROM書き込みを実行 281 | a_serial.println("Try to write EEPROM: "); 282 | mrd_eeprom_dump_serial(a_write_data, a_len_byte, a_bhd, a_serial); // 書き込み内容をダンプ表示 283 | 284 | if (mrd_eeprom_write(a_write_data, a_protect, a_serial)) { 285 | a_serial.println("...Write OK."); 286 | } else { 287 | a_serial.println("...Write failed."); 288 | return false; 289 | } 290 | 291 | // EEPROM読み込みを実行 292 | a_serial.println("Read EEPROM: "); 293 | UnionEEPROM a_data_tmp; 294 | mrd_eeprom_load(a_len_byte, a_serial); 295 | mrd_eeprom_dump_serial(a_data_tmp, a_len_byte, a_bhd, a_serial); // 読み込み内容をダンプ表示 296 | a_serial.println("...Read completed."); 297 | 298 | return true; 299 | } 300 | 301 | //------------------------------------------------------------------------------------ 302 | // 各種オペレーション 303 | //------------------------------------------------------------------------------------ 304 | 305 | /// @brief EEPROMから任意のshort型データを読み込む. 306 | /// @param index_y 配列の一次元目(0~2). 307 | /// @param index_x 配列の二次元目(0~89). 308 | /// @return short型データを返す. 309 | short mrd_eeprom_load_short(int index_y, int index_x) { 310 | return short(EEPROM.read(index_y * 90 + index_x)); 311 | } 312 | 313 | /// @brief EEPROMから任意のbyte型データを読み込む. 314 | /// @param index_y 配列の一次元目(0~2). 315 | /// @param index_x 配列の二次元目(0~179). 316 | /// @param low_high 下位ビットか上位ビットか. (0:low_bit, 1:high_bit) 317 | /// @return byte型データを返す. 318 | int8_t mrd_eeprom_load_byte(int index_y, int index_x, int low_high) // 319 | { 320 | return int8_t(EEPROM.read(index_y * 180 + index_x * 2 + low_high)); 321 | } 322 | 323 | #endif // __MERIDIAN_EEPROM_H__ 324 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/main.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_MAIN_H__ 2 | #define __MERIDIAN_MAIN_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | 7 | #include // 9軸センサBNO055用 8 | #include 9 | #include // MPU6050用 10 | #include // Meridianのライブラリ導入 11 | #include // SDカードやSPI通信用 12 | #include // SPI通信Master用 13 | #include 14 | MERIDIANFLOW::Meridian mrd; // ライブラリのクラスを mrdという名前でインスタンス化 15 | 16 | #include // ICSサーボのインスタンス設定 17 | IcsHardSerialClass ics_L(&Serial2, PIN_EN_L, SERVO_BAUDRATE_L, SERVO_TIMEOUT_L); 18 | IcsHardSerialClass ics_R(&Serial3, PIN_EN_R, SERVO_BAUDRATE_R, SERVO_TIMEOUT_R); 19 | IcsHardSerialClass ics_C(&Serial1, PIN_EN_C, SERVO_BAUDRATE_C, SERVO_TIMEOUT_C); 20 | 21 | //------------------------------------------------------------------------------------ 22 | // 列挙型 23 | //------------------------------------------------------------------------------------ 24 | 25 | enum UartLine { // サーボ系統の列挙型(L,R,C) 26 | L, // Left 27 | R, // Right 28 | C // Center 29 | }; 30 | 31 | enum ServoType { // サーボプロトコルのタイプ 32 | NOSERVO = 0, // サーボなし 33 | PWM_S = 1, // Single PWM (WIP) 34 | PCA9685 = 11, // I2C_PCA9685 to PWM (WIP) 35 | FTBRSX = 21, // FUTABA_RSxTTL (WIP) 36 | DXL1 = 31, // DYNAMIXEL 1.0 (WIP) 37 | DXL2 = 32, // DYNAMIXEL 2.0 (WIP) 38 | KOICS3 = 43, // KONDO_ICS 3.5 / 3.6 39 | KOPMX = 44, // KONDO_PMX (WIP) 40 | JRXBUS = 51, // JRPROPO_XBUS (WIP) 41 | FTCSTS = 61, // FEETECH_STS (WIP) 42 | FTCSCS = 62 // FEETECH_SCS (WIP) 43 | }; 44 | 45 | enum ImuAhrsType { // 6軸9軸センサ種の列挙型(NO_IMU, MPU6050_IMU, MPU9250_IMU, BNO055_AHRS) 46 | NO_IMU = 0, // IMU/AHRS なし. 47 | MPU6050_IMU = 1, // MPU6050 48 | MPU9250_IMU = 2, // MPU9250(未設定) 49 | BNO055_AHRS = 3 // BNO055 50 | }; 51 | 52 | enum PadType { // リモコン種の列挙型(PC, MERIMOTE, BLUERETRO, SBDBT, KRR5FH) 53 | PC = 0, // PCからのPD入力情報を使用 54 | MERIMOTE = 1, // MERIMOTE(未導入) 55 | BLUERETRO = 2, // BLUERETRO(未導入) 56 | SBDBT = 3, // SBDBT(未導入) 57 | KRR5FH = 4, // KRR5FH 58 | WIIMOTE = 5 // WIIMOTE(未導入) 59 | }; 60 | 61 | enum BinHexDec { // 数値表示タイプの列挙型(Bin, Hex, Dec) 62 | Bin = 0, // BIN 63 | Hex = 1, // HEX 64 | Dec = 2, // DEC 65 | }; 66 | 67 | //------------------------------------------------------------------------------------ 68 | // 変数 69 | //------------------------------------------------------------------------------------ 70 | 71 | // システム用の変数 72 | const int MRDM_BYTE = MRDM_LEN * 2; // Meridim配列のバイト型の長さ 73 | const int MRD_ERR = MRDM_LEN - 2; // エラーフラグの格納場所（配列の末尾から2つめ） 74 | const int MRD_ERR_u = MRD_ERR * 2 + 1; // エラーフラグの格納場所（上位8ビット） 75 | const int MRD_ERR_l = MRD_ERR * 2; // エラーフラグの格納場所（下位8ビット） 76 | const int MRD_CKSM = MRDM_LEN - 1; // チェックサムの格納場所（配列の末尾） 77 | const int PAD_LEN = 5; // リモコン用配列の長さ 78 | 79 | //------------------------------------------------------------------------------------ 80 | // クラス・構造体・共用体 81 | //------------------------------------------------------------------------------------ 82 | 83 | // Meridim配列用の共用体(SPI送信時に末尾の4バイトが0になる不具合の対策として+4バイト) 84 | typedef union { 85 | short sval[MRDM_LEN + 4]; // short型で90個の配列データ 86 | unsigned short usval[MRDM_LEN + 2]; // 上記のunsigned short型 87 | uint8_t bval[MRDM_BYTE + 4]; // byte型で180個の配列データ 88 | uint8_t ubval[MRDM_BYTE + 4]; // 上記のunsigned byte型 89 | } Meridim90Union; 90 | Meridim90Union s_spi_meridim; // SPI送信用配列(short型, センサや角度は100倍値) 91 | Meridim90Union r_spi_meridim; // SPI受信用配列 92 | Meridim90Union s_spi_meridim_dma; // SPI送信DMA用配列 93 | Meridim90Union r_spi_meridim_dma; // SPI受信DMA用配列 94 | Meridim90Union s_spi_meridim_dummy; // SPI送信ダミーデータ用配列 95 | 96 | // フラグ管理用の構造体 97 | struct MrdFlags { 98 | bool imuahrs_available = true; // メインセンサ値を読み取る間, サブスレッドによる書き込みを待機 99 | bool udp_board_passive = false; // UDP通信の周期制御がボード主導(false) か, PC主導(true)か 100 | bool count_frame_reset = false; // フレーム管理時計をリセットする 101 | bool stop_board_during = false; // ボードの末端処理をmeridim[MRD_STOP_FRAMES]ms止める 102 | bool eeprom_write_mode = false; // EEPROMへの書き込みモード 103 | bool eeprom_read_mode = false; // EEPROMからの読み込みモード 104 | bool eeprom_protect = EEPROM_PROTECT; // EEPROMの書き込みプロテクト 105 | bool eeprom_load = EEPROM_LOAD; // 起動時にEEPROMの内容を読み込む 106 | bool eeprom_set = EEPROM_SET; // 起動時にEEPROMに規定値をセット 107 | bool sdcard_write_mode = false; // SDCARDへの書き込みモード 108 | bool sdcard_read_mode = false; // SDCARDからの読み込みモード 109 | bool wire0_init = false; // I2C 0系統の初期化合否 110 | bool wire1_init = false; // I2C 1系統の初期化合否 111 | bool bt_busy = false; // Bluetoothの受信中フラグ（UDPコンフリクト回避用） 112 | bool spi_trans = true; // ESP32とのSPI通信の実施 113 | bool spi_rcvd = true; // SPIのデータ受信判定 114 | bool udp_rcvd = false; // UDPのデータ受信判定 115 | bool udp_busy = false; // UDPスレッドでの受信中フラグ（送信抑制） 116 | bool meridim_rcvd = false; // Meridimが正しく受信できたか 117 | bool servoL_drive = false; // L系統のサーボの送受信 118 | bool servoR_drive = false; // R系統のサーボの送受信 119 | bool servoC_drive = false; // C系統のサーボの送受信 120 | bool meridim_special = false; // 特殊なMeridaim配列を優先送信 121 | }; 122 | MrdFlags flg; 123 | 124 | // シーケンス番号管理用の構造体 125 | struct MrdSq { 126 | int s_increment = 0; // フレーム毎に0-59999をカウントし, 送信 127 | int r_expect = 0; // フレーム毎に0-59999をカウントし, 受信値と比較 128 | }; 129 | MrdSq mrdsq; 130 | 131 | // タイマー管理用の構造体 132 | struct MrdTimer { 133 | long frame_ms = FRAME_DURATION; // 1フレームあたりの単位時間(ms) 134 | int count_loop = 0; // サイン計算用の循環カウンタ 135 | int count_loop_dlt = 2; // サイン計算用の循環カウンタを1フレームにいくつ進めるか 136 | int count_loop_max = 359999; // 循環カウンタの最大値 137 | int count_pad_interval = 0; // JOYPADのデータを読みに行くためのフレームカウント★ 138 | unsigned long count_frame = 0; // メインフレームのカウント 139 | }; 140 | MrdTimer tmr; 141 | 142 | // エラーカウント管理用の構造体 143 | struct MrdErr { 144 | int esp_pc = 0; // PCの受信エラー（ESP32からのUDP） 145 | int pc_esp = 0; // ESP32の受信エラー（PCからのUDP） 146 | int esp_tsy = 0; // Teensyの受信エラー（ESP32からのSPI） 147 | int tsy_esp = 0; // ESP32の受信エラー（TeensyからのSPI） 148 | int esp_skip = 0; // UDP→ESP受信のカウントの連番スキップ回数 149 | int tsy_skip = 0; // ESP→Teensy受信のカウントの連番スキップ回数 150 | int pc_skip = 0; // PC受信のカウントの連番スキップ回数 151 | }; 152 | MrdErr err; 153 | 154 | // リモコン値格納用の共用体 155 | typedef union { 156 | short sval[PAD_LEN]; // short型で4個の配列データを持つ 157 | uint16_t usval[PAD_LEN]; // 上記のunsigned short型 158 | int8_t bval[PAD_LEN * 2]; // 上記のbyte型 159 | uint8_t ubval[PAD_LEN * 2]; // 上記のunsigned byte型 160 | uint64_t ui64val[1]; // 上記のunsigned int16型 161 | // [0]button, [1]pad.stick_L_x:pad.stick_L_y, 162 | // [2]pad.stick_R_x:pad.stick_R_y, [3]pad.L2_val:pad.R2_val 163 | } PadUnion; 164 | PadUnion pad_array = {0}; // PAD値の格納用配列(一次転記) 165 | PadUnion pad_new = {0}; // PAD値の格納用配列(二次転記) 166 | PadUnion pad_i2c = {0}; // PAD値のi2c送受信用配列 167 | 168 | // リモコンのアナログ入力データ用の構造体 169 | struct PadValue { 170 | unsigned short stick_R = 0; 171 | int stick_R_x = 0; 172 | int stick_R_y = 0; 173 | unsigned short stick_L = 0; 174 | int stick_L_x = 0; 175 | int stick_L_y = 0; 176 | unsigned short stick_L2R2V = 0; 177 | int R2_val = 0; 178 | int L2_val = 0; 179 | }; 180 | PadValue pad_analog; 181 | 182 | // 6軸or9軸センサー用の構造体 183 | struct AhrsValue { 184 | Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire); // BNO055のインスタンス 185 | MPU6050 mpu6050; // MPU6050のインスタンス 186 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 187 | uint8_t devStatus; // return status after each device operation (0 = success, 188 | // !0 = error) 189 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 190 | uint8_t fifoBuffer[64]; // FIFO storage buffer 191 | Quaternion q; // [w, x, y, z] quaternion container 192 | VectorFloat gravity; // [x, y, z] gravity vector 193 | float ypr[3]; // [roll, pitch, yaw] roll/pitch/yaw container and gravity 194 | // vector 195 | float yaw_origin = 0; // ヨー軸の補正センター値 196 | float yaw_source = 0; // ヨー軸のソースデータ保持用 197 | float read[16]; // mpuから読み込んだ一次データ 198 | // acc_x,y,z,gyro_x,y,z,mag_x,y,z,gr_x,y,z,rpy_r,p,y,temp 199 | float zeros[16] = {0}; // リセット用 200 | float ave_data[16]; // 上記の移動平均値を入れる 201 | float result[16]; // 加工後の最新のmpuデータ（二次データ） 202 | float stock_data[IMUAHRS_STOCK][16]; // 上記の移動平均値計算用のデータストック 203 | int stock_count = 0; // 上記の移動平均値計算用のデータストックを輪番させる時の変数 204 | VectorInt16 aa; // [x, y, z] 加速度センサの測定値 205 | VectorInt16 gyro; // [x, y, z] 角速度センサの測定値 206 | VectorInt16 mag; // [x, y, z] 磁力センサの測定値 207 | long temperature; // センサの温度測定値 208 | }; 209 | AhrsValue ahrs; 210 | 211 | // サーボパラメータ用の構造体 212 | struct ServoParam { 213 | // サーボの最大接続 (サーボ送受信のループ処理数） 214 | int num_max; 215 | 216 | // 各サーボのマウントありなし(config.hで設定) 217 | int ixl_mount[IXL_MAX]; // L系統サーボのマウント有無 218 | int ixr_mount[IXR_MAX]; // R系統サーボのマウント有無 219 | int ixc_mount[IXC_MAX]; // C系統サーボのマウント有無 220 | 221 | // 各サーボのコード上のインデックスに対し, 実際に呼び出すハードウェアのID番号(config.hで設定) 222 | int ixl_id[IXL_MAX]; // L系統の実サーボ呼び出しID番号 223 | int ixr_id[IXR_MAX]; // R系統の実サーボ呼び出しID番号 224 | int ixc_id[IXC_MAX]; // C系統の実サーボ呼び出しID番号 225 | 226 | // 各サーボの正逆方向補正用配列(config.hで設定) 227 | int ixl_cw[IXL_MAX]; // L系統サーボの正逆 228 | int ixr_cw[IXR_MAX]; // R系統サーボの正逆 229 | int ixc_cw[IXC_MAX]; // C系統サーボの正逆 230 | 231 | // 各サーボの直立ポーズトリム値(config.hで設定) 232 | float ixl_trim[IXL_MAX]; // L系統サーボのトリム値 233 | float ixr_trim[IXR_MAX]; // R系統サーボのトリム値 234 | float ixc_trim[IXC_MAX]; // C系統サーボのトリム値 235 | 236 | // 各サーボのベンダーと型番(config.hで設定) 237 | float ixl_type[IXL_MAX]; // L系統 238 | float ixr_type[IXR_MAX]; // R系統 239 | float ixc_type[IXC_MAX]; // C系統 240 | 241 | // 各サーボのポジション値(degree) 242 | float ixl_tgt[IXL_MAX] = {0}; // L系統サーボの目標値 243 | float ixr_tgt[IXR_MAX] = {0}; // R系統サーボの目標値 244 | float ixc_tgt[IXC_MAX] = {0}; // C系統サーボの目標値 245 | float ixl_tgt_past[IXL_MAX] = {0}; // L系統サーボの前回の値 246 | float ixr_tgt_past[IXR_MAX] = {0}; // R系統サーボの前回の値 247 | float ixc_tgt_past[IXC_MAX] = {0}; // C系統サーボの前回の値 248 | 249 | // 各サーボの実行コマンド値(degree) 250 | float ixl_cmd[IXL_MAX] = {0}; // L系統サーボのコマンド 251 | float ixr_cmd[IXR_MAX] = {0}; // R系統サーボのコマンド 252 | float ixc_cmd[IXC_MAX] = {0}; // C系統サーボのコマンド 253 | 254 | // サーボのエラーカウンタ配列 255 | int ixl_err[IXL_MAX] = {0}; // L系統サーボのエラーカウンタ配列 256 | int ixr_err[IXR_MAX] = {0}; // R系統サーボのエラーカウンタ配列 257 | int ixc_err[IXC_MAX] = {0}; // C系統サーボのエラーカウンタ配列 258 | 259 | // サーボのコンディションステータス配列 260 | uint16_t ixl_stat[IXL_MAX] = {0}; // L系統サーボのコンディションステータス配列 261 | uint16_t ixr_stat[IXR_MAX] = {0}; // R系統サーボのコンディションステータス配列 262 | uint16_t ixc_stat[IXC_MAX] = {0}; // C系統サーボのコンディションステータス配列 263 | }; 264 | ServoParam sv; 265 | 266 | // モニタリング設定用の構造体 267 | struct MrdMonitor { 268 | bool frame_delay = MONITOR_FRAME_DELAY; // フレーム遅延時間を表示 269 | bool flow = MONITOR_FLOW; // フローを表示 270 | bool all_err = MONITOR_ERR_ALL; // 全経路の受信エラー率を表示 271 | bool servo_err = MONITOR_ERR_SERVO; // サーボエラーを表示 272 | bool seq_num = MONITOR_SEQ; // シーケンス番号チェックを表示 273 | bool pad = MONITOR_PAD; // リモコンのデータを表示 274 | }; 275 | 276 | MrdMonitor monitor; 277 | 278 | #include "mrd_disp.h" 279 | MrdMsgHandler mrd_disp(Serial); 280 | 281 | #include "mrd_sd.h" 282 | MrdSdHandler mrd_sd(Serial); 283 | 284 | //================================================================================================================ 285 | // 関 数 各 種 286 | //================================================================================================================ 287 | 288 | // 予約関数 289 | void mrd_countup_errs(); 290 | 291 | #endif //__MERIDIAN_MAIN_H__ 292 | -------------------------------------------------------------------------------- /Meridian_TWIN_ESP32/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Meridian_TWIN_for_ESP32 By Izumi Ninagawa & Meridian Project 2 | // MIT Licenced. 3 | // 4 | // Meridan TWIN ESP32用スクリプト 5 | // 20240804 第三次リファクタリング. モジュール化と表記法の共通化. 6 | // 20240804 mrd_writedim90の概念を導入. 7 | // 20240809 wiiリモコン, ヌンチャクに対応. Homeボタンでヌンチャクスティックのキャリブレーション. 8 | // 20240819 変数名をTWIN間で整合. EEPROM関連は調整中. 9 | // 20250505 トリム調整機能対応に合わせてコメントや変数名等微調整. 10 | 11 | #define VERSION "Meridian_TWIN_for_ESP32_v1.1.1_2025.05.05" // バージョン表示 12 | 13 | //================================================================================================================ 14 | // 初期設定 15 | //================================================================================================================ 16 | 17 | // ヘッダファイルの読み込み 18 | #include "main.h" 19 | #include "config.h" 20 | #include "keys.h" 21 | 22 | #include "mrd_bt_pad.h" 23 | #include "mrd_eeprom.h" 24 | #include "mrd_util.h" 25 | #include "mrd_wifi.h" 26 | 27 | // ライブラリ導入 28 | #include 29 | #include // DMAでSPI通信を高速化するめのライブラリ 30 | ESP32DMASPI::Slave slave; 31 | 32 | //================================================================================================================ 33 | // SETUP 34 | //================================================================================================================ 35 | void setup() { 36 | 37 | // シリアルモニターの設定 38 | Serial.begin(SERIAL_PC_BPS); 39 | // シリアルモニターの確立待ち 40 | unsigned long start_time = millis(); 41 | while (!Serial && (millis() - start_time < SERIAL_PC_TIMEOUT)) { // タイムアウトもチェック 42 | delay(1); 43 | } 44 | 45 | // 起動メッセージ表示 46 | mrd_disp.hello_twin_esp(VERSION, SERIAL_PC_BPS, SPI0_SPEED); 47 | 48 | // EEPROMの初期化 49 | // mrd_eeprom_init(EEPROM_BYTE); 50 | 51 | // EEPROMの内容のダンプ表示 52 | // mrd_eeprom_dump_at_boot(EEPROM_BYTE, EEPROM_DUMP, EEPROM_STYLE, Serial); 53 | 54 | // EEPROMのリードライトテスト 55 | // mrd_eeprom_write_read_check(mrd_eeprom_make_data_from_config(), EEPROM_BYTE, // 56 | // CHECK_EEPROM_RW, EEPROM_PROTECT, EEPROM_STYLE, Serial); 57 | 58 | // WiFiの初期化と開始 59 | mrd_disp.esp_wifi(WIFI_AP_SSID); 60 | if (mrd_wifi_init(udp, WIFI_AP_SSID, WIFI_AP_PASS, Serial)) { 61 | // wifiIPの表示 62 | mrd_disp.esp_ip(MODE_FIXED_IP, WIFI_SEND_IP, FIXED_IP_ADDR); 63 | } 64 | 65 | // SPI送受信用DMAの設定(バッファサイズは4の倍数, 末尾に4バイト分0が入る不具合対策で+4) 66 | s_spi_meridim_dma = slave.allocDMABuffer(MRDM_BYTE + 4); // DMAバッファ設定 67 | r_spi_meridim_dma = slave.allocDMABuffer(MRDM_BYTE + 4); // DMAバッファ設定 68 | 69 | // SPI送受信バッファをリセット 70 | memset(s_spi_meridim_dma, 0, MRDM_BYTE + 4); // ※+4は不具合対策 71 | memset(r_spi_meridim_dma, 0, MRDM_BYTE + 4); // ※+4は不具合対策 72 | 73 | // SPI通信の初回送信データをセット 74 | memset(s_spi_meridim.bval, 0, MRDM_BYTE + 4); // ※+4は不具合対策 75 | s_spi_meridim.sval[MRD_CKSM] = mrd.cksm_val(s_spi_meridim.sval, MRDM_LEN); // チェックサムを格納 76 | memcpy(s_spi_meridim_dma, s_spi_meridim.bval, MRDM_BYTE + 4); // 送信データをDMAバッファに転記 77 | 78 | // SPI通信の設定 79 | slave.setDataMode(SPI_MODE3); 80 | slave.setMaxTransferSize(MRDM_BYTE + 4); 81 | slave.setDMAChannel(2); // 専用メモリの割り当て(1か2のみ) 82 | slave.setQueueSize(1); // キューサイズ とりあえず1 83 | slave.begin(); // 引数を指定しなければデフォルトのSPI (SPI2,HSPIを利用) 84 | // ピン番号は CS: 15, CLK: 14, MOSI: 13, MISO: 12 85 | 86 | // Bluetoothの開始と表示(WIIMOTE) 87 | if (MOUNT_PAD == WIIMOTE) { // Bluetooth用スレッドの開始 88 | // BT接続確認用LED設定 89 | pinMode(PIN_LED_BT, OUTPUT); 90 | digitalWrite(PIN_LED_BT, HIGH); 91 | mrd_bt_settings(MOUNT_PAD, PAD_INIT_TIMEOUT, wiimote, PIN_LED_BT, Serial); 92 | xTaskCreatePinnedToCore(Core0_BT_r, "Core0_BT_r", 2048, NULL, 5, &thp[2], 0); 93 | } 94 | 95 | // 開始メッセージ 96 | mrd_disp.flow_start_twin_esp(); 97 | } 98 | 99 | //================================================================================================================ 100 | // MAIN LOOP 101 | //================================================================================================================ 102 | void loop() { 103 | 104 | //------------------------------------------------------------------------------------ 105 | // [ 1 ] UDP受信待受ループ (PC → ESP32) 106 | //------------------------------------------------------------------------------------ 107 | mrd.monitor_check_flow("[1]", monitor.flow); // 動作チェック用シリアル表示 108 | 109 | // @[1-1] UDPの受信待ち受けループ 110 | if (flg.udp_receive_mode) // UDPの受信実施フラグの確認 (モード確認) 111 | { 112 | unsigned long start_tmp = millis(); 113 | 114 | flg.udp_busy = true; // UDP使用中フラグをアゲる 115 | flg.udp_rcvd = false; // UDP受信完了フラグをサゲる 116 | while (!flg.udp_rcvd) { 117 | // UDP受信処理 118 | if (mrd_wifi_udp_receive(r_udp_meridim.bval, MRDM_BYTE, udp)) // 受信確認 119 | { 120 | flg.udp_rcvd = true; // UDP受信完了フラグをアゲる 121 | } 122 | 123 | // タイムアウト抜け処理 124 | unsigned long current_tmp = millis(); 125 | if (current_tmp - start_tmp >= UDP_TIMEOUT) { 126 | if (millis() > MONITOR_SUPPRESS_DURATION) { // 起動直後はエラー表示を抑制 127 | Serial.println("UDP timeout"); 128 | } 129 | flg.udp_rcvd = false; 130 | break; 131 | } 132 | delay(1); 133 | } 134 | } 135 | 136 | flg.udp_busy = false; // UDP使用中フラグをサゲる 137 | 138 | // @[1-end] この時点で r_udp_meridim にupdから届いた最新データが格納されている. 139 | 140 | //------------------------------------------------------------------------------------ 141 | // [ 2 ] UDP受信品質チェック (PC → ESP32) 142 | //------------------------------------------------------------------------------------ 143 | mrd.monitor_check_flow("[2]", monitor.flow); // 動作チェック用シリアル表示 144 | 145 | // @[2-1] UDP受信データ r_udp_meridim のチェックサムを確認. 146 | if (mrd.cksm_rslt(r_udp_meridim.sval, MRDM_LEN)) { 147 | 148 | // UDP受信配列から UDP送信配列にデータを転写 149 | memcpy(s_spi_meridim.bval, r_udp_meridim.bval, MRDM_BYTE + 4); 150 | 151 | // エラービット14番(ESP32のPCからのUDP受信エラー検出)をサゲる 152 | mrd_clear_bit16(s_spi_meridim.usval[MRD_ERR], ERRBIT_14_PC_ESP); 153 | 154 | } else { // チェックサムがNGならバッファから転記せず前回のデータを使用する 155 | 156 | // エラービット14番(ESP32のPCからのUDP受信エラー検出)をアゲる 157 | mrd_set_bit16(s_spi_meridim.usval[MRD_ERR], ERRBIT_14_PC_ESP); 158 | 159 | err.pc_esp++; 160 | } 161 | // @ この時点で最新データは s_spi_meridim に転記済み. 162 | 163 | // @[2-2] シーケンス番号チェック 164 | 165 | // シーケンス番号予想値の生成(カウントアップ) 166 | mrdsq.r_expect = mrd.seq_predict_num(mrdsq.r_expect); // 0〜59999をループ 167 | 168 | // シーケンス番号のシリアルモニタ表示 169 | mrd_disp.seq_number(mrdsq.r_expect, r_udp_meridim.usval[MRD_SEQ], monitor.seq_num); 170 | 171 | // @[2-3] シーケンス番号予想値と受信値が合致しているかのチェック 172 | 173 | // 受信シーケンス番号の値が予想通りなら, 174 | if (mrdsq.r_expect == r_udp_meridim.usval[MRD_SEQ]) { 175 | 176 | // エラービット10番[ESP受信のスキップ検出]をサゲる 177 | mrd_clear_bit16(s_spi_meridim.usval[MRD_ERR], ERRBIT_10_UDP_ESP_SKIP); 178 | flg.meridim_rcvd = true; // Meridim受信成功フラグをアゲる 179 | 180 | } else { // 受信シーケンス番号の値が予想と違ったら, 181 | 182 | // 今回の受信番号を予想結果としてキープ 183 | mrdsq.r_expect = r_udp_meridim.usval[MRD_SEQ]; 184 | 185 | // エラービット10番[ESP受信のスキップ検出]をアゲる 186 | mrd_set_bit16(s_spi_meridim.usval[MRD_ERR], ERRBIT_10_UDP_ESP_SKIP); 187 | 188 | // シーケンス番号が前回と同じでなければ, 189 | if (mrdsq.r_past != r_udp_meridim.usval[MRD_SEQ]) { 190 | err.esp_skip++; 191 | flg.meridim_rcvd = false; // Meridim受信成功フラグをサゲる 192 | } 193 | // シーケンス番号が前回と同じであれば, 受信スキップは起こしていないと判定するが, 194 | // Meridim受信成功フラグをサゲの状態にし, このフレームでの多重処理を防ぐ. 195 | } 196 | mrdsq.r_past = r_udp_meridim.usval[MRD_SEQ]; // 受信シーケンス番号をキープ 197 | 198 | // @[2-4] エラーリポートの表示 199 | mrd_disp.all_err(MONITOR_ERR_ALL, err); 200 | 201 | // @[2-end] この時点での最新データは s_spi_meridim. 202 | // チェックサムとシーケンス番号チェックに適合しなかった場合は前回のデータ. 203 | // エラーフラグを操作済みのため、s_spi_meridimのチェックサムは合わない. 204 | 205 | //------------------------------------------------------------------------------------ 206 | // [ 3 ] 受信値による処理 (ESP32) 207 | //------------------------------------------------------------------------------------ 208 | mrd.monitor_check_flow("[3]", monitor.flow); // 動作チェック用シリアル表示 209 | 210 | // @[3-1] マスターコマンドによる処理 211 | execute_master_command_from_PC(s_spi_meridim, flg.meridim_rcvd); 212 | 213 | // @[3-end] この時点でPCから受信したデータ(s_spi_meridim)に基づく処理が完了している. 214 | 215 | //------------------------------------------------------------------------------------ 216 | // [ 4 ] SPI送信データ作成 (ESP32 → Teensy) 217 | //------------------------------------------------------------------------------------ 218 | mrd.monitor_check_flow("[4]", monitor.flow); // 動作チェック用シリアル表示 219 | 220 | // @[4-1] ユーザー定義の送信データの書き込み 221 | // ・SPIに送るデータをここで作成, s_spi_meridim.svalに書き込み 222 | 223 | // @[4-2] リモコンデータの書き込み 224 | if (MOUNT_PAD > 0) { // リモコンがマウントされていれば 225 | 226 | // リモコンデータの読み込み 227 | // → WIIMOTEの場合は別スレッドにてpad_array を自動更新 228 | 229 | // リモコンの値をmeridimに格納する 230 | mrd_meriput90_pad(s_spi_meridim, pad_array, PAD_BUTTON_MARGE); 231 | } 232 | 233 | // @[4-3] フレームスキップ検出用のカウントを転記して格納 (PCからのカウントと同じ値をESPに転送) 234 | // → PCから受け取った値がs_spi_meridim.sval[MRD_SEQ]に入っているのでここでは何もしない 235 | 236 | // @[4-4] チェックサムの更新 237 | mrd_meriput90_cksm(s_spi_meridim); 238 | 239 | // @[4-end] ここで SPIに送信するs_spi_meridim のデータが完成. 240 | 241 | //------------------------------------------------------------------------------------ 242 | // [ 5 ] SPI送受信の実行(送信後に受信の待ち受け) (ESP32 → Teensy, Teensy → ESP32) 243 | //------------------------------------------------------------------------------------ 244 | mrd.monitor_check_flow("[5]", monitor.flow); // 動作チェック用シリアル表示 245 | 246 | flg.spi_rcvd = false; // SPI受信完了フラグをサゲる 247 | 248 | // SPIを送信し, ダミーではない実データを待ち受けるループ 249 | while (!flg.spi_rcvd) { 250 | 251 | // @[5-1] SPIのトランザクションが空ならデータをキューに装填 252 | if (slave.remained() == 0) { 253 | memcpy(s_spi_meridim_dma, s_spi_meridim.bval, MRDM_BYTE + 4); 254 | slave.queue(r_spi_meridim_dma, s_spi_meridim_dma, MRDM_BYTE + 4); 255 | } 256 | 257 | // @[5-2] SPIの送受信 258 | if (slave.available()) { // 送受信の完了待ち 259 | 260 | // DMAの受信データをtmp配列に転記 261 | memcpy(tmp_meridim.bval, r_spi_meridim_dma, MRDM_BYTE + 4); 262 | slave.pop(); // DMAのデータ配列の先頭を削除 263 | 264 | // 受信データが実データなら処理後に抜ける, ダミーデータなら次のデータを待つ 265 | if (tmp_meridim.sval[0] != MCMD_DUMMY_DATA) { 266 | 267 | // @[5-3] 受信したSPI実データのチェックサム 268 | if (mrd.cksm_rslt(tmp_meridim.sval, MRDM_LEN)) { 269 | 270 | // チェックサムがOKなら, DMAからUDP送信配列に転記 271 | memcpy(s_udp_meridim.bval, tmp_meridim.bval, MRDM_BYTE); 272 | // エラービット12番[ESP32のSPI受信エラー]をサゲる 273 | mrd_clear_bit16(s_udp_meridim.usval[MRD_ERR], ERRBIT_12_TSY_ESP); 274 | mrd_meriput90_cksm(s_udp_meridim); // チェックサムの更新 275 | flg.meridim_rcvd = true; // Meridim受信成功フラグをアゲる 276 | flg.spi_rcvd = true; // SPI受信完了フラグをアゲてループを抜ける 277 | 278 | } else { // チェックサムがNGなら, 前回の受信値を使用する 279 | // エラービット12番[ESP32のSPI受信エラー]をアゲる 280 | mrd_set_bit16(s_udp_meridim.usval[MRD_ERR], ERRBIT_12_TSY_ESP); 281 | mrd_meriput90_cksm(s_udp_meridim); // チェックサムの更新 282 | flg.meridim_rcvd = false; // Meridim受信成功フラグをサゲる 283 | flg.spi_rcvd = true; // SPI受信完了フラグをアゲてループを抜ける 284 | } 285 | } 286 | } 287 | delay(1); 288 | } 289 | 290 | // @[5-end] s_spi_meridimのSPIに送信が完了し,s_udp_meridimにチェック済みの受信データが格納される. 291 | // SPI受信失敗なら,前回のSPI受信データにエラーフラグとチェックサムを上書きしたものを格納. 292 | 293 | //------------------------------------------------------------------------------------ 294 | // [ 6 ] UDP送信データ作成 (ESP32 → PC) 295 | //------------------------------------------------------------------------------------ 296 | mrd.monitor_check_flow("[6]", monitor.flow); // 動作チェック用シリアル表示 297 | 298 | // @[6-1] 必要に応じてs_udp_meridimのシーケンス番号の確認をしてもよい 299 | // (略) 300 | 301 | // @[6-2] マスターコマンドによる処理 302 | execute_master_command_from_Tsy(s_udp_meridim, flg.meridim_rcvd); 303 | 304 | // @[6-3] このESP32内で計算処理したデータをs_udp_meridim.svalに格納する 305 | // ・Teensy→ESP32→PCという経路 306 | // ・Teensyからのリモコン値等はここでのみ補足可能 307 | 308 | // @[6-4] チェックサムの更新 309 | // mrd_meriput90_cksm(s_udp_meridim); 310 | 311 | // @[6-end] s_udp_meridimにPCに送信用の最新データが格納された状態. 312 | 313 | //------------------------------------------------------------------------------------ 314 | // [ 7 ] UDP送信実行 (ESP32 → PC) 315 | //------------------------------------------------------------------------------------ 316 | mrd.monitor_check_flow("[7]\n", monitor.flow); // デバグ用フロー表示 317 | 318 | // @[7-1] UDP送信の実行 319 | 320 | if (flg.udp_send_mode) // UDPの送信実施フラグの確認 (モード確認) 321 | { 322 | flg.udp_busy = true; // UDP使用中フラグをアゲる 323 | mrd_wifi_udp_send(s_udp_meridim.bval, MRDM_BYTE, udp); 324 | flg.udp_busy = false; // UDP使用中フラグをサゲる 325 | flg.udp_rcvd = false; // UDP受信完了フラグをサゲる 326 | } 327 | 328 | // @[7-end] s_udp_meridimをPCに送信完了. [ 1 ]のudpの受信待ち受けへ. 329 | } 330 | 331 | //================================================================================================================ 332 | // [ 関 数 各 種 333 | //================================================================================================================ 334 | 335 | /// @brief PCから受けたMaster Commandを実行する. 受信コマンドに基づき, 異なる処理を行う. 336 | /// @param a_meridim 実行したいコマンドの入ったMeridim配列を渡す. 337 | /// @param a_flg_exe Meridimの受信成功判定フラグを渡す. 338 | /// @return コマンドを実行した場合はtrue, しなかった場合はfalseを返す. 339 | bool execute_master_command_from_PC(Meridim90Union a_meridim, bool a_flg_exe) { 340 | switch (a_meridim.sval[MRD_MASTER]) { 341 | case MCMD_TEST_VALUE: // ダミーコード 342 | flg.test_value = true; // ダミーコード 343 | return true; 344 | 345 | default: 346 | return true; 347 | } 348 | return false; 349 | } 350 | 351 | /// @brief Teensyから受けたMaster Commandを実行する. 受信コマンドに基づき, 異なる処理を行う. 352 | /// @param a_meridim 実行したいコマンドの入ったMeridim配列を渡す. 353 | /// @param a_flg_exe Meridimの受信成功判定フラグを渡す. 354 | /// @return コマンドを実行した場合はtrue, しなかった場合はfalseを返す. 355 | bool execute_master_command_from_Tsy(Meridim90Union a_meridim, bool a_flg_exe) { 356 | switch (a_meridim.sval[MRD_MASTER]) { 357 | case MCMD_TEST_VALUE: // ダミーコード 358 | flg.test_value = true; // ダミーコード 359 | return true; 360 | 361 | default: 362 | return true; 363 | } 364 | return false; 365 | } 366 | -------------------------------------------------------------------------------- /Meridian_TWIN_Tsy40/src/mrd_eeprom.h: -------------------------------------------------------------------------------- 1 | #ifndef __MERIDIAN_EEPROM_H__ 2 | #define __MERIDIAN_EEPROM_H__ 3 | 4 | // ヘッダファイルの読み込み 5 | #include "config.h" 6 | #include "main.h" 7 | 8 | // ライブラリ導入 9 | #include 10 | 11 | //================================================================================================================ 12 | // EEPROM関連の処理 13 | //================================================================================================================ 14 | // 注:EEPROM.beginはESP32では必要だが, Teensyでは不要. 15 | 16 | // EEPROM読み書き用共用体 17 | typedef union { 18 | uint8_t bval[EEPROM_BYTE]; // 1バイト単位で540個のデータを持つ 19 | int16_t saval[3][90]; // short型で3*90個の配列データを持つ 20 | uint16_t usaval[3][90]; // unsigned short型で3*90個の配列データを持つ 21 | int16_t sval[270]; // short型で270個のデータを持つ 22 | uint16_t usval[270]; // unsigned short型で270個のデータを持つ 23 | } UnionEEPROM; 24 | UnionEEPROM eeprom_write_data; // EEPROM書き込み用 25 | UnionEEPROM eeprom_read_data; // EEPROM読み込み用 26 | 27 | //------------------------------------------------------------------------------------ 28 | // EEPROM 読み込み 29 | //------------------------------------------------------------------------------------ 30 | 31 | /// @brief EEPROMの内容を読み込んで返す. 32 | /// @param a_len_byte EEPROMの使用サイズ. 33 | /// @param a_serial メッセージ表示用のハードウェアシリアル. 34 | /// @return UnionEEPROM のフォーマットで配列を返す. 35 | UnionEEPROM mrd_eeprom_load(int a_len_byte, Stream &a_serial) { 36 | if (a_len_byte > EEPROM.length()) // EEPROMのサイズを超えないようチェック 37 | { 38 | a_len_byte = EEPROM.length(); 39 | a_serial.println("Error: EEPROM address out of range."); 40 | } 41 | 42 | UnionEEPROM data_tmp; 43 | for (int i = 0; i < a_len_byte; i++) // データを読み込む時はbyte型 44 | { 45 | data_tmp.bval[i] = EEPROM.read(i); 46 | } 47 | return data_tmp; 48 | } 49 | 50 | //------------------------------------------------------------------------------------ 51 | // EEPROM ダンプ出力 52 | //------------------------------------------------------------------------------------ 53 | 54 | /// @brief EEPROM格納用の配列データをシリアルにダンプ出力する. 55 | /// @param a_data EEPROM用の配列データ. 56 | /// @param a_len_byte EEPROMのサイズ(byte). 57 | /// @param a_bhd ダンプリストの表示形式.(0:Bin, 1:Hex, 2:Dec) 58 | /// @param a_serial メッセージ表示用のハードウェアシリアル. 59 | /// @return 終了時にtrueを返す. 60 | bool mrd_eeprom_dump_serial(UnionEEPROM a_data, int a_len_byte, int a_bhd, Stream &a_serial) { 61 | a_serial.print("EEPROM Length "); 62 | a_serial.print(EEPROM.length()); // EEPROMの長さ表示 63 | a_serial.print(", Used Length "); 64 | a_serial.print(a_len_byte); // EEPROMの使用サイズ表示(BYTE) 65 | a_serial.println("byte, 16bit Dump;"); 66 | int k = 0; 67 | char hex_tmp[5]; 68 | for (int i = 0; i < int(a_len_byte / 2); i++) // 読み込むデータはshort型で作成 69 | { 70 | k++; 71 | if (a_bhd == Bin) { 72 | // 16桁のビット列を表示 73 | for (int bit = 15; bit >= 0; bit--) { 74 | a_serial.print((a_data.usval[i] >> bit) & 1); 75 | } 76 | } else if (a_bhd == Hex) { 77 | sprintf(hex_tmp, "%04X", (uint16_t)a_data.sval[i]); // 符号なし16ビットにキャストして表示 78 | a_serial.print(hex_tmp); 79 | } else { 80 | a_serial.print(a_data.sval[i], DEC); 81 | } 82 | 83 | if (a_bhd == Bin) { 84 | if (k % 90 == 0) { 85 | a_serial.println(",\n"); 86 | k = 0; 87 | } else if (k % 20 == 0) { 88 | a_serial.println("/"); 89 | } else if (k % 5 == 0) { 90 | a_serial.println(); 91 | } else { 92 | a_serial.print(" "); 93 | } 94 | } else { 95 | if (k % 90 == 0) { 96 | a_serial.println(","); 97 | k = 0; 98 | } else if (k % 10 == 0) { 99 | a_serial.print((k % 20 == 0) ? "\n" : " "); 100 | } else { 101 | a_serial.print(" "); 102 | } 103 | } 104 | } 105 | return true; 106 | } 107 | 108 | //------------------------------------------------------------------------------------ 109 | // EEPROM データ作成 110 | //------------------------------------------------------------------------------------ 111 | 112 | /// @brief サーボ設定構造体からEEPROM格納用の配列データを作成する 113 | /// @param a_sv サーボ設定を保持する構造体 114 | /// @return EEPROM格納用の配列データ(UnionEEPROM型) 115 | UnionEEPROM mrd_eeprom_make_data_from_config(const ServoParam &a_sv) { 116 | UnionEEPROM array_tmp = {0}; 117 | 118 | for (int i = 0; i < 15; i++) { 119 | // 各サーボのマウント有無と方向(正転・逆転) 120 | uint16_t l_tmp = 0; 121 | uint16_t r_tmp = 0; 122 | 123 | // bit0 : マウント 124 | if (sv.ixl_mount[i]) 125 | l_tmp |= 0x0001; 126 | if (sv.ixr_mount[i]) 127 | r_tmp |= 0x0001; 128 | 129 | // bit1-7 : サーボ ID 130 | l_tmp |= static_cast(sv.ixl_id[i] & 0x7F) << 1; 131 | r_tmp |= static_cast(sv.ixr_id[i] & 0x7F) << 1; 132 | 133 | // bit8 : サーボ回転方向 (正転なら1, 逆転なら0) 134 | if (sv.ixl_cw[i] > 0) 135 | l_tmp |= 0x0100; 136 | if (sv.ixr_cw[i] > 0) 137 | r_tmp |= 0x0100; 138 | 139 | // サーボのマウント有無, ID, 回転方向のデータ格納 140 | array_tmp.saval[1][20 + i * 2] = l_tmp; 141 | array_tmp.saval[1][50 + i * 2] = r_tmp; 142 | 143 | // 各サーボの直立デフォルト角度(degree → float short*100)の格納 144 | array_tmp.saval[1][21 + i * 2] = mrd.float2HfShort(a_sv.ixl_trim[i]); 145 | array_tmp.saval[1][51 + i * 2] = mrd.float2HfShort(a_sv.ixr_trim[i]); 146 | } 147 | return array_tmp; 148 | } 149 | 150 | /// @brief 現在のサーボ位置からEEPROM格納用のトリムデータを作成し、EEPROM配列に入れる. 151 | /// @param a_data EEPROM書き込み用の配列データ. 152 | /// @param a_sv サーボパラメータの構造体. 153 | /// @return EEPROM格納用の配列データを返す. 154 | UnionEEPROM mrd_eeprom_make_trim_from_current_lite(UnionEEPROM a_data, ServoParam a_sv) { 155 | for (int i = 0; i < 15; i++) { 156 | // L系統サーボの直立デフォルト値 degree 157 | a_data.saval[1][21 + i * 2] = short(mrd.float2HfShort(a_sv.ixl_tgt[i] - a_sv.ixl_trim[i])); 158 | // R系統サーボの直立デフォルト値 degree 159 | a_data.saval[1][51 + i * 2] = short(mrd.float2HfShort(a_sv.ixr_tgt[i] - a_sv.ixr_trim[i])); 160 | }; 161 | return a_data; 162 | } 163 | 164 | //------------------------------------------------------------------------------------ 165 | // EEPROM 書き込み 166 | //------------------------------------------------------------------------------------ 167 | 168 | /// @brief EEPROMを0でフォーマットする 169 | /// @param a_flg_protect EEPROMの書き込み許可があるかどうかのブール値. 170 | /// @param a_len_byte EEPROMの使用サイズ. 171 | /// @param a_serial メッセージ表示用のハードウェアシリアル. 172 | /// @return EEPROMの書き込みと読み込みが成功した場合はtrueを, 書き込まなかった場合はfalseを返す. 173 | bool mrd_eeprom_zero_format(bool a_flg_protect, int a_len_byte, Stream &a_serial) { 174 | a_serial.println("Try to EEPROM zero format..."); 175 | 176 | if (a_flg_protect) { // EEPROM書き込み実施フラグをチェック 177 | return false; 178 | } 179 | if (flg.eeprom_protect) // config.hのEEPROM書き込みプロテクトをチェック 180 | { 181 | a_serial.println("EEPROM is protected. To unprotect, please set 'EEPROM_PROTECT' to false."); 182 | return false; 183 | } 184 | 185 | // EEPROM書き込み 186 | for (int i = 0; i < EEPROM_BYTE; i++) // データを書き込む時はbyte型 187 | { 188 | if (i >= EEPROM.length()) // EEPROMのサイズを超えないようチェック 189 | { 190 | a_serial.println("Error: EEPROM address out of range."); 191 | return false; 192 | } 193 | // 書き込みデータがEEPROM内のデータと違う場合のみ書き込みをセット 194 | EEPROM.write(i, 0); 195 | } 196 | a_serial.println("EEPROM zero format finished."); 197 | return true; 198 | } 199 | 200 | /// @brief EEPROMにEEPROM格納用の配列データを書き込む. 201 | /// @param a_data EEPROM書き込み用の配列データ. 202 | /// @param a_flg_protect EEPROMの書き込み許可があるかどうかのブール値. 203 | /// @param a_serial メッセージ表示用のハードウェアシリアル. 204 | /// @return EEPROMの書き込みと読み込みが成功した場合はtrueを, 書き込まなかった場合はfalseを返す. 205 | bool mrd_eeprom_write(UnionEEPROM a_data, bool a_flg_protect, Stream &a_serial) { 206 | if (a_flg_protect) { // EEPROM書き込み実施フラグをチェック 207 | return false; 208 | } 209 | if (flg.eeprom_protect) // config.hのEEPROM書き込みプロテクトをチェック 210 | { 211 | a_serial.println("EEPROM is protected. To unprotect, please set 'EEPROM_PROTECT' to false."); 212 | return false; 213 | } 214 | 215 | // EEPROM書き込み 216 | byte old_value_tmp; // EEPROMにすでに書き込んであるデータ 217 | bool flg_renew_tmp = false; // 書き込みコミットを実施するかのフラグ 218 | for (int i = 0; i < EEPROM_BYTE; i++) // データを書き込む時はbyte型 219 | { 220 | if (i >= EEPROM.length()) // EEPROMのサイズを超えないようチェック 221 | { 222 | a_serial.println("Error: EEPROM address out of range."); 223 | return false; 224 | } 225 | old_value_tmp = EEPROM.read(i); 226 | // 書き込みデータがEEPROM内のデータと違う場合のみ書き込みをセット 227 | if (old_value_tmp != a_data.bval[i]) { 228 | EEPROM.write(i, a_data.bval[i]); 229 | flg_renew_tmp = true; 230 | } 231 | } 232 | if (flg_renew_tmp) // 変更箇所があれば書き込みを実施 233 | { 234 | // EEPROM.commit(); // 書き込みを確定にcommitがESP32では必要.Teensyでは不要, 235 | a_serial.print("Value updated "); 236 | return true; 237 | } else { 238 | a_serial.print("Same value "); 239 | } 240 | return false; 241 | } 242 | 243 | //------------------------------------------------------------------------------------ 244 | // EEPROM データ作成→書き込み 245 | //------------------------------------------------------------------------------------ 246 | 247 | /// @brief EEPROMにサーボの設定とデフォルト位置を保存する. 248 | /// @param a_data EEPROM書き込み用の配列データ. 249 | /// @param a_len_byte EEPROMの使用サイズ. 250 | /// @return UnionEEPROM のフォーマットで配列を返す. 251 | bool mrd_eeprom_set(UnionEEPROM a_data, int a_len_byte) // 252 | { 253 | if (flg.eeprom_set) { 254 | // a_data = mrd_eeprom_make_data_from_config_lite(); 255 | 256 | // EEPROM書き込み 257 | for (int i = 0; i < a_len_byte; i++) // データを書き込む時はbyte型 258 | { 259 | EEPROM.write(i, a_data.bval[i]); 260 | } 261 | // Serial.println(); 262 | return true; 263 | } 264 | return false; 265 | } 266 | 267 | /// @brief EEPROM格納用の配列データをシリアルにダンプ出力する.(起動時用) 268 | /// @param a_len_byte EEPROMのサイズ(byte). 269 | /// @param a_flg_do 実施するか否か. 270 | /// @param a_bhd ダンプリストの表示形式.(0:Bin, 1:Hex, 2:Dec) 271 | /// @return 終了時にtrueを返す. 272 | bool mrd_eeprom_dump_at_boot(int a_len_byte, bool a_flg_do, int a_bhd, Stream &a_serial) { 273 | if (a_flg_do) { 274 | mrd_eeprom_dump_serial(mrd_eeprom_load(a_len_byte, a_serial), a_len_byte, a_bhd, a_serial); 275 | return true; 276 | } 277 | return false; 278 | } 279 | 280 | /// @brief EEPROMに設定値を書き込み, その後で読み込んで内容を確認し, シリアルポートに出力する. 281 | /// @param a_write_data EEPROM書き込み用の配列データ. 282 | /// @param a_a_flg_do EEPROMの読み書きチェックを実施するかのブール値. 283 | /// @param a_protect EEPROMの書き込み許可があるかどうかのブール値. 284 | /// @param a_bhd ダンプリストの表示形式.(0:Bin, 1:Hex, 2:Dec) 285 | /// @return EEPROMの書き込みと読み込みが成功した場合はtrueを, それ以外はfalseを返す. 286 | bool mrd_eeprom_write_read_check(UnionEEPROM a_write_data, int a_len_byte, bool a_flg_do, 287 | bool a_protect, int a_bhd, Stream &a_serial) { 288 | if (!a_flg_do) // EEPROMの読み書きチェックを実施するか 289 | { 290 | return false; 291 | } 292 | 293 | // EEPROM書き込みを実行 294 | a_serial.println("Try to write EEPROM: "); 295 | mrd_eeprom_dump_serial(a_write_data, a_len_byte, a_bhd, a_serial); // 書き込み内容をダンプ表示 296 | 297 | if (mrd_eeprom_write(a_write_data, a_protect, a_serial)) { 298 | a_serial.println("...Write OK."); 299 | } else { 300 | a_serial.println("...Write failed."); 301 | return false; 302 | } 303 | 304 | // EEPROM読み込みを実行 305 | a_serial.println("Read EEPROM: "); 306 | UnionEEPROM a_data_tmp; 307 | mrd_eeprom_load(a_len_byte, a_serial); 308 | mrd_eeprom_dump_serial(a_data_tmp, a_len_byte, a_bhd, a_serial); // 読み込み内容をダンプ表示 309 | a_serial.println("...Read completed."); 310 | 311 | return true; 312 | } 313 | 314 | /// @brief EEPROMの内容を読み込んで返す. 315 | /// @return UnionEEPROM のフォーマットで配列を返す. 316 | UnionEEPROM mrd_eeprom_read() { 317 | UnionEEPROM read_data_tmp = {0}; // ゼロ初期化を明示的に行う 318 | for (int i = 0; i < EEPROM_BYTE; i++) // データを読み込む時はbyte型 319 | { 320 | read_data_tmp.bval[i] = EEPROM.read(i); 321 | } 322 | return read_data_tmp; 323 | } 324 | 325 | /// @brief EEPROMの内容を読み込みサーボ値構造体に反映する. 326 | /// @param a_sv サーボ設定を保持する構造体. 327 | /// @param a_monitor シリアルモニタへのデータ表示. 328 | /// @param a_serial 出力先シリアルの指定. 329 | /// @return 終了時にtrueを返す. 330 | bool mrd_eeprom_load_servosettings(ServoParam &a_sv, bool a_monitor, Stream &a_serial) { 331 | a_serial.println("Load and set servo settings from EEPROM."); 332 | UnionEEPROM array_tmp = mrd_eeprom_read(); 333 | for (int i = 0; i < a_sv.num_max; i++) { 334 | // 各サーボのマウント有無 335 | a_sv.ixl_mount[i] = static_cast(array_tmp.saval[1][20 + i * 2] & 0x0001); // bit0:マウント有無 336 | a_sv.ixr_mount[i] = static_cast(array_tmp.saval[1][50 + i * 2] & 0x0001); // bit0:マウント有無 337 | // 各サーボの実サーボ呼び出しID番号 338 | a_sv.ixl_id[i] = static_cast(array_tmp.saval[1][20 + i * 2] >> 1 & 0x007F); // bit1–7:サーボID 339 | a_sv.ixr_id[i] = static_cast(array_tmp.saval[1][50 + i * 2] >> 1 & 0x007F); // bit1–7:サーボID 340 | // 各サーボの回転方向(正転・逆転) 341 | a_sv.ixl_cw[i] = static_cast((array_tmp.saval[1][20 + i * 2] >> 8) & 0x0001) ? 1 : -1; // bit8:回転方向 342 | a_sv.ixr_cw[i] = static_cast((array_tmp.saval[1][50 + i * 2] >> 8) & 0x0001) ? 1 : -1; // bit8:回転方向 343 | // 各サーボの直立デフォルト角度,トリム値(degree小数2桁までを100倍した値で格納されているものを展開) 344 | a_sv.ixl_trim[i] = array_tmp.saval[1][21 + i * 2] * 0.01f; 345 | a_sv.ixr_trim[i] = array_tmp.saval[1][51 + i * 2] * 0.01f; 346 | 347 | if (a_monitor) { 348 | a_serial.print("L-idx:"); 349 | a_serial.print(mrd_pddstr(i, 2, 0, false)); 350 | a_serial.print(", id:"); 351 | a_serial.print(mrd_pddstr(sv.ixl_id[i], 2, 0, false)); 352 | a_serial.print(", mt:"); 353 | a_serial.print(mrd_pddstr(sv.ixl_mount[i], 1, 0, false)); 354 | a_serial.print(", cw:"); 355 | a_serial.print(mrd_pddstr(sv.ixl_cw[i], 1, 0, true)); 356 | a_serial.print(", trm:"); 357 | a_serial.print(mrd_pddstr(sv.ixl_trim[i], 7, 2, true)); 358 | a_serial.print(" R-idx: "); 359 | a_serial.print(mrd_pddstr(i, 2, 0, false)); 360 | a_serial.print(", id:"); 361 | a_serial.print(mrd_pddstr(sv.ixr_id[i], 2, 0, false)); 362 | a_serial.print(", mt:"); 363 | a_serial.print(mrd_pddstr(sv.ixr_mount[i], 1, 0, false)); 364 | a_serial.print(", cw:"); 365 | a_serial.print(mrd_pddstr(sv.ixr_cw[i], 1, 0, true)); 366 | a_serial.print(", trm:"); 367 | a_serial.println(mrd_pddstr(sv.ixr_trim[i], 7, 2, true)); 368 | } 369 | } 370 | return true; 371 | } 372 | 373 | //------------------------------------------------------------------------------------ 374 | // 各種オペレーション 375 | //------------------------------------------------------------------------------------ 376 | 377 | /// @brief EEPROMから任意のshort型データを読み込む. 378 | /// @param index_y 配列の一次元目(0~2). 379 | /// @param index_x 配列の二次元目(0~89). 380 | /// @return short型データを返す. 381 | short mrd_eeprom_load_short(int index_y, int index_x) { 382 | return short(EEPROM.read(index_y * 90 + index_x)); 383 | } 384 | 385 | /// @brief EEPROMから任意のbyte型データを読み込む. 386 | /// @param index_y 配列の一次元目(0~2). 387 | /// @param index_x 配列の二次元目(0~179). 388 | /// @param low_high 下位ビットか上位ビットか. (0:low_bit, 1:high_bit) 389 | /// @return byte型データを返す. 390 | int8_t mrd_eeprom_load_byte(int index_y, int index_x, int low_high) // 391 | { 392 | return int8_t(EEPROM.read(index_y * 180 + index_x * 2 + low_high)); 393 | } 394 | 395 | #endif // __MERIDIAN_EEPROM_H__ 396 | --------------------------------------------------------------------------------