├── .gitignore ├── .travis.yml ├── .vscode ├── arduino.json └── extensions.json ├── README.md ├── include └── readme.txt ├── info ├── ActionFor2.1.txt ├── ActionTable.png ├── AddingSensor.txt ├── Alpha 1 系列PC通信协议.jpg ├── Alpha 1 系列蓝牙通信协议.pdf ├── Alpha 2.pdf ├── Alpha1系列蓝牙通讯协议（英文）.pdf ├── ESP8266_Download.png ├── EventHandling.txt ├── G2.txt ├── HaiLzd │ └── 舵机角度模式修改及偏差命令修改说明 │ │ ├── 2018.9.8.240度.默认保护.ID1.115200.无初始V1.49.up │ │ └── 舵机修改说明.txt ├── Info.xlsx ├── MP3-TF-16P使用说明书V1.6.pdf ├── MP3-TF-16P调试手册V1.6.pdf ├── PSX.txt ├── SPPIFFS_Files.txt ├── Serial-2_to_1.png ├── UBT-action │ ├── #小苹果.hts │ ├── 01-default-01.aesx │ ├── 02-Right_punch-03.aesx │ ├── 03-Left_punch-03.aesx │ ├── 04-turn_Right-05.aesx │ ├── 05-turn_Left-05.aesx │ ├── 07-stand_up_from_the_back-7.aesx │ ├── 08-move_Rightward-10.aesx │ ├── 09-move_Leftward-10.aesx │ ├── 10-move_back-14.aesx │ ├── 11-stand_up_from_front-13.aesx │ ├── 12-move_forward-20.aesx │ ├── 13-push_up-30.aesx │ ├── Left.dat │ ├── 前进.hts │ └── 江南style.hts ├── UBTECH_Servo.pdf ├── g2.xlsx ├── ps2解码通讯手册V1.3.doc └── 杭州海灵总线舵机协议.pdf ├── lib ├── Buffer │ ├── Buffer.cpp │ ├── Buffer.h │ └── Examples │ │ └── CheckBuffer │ │ └── CheckBuffer.ino ├── Dependency.txt ├── EventHandler │ ├── DummyData.cpp │ ├── EdsDrivers.h │ ├── EventData.cpp │ ├── EventData.h │ ├── EventDataSource.cpp │ ├── EventDataSource.h │ ├── EventHandler.cpp │ ├── EventHandler.h │ └── drivers │ │ ├── EdsBattery │ │ ├── EdsBattery.cpp │ │ └── EdsBattery.h │ │ ├── EdsMaze │ │ ├── EdsMaze.cpp │ │ └── EdsMaze.h │ │ ├── EdsMpu6050 │ │ ├── EdsMpu6050.cpp │ │ └── EdsMpu6050.h │ │ ├── EdsPsxButton │ │ ├── EdsPsxButton.cpp │ │ └── EdsPsxButton.h │ │ ├── EdsSonic │ │ ├── EdsSonic.cpp │ │ └── EdsSonic.h │ │ └── EdsTouch │ │ ├── EdsTouch.cpp │ │ └── EdsTouch.h ├── MP3TF16P │ ├── MP3TF16P.cpp │ └── MP3TF16P.h ├── MyData │ ├── MyData.cpp │ └── MyData.h ├── MyDebugger │ ├── MyDebugger.cpp │ └── MyDebugger.h ├── RobotServo │ ├── RobotServo.cpp │ ├── RobotServo.h │ ├── baseServo.cpp │ ├── baseServo.h │ └── drivers │ │ ├── HaiLzd │ │ ├── HLServo.cpp │ │ └── HLServo.h │ │ ├── UBT │ │ ├── UBTServo.cpp │ │ └── UBTServo.h │ │ └── _Sample_Servo │ │ ├── _Sample_Servo.cpp │ │ └── _Sample_Servo.h ├── SSBoard │ ├── SSBoard.cpp │ └── SSBoard.h ├── SimpleWiFiManager │ ├── SimpleWiFiManager.cpp │ ├── SimpleWiFiManager.h │ ├── SimpleWiFiManager_Handler.cpp │ ├── SimpleWiFiManager_Receiver.cpp │ ├── examples │ │ ├── WebServer │ │ │ └── webserver.ino │ │ ├── WiFiServer │ │ │ └── wifiserver.ino │ │ └── basic │ │ │ └── basic.ino │ ├── library.json │ └── library.properties ├── myUtil │ ├── myUtil.cpp │ └── myUtil.h └── readme.txt ├── platformio.ini └── src ├── ActionData.cpp ├── ActionData.h ├── ComboData.cpp ├── ComboData.h ├── Command_Generic.ino ├── EyeLed.h ├── EyeLed.ino ├── HAILZD_Command.ino ├── OTA.h ├── OTA.ino ├── RESULT.h ├── RobotConfig.cpp ├── RobotConfig.h ├── RobotControl.ino ├── RobotEventHandler.h ├── RobotEventHandler.ino ├── RobotMaintence.ino ├── SystemAction.ino ├── UBTBT_Command.ino ├── UBTCB_Command.ino ├── UBTSV_Command.ino ├── UBT_Common.ino ├── UTIL.h ├── UTIL.ino ├── V1_Command.ino ├── V2_CheckAction.ino ├── V2_Command.h ├── V2_Command.ino ├── message.h └── robot.h /.gitignore: -------------------------------------------------------------------------------- 1 | .pioenvs 2 | .piolibdeps 3 | .vscode/.browse.c_cpp.db* 4 | .vscode/.browse.vc.db 5 | .vscode/c_cpp_properties.json 6 | .vscode/launch.json 7 | .vscode/settings.json 8 | .vscode/ipch -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # Continuous Integration (CI) is the practice, in software 2 | # engineering, of merging all developer working copies with a shared mainline 3 | # several times a day < http://docs.platformio.org/page/ci/index.html > 4 | # 5 | # Documentation: 6 | # 7 | # * Travis CI Embedded Builds with PlatformIO 8 | # < https://docs.travis-ci.com/user/integration/platformio/ > 9 | # 10 | # * PlatformIO integration with Travis CI 11 | # < http://docs.platformio.org/page/ci/travis.html > 12 | # 13 | # * User Guide for `platformio ci` command 14 | # < http://docs.platformio.org/page/userguide/cmd_ci.html > 15 | # 16 | # 17 | # Please choice one of the following templates (proposed below) and uncomment 18 | # it (remove "# " before each line) or use own configuration according to the 19 | # Travis CI documentation (see above). 20 | # 21 | 22 | 23 | # 24 | # Template #1: General project. Test it using existing `platformio.ini`. 25 | # 26 | 27 | # language: python 28 | # python: 29 | # - "2.7" 30 | # 31 | # sudo: false 32 | # cache: 33 | # directories: 34 | # - "~/.platformio" 35 | # 36 | # install: 37 | # - pip install -U platformio 38 | # - platformio update 39 | # 40 | # script: 41 | # - platformio run 42 | 43 | 44 | # 45 | # Template #2: The project is intended to by used as a library with examples 46 | # 47 | 48 | # language: python 49 | # python: 50 | # - "2.7" 51 | # 52 | # sudo: false 53 | # cache: 54 | # directories: 55 | # - "~/.platformio" 56 | # 57 | # env: 58 | # - PLATFORMIO_CI_SRC=path/to/test/file.c 59 | # - PLATFORMIO_CI_SRC=examples/file.ino 60 | # - PLATFORMIO_CI_SRC=path/to/test/directory 61 | # 62 | # install: 63 | # - pip install -U platformio 64 | # - platformio update 65 | # 66 | # script: 67 | # - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N 68 | -------------------------------------------------------------------------------- /.vscode/arduino.json: -------------------------------------------------------------------------------- 1 | { 2 | "board": "esp8266:esp8266:d1", 3 | "configuration": "CpuFrequency=80,FlashSize=4M1M,LwIPVariant=v2mss536,Debug=Disabled,DebugLevel=None____,FlashErase=none,UploadSpeed=921600", 4 | "port": "COM9" 5 | } -------------------------------------------------------------------------------- /.vscode/extensions.json: -------------------------------------------------------------------------------- 1 | { 2 | // See http://go.microsoft.com/fwlink/?LinkId=827846 3 | // for the documentation about the extensions.json format 4 | "recommendations": [ 5 | "platformio.platformio-ide" 6 | ] 7 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RobotControl 2 | 3 | Arduino ESP8266 project for UBTech Alpha 1S Controller. 4 | 5 | Initial version 1.0 comes from the sample in my UBTech library. 6 | 7 | Versionn 2.0 will define a new set of communization protocol. 8 | In this version, it will support both V1 & V2 commands. 9 | 10 | 11 | ## Major changes in V2, V2.2, V2.3 12 | 13 | - New V2.0 communication protocol to avoid misinterpretation 14 | (some V1 commands are kept for bluetooth control) 15 | - Support control from USB, Bluetooth and Wifi (connect to router or AP mode) 16 | - Action setup by reading position from robot 17 | - Convert action from UBT action file in aesx and hts format 18 | - Play MP3 with action 19 | - Use MPU 6050 to detect current situation 20 | - Define the action to stand up after fall down for both face up & face down cases 21 | - Touch sensor and response for single, double and triple click, and long touch. 22 | - Define events via Blockly UI in MyAlphaRobot v2.2+ 23 | - Add Sub-System Bus for other device, e.g. Ultrasonic, PSX Control 24 | - Add USB-TTL mode to work as testing board for serial servos. 25 | - Option to set action speed 26 | - ...... 27 | 28 | 29 | ## Dependency 30 | 31 | - [OLED12864] library 32 | 33 | Already included in lib_deps. 34 | 35 | ## Installation 36 | 37 | This is a Visual Studio Code project with PlatformIO IDE. 38 | 39 | - Install [Visual Studio Code] 40 | - Install the PlatformIO IDE extension 41 | - Open the folder in Visual Studio Code 42 | 43 | ## Hardware 44 | 45 | - ESP8266 board 46 | - HC-05 Bluetooth module (optional) 47 | - MP3-TF-16P MP3 player (optional) 48 | - MPU6050 module (optional) 49 | - TTP223 touch sensor (optional) 50 | - SSD1306 OLED module (optional) 51 | - Sub-system boards (optional) 52 | 53 | ## Pin Assignment for ESP8266 Board 54 | 55 | | Pin | Usage | 56 | | ------ | ------ | 57 | | GPIO-2 | Connect to Rx of serial console for debug output | 58 | | GPIO-4 | Connect to SDA of I2C bus | 59 | | GPIO-5 | Connect to SCL of I2C bus | 60 | | GPIO-12 | Connect to signal pin of Servo | 61 | | GPIO-13 | Connect to Touch sensor | 62 | | GPIO-14 | Connect to Sub-System Bus (for PSX Control, Ultrasonic, ...) | 63 | | GPIO-15 | Connect to head led | 64 | | GPIO-16 | Connect to Rx of MP3 module | 65 | | Rx | Connect to Tx of HC-05 | 66 | | Tx | Connect to Rx of HC-05 | 67 | | I2C | for MPU6050 and OLED display | 68 | 69 | The PC setup program MyAlphaRobot can be found in https://github.com/Super169/MyAlphaRobotV2/releases/tag/MyAlphaRobot_2.3 70 | ![MyAlphaRobot](https://raw.githubusercontent.com/Super169/images/master/MyAlphaRobot/2.3/main-02-serial.png) 71 | 72 | The new version support event setting by Blockly UI. 73 | 74 | ![MyAlphaRobot](https://raw.githubusercontent.com/Super169/images/master/MyAlphaRobot/2.3/event_handler.png) 75 | 76 | And also online update firmware up to 921600bps, it takes less than 10 seconds to flash the firmware. 77 | 78 | ![MyAlphaRobot](https://raw.githubusercontent.com/Super169/images/master/MyAlphaRobot/2.3/burn-01.png) 79 | 80 | You need a ESP8266 control board in order to use this firmware. 81 | 82 | You can make a simple PCB like this, 83 | ![PCBLayout](https://raw.githubusercontent.com/Super169/images/master/RobotControlV2.0/PCB_v2_7.png) 84 | 85 | I just make one as below, I have removed the HC-05 as it can already controlled by smartphone via WiFi. 86 | ![MyPCB](https://raw.githubusercontent.com/Super169/images/master/RobotControlV2.0/MyPCB_3.png) 87 | 88 | 89 | and this is my testing environment for program development, including PSX Control and Ultrasonic Sensor. 90 | 91 | ![Robot](https://raw.githubusercontent.com/Super169/images/master/RobotControlV2.0/MyPCB_4.png) 92 | 93 | 94 | 95 | Or if you cannot make the PCB yourself, you can buy a ready to use control board from Taobo. 96 | https://item.taobao.com/item.htm?id=571368655206 97 | 98 | ![PCBLayout](https://raw.githubusercontent.com/Super169/images/master/RobotControlV2.0/TB-03.png) 99 | 100 | 101 | 102 | 103 | 104 | ENJOY! 105 | 106 | [UBTech]: 107 | [OLED12864]: 108 | [espsoftwareserial]: 109 | [Visual Studio Code]: 110 | -------------------------------------------------------------------------------- /include/readme.txt: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project header files. 3 | 4 | A header file is a file containing C declarations and macro definitions 5 | to be shared between several project source files. You request the use of a 6 | header file in your project source file (C, C++, etc) located in `src` folder 7 | by including it, with the C preprocessing directive `#include'. 8 | 9 | ```src/main.c 10 | 11 | #include "header.h" 12 | 13 | int main (void) 14 | { 15 | ... 16 | } 17 | ``` 18 | 19 | Including a header file produces the same results as copying the header file 20 | into each source file that needs it. Such copying would be time-consuming 21 | and error-prone. With a header file, the related declarations appear 22 | in only one place. If they need to be changed, they can be changed in one 23 | place, and programs that include the header file will automatically use the 24 | new version when next recompiled. The header file eliminates the labor of 25 | finding and changing all the copies as well as the risk that a failure to 26 | find one copy will result in inconsistencies within a program. 27 | 28 | In C, the usual convention is to give header files names that end with `.h'. 29 | It is most portable to use only letters, digits, dashes, and underscores in 30 | header file names, and at most one dot. 31 | 32 | Read more about using header files in official GCC documentation: 33 | 34 | * Include Syntax 35 | * Include Operation 36 | * Once-Only Headers 37 | * Computed Includes 38 | 39 | https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html 40 | -------------------------------------------------------------------------------- /info/ActionFor2.1.txt: -------------------------------------------------------------------------------- 1 | Changes in ActionData 2 | 3 | - Define AD_PBUFFER_SIZE (try using 10 first, most action is good enough) 4 | - MAX_POSE up to 65535 (use double byte for pose pointer) 5 | - AD_DATA_SIZE reduced from 12000 to 600 6 | - _data size 7 | 8 | - New variable: 9 | uint16_t _poseOffset 10 | 11 | Upload action header - reset pose offset 12 | _poseOffset = 0; 13 | 14 | Upload Pose 15 | put pose to (poseId - _poseOffset 16 | if (poseId = poseCnt - 1) { 17 | save to SPIFFS 18 | } 19 | if (poseId % pose_buffer_size) == 0 { 20 | Save to SPIFFS 21 | clear Pose data 22 | _poseOffset += pose_buffer_size 23 | } 24 | 25 | V2 -------------------------------------------------------------------------------- /info/ActionTable.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/ActionTable.png -------------------------------------------------------------------------------- /info/AddingSensor.txt: -------------------------------------------------------------------------------- 1 | 1) EventData add DEVICE 2 | 3 | 2) add #define 4 | ED_SIZE_???? 5 | ED_COUNT_???? 6 | 7 | 3) ED_MAX_DEVICE 8 | 9 | 4) private: 10 | _size 11 | _idCount 12 | 13 | 5) Add driver for new device 14 | 15 | 6) Update EdsDriver to includ new driver -------------------------------------------------------------------------------- /info/Alpha 1 系列PC通信协议.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/Alpha 1 系列PC通信协议.jpg -------------------------------------------------------------------------------- /info/Alpha 1 系列蓝牙通信协议.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/Alpha 1 系列蓝牙通信协议.pdf -------------------------------------------------------------------------------- /info/Alpha 2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/Alpha 2.pdf -------------------------------------------------------------------------------- /info/Alpha1系列蓝牙通讯协议（英文）.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/Alpha1系列蓝牙通讯协议（英文）.pdf -------------------------------------------------------------------------------- /info/ESP8266_Download.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/ESP8266_Download.png -------------------------------------------------------------------------------- /info/EventHandling.txt: -------------------------------------------------------------------------------- 1 | RobotEventHandler.ino 2 | 3 | - Data Collection 4 | eds[device]->GetData() 5 | 6 | 7 | EdsDataSource.h (generic) 8 | - IsReady: millis() > _nextReportMs 9 | 10 | _lastDataReady : data read for previous checking (can be few rounds before) 11 | _thisDataReady : data read for this round 12 | _lastReportMs : last value report time 13 | _lastValueHandled : indicate if last value has event handled 14 | 15 | 16 | 17 | /* 18 | * PostHandling (eventMatched, isRelated, pending) 19 | * - eventMatched : this round has event matched 20 | * - isRelated : the matched event is related to this device 21 | * - pending : some event pending on this data within threadhold: see EventHander.cpp 22 | * condition require to match for continous time before confirmed (e.g. Mpu6050) 23 | */ 24 | Default action: 25 | 1) with data but not checked : force check in next time 26 | 2) post handling delay : _delayCheckMs : 1000 27 | 4) pending : _pendingCheckMs : 20 28 | 5) no data or data not match : _continueCheckMs : 50 29 | // may consider to separate no data & data not match with different setting 30 | 31 | EdsMpu6050: 32 | edsMpu6050.Setup(EDS_MPU6050_I2CADDR, (1000 / config.mpuPositionCheckFreq()), config.mpuCheckFreq()); 33 | void EdsMpu6050::Setup(uint8_t i2cAddr, uint16_t threadhold, uint16_t elapseMs) { 34 | threadhold : (1000 / config.mpuPositionCheckFreq()) : sensitivity 35 | _continueCheckMs : config.mpuCheckFreq() 36 | 37 | 38 | EdsTouch: 39 | edsTouch.Setup(EDS_TOUCH_GPIO, config.touchDetectPeriod(), config.touchReleasePeriod()); 40 | void EdsTouch::Setup(uint8_t gpioPin, unsigned long touchDetectPeriod, unsigned long touchReleasePeriod) { 41 | ** Further study GetData for details 42 | 43 | EdsPsxButton: 44 | edsPsxButton.Setup(&ssb, config.psxCheckMs(), config.psxNoEventMs(), config.psxIgnoreRepeatMs()); 45 | void EdsPsxButton::Setup(SSBoard *ssb, uint8_t normalCheckMs, uint8_t noEventMs, uint16_t ignoreRepeatMs) { 46 | 47 | _lastReportValue : last value to be handled 48 | _noEventMs : elapse time if last value being checked 49 | _normalCheckMs : elapse time if last value is not checked 50 | _ignoreRepeatMs : elapse time to ignore repeated value 51 | 52 | 53 | _normalCheckMs : config.psxCheckMs() 54 | _noEventMs : config.psxNoEventMs() 55 | _ignoreRepeatMs : config.psxIgnoreRepeatMs() 56 | 57 | GetData() 58 | - Ignore if not change, and handled, and within ignoeRepeatMs 59 | (_prevDataRady && (_lastReportValue == data) && (_lastValueHandled) && ((millis() - _lastReportMS) < _ignoreRepeatMs)) 60 | 61 | PostHandling() 62 | - If !eventMatched || isRelated : no event match, or handled => _noEventMs 63 | * !isRelated can be blocked by other event 64 | - else => _normalCheckMs 65 | 66 | EdsSonic: 67 | edsSonic.Setup(&ssb); 68 | void EdsSonic::Setup(SSBoard *ssb) { 69 | 70 | * need to use threadHold? 71 | * 72 | 73 | EdsBattery: 74 | edsBattery.Setup(config.batteryMinValue(), config.batteryMaxValue(), config.batteryNormalSec() * 1000, config.batteryAlarmSec() * 1000); 75 | void EdsBattery::Setup(uint16_t minVoltage, uint16_t maxVoltage, uint16_t normalCheckMs, uint16_t alarmtIntervalMs) { 76 | 77 | threadHold : EBAT_THREADHOLD 78 | _delayCheckMs : config.batteryAlarmSec() * 1000 79 | _pendingCheckMs : _data->Threadhold(_Device) / 10 (i.e. check 10 times to confirm) 80 | _continueCheckMs : config.batteryNormalSec() * 1000 81 | 82 | -------------------------------------------------------------------------------- /info/G2.txt: -------------------------------------------------------------------------------- 1 | g1 0 - 180 2 | 3 | 01: 0 - 235 ( 90 - 130) : -130 ~ +105 4 | 02: 160 - 0 ( 90 - 70) : -70 ~ +90 5 | 03: 200 - 60 ( 90 - 140) : 140 6 | 7 | 04: 0 - 235 ( 90 - 110) : -130 ~ +105 8 | 05: 160 - 0 ( 90 - 90) : +70 ~ -90 9 | 06: 40 - 180 ( 90 - 100) : 140 10 | 11 | 07: 200 - 100 ( 90 - 120) : 110 : +80 ~ -20 12 | 08: 20 - 210 (120 - 120) : 190 : -100 ~ +90 13 | 09: 30 - 220 ( 60 - 120) : 190 : -90 ~ +100 14 | 10: 30 - 210 ( 90 - 120) : 180 : -90 ~ +90 15 | 11: 105 - 195 ( 90 - 120) : 90 : -15 ~ +75 16 | 17 | 12: 140 - 40 ( 90 - 120) : 100 : +20 ~ -80 18 | 13: 30 - 220 ( 60 - 120) : 190 : -90 ~ +100 19 | 14: 20 - 210 (120 - 120) : 190 : -100 ~ +90 20 | 15: 30 - 210 ( 90 - 120) : 180 : -90 ~ +90 21 | 16: 45 - 135 ( 90 - 120) : 90 : -75 ~ +15 -------------------------------------------------------------------------------- /info/HaiLzd/舵机角度模式修改及偏差命令修改说明/2018.9.8.240度.默认保护.ID1.115200.无初始V1.49.up: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/HaiLzd/舵机角度模式修改及偏差命令修改说明/2018.9.8.240度.默认保护.ID1.115200.无初始V1.49.up -------------------------------------------------------------------------------- /info/HaiLzd/舵机角度模式修改及偏差命令修改说明/舵机修改说明.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/HaiLzd/舵机角度模式修改及偏差命令修改说明/舵机修改说明.txt -------------------------------------------------------------------------------- /info/Info.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/Info.xlsx -------------------------------------------------------------------------------- /info/MP3-TF-16P使用说明书V1.6.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/MP3-TF-16P使用说明书V1.6.pdf -------------------------------------------------------------------------------- /info/MP3-TF-16P调试手册V1.6.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/MP3-TF-16P调试手册V1.6.pdf -------------------------------------------------------------------------------- /info/PSX.txt: -------------------------------------------------------------------------------- 1 | A8 8A 02 01 03 ED 2 | 3 | No connected: 4 | A8 8A 0B 01 00 41 5B FF FF FF FF FF FF A2 ED 5 | A8 8A 0B 01 00 41 5E FF FF FF FF FF FF A5 ED 6 | A8 8A 0B 01 00 61 5F FF FF FF FF FF FF C6 ED 7 | A8 8A 0B 01 00 41 5E FF FF FF FF FF FF A5 ED 8 | A8 8A 0B 01 00 61 7A FF FF FF FF FF FF E1 ED 9 | A8 8A 0B 01 00 61 5E FF FF FF FF FF FF C5 ED 10 | A8 8A 0B 01 00 61 5B FF FF FF FF FF FF C2 ED 11 | A8 8A 0B 01 00 41 5E FF FF FF FF FF FF A5 ED 12 | A8 8A 0B 01 00 41 5E FF FF FF FF FF FF A5 ED 13 | A8 8A 0B 01 00 61 5E FF FF FF FF FF FF C5 ED 14 | 15 | Connected 16 | A8 8A 0B 01 00 61 DA FF FF FF FF FF FF 41 ED 17 | A8 8A 0B 01 00 41 5B FF FF FF FF FF FF A2 ED 18 | A8 8A 0B 01 00 41 5F FF FF FF FF FF FF A6 ED 19 | A8 8A 0B 01 00 41 7B FF FF FF FF FF FF C2 ED 20 | A8 8A 0B 01 00 41 5F FF FF FF FF FF FF A6 ED 21 | A8 8A 0B 01 00 41 5B FF FF FF FF FF FF A2 ED 22 | A8 8A 0B 01 00 41 5E FF FF FF FF FF FF A5 ED 23 | A8 8A 0B 01 00 41 5E FF FF FF FF FF FF A5 ED 24 | A8 8A 0B 01 00 41 5E FF FF FF FF FF FF A5 ED 25 | A8 8A 0B 01 00 61 5A FF FF FF FF FF FF C1 ED 26 | -------------------- ** ** ----------------- 27 | 00 01 02 03 04 05 06 07 08 28 | 29 | L1: 30 | FF FB 31 | 1111 1111 1111 1011 32 | 33 | 34 | L2: 35 | FF FE 36 | 1111 1111 1111 1110 37 | 38 | R1: 39 | FF F7 40 | 1111 1111 1111 0111 41 | 42 | R2: 43 | FF FD 44 | 1111 1111 1111 1101 45 | 46 | Up: 47 | EF FF 48 | 1110 1111 1111 1111 49 | 50 | Down: 51 | BF FF 52 | 1011 1111 1111 1111 53 | 54 | Left: 55 | 7F FF 56 | 0111 1111 1111 1111 57 | 58 | Right: 59 | DF FF 60 | 1101 1111 1111 1111 61 | 62 | Triangle: 63 | FF EF 64 | 1111 1111 1110 1111 65 | 66 | Cross: 67 | FF BF 68 | 1111 1111 1011 1111 69 | 70 | Square: 71 | FF 7F 72 | 1111 1111 0111 1111 73 | 74 | Circle: 75 | FF DF 76 | 1111 1111 1101 1111 77 | 78 | 79 | Select: 80 | FE FF 81 | 1111 1110 1111 1111 82 | 83 | Start: 84 | F7 FF 85 | 1111 0111 1111 1111 86 | -------------------------------------------------------------------------------- /info/SPPIFFS_Files.txt: -------------------------------------------------------------------------------- 1 | Simple WiFi Manager Setup: /system/wifi.config 2 | Robot Configuration: /robot/config.dat 3 | Robot Actions: /alpha/action/???.dat 4 | Robot Events: /alpha/event/idle.event, /alpha/event/busy.event -------------------------------------------------------------------------------- /info/Serial-2_to_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/Serial-2_to_1.png -------------------------------------------------------------------------------- /info/UBT-action/#小苹果.hts: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/#小苹果.hts -------------------------------------------------------------------------------- /info/UBT-action/01-default-01.aesx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/01-default-01.aesx -------------------------------------------------------------------------------- /info/UBT-action/02-Right_punch-03.aesx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/02-Right_punch-03.aesx -------------------------------------------------------------------------------- /info/UBT-action/03-Left_punch-03.aesx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/03-Left_punch-03.aesx -------------------------------------------------------------------------------- /info/UBT-action/04-turn_Right-05.aesx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/04-turn_Right-05.aesx -------------------------------------------------------------------------------- /info/UBT-action/05-turn_Left-05.aesx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/05-turn_Left-05.aesx -------------------------------------------------------------------------------- /info/UBT-action/07-stand_up_from_the_back-7.aesx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/07-stand_up_from_the_back-7.aesx -------------------------------------------------------------------------------- /info/UBT-action/08-move_Rightward-10.aesx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/08-move_Rightward-10.aesx -------------------------------------------------------------------------------- /info/UBT-action/09-move_Leftward-10.aesx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/09-move_Leftward-10.aesx -------------------------------------------------------------------------------- /info/UBT-action/10-move_back-14.aesx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/10-move_back-14.aesx -------------------------------------------------------------------------------- /info/UBT-action/11-stand_up_from_front-13.aesx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/11-stand_up_from_front-13.aesx -------------------------------------------------------------------------------- /info/UBT-action/12-move_forward-20.aesx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/12-move_forward-20.aesx -------------------------------------------------------------------------------- /info/UBT-action/13-push_up-30.aesx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/13-push_up-30.aesx -------------------------------------------------------------------------------- /info/UBT-action/Left.dat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/Left.dat -------------------------------------------------------------------------------- /info/UBT-action/前进.hts: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/前进.hts -------------------------------------------------------------------------------- /info/UBT-action/江南style.hts: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBT-action/江南style.hts -------------------------------------------------------------------------------- /info/UBTECH_Servo.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/UBTECH_Servo.pdf -------------------------------------------------------------------------------- /info/g2.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/g2.xlsx -------------------------------------------------------------------------------- /info/ps2解码通讯手册V1.3.doc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/ps2解码通讯手册V1.3.doc -------------------------------------------------------------------------------- /info/杭州海灵总线舵机协议.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Super169/RobotControl/f78a1836d2cc601394d08369bb540d7717100f35/info/杭州海灵总线舵机协议.pdf -------------------------------------------------------------------------------- /lib/Buffer/Buffer.cpp: -------------------------------------------------------------------------------- 1 | #include "Buffer.h" 2 | 3 | Buffer::Buffer(uint16_t size) 4 | { 5 | init(size); 6 | } 7 | 8 | Buffer::~Buffer() { 9 | free(_buffer); 10 | } 11 | 12 | void Buffer::reset() { 13 | _head = _tail = 0; 14 | } 15 | 16 | void Buffer::init(uint16_t size) 17 | { 18 | _size = size; 19 | if (size == 0) return; 20 | _buffer = (byte *) malloc(size); 21 | reset(); 22 | } 23 | 24 | 25 | uint16_t Buffer::available() { 26 | if (!_size) return 0; 27 | int count = _tail - _head; 28 | if (count < 0) count += _size; 29 | return count; 30 | } 31 | 32 | byte Buffer::peek() { 33 | if ((!_size) || (_head == _tail)) return 0; 34 | return _buffer[_head]; 35 | } 36 | 37 | byte Buffer::peek(uint16_t pos) { 38 | if ((!_size) || (_head == _tail)) return 0; 39 | if (pos >= available()) return 0; 40 | int peekPos = (_head + pos) % _size; 41 | return _buffer[peekPos]; 42 | } 43 | 44 | bool Buffer::peek(byte *storage, uint16_t count) { 45 | if (available() < count) return false; 46 | byte *startPos = _buffer + _head; 47 | if (_tail > _head) { 48 | // read staight forward 49 | memcpy(storage, startPos, count); 50 | return true; 51 | } 52 | // part I, read to the end 53 | int size1 = (_size - _head); 54 | int size2 = count - size1; 55 | memcpy(storage, startPos, size1); 56 | 57 | startPos = storage + size1; 58 | memcpy(startPos, _buffer, size2); 59 | _head = size2; 60 | return true; 61 | } 62 | 63 | 64 | byte Buffer::read() { 65 | if ((!_size) || (_head == _tail)) return 0; 66 | byte data = _buffer[_head]; 67 | _head = (_head + 1) % _size; 68 | if (_head == _tail) reset(); // reset the point to reduce the chance of multiple read for block read 69 | return data; 70 | } 71 | 72 | bool Buffer::read(byte *storage, uint16_t count) { 73 | if (!peek(storage, count)) return false; 74 | _head = (_head + count) % _size; 75 | if (_head == _tail) reset(); // reset the point to reduce the chance of multiple read for block read 76 | return true; 77 | } 78 | 79 | bool Buffer::skip(uint16_t count) { 80 | if (available() < count) return false; 81 | _head = (_head + count) % _size; 82 | if (_head == _tail) reset(); 83 | return true; 84 | } 85 | 86 | bool Buffer::write(byte data) { 87 | if (!_size) return false; 88 | int newTail = (_tail + 1) % _size; 89 | if (newTail == _head) { 90 | // buffer full, need to do somehting 91 | return false; 92 | } 93 | _buffer[_tail] = data; 94 | _tail = newTail; 95 | return true; 96 | } -------------------------------------------------------------------------------- /lib/Buffer/Buffer.h: -------------------------------------------------------------------------------- 1 | #ifndef _BUFFER_H_ 2 | #define _BUFFER_H_ 3 | 4 | #include 5 | 6 | class Buffer { 7 | public: 8 | Buffer(uint16_t size = 0); 9 | ~Buffer(); 10 | void init(uint16_t size); 11 | void reset(); 12 | uint16_t available(); 13 | byte peek(); 14 | byte peek(uint16_t ptr); 15 | bool peek(byte *storage, uint16_t count); 16 | byte write(); 17 | byte read(); 18 | bool read(byte *storage, uint16_t count); 19 | bool skip() { return skip(1); } 20 | bool skip(uint16_t count); 21 | bool write(byte data); 22 | 23 | int head() { return _head; } 24 | int tail() { return _tail; } 25 | 26 | private: 27 | uint16_t _size; 28 | byte *_buffer; 29 | int _head; 30 | int _tail; 31 | 32 | }; 33 | 34 | #endif -------------------------------------------------------------------------------- /lib/Buffer/Examples/CheckBuffer/CheckBuffer.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Buffer.h" 3 | 4 | Buffer buffer(64); 5 | 6 | void setup() { 7 | delay(1000); 8 | Serial.begin(115200); 9 | 10 | if (!buffer.available()) Serial.println(F("Buffer empty")); 11 | 12 | for (int i = 0; i < 100; i++) buffer.write(i); 13 | 14 | Serial.print("\nData in buffer: "); 15 | Serial.println(buffer.available()); 16 | 17 | Serial.print("\nRead data: "); 18 | Serial.println(buffer.read());; 19 | Serial.print("Data in buffer: "); 20 | Serial.println(buffer.available()); 21 | 22 | Serial.print("\nPeek data: "); 23 | Serial.println(buffer.peek());; 24 | Serial.print("Data in buffer: "); 25 | Serial.println(buffer.available()); 26 | 27 | byte data[50]; 28 | if (buffer.read(data, 50)) { 29 | Serial.print("\nRead data: "); 30 | ShowData(data, 50); 31 | Serial.print("Data in buffer: "); 32 | Serial.println(buffer.available()); 33 | } 34 | 35 | if (buffer.peek(data, 50)) { 36 | Serial.print("\nPeek data: "); 37 | ShowData(data, 50); 38 | Serial.print("Data in buffer: "); 39 | Serial.println(buffer.available()); 40 | } 41 | 42 | if (buffer.read(data, 50)) { 43 | Serial.print("\nAnother 50 data read"); 44 | } else { 45 | Serial.print("\nNot enough data for peek"); 46 | } 47 | 48 | if (buffer.peek(data, 50)) { 49 | Serial.print("\nAnother 50 data peek"); 50 | } else { 51 | Serial.print("\nNot enough data for peek"); 52 | } 53 | 54 | 55 | for (int i = 0; i < 100; i++) buffer.write(i); 56 | 57 | Serial.print("\nData in buffer: "); 58 | Serial.println(buffer.available()); 59 | 60 | Serial.print("\nPeek data: "); 61 | Serial.println(buffer.peek());; 62 | Serial.print("Data in buffer: "); 63 | Serial.println(buffer.available()); 64 | 65 | Serial.print("\nRead data: "); 66 | Serial.println(buffer.read());; 67 | Serial.print("Data in buffer: "); 68 | Serial.println(buffer.available()); 69 | 70 | if (buffer.peek(data, 50)) { 71 | Serial.print("\nPeek data: "); 72 | ShowData(data, 50); 73 | Serial.print("Data in buffer: "); 74 | Serial.println(buffer.available()); 75 | } 76 | 77 | if (buffer.read(data, 50)) { 78 | Serial.print("\nRead data: "); 79 | ShowData(data, 50); 80 | Serial.print("Data in buffer: "); 81 | Serial.println(buffer.available()); 82 | } 83 | 84 | Serial.print("\nHead: "); 85 | Serial.print(buffer.head()); 86 | Serial.print(" ; Tail: "); 87 | Serial.print(buffer.tail()); 88 | Serial.print(" => available: "); 89 | Serial.print(buffer.available()); 90 | 91 | int count = buffer.available(); 92 | if (buffer.read(data, count)) { 93 | Serial.print("\nRead all data out!"); 94 | ShowData(data, count); 95 | Serial.print("Data in buffer: "); 96 | Serial.print("\nData in buffer: "); 97 | Serial.println(buffer.available()); 98 | } 99 | 100 | Serial.print("\nHead: "); 101 | Serial.print(buffer.head()); 102 | Serial.print(" ; Tail: "); 103 | Serial.print(buffer.tail()); 104 | Serial.print(" => available: "); 105 | Serial.print(buffer.available()); 106 | 107 | } 108 | 109 | void loop() { 110 | 111 | } 112 | 113 | void ShowData(byte *data, uint16_t count) { 114 | for (int i = 0; i < count; i++) { 115 | Serial.print(data[i]); 116 | Serial.print(" "); 117 | } 118 | Serial.println(); 119 | } 120 | 121 | -------------------------------------------------------------------------------- /lib/Dependency.txt: -------------------------------------------------------------------------------- 1 | Already included in lib_deps of platformio.txt: 2 | 3 | OLED12864=https://github.com/Super169/OLED12864/archive/RobotControl_2.0.zip 4 | 5 | 6 | -------------------------------------------------------------------------------- /lib/EventHandler/DummyData.cpp: -------------------------------------------------------------------------------- 1 | #include "EventHandler.h" 2 | 3 | void EventHandler::LoadDummyData() { 4 | _evtCount = 10; 5 | uint8_t evt[10][12] = { 6 | {0x00, 0x02, 0x04, 0x00, 0x00, 0x03, 0x90, 0x01, 0x00, 0x00, 0x00, 0x00}, 7 | {0x00, 0x01, 0x01, 0x00, 0x02, 0x03, 0xC0, 0xC7, 0x01, 0x0F, 0x00, 0x00}, 8 | {0x00, 0x01, 0x01, 0x00, 0x02, 0x03, 0xC0, 0xC7, 0x01, 0x05, 0x00, 0x00}, 9 | {0x00, 0x01, 0x01, 0x00, 0x02, 0x02, 0x40, 0x38, 0x01, 0x06, 0x00, 0x00}, 10 | {0x00, 0x01, 0x02, 0x00, 0x00, 0x01, 0x01, 0x00, 0x01, 0x0B, 0x00, 0x00}, 11 | {0x00, 0x01, 0x02, 0x00, 0x00, 0x01, 0x02, 0x00, 0x01, 0x0C, 0x00, 0x00}, 12 | {0x00, 0x01, 0x02, 0x00, 0x00, 0x01, 0x03, 0x00, 0x01, 0x0D, 0x00, 0x00}, 13 | {0x00, 0x01, 0x02, 0x00, 0x00, 0x01, 0x04, 0x00, 0x01, 0x0E, 0x00, 0x00}, 14 | {0x00, 0x01, 0x05, 0x00, 0x00, 0x03, 0x0A, 0x00, 0x04, 0x00, 0x02, 0x00}, 15 | {0x00, 0x01, 0x05, 0x00, 0x00, 0x03, 0x1E, 0x00, 0x04, 0x00, 0x03, 0x00} 16 | }; 17 | 18 | size_t size = _evtCount * sizeof(EVENT); 19 | _events = (EVENT *) malloc(size); 20 | memcpy((void *) _events, (void *) evt, size); 21 | 22 | Serial.printf("Data dump of _events[%d]:\n", _evtCount); 23 | for (int i = 0; i < _evtCount; i++) { 24 | for (int j = 0; j < 12; j++) { 25 | Serial.printf("%02X ", _events[i].buffer[j]); 26 | } 27 | Serial.printf("\n"); 28 | } 29 | 30 | CheckEventsRequirement(); 31 | 32 | } 33 | 34 | 35 | -------------------------------------------------------------------------------- /lib/EventHandler/EdsDrivers.h: -------------------------------------------------------------------------------- 1 | #ifndef _EDS_DRIVERS_H_ 2 | #define _EDS_DRIVERS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #endif 12 | -------------------------------------------------------------------------------- /lib/EventHandler/EventData.cpp: -------------------------------------------------------------------------------- 1 | #include "EventData.h" 2 | 3 | 4 | EventData::EventData() 5 | { 6 | // DevOffset - location of starting point of device, flag has 1 byte only 7 | // offset - location of starting point of data, dat can have multiple bytes 8 | _devCount = 0; 9 | _dataSize = 0; 10 | for (int i = 0; i <= ED_MAX_DEVICE; i++) { 11 | _offset[i] = _dataSize; 12 | _devOffset[i] = _devCount; 13 | _dataSize += _size[i] * _idCount[i]; 14 | _devCount += _idCount[i]; 15 | } 16 | _ready = (bool *) malloc(_dataSize); 17 | _threadhold = (uint16_t *) malloc(_devCount * sizeof(uint16_t)); 18 | Clear(); 19 | } 20 | 21 | EventData::~EventData() 22 | { 23 | } 24 | 25 | void EventData::Clear() { 26 | for (int i = 0; i < ED_DATA_SIZE; i++) { 27 | _data[i] = ED_INVALID_DATA; 28 | } 29 | // for safety, do not use memset as data type may changed to multiple bytes later 30 | for (int i = 0; i < _dataSize; i++) { 31 | _ready[i] = false; 32 | } 33 | for (int i = 0; i < _devCount; i++) { 34 | _threadhold[i] = 0; 35 | } 36 | } 37 | 38 | bool EventData::IsValid(uint8_t device, uint8_t devId, uint8_t target) { 39 | if (device > ED_MAX_DEVICE) return false; 40 | if (devId > MaxId(device)) return false; 41 | return (target < _size[device]); 42 | } 43 | 44 | bool EventData::IsReady(uint8_t device, uint8_t devId, uint8_t target) { 45 | if (!IsValid(device, devId, target)) return false; 46 | return _ready[Offset(device, devId, target)]; 47 | } 48 | 49 | bool EventData::MarkReady(uint8_t device, uint8_t devId, uint8_t target, bool ready) { 50 | if (!IsValid(device, devId, target)) return false; 51 | _ready[Offset(device, devId, target)] = ready; 52 | return true; 53 | } 54 | 55 | uint8_t EventData::Offset(uint8_t device, uint8_t devId, uint8_t target) { 56 | // no need to validate for private function 57 | return _offset[device] + devId * _size[device] + target; 58 | } 59 | 60 | uint8_t EventData::DevOffset(uint8_t device, uint8_t devId) { 61 | // no need to validate for private function 62 | return _devOffset[device] + devId; 63 | } 64 | 65 | uint8_t EventData::IdCount(uint8_t device) { 66 | if (device > ED_MAX_DEVICE) return 0; 67 | return _idCount[device]; 68 | } 69 | 70 | uint8_t EventData::MaxId(uint8_t device) { 71 | if (device > ED_MAX_DEVICE) return 0; 72 | return _idCount[device] - 1; 73 | } 74 | 75 | bool EventData::SetThreadhold(uint8_t device, uint8_t devId, uint16_t threadhold) { 76 | if (!IsValid(device, devId)) return false; 77 | _threadhold[DevOffset(device, devId)] = threadhold; 78 | return true; 79 | } 80 | 81 | uint16_t EventData::Threadhold(uint8_t device, uint8_t devId) { 82 | if (!IsValid(device, devId)) return 0; 83 | return _threadhold[DevOffset(device, devId)]; 84 | } 85 | 86 | uint8_t EventData::DeviceDataSize(uint8_t device) { 87 | if (device > ED_MAX_DEVICE) return 0; 88 | return _size[device]; 89 | } 90 | 91 | 92 | bool EventData::SetData(uint8_t device, uint8_t devId, uint8_t target, int16_t value) { 93 | if (!IsValid(device, devId, target)) return false; 94 | uint8_t offset = Offset(device, devId, target); 95 | _data[offset] = value; 96 | MarkReady(device, devId, target, true); 97 | return true; 98 | } 99 | 100 | int16_t EventData::GetData(uint8_t device, uint8_t devId, uint8_t target) { 101 | if (!IsReady(device, devId, target)) return ED_INVALID_DATA; 102 | uint8_t offset = Offset(device, devId, target); 103 | return _data[offset]; 104 | } 105 | 106 | 107 | void EventData::DumpData(Stream *output) { 108 | // Hard to read if just dump the array. 109 | output->printf("\nEventData::DumpData:\n"); 110 | uint8_t device, devCount; 111 | 112 | output->printf("Max Device: %d\n", ED_MAX_DEVICE); 113 | for (int i = 0; i <= ED_MAX_DEVICE; i++) { 114 | output->printf("%d: %d, %d, [ %d , %d ], [ %d , %d ]\n", 115 | i, _size[i], _idCount[i], 116 | _offset[i], Offset(i,0,0), 117 | _devOffset[i], DevOffset(i, 0)); 118 | } 119 | output->printf("\n"); 120 | 121 | device = (uint8_t) DEVICE::mpu; 122 | devCount = _idCount[device]; 123 | output->printf("\nMPU (device: %d x %d): \n", device, devCount); 124 | for (int id = 0; id < devCount; id++) { 125 | output->printf("-- Id: %d => ", id); 126 | ShowValue(output, Offset(device, id, 0), 0); 127 | ShowValue(output, Offset(device, id, 1), 0); 128 | ShowValue(output, Offset(device, id, 2), 0); 129 | output->printf("\n"); 130 | } 131 | 132 | device = (uint8_t) DEVICE::touch; 133 | devCount = _idCount[device]; 134 | output->printf("\nTouch (device: %d x %d): \n", device, devCount); 135 | for (int id = 0; id < devCount; id++) { 136 | output->printf("-- Id: %d => ", id); 137 | ShowValue(output, Offset(device, id, 0), 0); 138 | output->printf("\n"); 139 | } 140 | 141 | device = (uint8_t) DEVICE::psx_button; 142 | devCount = _idCount[device]; 143 | output->printf("\nPSX (device: %d x %d): \n", device, devCount); 144 | for (int id = 0; id < devCount; id++) { 145 | output->printf("-- Id: %d => ", id); 146 | ShowValue(output, Offset(device, id, 0), 2); 147 | output->printf("\n"); 148 | } 149 | 150 | device = (uint8_t) DEVICE::battery; 151 | devCount = _idCount[device]; 152 | output->printf("\nBattery (device: %d x %d): \n", device, devCount); 153 | for (int id = 0; id < devCount; id++) { 154 | output->printf("-- Id: %d => ", id); 155 | ShowValue(output, Offset(device, id, 0), 0); 156 | ShowValue(output, Offset(device, id, 1), 0); 157 | output->printf("\n"); 158 | } 159 | 160 | device = (uint8_t) DEVICE::sonic; 161 | devCount = _idCount[device]; 162 | output->printf("\nSonic (device: %d x %d): \n", device, devCount); 163 | for (int id = 0; id < devCount; id++) { 164 | output->printf("-- Id: %d => ", id); 165 | ShowValue(output, Offset(device, id, 0), 0); 166 | output->printf("\n"); 167 | } 168 | 169 | output->printf("\n\n"); 170 | } 171 | 172 | void EventData::DumpData(Stream *output, uint8_t device) { 173 | } 174 | 175 | 176 | void EventData::ShowValue(Stream *output, uint8_t idx, uint8_t mode) { 177 | uint16_t u16; 178 | if (_ready[idx]) { 179 | switch (mode) { 180 | case 1: // HEX 181 | u16 = _data[idx]; 182 | output->printf("%04X ", u16); 183 | break; 184 | case 2: // Binary 185 | u16 = _data[idx]; 186 | // char buffer[33]; 187 | // itoa(u16, buffer, 2); 188 | // output->printf("%04X [%s] : ", u16, buffer); 189 | // Split to 4 digits group for easy reading 190 | output->printf("%04X [", u16); 191 | for (int i = 0; i < 4; i++) { 192 | for (int j = 0; j < 4; j++) { 193 | int pos = 15 - (i * 4 + j); 194 | uint16_t mask = 1 << pos; 195 | output->printf(((u16 & mask) == 0 ? "0" : "1")); 196 | } 197 | if (i < 3) output->printf(" "); 198 | } 199 | output->printf("]"); 200 | break; 201 | default: // DEC 202 | output->printf("%d ", _data[idx]); 203 | break; 204 | } 205 | } else { 206 | output->printf("---- "); 207 | } 208 | } 209 | -------------------------------------------------------------------------------- /lib/EventHandler/EventData.h: -------------------------------------------------------------------------------- 1 | #ifndef _EVENTDATA_H_ 2 | #define _EVENTDATA_H_ 3 | #include 4 | 5 | #define ED_INVALID_DATA -32768 6 | 7 | #define ED_SIZE_MPU 3 8 | #define ED_SIZE_TOUCH 1 9 | #define ED_SIZE_PSXBUTTON 1 10 | #define ED_SIZE_BATTERY 2 11 | #define ED_SIZE_SONIC 1 12 | #define ED_SIZE_MAZE 1 13 | 14 | #define ED_COUNT_MPU 1 15 | #define ED_COUNT_TOUCH 1 16 | #define ED_COUNT_PSXBUTTON 1 17 | #define ED_COUNT_BATTERY 1 18 | #define ED_COUNT_SONIC 4 19 | #define ED_COUNT_MAZE 1 20 | 21 | /* 22 | #define ED_OFFSET_MPU 0 23 | #define ED_OFFSET_TOUCH 3 24 | #define ED_OFFSET_PSXBUTTON 4 25 | #define ED_OFFSET_BATTERY 5 26 | #define ED_OFFSET_SONIC 7 27 | */ 28 | 29 | #define ED_MAX_DEVICE 6 30 | #define ED_DATA_SIZE 64 31 | 32 | 33 | class EventData { 34 | 35 | public: 36 | 37 | enum class DEVICE : uint8_t { 38 | mpu = 1, touch = 2, psx_button = 3, battery = 4, sonic = 5, maze = 6 39 | }; 40 | 41 | enum class MPU_TARGET : uint8_t { 42 | x = 0, y = 1, z = 2 43 | }; 44 | 45 | enum class BATTERY_TARGET : uint8_t { 46 | reading = 0, level = 1 47 | }; 48 | 49 | EventData(); 50 | ~EventData(); 51 | 52 | bool IsValid(uint8_t device, uint8_t devId, uint8_t target); 53 | inline bool IsValid(uint8_t device, uint8_t devId) { return IsValid(device, devId, 0); } 54 | bool IsReady(uint8_t device, uint8_t devId, uint8_t target); 55 | bool MarkReady(uint8_t device, uint8_t devId, uint8_t target, bool ready); 56 | void Clear(); 57 | 58 | 59 | bool SetData(uint8_t device, uint8_t devId, uint8_t target, int16_t value); 60 | int16_t GetData(uint8_t device, uint8_t devId, uint8_t target); 61 | 62 | bool SetData(DEVICE device, uint8_t devId, uint8_t target, int16_t value) { 63 | return SetData((uint8_t) device, devId, target, value); 64 | } 65 | int16_t GetData(DEVICE device, uint8_t devId, uint8_t target) { 66 | return GetData((uint8_t) device, devId, target); 67 | } 68 | 69 | // In the first version, devId is not used, so let's allow to skip this parameter in common interface first 70 | bool SetData(DEVICE device, uint8_t target, int16_t value) { 71 | return SetData((uint8_t) device, 0, target, value); 72 | } 73 | int16_t GetData(DEVICE device, uint8_t target) { 74 | return GetData((uint8_t) device, 0, target); 75 | } 76 | 77 | uint8_t DeviceDataSize(uint8_t device); 78 | 79 | uint8_t Offset(uint8_t device, uint8_t devId, uint8_t target); 80 | uint8_t DevOffset(uint8_t device, uint8_t devId); 81 | uint8_t IdCount(uint8_t device); 82 | uint8_t MaxId(uint8_t device); 83 | uint8_t DevCount() { return _devCount; } 84 | 85 | bool SetThreadhold(uint8_t device, uint8_t devId, uint16_t threadhold); 86 | uint16_t Threadhold(uint8_t device, uint8_t devId); 87 | 88 | void DumpData(Stream *output); 89 | void DumpData(Stream *output, uint8_t device); 90 | 91 | // ------ 92 | // TODO: remove these method once all program can handle multiple ID for device 93 | bool SetThreadhold(uint8_t device, uint16_t threadhold) { 94 | return SetThreadhold(device, 0, threadhold); 95 | } 96 | uint16_t Threadhold(uint8_t device) { 97 | return Threadhold(device, 0); 98 | } 99 | // ------ 100 | 101 | 102 | private: 103 | const uint8_t _size[ED_MAX_DEVICE + 1] = 104 | {0, ED_SIZE_MPU, ED_SIZE_TOUCH, ED_SIZE_PSXBUTTON, ED_SIZE_BATTERY, ED_SIZE_SONIC, ED_SIZE_MAZE}; 105 | const uint8_t _idCount[ED_MAX_DEVICE + 1] = 106 | {0, ED_COUNT_MPU, ED_COUNT_TOUCH, ED_COUNT_PSXBUTTON, ED_COUNT_BATTERY, ED_COUNT_SONIC, ED_COUNT_MAZE}; 107 | // const uint8_t _offset[ED_MAX_DEVICE + 1] = {0, ED_OFFSET_MPU, ED_OFFSET_TOUCH, ED_OFFSET_PSXBUTTON, ED_OFFSET_BATTERY, ED_OFFSET_SONIC }; 108 | // uint16_t _threadhold[ED_MAX_DEVICE + 1] = {0, 0, 0, 0, 0,0 }; 109 | 110 | uint8_t _offset[ED_MAX_DEVICE + 1]; 111 | uint8_t _devOffset[ED_MAX_DEVICE + 1]; 112 | 113 | 114 | uint8_t _devCount; // actual device, not the count of device type 115 | uint8_t _dataSize; 116 | 117 | 118 | int16_t _data[ED_DATA_SIZE]; 119 | 120 | // Control flags: use bool array instead of bit control table at this moment 121 | // bool _ready[ED_DATA_SIZE]; 122 | // uint16_t _threadhold[ED_MAX_DEVICE + 1]; 123 | bool *_ready; 124 | uint16_t *_threadhold; 125 | 126 | 127 | void ShowValue(Stream *output, uint8_t idx, uint8_t mode = 0); 128 | 129 | }; 130 | 131 | 132 | #endif 133 | -------------------------------------------------------------------------------- /lib/EventHandler/EventDataSource.cpp: -------------------------------------------------------------------------------- 1 | #include "EventDataSource.h" 2 | 3 | 4 | bool EventDataSource::SetEnabled(bool enabled) { 5 | _isEnabled = enabled; 6 | return IsEnabled(); 7 | } 8 | 9 | bool EventDataSource::IsAvailable() { 10 | return (_isAvailable); 11 | } 12 | 13 | bool EventDataSource::IsEnabled() { 14 | return (_isAvailable && _isEnabled); 15 | } 16 | 17 | bool EventDataSource::IsReady() { 18 | return (_isAvailable && _isEnabled && !_isSuspended && (millis() > _nextReportMs)); 19 | } 20 | 21 | void EventDataSource::Suspend(bool suspend) { 22 | _isSuspended = suspend; 23 | } 24 | 25 | void EventDataSource::Config(EventData *data, MyDebugger *dbg, byte devId) { 26 | _data = data; 27 | _dbg = dbg; 28 | _DevId = devId; 29 | } 30 | 31 | void EventDataSource::SetNextReportTime() { 32 | _nextReportMs = millis() + _reportInterval; 33 | } 34 | 35 | /* 36 | * PostHandling (eventMatched, isRelated, pending) 37 | * - eventMatched : this round has event matched 38 | * - isRelated : the matched event is related to this device 39 | * - pending : some event pending on this data within threadhold: see EventHander.cpp 40 | * condition require to match for continous time before confirmed (e.g. Mpu6050) 41 | */ 42 | void EventDataSource::PostHandler(bool eventMatched, bool isRelated, bool pending) { 43 | if (!IsReady()) return; 44 | if (_dbg->require(210)) _dbg->log(210,0,"EventDataSource[%d]::PostHandler(%d,%d,%d)",_Device, eventMatched, isRelated, pending); 45 | if (_thisDataReady) { 46 | if (isRelated) { 47 | // Current data has event matched 48 | _nextReportMs = millis() + _delayCheckMs; 49 | } else if (eventMatched) { 50 | // Maybe handled with other events, should check again in next round 51 | _nextReportMs = 0; 52 | } else { 53 | // Current data did not trigger any event 54 | _nextReportMs = millis() + _continueCheckMs; 55 | } 56 | } else if (pending) { 57 | _nextReportMs = millis() + _pendingCheckMs; 58 | } else if (_thisDataError) { 59 | _nextReportMs = millis() + _continueCheckMs; 60 | } else { 61 | // not ready at GetData, but ready now 62 | // Do nothing and let it getData again in next round 63 | } 64 | 65 | } 66 | -------------------------------------------------------------------------------- /lib/EventHandler/EventDataSource.h: -------------------------------------------------------------------------------- 1 | #ifndef _EVENT_DATA_SOURCE_H_ 2 | #define _EVENT_DATA_SOURCE_H_ 3 | 4 | #include "ESP8266WiFi.h" 5 | #include "EventData.h" 6 | #include "MyDebugger.h" 7 | 8 | // Default setting for checking frequency, don't make it too busy 9 | #define EDS_PENDING_CHECK_MS 20 10 | #define EDS_CONTINUE_CHECK_MS 50 11 | #define EDS_DELAY_CHECK_MS 1000 12 | #define EDS_HANDLED_CHECK_MS 1000 13 | 14 | class EventDataSource { 15 | 16 | protected: 17 | EventData *_data = NULL; 18 | MyDebugger *_dbg; 19 | 20 | bool _isAvailable = false; 21 | bool _isEnabled = true; 22 | bool _isSuspended = false; 23 | bool _enableDebug = true; 24 | /* 25 | _lastDataReady vs _thisDataReady 26 | In most situation, _lastDataReady should be the same as _thisDataReady as the current value will be updated to last value. 27 | But for some special case, it need to check if the previous value has been handled, and it may skip current reading under some condition. 28 | E.g. PSX Button 29 | Since it will keep checking the button status, while this status will repeat for 1s if no other button pressed. 30 | When it checked again within 1s, it need to ignore this button, but it need to keep the status of some value handled. 31 | In this case, _lastDataReady is true, but _thisDataReady is false. 32 | */ 33 | bool _lastDataReady = false; 34 | bool _thisDataReady = false; 35 | bool _thisDataError = false; 36 | 37 | unsigned long _lastReportMS = 0; 38 | bool _lastValueHandled = false; 39 | unsigned long _reportInterval = 1000; // by default, each sensor will be reported once per second 40 | unsigned long _nextReportMs = 0; 41 | 42 | unsigned long _pendingCheckMs = EDS_PENDING_CHECK_MS; 43 | unsigned long _continueCheckMs = EDS_CONTINUE_CHECK_MS; 44 | unsigned long _delayCheckMs = EDS_DELAY_CHECK_MS; 45 | 46 | uint8_t _Device = 0; 47 | uint8_t _DevId = 0; 48 | 49 | void Config(EventData *data, MyDebugger *dbg, byte devId = 0); 50 | void SetNextReportTime(); 51 | 52 | public: 53 | EventDataSource() {} 54 | ~EventDataSource() {} 55 | 56 | uint8_t Device() { return _Device; } 57 | uint8_t DevId() { return _DevId; } 58 | 59 | bool SetEnabled(bool enabled); 60 | bool IsAvailable(); 61 | bool IsEnabled(); 62 | bool IsReady(); 63 | void Suspend(bool suspend); 64 | virtual bool GetData() = 0; 65 | bool ForceGetData() { 66 | _nextReportMs = 0; 67 | return GetData(); 68 | } 69 | virtual void PostHandler(bool eventMatched, bool isRelated, bool pending); 70 | 71 | private: 72 | 73 | 74 | }; 75 | 76 | #endif 77 | 78 | -------------------------------------------------------------------------------- /lib/EventHandler/EventHandler.h: -------------------------------------------------------------------------------- 1 | #ifndef _EVENTHANDLER_H_ 2 | #define _EVENTHANDLER_H_ 3 | #include 4 | #include "EventData.h" 5 | #include 6 | #define FILE_READ "r" 7 | #define FILE_WRITE "w" 8 | 9 | class EventHandler { 10 | 11 | public: 12 | 13 | enum class CHECK_MODE : uint8_t { 14 | match = 1, greater = 2, less = 3, button = 4 15 | }; 16 | 17 | enum class MPU6050_TARGET : uint8_t { 18 | x = 0, y = 1, z = 2 19 | }; 20 | 21 | enum class ACTION_TYPE : uint8_t { 22 | na = 0, playAction = 1, stopAction = 2, headLed = 3, mp3PlayMp3 = 4, mp3PlayFile = 5, mp3Stop = 6, gpio = 7, system_action = 8, servo = 9, sonic = 10 23 | }; 24 | 25 | enum class EVENT_TYPE : uint8_t { 26 | na = 0, handler = 1, preCond = 2 27 | }; 28 | 29 | 30 | union CONDITION { 31 | uint8_t buffer[6]; 32 | struct { 33 | uint8_t device; 34 | uint8_t devId; 35 | uint8_t target; 36 | uint8_t checkMode; 37 | int16_t value; 38 | } data; 39 | }; 40 | 41 | union ACTION { 42 | uint8_t buffer[4]; 43 | struct { 44 | uint8_t type; 45 | uint8_t parm_1; 46 | uint8_t parm_2; 47 | uint8_t parm_3; 48 | } data; 49 | struct { 50 | uint8_t type; 51 | uint8_t parm_1; 52 | uint16_t parm_u16; 53 | } u16data; 54 | }; 55 | 56 | union EVENT { 57 | uint8_t buffer[12]; 58 | struct { 59 | uint8_t seq; 60 | uint8_t type; 61 | CONDITION condition; 62 | ACTION action; 63 | } data; 64 | }; 65 | 66 | EventHandler(EventData *data); 67 | ~EventHandler(); 68 | 69 | void ReleaseMemory(); 70 | void SetCount(uint8_t count); 71 | void Reset(uint8_t count); 72 | bool FillData(uint8_t startIdx, uint8_t count, byte *buffer); 73 | bool Clone(EventHandler *source); 74 | bool IsValid(); 75 | 76 | bool LoadData(String filename); 77 | void LoadDummyData(); 78 | bool SaveData(String filename); 79 | bool IsRequired(uint8_t device, uint8_t devId); 80 | bool LastEventRelated(uint8_t device, uint8_t devId); 81 | bool IsPending(uint8_t device, uint8_t devId); 82 | 83 | EVENT CheckEvents(); 84 | uint16_t Count() { return _evtCount; } 85 | EVENT *Events() { return _events; } 86 | 87 | void ResetEventLastMs(); 88 | void DumpEvents(Stream *output); 89 | 90 | private: 91 | uint16_t _evtCount; 92 | EVENT *_events; 93 | unsigned long *_eventLastMs; 94 | 95 | EventData *_data; 96 | bool *_isRequired; 97 | void CheckEventsRequirement(); // mark flag in _isRequired 98 | 99 | // bool _lastEventRelated[ED_MAX_DEVICE + 1]; 100 | bool *_lastEventRelated; 101 | bool MatchCondition(uint16_t idx, CONDITION cod); 102 | 103 | size_t FileSize(const char *fileName); 104 | bool ReadFile(const char *fileName, uint8_t *buffer, size_t size); 105 | bool DeleteFile(const char *fileName, bool mustExists = false); 106 | bool WriteFile(const char *fileName, uint8_t *buffer, size_t size); 107 | }; 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /lib/EventHandler/drivers/EdsBattery/EdsBattery.cpp: -------------------------------------------------------------------------------- 1 | #include "EdsBattery.h" 2 | 3 | EdsBattery::EdsBattery(EventData *data, MyDebugger *dbg, byte devId) { 4 | _Device = (uint8_t) EventData::DEVICE::battery; 5 | Config(data, dbg, devId); 6 | _isAvailable = true; 7 | } 8 | 9 | EdsBattery::~EdsBattery() { 10 | } 11 | 12 | void EdsBattery::Setup(uint16_t minVoltage, uint16_t maxVoltage, uint16_t normalCheckMs, uint16_t alarmtIntervalMs) { 13 | if (_dbg->require(110)) _dbg->log(110, 0, "EdsBattery::Setup(%d,%d,%d,%d)", minVoltage, maxVoltage, normalCheckMs, alarmtIntervalMs); 14 | _minVoltage = minVoltage; 15 | _maxVoltage = maxVoltage; 16 | 17 | _data->SetThreadhold(_Device, EBAT_THREADHOLD); 18 | _pendingCheckMs = _data->Threadhold(_Device) / 10; 19 | _continueCheckMs = normalCheckMs; 20 | _delayCheckMs = alarmtIntervalMs; 21 | 22 | } 23 | 24 | bool EdsBattery::GetData() { 25 | _thisDataReady = false; 26 | if (!IsReady()) return false; 27 | _lastDataReady = false; 28 | 29 | uint16_t v = analogRead(0); 30 | int iPower = GetPower(v); 31 | 32 | /* 33 | _dbg->msg("Battery: %d, %d, %d", _Device, _DevId, (uint8_t) EventData::BATTERY_TARGET::reading); 34 | _dbg->msg("Battery reading: %d (%d - %d)", v, _minVoltage, _maxVoltage); 35 | _dbg->msg("Battery level: %d", iPower); 36 | */ 37 | 38 | _data->SetData(_Device, _DevId, (uint8_t) EventData::BATTERY_TARGET::reading, v); 39 | _data->SetData(_Device, _DevId, (uint8_t) EventData::BATTERY_TARGET::level, iPower); 40 | _lastDataReady = true; 41 | _thisDataReady = true; 42 | _lastReportMS = millis(); 43 | _lastReportReading = v; 44 | _lastReportLevel = iPower; 45 | if (_dbg->require(100)) _dbg->log(100,0,"Battery: %d, %d%% ", v, iPower); 46 | return true; 47 | } 48 | 49 | byte EdsBattery::GetPower(uint16_t v) { 50 | // Power in precentage instead of voltage 51 | float power = ((float) (v - _minVoltage) / (_maxVoltage - _minVoltage) * 100.0f); 52 | int iPower = (int) (power + 0.5); // round up 53 | if (iPower > 100) iPower = 100; 54 | if (iPower < 0) iPower = 0; 55 | return (byte) iPower; 56 | } 57 | 58 | /* 59 | * PostHandler 60 | */ 61 | /* 62 | void EdsBattery::PostHandler(bool eventMatched, bool isRelated, bool pending) { 63 | if (!IsReady()) return; 64 | if (_thisDataReady && isRelated) { 65 | _nextReportMs = millis() + _alarmIntervalMs; 66 | } else if (pending) { 67 | _dbg->msg("edsBattery pending"); 68 | _nextReportMs = millis() + EDS_PENDING_CHECK_MS; 69 | } else { 70 | _dbg->msg("edsBattery normal"); 71 | _nextReportMs = millis() + _normalCheckMs; 72 | } 73 | } 74 | */ -------------------------------------------------------------------------------- /lib/EventHandler/drivers/EdsBattery/EdsBattery.h: -------------------------------------------------------------------------------- 1 | #ifndef _EDS_BATTERY_H_ 2 | #define _EDS_BATTERY_H_ 3 | 4 | // Keyword: EBAT 5 | 6 | #include "EventHandler.h" 7 | #include "EventDataSource.h" 8 | #include "SSBoard.h" 9 | 10 | #define EBAT_THREADHOLD 1000 11 | 12 | /* 13 | * Event Data Source for Battery 14 | * 15 | */ 16 | 17 | class EdsBattery : public EventDataSource { 18 | public: 19 | EdsBattery(EventData *data, MyDebugger *dbg, byte devId = 0); 20 | ~EdsBattery(); 21 | 22 | // void Initialize(EventData *data) override; 23 | void Setup(uint16_t minVoltage, uint16_t maxVoltage, uint16_t normalCheckMs, uint16_t alarmIntervalMs); 24 | bool GetData() override; 25 | // void PostHandler(bool eventMatched, bool isRelated, bool pending) override; 26 | 27 | byte GetPower(uint16_t v); 28 | 29 | private: 30 | 31 | uint16_t _minVoltage = 0; 32 | uint16_t _maxVoltage = 0; 33 | uint16_t _lastReportReading = 0; 34 | uint16_t _lastReportLevel = 0; 35 | 36 | }; 37 | 38 | #endif -------------------------------------------------------------------------------- /lib/EventHandler/drivers/EdsMaze/EdsMaze.cpp: -------------------------------------------------------------------------------- 1 | #include "EdsMaze.h" 2 | 3 | EdsMaze::EdsMaze(EventData *data, MyDebugger *dbg, byte devId) { 4 | _Device = (uint8_t) EventData::DEVICE::maze; 5 | Config(data, dbg, devId); 6 | _isAvailable = false; 7 | } 8 | 9 | EdsMaze::~EdsMaze() { 10 | } 11 | 12 | /* 13 | void EdsMaze::Initialize(EventData *data) { 14 | _data = data; 15 | } 16 | */ 17 | 18 | /* 19 | * Ping: 20 | * Send: A8 8A 04 02 00 01 07 ED 21 | * Return: A8 8A 05 02 00 01 00 08 ED 22 | */ 23 | 24 | void EdsMaze::Setup(SSBoard *ssb, RobotServo *rs, uint8_t wallDistance, 25 | uint8_t servoId, bool servoReversed, uint16_t servoMoveMs, uint16_t servoWaitMs, 26 | unsigned long continueCheckms, unsigned long delayCheckMs) { 27 | if (_dbg->require(110)) _dbg->log(110, 0, "EdsMaze::Setup(*ssb)"); 28 | _ssb = ssb; 29 | _rs = rs; 30 | _servoId = servoId; 31 | _wallDistance = wallDistance; 32 | 33 | _servoReversed = servoReversed; 34 | _servoMoveMs = servoMoveMs; 35 | _servoWaitMs = servoWaitMs; 36 | 37 | // Check if device available 38 | 39 | 40 | 41 | Ping(); 42 | if (_isAvailable) { 43 | if (_dbg->require(10)) _dbg->log(10, 0, "Maze Solver activated"); 44 | } else { 45 | if (_dbg->require(10)) _dbg->log(10, 0, "Maze Solver not available"); 46 | } 47 | _data->SetThreadhold(_Device, 0); 48 | _pendingCheckMs = 0; 49 | _continueCheckMs = continueCheckms; 50 | _delayCheckMs = delayCheckMs; 51 | } 52 | 53 | bool EdsMaze::Ping() { 54 | _isAvailable = false; 55 | if (!Ping(EDS_MAZE_FRONT_ID)) return false; 56 | // use 3 sensor if no servo defined 57 | if (_servoId) { 58 | if (!_rs->exists(_servoId)) return false; 59 | } else { 60 | if (!Ping(EDS_MAZE_LEFT_ID)) return false; 61 | if (!Ping(EDS_MAZE_RIGHT_ID)) return false; 62 | } 63 | _isAvailable = true; 64 | return true; 65 | } 66 | 67 | bool EdsMaze::Ping(byte id) { 68 | byte cmd[] = {0xA8, 0x8A, 0x04, 0x02, 0x00, 0x01, 0x07, 0xED}; 69 | cmd[4] = id; 70 | bool ready = false; 71 | byte tryCnt = 0; 72 | while (tryCnt++ < 5) { 73 | ready = _ssb->SendCommand((byte *) cmd, true); 74 | if (ready) break; 75 | } 76 | return (ready); 77 | } 78 | 79 | /* 80 | * Get Data: 81 | * Send: A8 8A 06 02 00 02 28 02 34 ED 82 | * Return: A8 8A 08 02 00 02 28 02 00 B4 EA ED （B4，180cm） 83 | */ 84 | 85 | bool EdsMaze::GetData() { 86 | _thisDataReady = false; 87 | _thisDataError = false; 88 | 89 | if (!IsReady()) return false; 90 | 91 | uint16 disFront, disLeft, disRight; 92 | 93 | if (_servoId) { 94 | if (!GetOneSensorData(&disFront, &disLeft, &disRight)) return false; 95 | } else { 96 | if (!GetThreeSensorData(&disFront, &disLeft, &disRight)) return false; 97 | } 98 | 99 | uint16_t action; 100 | 101 | if (disLeft >= _wallDistance) { 102 | action = EDS_MAZE_GO_LEFT; 103 | } else if (disFront >= _wallDistance) { 104 | action = EDS_MAZE_GO_FRONT; 105 | } else if (disRight >= _wallDistance) { 106 | action = EDS_MAZE_GO_RIGHT; 107 | } else { 108 | action = EDS_MAZE_TURN_BACK; 109 | } 110 | 111 | _data->SetData(_Device, _DevId, 0, action); 112 | if ((_dbg->require(100))) _dbg->log(100,0,"Maze: [%d,%d] : (%d, %d, %d) => %d", 113 | _Device, _DevId, 114 | disFront, disLeft, disRight, action); 115 | _thisDataReady = true; 116 | 117 | return true; 118 | } 119 | 120 | 121 | bool EdsMaze::GetThreeSensorData(uint16 *disFront, uint16 *disLeft, uint16 *disRight) { 122 | bool showMsg = false; 123 | if (millis() - _lastMsgMs > 1000) { 124 | _dbg->log(10,0,"EdsMaze::GetThreeSensorData()"); 125 | showMsg = true; 126 | _lastMsgMs = millis(); 127 | } 128 | if (!ReadSensor(EDS_MAZE_FRONT_ID, disFront)) return false; 129 | if (!ReadSensor(EDS_MAZE_LEFT_ID, disLeft)) return false; 130 | if (!ReadSensor(EDS_MAZE_RIGHT_ID, disRight)) return false; 131 | if (showMsg) { 132 | _dbg->log(10, 0, "=> %d, %d, %d", *disLeft, *disFront, *disRight); 133 | } 134 | return true; 135 | } 136 | 137 | bool EdsMaze::GetOneSensorData(uint16 *disFront, uint16 *disLeft, uint16 *disRight) { 138 | bool showMsg = false; 139 | if (millis() - _lastMsgMs > 1000) { 140 | _dbg->log(10,0,"EdsMaze::GetOneSensorData()"); 141 | showMsg = true; 142 | _lastMsgMs = millis(); 143 | } 144 | if (!_rs->exists(_servoId)) return false; 145 | // 0 - left 146 | // 90 - center 147 | // 180 - right 148 | // checking left, right, center 149 | _rs->goAngle(_servoId, (_servoReversed ? 180 : 0) , _servoMoveMs); 150 | delay(_servoWaitMs); // 1s is good enough; maybe add parameter to control later 151 | if (!ReadSensor(EDS_MAZE_FRONT_ID, disLeft)) return false; 152 | 153 | _rs->goAngle(_servoId, (_servoReversed ? 0 : 180) , _servoMoveMs); 154 | delay(_servoWaitMs); // 1s is good enough; maybe add parameter to control later 155 | if (!ReadSensor(EDS_MAZE_FRONT_ID, disRight)) return false; 156 | 157 | _rs->goAngle(_servoId, 90, _servoMoveMs); 158 | delay(_servoWaitMs); // 1s is good enough; maybe add parameter to control later 159 | if (!ReadSensor(EDS_MAZE_FRONT_ID, disFront)) return false; 160 | 161 | if (showMsg) { 162 | _dbg->log(10, 0, "GetOneSensorData (%d) => %d, %d, %d", _servoReversed, *disLeft, *disFront, *disRight); 163 | } 164 | 165 | return true; 166 | } 167 | 168 | bool EdsMaze::ReadSensor(byte id, uint16 *data) { 169 | byte cmd[] = { 0xA8, 0x8A, 0x06, 0x02, 0x00, 0x02, 0x28, 0x02, 0x34, 0xED }; 170 | cmd[4] = id; 171 | 172 | unsigned long startMs = millis(); 173 | if (!_ssb->SendCommand((byte *) cmd, true)) { 174 | _thisDataError = true; 175 | _dbg->log(10, 0, "Fail getting sonic sensor data"); 176 | return false; 177 | } 178 | 179 | unsigned long diff = millis() - startMs; 180 | //_dbg->msg("It taks %d ms to read PSX", diff); 181 | 182 | Buffer *result = _ssb->ReturnBuffer(); 183 | // Data returned: A8 8A 08 02 00 02 28 02 00 {*} EA ED 184 | 185 | *data = result->peek(8) << 8 | result->peek(9); 186 | return true; 187 | } -------------------------------------------------------------------------------- /lib/EventHandler/drivers/EdsMaze/EdsMaze.h: -------------------------------------------------------------------------------- 1 | #ifndef _EDS_MAZE_H_ 2 | #define _EDS_MAZE_H_ 3 | 4 | // Keyword: EPB_ 5 | 6 | #include "EventHandler.h" 7 | #include "EventDataSource.h" 8 | #include "SSBoard.h" 9 | #include "RobotServo.h" 10 | 11 | #define EDS_MAZE_FRONT_ID 0 12 | #define EDS_MAZE_LEFT_ID 1 13 | #define EDS_MAZE_RIGHT_ID 2 14 | 15 | #define EDS_MAZE_GO_FRONT 0 16 | #define EDS_MAZE_GO_LEFT 1 17 | #define EDS_MAZE_GO_RIGHT 2 18 | #define EDS_MAZE_TURN_BACK 3 19 | 20 | 21 | 22 | /* 23 | * Event Data Source for PSX Button 24 | * 25 | * 26 | * Ping: 27 | * Send: A8 8A 04 02 00 01 07 ED 28 | * Return: A8 8A 05 02 00 01 00 08 ED 29 | * 30 | * 31 | * Get Data: 32 | * Send: A8 8A 06 02 00 02 28 02 34 ED 33 | * Return: A8 8A 08 02 00 02 28 02 00 B4 EA ED （B4，180cm） 34 | */ 35 | 36 | 37 | class EdsMaze : public EventDataSource { 38 | public: 39 | EdsMaze(EventData *data, MyDebugger *dbg, byte devId = 0); 40 | ~EdsMaze(); 41 | 42 | void Setup(SSBoard *ssb, RobotServo *rs, uint8_t wallDistance, 43 | uint8_t servoId, bool servoReversed, uint16_t servoMoveMs, uint16_t servoWaitMs, 44 | unsigned long continueCheckms, unsigned long delayCheckMs); 45 | bool GetData() override; 46 | // void PostHandler(bool eventMatched, bool isRelated, bool pending) override; 47 | 48 | private: 49 | uint8_t _servoId = 0; 50 | uint8_t _wallDistance = 100; 51 | SSBoard *_ssb; 52 | RobotServo *_rs; 53 | 54 | bool Ping(); 55 | bool Ping(byte id); 56 | bool ReadSensor(byte id, uint16 *data); 57 | 58 | bool GetOneSensorData(uint16 *disFront, uint16 *disLeft, uint16 *disRight); 59 | bool GetThreeSensorData(uint16 *disFront, uint16 *disLeft, uint16 *disRight); 60 | 61 | unsigned long _lastMsgMs = 0; 62 | 63 | bool _servoReversed = false; 64 | unsigned long _servoMoveMs = 500; 65 | unsigned long _servoWaitMs = 2000; 66 | 67 | 68 | // uint8_t _normalCheckMs = 0; 69 | // uint8_t _noEventMs = 0; 70 | // uint16_t _ignoreRepeatMs = 0; 71 | // uint16_t _lastReportValue = 0; 72 | }; 73 | 74 | #endif -------------------------------------------------------------------------------- /lib/EventHandler/drivers/EdsMpu6050/EdsMpu6050.cpp: -------------------------------------------------------------------------------- 1 | #include "EdsMpu6050.h" 2 | 3 | 4 | 5 | EdsMpu6050::EdsMpu6050(EventData *data, MyDebugger *dbg, byte devId) { 6 | _Device = (uint8_t) EventData::DEVICE::mpu; 7 | Config(data, dbg, devId); 8 | _isAvailable = false; 9 | } 10 | 11 | EdsMpu6050::~EdsMpu6050() { 12 | } 13 | 14 | void EdsMpu6050::Setup(uint8_t i2cAddr, uint16_t threadhold, uint16_t elapseMs) { 15 | if (_dbg->require(110)) _dbg->log(110, 0, "EdsMpu6050::Setup(0x%02X, threadhold: %d ms, elapse: %d ms)", i2cAddr, threadhold, elapseMs); 16 | _i2cAddr = i2cAddr; 17 | _threadhold = threadhold; 18 | _continueCheckMs = elapseMs; 19 | _delayCheckMs = EMPU_DELAY_CHECK_MS; 20 | 21 | _data->SetThreadhold(_Device, threadhold); 22 | 23 | Wire.begin(); 24 | // Check if MPU available 25 | Wire.beginTransmission(_i2cAddr); 26 | uint8_t i2cError = Wire.endTransmission(); 27 | _isAvailable = (i2cError == 0); 28 | if (_isAvailable) { 29 | Wire.beginTransmission(_i2cAddr); 30 | Wire.write(0x6B); // PWR_MGMT_1 register 31 | Wire.write(0); // set to zero (wakes up the MPU-6050) 32 | Wire.endTransmission(true); 33 | if (_dbg->require(10)) _dbg->log(10, 0, "MPU6050 initialized."); 34 | } else { 35 | if (_dbg->require(10)) _dbg->log(10, 0, "MPU6050 is not available."); 36 | } 37 | } 38 | 39 | 40 | bool EdsMpu6050::GetData() { 41 | _thisDataReady = false; 42 | _thisDataError = false; 43 | if (!IsReady()) return false; 44 | 45 | if (!GetMpuData()) { 46 | _thisDataError = true; 47 | return false; 48 | } 49 | 50 | _data->SetData(_Device, _DevId, 0, _ax); 51 | _data->SetData(_Device, _DevId, 1, _ay); 52 | _data->SetData(_Device, _DevId, 2, _az); 53 | if (_dbg->require(100)) _dbg->log(100,0,"MPU 6050: ( %d , %d , %d )", _ax, _ay, _az); 54 | 55 | _thisDataReady = true; 56 | 57 | return true; 58 | } 59 | 60 | /* 61 | void EdsMpu6050::PostHandler(bool eventMatched, bool isRelated, bool pending) { 62 | _nextReportMs = millis() + _elapseMs; 63 | } 64 | */ 65 | 66 | // public function for using outside EDS 67 | bool EdsMpu6050::GetMpuData() { 68 | if (!IsAvailable()) return false; 69 | _ax = _ay = _az = _tmp = _gx = _gy = _gz = 0; 70 | Wire.beginTransmission(_i2cAddr); 71 | Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) 72 | Wire.endTransmission(false); 73 | Wire.requestFrom(_i2cAddr, (size_t) EMPU_DATA_SIZE,true); // request a total of 14 registers 74 | memset(_mpuBuffer, 0, EMPU_DATA_SIZE); 75 | for (int i = 0; i < EMPU_DATA_SIZE; i++) { 76 | if (Wire.available()) { 77 | _mpuBuffer[i] = Wire.read(); 78 | } else { 79 | _dbg->printf("Incomplete MPU data, only %d bytes returned\n", i); 80 | memset(_mpuBuffer, 0, EMPU_DATA_SIZE); 81 | return false; 82 | } 83 | } 84 | _ax = _mpuBuffer[0] << 8 | _mpuBuffer[1]; 85 | _ay = _mpuBuffer[2] << 8 | _mpuBuffer[3]; 86 | _az = _mpuBuffer[4] << 8 | _mpuBuffer[5]; 87 | _gx = _mpuBuffer[8] << 8 | _mpuBuffer[9]; 88 | _gy = _mpuBuffer[10] << 8 | _mpuBuffer[11]; 89 | _gz = _mpuBuffer[12] << 8 | _mpuBuffer[13]; 90 | 91 | return true; 92 | } -------------------------------------------------------------------------------- /lib/EventHandler/drivers/EdsMpu6050/EdsMpu6050.h: -------------------------------------------------------------------------------- 1 | #ifndef _EDS_MPU6050_H_ 2 | #define _EDS_MPU6050_H_ 3 | 4 | // Keyword: EMPU_ 5 | 6 | #include 7 | #include "EventHandler.h" 8 | #include "EventDataSource.h" 9 | 10 | #define EMPU_DEFAULT_ADDR 0x68 11 | #define EMPU_DATA_SIZE 14 12 | #define EMPU_RESULT_SIZE 20 13 | #define EMPU_DELAY_CHECK_MS 5000 14 | 15 | class EdsMpu6050 : public EventDataSource { 16 | public: 17 | EdsMpu6050(EventData *data, MyDebugger *dbg, byte devId = 0); 18 | ~EdsMpu6050(); 19 | 20 | void Setup(uint8_t i2cAddr, uint16_t threadhold, uint16_t elapseMs); 21 | bool GetData() override; 22 | // void PostHandler(bool eventMatched, bool isRelated, bool pending) override; 23 | 24 | bool GetMpuData(); 25 | uint8_t *MpuBuffer() { return (uint8_t *) _mpuBuffer; } 26 | 27 | inline int16_t ax() { return _ax; } 28 | inline int16_t ay() { return _ay; } 29 | inline int16_t az() { return _az; } 30 | 31 | inline int16_t gx() { return _gx; } 32 | inline int16_t gy() { return _gy; } 33 | inline int16_t gz() { return _gz; } 34 | 35 | private: 36 | uint8_t _i2cAddr = 0x68; 37 | uint8_t _mpuBuffer[EMPU_DATA_SIZE]; 38 | int16_t _ax, _ay, _az; 39 | int16_t _gx, _gy, _gz; 40 | int16_t _tmp; 41 | int8_t _actionSign; 42 | uint16_t _threadhold; 43 | }; 44 | 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /lib/EventHandler/drivers/EdsPsxButton/EdsPsxButton.cpp: -------------------------------------------------------------------------------- 1 | #include "EdsPsxButton.h" 2 | 3 | EdsPsxButton::EdsPsxButton(EventData *data, MyDebugger *dbg, byte devId) { 4 | _Device = (uint8_t) EventData::DEVICE::psx_button; 5 | Config(data, dbg, devId); 6 | _isAvailable = false; 7 | } 8 | 9 | EdsPsxButton::~EdsPsxButton() { 10 | } 11 | 12 | /* 13 | void EdsPsxButton::Initialize(EventData *data) { 14 | _data = data; 15 | } 16 | */ 17 | 18 | void EdsPsxButton::Setup(SSBoard *ssb, uint8_t normalCheckMs, uint8_t noEventMs, uint16_t ignoreRepeatMs) { 19 | if (_dbg->require(110)) _dbg->log(110, 0, "EdsPsxButton::Setup(*ssb)"); 20 | _ssb = ssb; 21 | _normalCheckMs = normalCheckMs; 22 | _noEventMs = noEventMs; 23 | _ignoreRepeatMs = ignoreRepeatMs; 24 | 25 | // Check if device available 26 | 27 | Ping(); 28 | if (_isAvailable) { 29 | if (_dbg->require(10)) _dbg->log(10, 0, "PSX Conbroller detected"); 30 | } else { 31 | if (_dbg->require(10)) _dbg->log(10, 0, "PSX Conbroller not available"); 32 | } 33 | } 34 | 35 | bool EdsPsxButton::Ping() { 36 | byte cmd[] = {0xA8, 0x8A, 0x04, 0x01, 0x00, 0x01, 0x06, 0xED}; 37 | cmd[4] = _DevId; 38 | 39 | byte tryCnt = 0; 40 | while (tryCnt++ < 5) { 41 | _isAvailable = _ssb->SendCommand((byte *) cmd, true); 42 | if (_isAvailable) break; 43 | } 44 | return (_isAvailable); 45 | } 46 | 47 | 48 | bool EdsPsxButton::GetData() { 49 | _thisDataReady = false; 50 | _thisDataError = false; 51 | if (!IsReady()) return false; 52 | 53 | bool _prevDataRady = _lastDataReady; 54 | _lastDataReady = false; 55 | 56 | 57 | // byte cmd[] = {0xA8, 0x8A, 0x02, 0x01, 0x03, 0xED}; 58 | // byte cmd[] = { 0xA8, 0x8A, 0x07, 0x01, 0x00, 0x03, 0x2E, 0x01, 0x01, 0x3C, 0xED }; 59 | byte cmd[] = { 0xA8, 0x8A, 0x06, 0x01, 0x00, 0x02, 0x28, 0x02, 0x33, 0xED }; 60 | cmd[4] = _DevId; 61 | 62 | unsigned long startMs = millis(); 63 | if (!_ssb->SendCommand((byte *) cmd, true)) { 64 | _thisDataError = true; 65 | return false; 66 | } 67 | 68 | unsigned long diff = millis() - startMs; 69 | //_dbg->msg("It taks %d ms to read PSX", diff); 70 | 71 | Buffer *result = _ssb->ReturnBuffer(); 72 | // Data returned: A8 8A 0B 01 ?? ?? ?? {1} {2} .... 73 | // New return: A8 8A 08 01 00 02 28 02 [ EF 00 ] 24 ED 74 | uint16_t data; 75 | byte *button = (byte *) &data; 76 | button[0] = result->peek(8); 77 | button[1] = result->peek(9); 78 | 79 | // Due to the behavious of PSX control board, it will repeat the value within 1s 80 | // Need to skip repeated value if handled 81 | if (_prevDataRady && (_lastReportValue == data) && (_lastValueHandled) && ((millis() - _lastReportMS) < _ignoreRepeatMs)) { 82 | // ignore this value as it has just been handled 83 | // _dbg->msg("PSX Button already handled: %d, %04X : %04X, %d, %ld", _prevDataRady, _lastReportValue, data, _lastValueHandled, millis() - _lastReportMS); 84 | _lastDataReady = true; 85 | return false; 86 | } else { 87 | // _dbg->msg("PSX Button NOT handled: %d, %04X : %04X, %d, %ld", _prevDataRady, _lastReportValue, data, _lastValueHandled, millis() - _lastReportMS); 88 | } 89 | _data->SetData(_Device, _DevId, 0, data); 90 | _lastDataReady = true; 91 | _thisDataReady = true; 92 | _lastReportMS = millis(); 93 | _lastReportValue = data; 94 | _lastValueHandled = false; 95 | if ((data != 0xFFFF) && (_dbg->require(100))) _dbg->log(100,0,"PSX Button: [%d,%d,%d] %02X %02X => %04X", _Device, _DevId, 0, button[0], button[1], data); 96 | return (data != 0xFFFF); 97 | } 98 | 99 | 100 | /* 101 | * PostHandler 102 | * Can wait longer if data reported and handled 103 | */ 104 | void EdsPsxButton::PostHandler(bool eventMatched, bool isRelated, bool pending) { 105 | if (!IsReady()) return; 106 | if (_dbg->require(210)) _dbg->log(210,0,"EdsPsxButton::PostHandler(%d,%d,%d)",eventMatched, isRelated, pending); 107 | if (_thisDataReady) _lastValueHandled = isRelated; 108 | // wait longer if 109 | // - button pressed, and no event required or handled: i.e. !eventMatched 110 | if ((_lastDataReady) && (_lastReportValue != 0xFFFF) && ((!eventMatched) || (isRelated))) { 111 | _nextReportMs = millis() + _noEventMs; 112 | } else { 113 | _nextReportMs = millis() + _normalCheckMs; 114 | } 115 | } 116 | 117 | void EdsPsxButton::Shock() { 118 | if (!IsAvailable()) return; 119 | // byte cmd[] = {0xA8, 0x8A, 0x02, 0x02, 0x04, 0xED}; 120 | byte cmd[] = {0xA8, 0x8A, 0x07, 0x01, 0x00, 0x03, 0x2F, 0x01, 0x01, 0x3C, 0xED}; 121 | _ssb->SendCommand((byte *) cmd, true); 122 | } -------------------------------------------------------------------------------- /lib/EventHandler/drivers/EdsPsxButton/EdsPsxButton.h: -------------------------------------------------------------------------------- 1 | #ifndef _EDS_PSX_BUTTON_H_ 2 | #define _EDS_PSX_BUTTON_H_ 3 | 4 | // Keyword: EPB_ 5 | 6 | #include "EventHandler.h" 7 | #include "EventDataSource.h" 8 | #include "SSBoard.h" 9 | 10 | #define EPB_NO_BUTTON 0xFFFF 11 | // #define EPB_CONTINUE_CHECK_MS 20 12 | // #define EPB_NO_EVENT_MS 100 13 | // #define EPB_IGNORE_REPEAT_TIME 200 14 | 15 | /* 16 | * Event Data Source for PSX Button 17 | * 18 | * Get Data: send "A8 8A 02 01 03 ED" to bus 19 | * wait for return "A8 8A 0B 01 -- -- -- ?? ?? -- -- -- -- -- ED" 20 | */ 21 | /* 22 | * New sub-system board command: 23 | * 24 | * Get Data: A8 8A 06 01 00 02 28 02 33 ED 25 | * Return: A8 8A 08 01 00 02 28 02 [ EF 00 ] 24 ED 26 | * 0 1 2 3 4 5 6 7 8 9 27 | * 28 | * 29 | * Shock: A8 8A 07 01 00 03 2E 01 01 3C ED wrong 30 | * A8 8A 07 01 00 03 2F 01 01 3C ED 31 | * 32 | */ 33 | 34 | 35 | class EdsPsxButton : public EventDataSource { 36 | public: 37 | EdsPsxButton(EventData *data, MyDebugger *dbg, byte devId = 0); 38 | ~EdsPsxButton(); 39 | 40 | // void Initialize(EventData *data) override; 41 | void Setup(SSBoard *ssb, uint8_t normalCheckMs, uint8_t noEventMs, uint16_t ignoreRepeatMs); 42 | bool GetData() override; 43 | void PostHandler(bool eventMatched, bool isRelated, bool pending) override; 44 | void Shock(); 45 | 46 | private: 47 | SSBoard *_ssb; 48 | bool Ping(); 49 | 50 | uint8_t _normalCheckMs = 0; 51 | uint8_t _noEventMs = 0; 52 | uint16_t _ignoreRepeatMs = 0; 53 | 54 | 55 | uint16_t _lastReportValue = 0; 56 | }; 57 | 58 | #endif -------------------------------------------------------------------------------- /lib/EventHandler/drivers/EdsSonic/EdsSonic.cpp: -------------------------------------------------------------------------------- 1 | #include "EdsSonic.h" 2 | 3 | EdsSonic::EdsSonic(EventData *data, MyDebugger *dbg, byte devId) { 4 | _Device = (uint8_t) EventData::DEVICE::sonic; 5 | Config(data, dbg, devId); 6 | _isAvailable = false; 7 | } 8 | 9 | EdsSonic::~EdsSonic() { 10 | } 11 | 12 | /* 13 | void EdsSonic::Initialize(EventData *data) { 14 | _data = data; 15 | } 16 | */ 17 | 18 | /* 19 | * Ping: 20 | * Send: A8 8A 04 02 00 01 07 ED 21 | * Return: A8 8A 05 02 00 01 00 08 ED 22 | */ 23 | 24 | void EdsSonic::Setup(SSBoard *ssb, unsigned long continueCheckms, unsigned long delayCheckMs ) { 25 | if (_dbg->require(110)) _dbg->log(110, 0, "EdsSonic::Setup(*ssb)"); 26 | _ssb = ssb; 27 | // Check if device available 28 | 29 | Ping(); 30 | if (_isAvailable) { 31 | if (_dbg->require(10)) _dbg->log(10, 0, "Sonic Sensor detected"); 32 | } else { 33 | if (_dbg->require(10)) _dbg->log(10, 0, "Sonic Sensor not available"); 34 | } 35 | _data->SetThreadhold(_Device, 0); 36 | _pendingCheckMs = 0; 37 | _continueCheckMs = continueCheckms; 38 | _delayCheckMs = delayCheckMs; 39 | } 40 | 41 | bool EdsSonic::Ping() { 42 | byte cmd[] = {0xA8, 0x8A, 0x04, 0x02, 0x00, 0x01, 0x07, 0xED}; 43 | cmd[4] = _DevId; 44 | 45 | byte tryCnt = 0; 46 | while (tryCnt++ < 5) { 47 | _isAvailable = _ssb->SendCommand((byte *) cmd, true); 48 | if (_isAvailable) break; 49 | } 50 | return (_isAvailable); 51 | } 52 | 53 | /* 54 | * Get Data: 55 | * Send: A8 8A 06 02 00 02 28 02 34 ED 56 | * Return: A8 8A 08 02 00 02 28 02 00 B4 EA ED （B4，180cm） 57 | */ 58 | 59 | bool EdsSonic::GetData() { 60 | _thisDataReady = false; 61 | _thisDataError = false; 62 | 63 | if (!IsReady()) return false; 64 | 65 | byte cmd[] = { 0xA8, 0x8A, 0x06, 0x02, 0x00, 0x02, 0x28, 0x02, 0x34, 0xED }; 66 | cmd[4] = _DevId; 67 | 68 | unsigned long startMs = millis(); 69 | if (!_ssb->SendCommand((byte *) cmd, true)) { 70 | _thisDataError = true; 71 | return false; 72 | } 73 | 74 | 75 | unsigned long diff = millis() - startMs; 76 | //_dbg->msg("It taks %d ms to read PSX", diff); 77 | 78 | Buffer *result = _ssb->ReturnBuffer(); 79 | // Data returned: A8 8A 08 02 00 02 28 02 00 {*} EA ED 80 | 81 | uint16 data = result->peek(8) << 8 | result->peek(9); 82 | _data->SetData(_Device, _DevId, 0, data); 83 | if ((_dbg->require(100))) _dbg->log(100,0,"Sonic: [%d,%d,%d] => %d", _Device, _DevId, 0, data); 84 | _thisDataReady = true; 85 | 86 | return true; 87 | } 88 | 89 | -------------------------------------------------------------------------------- /lib/EventHandler/drivers/EdsSonic/EdsSonic.h: -------------------------------------------------------------------------------- 1 | #ifndef _EDS_SONIC_H_ 2 | #define _EDS_SONIC_H_ 3 | 4 | // Keyword: EPB_ 5 | 6 | #include "EventHandler.h" 7 | #include "EventDataSource.h" 8 | #include "SSBoard.h" 9 | 10 | /* 11 | * Event Data Source for PSX Button 12 | * 13 | * 14 | * Ping: 15 | * Send: A8 8A 04 02 00 01 07 ED 16 | * Return: A8 8A 05 02 00 01 00 08 ED 17 | * 18 | * 19 | * Get Data: 20 | * Send: A8 8A 06 02 00 02 28 02 34 ED 21 | * Return: A8 8A 08 02 00 02 28 02 00 B4 EA ED （B4，180cm） 22 | */ 23 | 24 | 25 | class EdsSonic : public EventDataSource { 26 | public: 27 | EdsSonic(EventData *data, MyDebugger *dbg, byte devId = 0); 28 | ~EdsSonic(); 29 | 30 | void Setup(SSBoard *ssb, unsigned long continueCheckms, unsigned long delayCheckMs); 31 | bool GetData() override; 32 | // void PostHandler(bool eventMatched, bool isRelated, bool pending) override; 33 | 34 | private: 35 | SSBoard *_ssb; 36 | bool Ping(); 37 | // uint8_t _normalCheckMs = 0; 38 | // uint8_t _noEventMs = 0; 39 | // uint16_t _ignoreRepeatMs = 0; 40 | // uint16_t _lastReportValue = 0; 41 | }; 42 | 43 | #endif -------------------------------------------------------------------------------- /lib/EventHandler/drivers/EdsTouch/EdsTouch.cpp: -------------------------------------------------------------------------------- 1 | #include "EdsTouch.h" 2 | 3 | 4 | EdsTouch::EdsTouch(EventData *data, MyDebugger *dbg, byte devId) { 5 | _Device = (uint8_t) EventData::DEVICE::touch; 6 | Config(data, dbg, devId); 7 | _isAvailable = true; 8 | } 9 | 10 | EdsTouch::~EdsTouch() { 11 | } 12 | 13 | void EdsTouch::Setup(uint8_t gpioPin, unsigned long touchDetectPeriod, unsigned long touchReleasePeriod) { 14 | if (_dbg->require(110)) _dbg->log(110, 0, "EdsTouch::Setup(GPIO: %d, Detect: %ld ms, Release: %ld ms)", gpioPin, touchDetectPeriod, touchReleasePeriod); 15 | _gpioPin = gpioPin; 16 | _touchDetectPeriod = touchDetectPeriod; 17 | _touchReleasePeriod = touchReleasePeriod; 18 | pinMode(_gpioPin, INPUT); 19 | } 20 | 21 | void EdsTouch::ResetTouchAction() { 22 | if (_dbg->require(210)) _dbg->log(210,0,"EdsTouch::ResetTouchAction\n"); 23 | _touchStartMs = 0; 24 | _touchReleaseMs = 0; 25 | _touchCount = 0; 26 | _waitRelease = false; 27 | } 28 | 29 | bool EdsTouch::IsTouchPressed() { 30 | pinMode(_gpioPin,INPUT); 31 | return digitalRead(_gpioPin); 32 | } 33 | 34 | uint8_t EdsTouch::CheckTouchAction() { 35 | 36 | uint8_t retCode = ETU_TOUCH_NONE; 37 | if (!IsReady()) return retCode; 38 | if (_dbg->require(210)) _dbg->log(210,0,"EdsTouch::CheckTouchAction()"); 39 | 40 | unsigned long currMs = millis(); // for safety, use same time in ms for all checking later 41 | bool currStatus = IsTouchPressed(); 42 | 43 | if (currStatus != _lastStatus) { 44 | if (currStatus) { 45 | _touchCount++; 46 | _touchReleaseMs = 0; 47 | if (!_touchStartMs) _touchStartMs = millis(); 48 | } else { 49 | _touchReleaseMs = currMs; 50 | } 51 | _lastStatus = currStatus; 52 | } 53 | 54 | if (_touchStartMs) { 55 | // Touch detected 56 | if ((millis() - _touchStartMs) > _touchDetectPeriod) { 57 | if (!_waitRelease) { 58 | _waitRelease = true; 59 | // End of detection 60 | if (_touchCount == 1) { 61 | if (currStatus) { 62 | // Long hold 63 | if (_dbg->require(200)) _dbg->log(200,0,"EdsTouch: Long hold"); 64 | retCode = ETU_TOUCH_LONG; 65 | } else { 66 | // Single click 67 | if (_dbg->require(200)) _dbg->log(200,0,"EdsTouch: Single click"); 68 | retCode = ETU_TOUCH_SINGLE; 69 | } 70 | } else if (_touchCount == 2) { 71 | // double click 72 | if (_dbg->require(200)) _dbg->log(200,0,"EdsTouch: Double click"); 73 | retCode = ETU_TOUCH_DOUBLE; 74 | } else { 75 | // triple click 76 | if (_dbg->require(200)) _dbg->log(200,0,"EdsTouch: Triple click"); 77 | retCode = ETU_TOUCH_TRIPLE; 78 | } 79 | } else { 80 | // must release for reasonable time to stop action 81 | if ((!currStatus) && ((currMs - _touchReleaseMs) > _touchReleasePeriod)) { 82 | ResetTouchAction(); 83 | } 84 | } 85 | } 86 | } 87 | 88 | return retCode; 89 | } 90 | 91 | 92 | bool EdsTouch::GetData() { 93 | _thisDataReady = false; 94 | if (!IsReady()) return false; 95 | _lastDataReady = false; 96 | uint8_t touchAction = CheckTouchAction(); 97 | _data->SetData(_Device, _DevId, 0, touchAction); 98 | _lastDataReady = true; 99 | _thisDataReady = true; 100 | _lastReportValue = touchAction; 101 | _lastReportMS = millis(); 102 | if ((touchAction != 0) && _dbg->require(100)) _dbg->log(100,0,"EdsTouch::GetData() => %d", touchAction); 103 | return (touchAction != 0); 104 | } 105 | 106 | void EdsTouch::PostHandler(bool eventMatched, bool isRelated, bool pending) { 107 | if (!IsReady()) return; 108 | if (_dbg->require(210)) _dbg->log(210,0,"EdsTouch::PostHandler(%d,%d,%d)",eventMatched, isRelated, pending); 109 | // Cannot use generic routine as it need to check the value != 0 to confirm value is not handled 110 | if ((_thisDataReady) && (_lastReportValue != 0) && ((!eventMatched) || (isRelated))) { 111 | _nextReportMs = millis() + _delayCheckMs; 112 | } else { 113 | _nextReportMs = millis() + _continueCheckMs; 114 | } 115 | } 116 | 117 | -------------------------------------------------------------------------------- /lib/EventHandler/drivers/EdsTouch/EdsTouch.h: -------------------------------------------------------------------------------- 1 | #ifndef _EDS_TOUCH_H_ 2 | #define _EDS_TOUCH_H_ 3 | 4 | 5 | // Keyword: ETU_ 6 | 7 | #include "EventHandler.h" 8 | #include "EventDataSource.h" 9 | #include "SSBoard.h" 10 | 11 | #define ETU_TOUCH_NONE 0 12 | #define ETU_TOUCH_SINGLE 1 13 | #define ETU_TOUCH_DOUBLE 2 14 | #define ETU_TOUCH_TRIPLE 3 15 | #define ETU_TOUCH_LONG 0xFF 16 | 17 | /* 18 | * Event Data Source for Battery 19 | * 20 | */ 21 | 22 | class EdsTouch : public EventDataSource { 23 | public: 24 | EdsTouch(EventData *data, MyDebugger *dbg, byte devId = 0); 25 | ~EdsTouch(); 26 | 27 | void Setup(uint8_t gpioPin, unsigned long touchDetectPeriod, unsigned long touchReleasePeriod); 28 | bool GetData() override; 29 | void PostHandler(bool eventMatched, bool isRelated, bool pending) override; 30 | 31 | private: 32 | uint8_t _gpioPin; 33 | 34 | unsigned long _touchDetectPeriod = 1500; 35 | unsigned long _touchReleasePeriod = 1000; 36 | bool _lastStatus = 0; 37 | unsigned long _touchStartMs = 0; 38 | unsigned long _touchReleaseMs = 0; 39 | unsigned long _touchCount = 0; 40 | bool _waitRelease = false; 41 | 42 | uint16_t _lastReportValue = 0; 43 | 44 | void ResetTouchAction(); 45 | bool IsTouchPressed(); 46 | uint8_t CheckTouchAction(); 47 | }; 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /lib/MP3TF16P/MP3TF16P.cpp: -------------------------------------------------------------------------------- 1 | #include "MP3TF16P.h" 2 | 3 | MP3TF16P::MP3TF16P(SoftwareSerial *ssData) 4 | { 5 | initObject(ssData, NULL); 6 | } 7 | 8 | MP3TF16P::MP3TF16P(SoftwareSerial *ssData, HardwareSerial *hsDebug) 9 | { 10 | initObject(ssData, hsDebug); 11 | } 12 | 13 | void MP3TF16P::initObject(SoftwareSerial *ssData, HardwareSerial *hsDebug) { 14 | _ss = ssData; 15 | _dbg = hsDebug; 16 | _enableDebug = (_dbg != NULL); 17 | } 18 | 19 | MP3TF16P::~MP3TF16P() { 20 | _ss = NULL; 21 | _dbg = NULL; 22 | } 23 | 24 | bool MP3TF16P::setDebug(bool debug) { 25 | if (_dbg == NULL) return false; 26 | _enableDebug = debug; 27 | return _enableDebug; 28 | } 29 | 30 | void MP3TF16P::begin() { 31 | _ss->begin(MP3_BAUD); 32 | delay(100); 33 | } 34 | 35 | void MP3TF16P::end() { 36 | _ss->end(); 37 | } 38 | 39 | void MP3TF16P::showCommand() { 40 | if (!_enableDebug) return; 41 | _dbg->printf("%08ld MP3 OUT>>", millis()); 42 | for (int i = 0; i < 10; i++) { 43 | _dbg->printf(" %02X", _buf[i]); 44 | } 45 | _dbg->println(); 46 | } 47 | 48 | bool MP3TF16P::sendCommand(bool expectReturn) { 49 | uint16_t sum = 0; 50 | for (int i = 1; i < 7; i++) { 51 | sum += _buf[i]; 52 | } 53 | sum = 0 - sum; 54 | _buf[7] = sum >> 8; 55 | _buf[8] = sum & 0xFF; 56 | 57 | if (_enableDebug) showCommand(); 58 | 59 | clearRxBuffer(); 60 | _ss->flush(); 61 | //_ss->enableTx(true); 62 | //delay(1); 63 | _ss->write(_buf, 10); 64 | //_ss->enableTx(false); 65 | if (expectReturn) return checkReturn(); 66 | clearRxBuffer(); 67 | return true; 68 | } 69 | 70 | void MP3TF16P::clearRxBuffer() { 71 | while (_ss->available() ) { 72 | _ss->read(); 73 | delay(2); 74 | } 75 | } 76 | 77 | bool MP3TF16P::checkReturn() { 78 | if (_enableDebug) _dbg->printf("%08ld MP3 checkReturn\n", millis()); 79 | unsigned long startMs = millis(); 80 | resetReturnBuffer(); 81 | byte ch; 82 | while ( ((millis() - startMs) < MP3_COMMAND_WAIT_TIME) && (!_ss->available()) ) delay(1); 83 | // unsigned long endMs = millis(); 84 | // if (_enableDebug) _dbg->printf("MP3 wait return from %d to %d\n", startMs, endMs); 85 | 86 | if (!_ss->available()) { 87 | // if (_enableDebug) _dbg->printf("MP3 no return after %d ms\n", MP3_COMMAND_WAIT_TIME); 88 | return false; 89 | } 90 | if (_enableDebug) { 91 | _dbg->printf("%08ld MP3 IN>>>", millis()); 92 | } 93 | // 10 ms is almost good for 10byte data 94 | delay(10); 95 | while (_ss->available()) { 96 | ch = (byte) _ss->read(); 97 | _retBuf[_retCnt++] = ch; 98 | if (_enableDebug) { 99 | _dbg->printf(" %02X", ch); 100 | /* 101 | _dbg->print((ch < 0x10 ? " 0" : " ")); 102 | _dbg->print(ch, HEX); 103 | */ 104 | } 105 | // extra delay to make sure transaction completed 106 | // ToDo: check data end? 107 | // But what can i do if not ended, seems not logical as only few bytes returned. 108 | // 1ms is already more than enough. 109 | if (!_ss->available()) delay(2); 110 | } 111 | if (_enableDebug) _dbg->println(); 112 | return true; 113 | } 114 | 115 | void MP3TF16P::sendSingleByteCommand(byte cmd) { 116 | resetCommandBuffer(); 117 | _buf[3] = cmd; 118 | sendCommand(); 119 | } 120 | 121 | void MP3TF16P::sendCommand(byte cmd, byte p1, byte p2) { 122 | resetCommandBuffer(); 123 | _buf[3] = cmd; 124 | _buf[5] = p1; 125 | _buf[6] = p2; 126 | sendCommand(); 127 | } 128 | 129 | void MP3TF16P::sendCommand(byte cmd, uint16_t parm) { 130 | byte p1 = parm >> 8; 131 | byte p2 = parm; 132 | sendCommand(cmd, p1, p2); 133 | } 134 | 135 | void MP3TF16P::playFile(uint16_t seq) { 136 | resetCommandBuffer(); 137 | _buf[3] = 0x03; 138 | _buf[5] = seq >> 8; 139 | _buf[6] = seq; 140 | sendCommand(); 141 | } 142 | 143 | void MP3TF16P::setVol(byte vol) { 144 | if (vol > 30) vol = 30; 145 | resetCommandBuffer(); 146 | _buf[3] = 0x06; 147 | _buf[6] = vol; 148 | sendCommand(); 149 | _vol = vol; 150 | } 151 | 152 | void MP3TF16P::playFolderFile(byte folder, byte seq) { 153 | resetCommandBuffer(); 154 | _buf[3] = 0x0F; 155 | _buf[5] = folder; 156 | _buf[6] = seq; 157 | sendCommand(); 158 | } 159 | 160 | // 7E FF 06 12 00 xx xx ?? ?? EF 161 | void MP3TF16P::playMp3File(uint16_t seq) { 162 | resetCommandBuffer(); 163 | _buf[3] = 0x12; 164 | _buf[5] = seq >> 8; 165 | _buf[6] = seq & 0xFF; 166 | sendCommand(); 167 | } 168 | 169 | void MP3TF16P::playAdFile(byte seq) { 170 | resetCommandBuffer(); 171 | _buf[3] = 0x13; 172 | _buf[6] = seq; 173 | sendCommand(); 174 | } 175 | 176 | uint8_t MP3TF16P::getVol() { 177 | return _vol; 178 | // Function will return local record instead of reading from MP3 module as it has been changed to single wire mode, no return from MP3 module 179 | /* 180 | resetCommandBuffer(); 181 | _buf[3] = 0x43; 182 | sendCommand(true); 183 | if (_retCnt == 10) { 184 | if ((_retBuf[0] == 0x7E) && (_retBuf[1] == 0xFF) && (_retBuf[2] == 0x06) && (_retBuf[3] == 0x43) && (_retBuf[9] == 0xEF)) { 185 | return _retBuf[6]; 186 | } 187 | } 188 | return 0xFF; 189 | */ 190 | } 191 | 192 | void MP3TF16P::adjVol(int diff) { 193 | if (diff == 0) return; 194 | uint8_t currVol = getVol(); 195 | if (currVol == 0xFF) return; 196 | if ((currVol == 0) && (diff < 0)) return; 197 | if ((currVol >= 30) && (diff > 0)) return; 198 | int iVol = currVol; 199 | iVol += diff; 200 | if (iVol < 0) iVol = 0; 201 | if (iVol > 30) iVol = 30; 202 | currVol = iVol; 203 | setVol(currVol); 204 | } -------------------------------------------------------------------------------- /lib/MP3TF16P/MP3TF16P.h: -------------------------------------------------------------------------------- 1 | #ifndef _MP3_TF_16P_H_ 2 | #define _MP3_TF_16P_H_ 3 | 4 | #include 5 | #include "HardwareSerial.h" 6 | #include "SoftwareSerial.h" 7 | 8 | #define MP3_BAUD 9600 9 | #define MP3_COMMAND_BUFFER_SIZE 10 10 | #define MP3_RETURN_BUFFER_SIZE 20 // Actually, 10 is enough, just for saftey 11 | 12 | #define MP3_COMMAND_WAIT_TIME 500 13 | 14 | const byte MP3_CMD[] = {0x7E, 0xFF, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xEF}; 15 | 16 | class MP3TF16P { 17 | public: 18 | MP3TF16P(SoftwareSerial *ssData); 19 | MP3TF16P(SoftwareSerial *ssData, HardwareSerial *hsDebug); 20 | ~MP3TF16P(); 21 | bool setDebug(bool debug); 22 | void begin(); 23 | void end(); 24 | void sendSingleByteCommand(byte cmd); 25 | void sendCommand(byte cmd, byte p1, byte p2); 26 | void sendCommand(byte cmd, uint16_t parm); 27 | inline void playNext() { sendSingleByteCommand(0x01); } 28 | inline void playPrev() { sendSingleByteCommand(0x02); } 29 | void playFile(uint16_t seq); // 0x03 30 | void setVol(byte vol); // 0x06 31 | inline void resetDevice() { sendSingleByteCommand(0x0C); } 32 | inline void play() { sendSingleByteCommand(0x0D); } 33 | inline void pause() { sendSingleByteCommand(0x0E); } 34 | void playFolderFile(byte folder, byte seq); // 0x0F 35 | inline void playAllLoop() { sendSingleByteCommand(0x11); } 36 | void playMp3File(uint16_t seq); // 0x12 37 | void playAdFile(byte seq); // 0x13 38 | inline void stopPlayAd() { sendSingleByteCommand(0x15); } 39 | inline void stop() { sendSingleByteCommand(0x16); } 40 | inline void playRandom() { sendSingleByteCommand(0x18); } 41 | uint8_t getVol(); 42 | void adjVol(int diff); 43 | inline void volUp() { adjVol(1); } 44 | inline void volDown() { adjVol(-1); } 45 | 46 | 47 | private: 48 | void initObject(SoftwareSerial *ssData, HardwareSerial *hsDebug); 49 | inline void resetCommandBuffer() { memcpy(_buf, MP3_CMD, MP3_COMMAND_BUFFER_SIZE); } 50 | inline void resetReturnBuffer() { memset(_retBuf, 0, MP3_RETURN_BUFFER_SIZE); _retCnt = 0; } 51 | inline bool sendCommand() { return sendCommand(false); } 52 | bool sendCommand(bool expectReturn); 53 | void showCommand(); 54 | void clearRxBuffer(); 55 | bool checkReturn(); 56 | SoftwareSerial *_ss; 57 | HardwareSerial *_dbg; 58 | bool _enableDebug; 59 | byte _buf[MP3_COMMAND_BUFFER_SIZE]; 60 | byte _retBuf[MP3_RETURN_BUFFER_SIZE]; 61 | byte _retCnt; 62 | byte _vol = 0xFF; 63 | 64 | }; 65 | 66 | #endif -------------------------------------------------------------------------------- /lib/MyData/MyData.cpp: -------------------------------------------------------------------------------- 1 | #include "MyData.h" 2 | 3 | MyData::MyData() { 4 | _type = MD_UNKNOWN; 5 | } 6 | 7 | 8 | MyData::MyData(String pLabel, String pKey, bool pValue, uint8_t pOffset) 9 | : _label(pLabel), _key(pKey), _offset(pOffset) 10 | { 11 | _type = MD_BOOL; 12 | _default._bool = pValue; 13 | _value._bool = pValue; 14 | _size = 1; 15 | } 16 | 17 | MyData::MyData(String pLabel, String pKey, int64_t pValue, uint8_t pOffset, uint8_t pSize) 18 | : _label(pLabel), _key(pKey), _offset(pOffset) 19 | { 20 | _type = MD_INTEGER; 21 | _default._int = pValue; 22 | _value._int = pValue; 23 | // can only be int8, int16, int32, int64, i.e. 1,2,4,8 24 | if ((pSize == 1) || (pSize == 2) || (pSize == 4) || (pSize == 8)) { 25 | _size = pSize; 26 | } else { 27 | _size = sizeof(int64_t); 28 | } 29 | } 30 | 31 | 32 | MyData::MyData(String pLabel, String pKey, double pValue, uint8_t pOffset, uint8_t pSize) 33 | : _label(pLabel), _key(pKey), _offset(pOffset) 34 | { 35 | _type = MD_FLOAT; 36 | _default._float = pValue; 37 | _value._float = pValue; 38 | // can only be float or double 39 | _size = (pSize == sizeof(float) ? pSize : sizeof(double)); 40 | } 41 | 42 | MyData::MyData(String pLabel, String pKey, String pValue, uint8_t pOffset, uint8_t pSize) 43 | : _label(pLabel), _key(pKey), _offset(pOffset) 44 | { 45 | _type = MD_STRING; 46 | _default._string = pValue; 47 | _value._string = pValue; 48 | _size = pSize; 49 | } 50 | 51 | void MyData::reset() { 52 | clear(); 53 | switch (_type) { 54 | case MD_BOOL: 55 | _value._bool = _default._bool; 56 | break; 57 | case MD_INTEGER: 58 | _value._int = _default._int; 59 | break; 60 | case MD_FLOAT: 61 | _value._float = _default._float; 62 | break; 63 | case MD_STRING: 64 | _value._string = _default._string; 65 | break; 66 | } 67 | } 68 | 69 | void MyData::clear() { 70 | // Clear the value for any type 71 | _value._bool = false; 72 | _value._int = 0; 73 | _value._float = 0.0; 74 | _value._string = ""; 75 | } 76 | 77 | 78 | bool MyData::setBool(bool pValue) { 79 | if (!isBool()) return false; 80 | _value._bool = pValue; 81 | return true; 82 | } 83 | 84 | bool MyData::setInt(int64_t pValue) { 85 | if (!isInt()) return false; 86 | _value._int = pValue; 87 | return true; 88 | } 89 | 90 | bool MyData::setFloat(double pValue) { 91 | if (!isFloat()) return false; 92 | _value._float = pValue; 93 | return true; 94 | } 95 | 96 | bool MyData::setString(String pValue) { 97 | if (!isString()) return false; 98 | _value._string = pValue; 99 | return true; 100 | } 101 | 102 | String MyData::getPrintable() { 103 | switch(_type) { 104 | case MD_BOOL: 105 | return (_value._bool ? "True" : "False"); 106 | case MD_INTEGER: 107 | return myUtil::getInt64String(_value._int); 108 | case MD_FLOAT: 109 | return String(_value._float); 110 | case MD_STRING: 111 | return _value._string; 112 | } 113 | return "<>"; 114 | } 115 | 116 | bool MyData::toBuffer(uint8_t *buffer) { 117 | if (_size == 0) return false; // This data cannot be copied 118 | uint8_t *target = buffer + _offset; 119 | switch (_type) { 120 | case MD_BOOL: 121 | { 122 | buffer[_offset] = getBool(); 123 | return true; 124 | } 125 | case MD_INTEGER: 126 | { 127 | int64_t value = getInt(); 128 | memcpy(target, &value, _size); 129 | return true; 130 | } 131 | case MD_FLOAT: 132 | { 133 | double value = getFloat(); 134 | memcpy(target, &value, _size); 135 | return true; 136 | } 137 | case MD_STRING: 138 | { 139 | memset(target, 0, _size); 140 | size_t copySize = getString().length(); 141 | if (copySize > _size) copySize = _size; 142 | memcpy(target, getString().c_str(), copySize); 143 | return true; 144 | } 145 | 146 | 147 | } 148 | return false; 149 | } 150 | 151 | bool MyData::fromBuffer(uint8_t *buffer) { 152 | if (_size == 0) return false; // This data cannot be copied 153 | uint8_t *source = buffer + _offset; 154 | switch (_type) { 155 | case MD_BOOL: 156 | { 157 | bool value = (bool) (*source); 158 | _value._bool = value; 159 | return true; 160 | } 161 | case MD_INTEGER: 162 | { 163 | _value._int = 0; 164 | memcpy(&_value._int, source, _size); 165 | return true; 166 | } 167 | case MD_FLOAT: 168 | { 169 | if (_size == sizeof(float)) { 170 | float *fptr = (float *) source; 171 | _value._float = (*fptr); 172 | } else { 173 | double *dptr = (double *) source; 174 | _value._float = (*dptr); 175 | } 176 | return true; 177 | } 178 | case MD_STRING: 179 | { 180 | char buffer[_size+1]; 181 | memset(buffer, 0, _size+1); 182 | memcpy(buffer, source, _size); 183 | _value._string = String(buffer); 184 | return true; 185 | } 186 | } 187 | return false; 188 | 189 | } 190 | -------------------------------------------------------------------------------- /lib/MyData/MyData.h: -------------------------------------------------------------------------------- 1 | #ifndef _MY_DATA_H_ 2 | #define _MY_DATA_H_ 3 | 4 | #include 5 | #include "myUtil.h" 6 | 7 | #define MD_UNKNOWN (0) 8 | #define MD_BOOL (1 << 0) 9 | #define MD_INTEGER (1 << 1) 10 | #define MD_FLOAT (1 << 2) 11 | #define MD_STRING (1 << 3) 12 | 13 | class MyData { 14 | public: 15 | 16 | // Becarefule, it seems that bool, int64_t cannot be distinguished 17 | MyData(); 18 | MyData(String pLabel, String pKey, bool pValue, uint8_t pOffset = 0); 19 | MyData(String pLabel, String pKey, int64_t pValue, uint8_t pOffset = 0, uint8_t pSize = 0); 20 | MyData(String pLabel, String pKey, double pValue, uint8_t pOffset = 0, uint8_t pSize = 0); 21 | MyData(String pLabel, String pKey, String pValue, uint8_t pOffset = 0, uint8_t pSize = 0); 22 | 23 | void reset(); 24 | void clear(); 25 | 26 | inline uint8_t type() { return _type; } 27 | inline String label() { return _label; } 28 | inline String key() { return _key; } 29 | inline uint8_t offset() { return _offset; } 30 | inline uint8_t size() { return _size; } 31 | 32 | 33 | inline bool getBool() { return _value._bool; } 34 | inline int64_t getInt() { return _value._int; } 35 | inline double getFloat() { return _value._float; } 36 | inline String getString() { return _value._string; } 37 | 38 | inline bool getBoolDefault() { return _default._bool; } 39 | inline int64_t getIntDefault() { return _default._int; } 40 | inline double getFloatDefault() { return _default._float; } 41 | inline String getStringDefault() { return _default._string; } 42 | String getPrintable(); 43 | 44 | 45 | // Becarefule, it seems that bool, int64_t cannot be distinguished 46 | inline bool set(bool pValue) { return setBool(pValue); } 47 | inline bool set(int64_t pValue) { return setInt(pValue); } 48 | inline bool set(double pValue) { return setFloat(pValue); } 49 | inline bool set(String pValue) { return setString(pValue); } 50 | 51 | bool setBool(bool pValue); 52 | bool setInt(int64_t pValue); 53 | bool setFloat(double pValue); 54 | bool setString(String pValue); 55 | 56 | inline bool isBool() { return (_type == MD_BOOL); } 57 | inline bool isInt() { return (_type == MD_INTEGER); } 58 | inline bool isFloat() { return (_type == MD_FLOAT); } 59 | inline bool isString() { return (_type == MD_STRING); } 60 | 61 | bool toBuffer(uint8_t *buffer); 62 | bool fromBuffer(uint8_t *buffer); 63 | 64 | 65 | private: 66 | struct CONTENT{ 67 | bool _bool; 68 | int64_t _int; 69 | double _float; 70 | String _string; 71 | }; 72 | 73 | uint8_t _type; 74 | String _label; 75 | String _key; 76 | CONTENT _default; 77 | CONTENT _value; 78 | uint8_t _offset; 79 | uint8_t _size; 80 | }; 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /lib/MyDebugger/MyDebugger.cpp: -------------------------------------------------------------------------------- 1 | #include "MyDebugger.h" 2 | 3 | MyDebugger::MyDebugger() { 4 | _enableDebug = false; 5 | _dbg = NULL; 6 | } 7 | 8 | MyDebugger::~MyDebugger() { 9 | 10 | } 11 | 12 | void MyDebugger::setOutput(Stream *output, bool enableDebug) { 13 | _dbg = output; 14 | _enableDebug = (enableDebug && (_dbg != NULL)); 15 | } 16 | 17 | void MyDebugger::enableDebug(bool value) { 18 | _enableDebug = value; 19 | } 20 | 21 | size_t MyDebugger::write(uint8_t b) { 22 | if ((!_enableDebug) || (_dbg == NULL)) return 0; 23 | return _dbg->write(b); 24 | } 25 | 26 | void MyDebugger::setLogLevel(uint8_t level) { 27 | _level = level; 28 | } 29 | 30 | bool MyDebugger::require(uint8_t level) { 31 | return (_level >= level); 32 | } 33 | 34 | // To provide standard message for debug 35 | // It can still use _dbg to send debug message if standard message is not requried. 36 | // Level: 37 | // 0 - critical, must be show anytime, or for temporary debug purpose 38 | // 100 - major result return 39 | // 110 - tracing, major function 40 | // 200 - minor result return 41 | // 210 - tracing, minor function 42 | // 43 | // Type: 44 | // 0 - full: header + content + footer 45 | // 1 - first: header + content 46 | // 2 - middle: content 47 | // 3 - end: content + footer 48 | // 49 | void MyDebugger::log(uint8_t level, uint8_t type, const char *format, ...) { 50 | 51 | if (!require(level)) return; 52 | 53 | switch (type) { 54 | case 0: 55 | case 1: 56 | _dbg->printf("%08ld ", millis()); 57 | break; 58 | } 59 | 60 | // source from Print::pritnf, as it cannot call printf wihtin the method 61 | // just modify write to _dbg->write as it's now not under Strem object 62 | char loc_buf[64]; 63 | char * temp = loc_buf; 64 | va_list arg; 65 | va_list copy; 66 | va_start(arg, format); 67 | va_copy(copy, arg); 68 | size_t len = vsnprintf(NULL, 0, format, arg); 69 | va_end(copy); 70 | if(len >= sizeof(loc_buf)){ 71 | temp = new char[len+1]; 72 | if(temp == NULL) { 73 | Serial.println("temp is null"); 74 | return; 75 | } 76 | } 77 | len = vsnprintf(temp, len+1, format, arg); 78 | _dbg->write((uint8_t*)temp, len); 79 | va_end(arg); 80 | if(len > 64){ 81 | delete[] temp; 82 | } 83 | // -- end of Print::print 84 | 85 | switch (type) { 86 | case 0: 87 | case 3: 88 | _dbg->printf("\n"); 89 | break; 90 | } 91 | delay(1); 92 | return; 93 | } 94 | 95 | 96 | // To provide standard message for debug 97 | // It can still use _dbg to send debug message if standard message is not requried. 98 | void MyDebugger::msg(const char *format, ...) { 99 | 100 | if (!isEnabled()) { 101 | return; 102 | } 103 | 104 | _dbg->printf("%08ld ", millis()); 105 | 106 | // source from Print::pritnf, as it cannot call printf wihtin the method 107 | // just modify write to _dbg->write as it's now not under Strem object 108 | char loc_buf[64]; 109 | char * temp = loc_buf; 110 | va_list arg; 111 | va_list copy; 112 | va_start(arg, format); 113 | va_copy(copy, arg); 114 | size_t len = vsnprintf(NULL, 0, format, arg); 115 | va_end(copy); 116 | if(len >= sizeof(loc_buf)){ 117 | temp = new char[len+1]; 118 | if(temp == NULL) { 119 | Serial.println("temp is null"); 120 | return; 121 | } 122 | } 123 | len = vsnprintf(temp, len+1, format, arg); 124 | _dbg->write((uint8_t*)temp, len); 125 | va_end(arg); 126 | if(len > 64){ 127 | delete[] temp; 128 | } 129 | // -- end of Print::print 130 | 131 | _dbg->printf("\n"); 132 | delay(1); 133 | return; 134 | } 135 | 136 | 137 | // To provide standard message for debug 138 | // It can still use _dbg to send debug message if standard message is not requried. 139 | void MyDebugger::msgh(const char *format, ...) { 140 | 141 | if (!isEnabled()) { 142 | return; 143 | } 144 | 145 | _dbg->printf("%08ld ", millis()); 146 | 147 | // source from Print::pritnf, as it cannot call printf wihtin the method 148 | // just modify write to _dbg->write as it's now not under Strem object 149 | char loc_buf[64]; 150 | char * temp = loc_buf; 151 | va_list arg; 152 | va_list copy; 153 | va_start(arg, format); 154 | va_copy(copy, arg); 155 | size_t len = vsnprintf(NULL, 0, format, arg); 156 | va_end(copy); 157 | if(len >= sizeof(loc_buf)){ 158 | temp = new char[len+1]; 159 | if(temp == NULL) { 160 | Serial.println("temp is null"); 161 | return; 162 | } 163 | } 164 | len = vsnprintf(temp, len+1, format, arg); 165 | _dbg->write((uint8_t*)temp, len); 166 | va_end(arg); 167 | if(len > 64){ 168 | delete[] temp; 169 | } 170 | // -- end of Print::print 171 | 172 | delay(1); 173 | return; 174 | } 175 | 176 | 177 | void MyDebugger::msgf(const char *format, ...) { 178 | 179 | if (!isEnabled()) { 180 | return; 181 | } 182 | 183 | // source from Print::pritnf, as it cannot call printf wihtin the method 184 | // just modify write to _dbg->write as it's now not under Strem object 185 | char loc_buf[64]; 186 | char * temp = loc_buf; 187 | va_list arg; 188 | va_list copy; 189 | va_start(arg, format); 190 | va_copy(copy, arg); 191 | size_t len = vsnprintf(NULL, 0, format, arg); 192 | va_end(copy); 193 | if(len >= sizeof(loc_buf)){ 194 | temp = new char[len+1]; 195 | if(temp == NULL) { 196 | Serial.println("temp is null"); 197 | return; 198 | } 199 | } 200 | len = vsnprintf(temp, len+1, format, arg); 201 | _dbg->write((uint8_t*)temp, len); 202 | va_end(arg); 203 | if(len > 64){ 204 | delete[] temp; 205 | } 206 | // -- end of Print::print 207 | 208 | delay(1); 209 | return; 210 | } -------------------------------------------------------------------------------- /lib/MyDebugger/MyDebugger.h: -------------------------------------------------------------------------------- 1 | #ifndef _MY_DEBUGGER_H_ 2 | #define _MY_DEBUGGER_H_ 3 | 4 | #ifdef ESP32 5 | #include 6 | #else 7 | #include 8 | #endif 9 | 10 | #include 11 | #include 12 | 13 | class MyDebugger : public Stream { 14 | public: 15 | MyDebugger(); 16 | ~MyDebugger(); 17 | void setOutput(Stream *output, bool enableDebug = true); 18 | void enableDebug(bool value); 19 | bool isEnabled() { return ((_enableDebug) && (_dbg != NULL)); } 20 | 21 | // Override pure virtual function in Stream 22 | int available() {return 0;} 23 | int read() {return 0;} 24 | int peek() { return 0; } 25 | void flush() { return; } 26 | 27 | void msg(const char * format, ...) __attribute__ ((format (printf, 2, 3))); 28 | void msgf(const char * format, ...) __attribute__ ((format (printf, 2, 3))); 29 | void msgh(const char * format, ...) __attribute__ ((format (printf, 2, 3))); // Shoudl be combined with msg, but need to change program, do it in next version 30 | 31 | bool require(uint8_t level); 32 | void log(uint8_t level, uint8_t type, const char * format, ...) __attribute__ ((format (printf, 4, 5))); 33 | 34 | void setLogLevel(uint8_t level); 35 | 36 | size_t write(uint8_t byte); 37 | using Print::write; // Since Strem has not define for write, must bring from Print 38 | 39 | private: 40 | bool _enableDebug; 41 | Stream *_dbg; 42 | uint8_t _level = 255; 43 | 44 | }; 45 | 46 | #endif -------------------------------------------------------------------------------- /lib/RobotServo/RobotServo.cpp: -------------------------------------------------------------------------------- 1 | #include "RobotServo.h" 2 | 3 | RobotServo::RobotServo() { 4 | } 5 | 6 | 7 | RobotServo::~RobotServo() { 8 | } 9 | 10 | -------------------------------------------------------------------------------- /lib/RobotServo/RobotServo.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROBOT_SERVO_H_ 2 | #define _ROBOT_SERVO_H_ 3 | 4 | #include "baseServo.h" 5 | 6 | #define _UBTServo_ 7 | //#define _HLServo_ 8 | 9 | #if defined(_UBTServo_) 10 | #include 11 | #elif defined(_HLServo_) 12 | #include 13 | #endif 14 | 15 | // Just a dummy class to control the interface for baseServo. 16 | // You can skip this class and use the servo class directly. 17 | class RobotServo { 18 | 19 | public: 20 | 21 | void setEnableTxCalback(void (*enableTxCallback)(bool)) { return _servo.setEnableTxCalback(enableTxCallback); } 22 | 23 | RobotServo(); 24 | bool begin(Stream *busPort, Stream *debugPort) { return _servo.begin(busPort, debugPort); } 25 | ~RobotServo(); 26 | 27 | byte servoType() { return _servo.servoType(); } 28 | 29 | void showInfo() { return _servo.showInfo(); } 30 | bool init(byte maxId, byte maxRetry) { return _servo.init(maxId, maxRetry); } 31 | bool initServo(byte id) { return _servo.initServo(id); } 32 | 33 | bool end() { return _servo.end(); } 34 | bool reset() { return _servo.reset(); } 35 | 36 | void enableDebug(bool value) { return _servo.enableDebug(value);} 37 | 38 | uint32_t getVersion(byte id) { return _servo.getVersion(id); } 39 | bool detectServo() { return _servo.detectServo(); } 40 | bool resetServo(byte id) { return _servo.resetServo(id); } 41 | 42 | bool exists(byte id) { return _servo.exists(id); } 43 | 44 | bool move(byte id, uint16_t pos, uint16_t time) { return _servo.move(id, pos, time); } 45 | bool move(byte cnt, byte *data) { return _servo.moveX(cnt, data); } 46 | bool goAngle(byte id, int16_t angle, uint16_t time) { return _servo.goAngle(id, angle, time); } 47 | 48 | uint16_t getAdjAngle(byte id) { return _servo.getAdjAngle(id); } 49 | uint16_t setAdjAngle(byte id, uint16 adjValue) {return _servo.setAdjAngle(id, adjValue); } 50 | 51 | byte servoCommand(byte *cmd) { return _servo.servoCommand(cmd); } 52 | 53 | byte setAngle(byte id, byte angle, byte minor) { return _servo.setAngle(id, angle, minor); } 54 | 55 | 56 | uint16_t lastPos(byte id) { return _servo.lastPos(id); } 57 | uint16_t lastAngle(byte id) { return _servo.lastAngle(id); } 58 | 59 | bool setLED(byte id, bool mode) { return _servo.setLED(id, mode); } 60 | bool setLED(bool mode) { return _servo.setLED(mode); } 61 | 62 | uint16_t getPos(byte id) { return _servo.getPos(id); } 63 | uint16_t getPos(byte id, bool lockAfterGet) { return _servo.getPos(id, lockAfterGet); } 64 | uint16_t getAngle(byte id) { return _servo.getAngle(id); } 65 | uint16_t getAngle(byte id, bool lockAfterGet) { return _servo.getAngle(id, lockAfterGet); } 66 | 67 | bool isLocked(byte id) { return _servo.isLocked(id); } 68 | bool lock(byte id) { return _servo.lock(id); } 69 | bool lock() { return _servo.lockAll(); } 70 | bool unlock(byte id) { return _servo.unlock(id); } 71 | bool unlock() { return _servo.unlockAll(); } 72 | 73 | int16_t minPos() { return _servo.minPos(); } 74 | int16_t maxPos() { return _servo.maxPos(); } 75 | int16_t minAngle() { return _servo.minAngle(); } 76 | int16_t maxAngle() { return _servo.maxAngle(); } 77 | 78 | int16_t servoCnt() { return _servo.servoCnt(); } 79 | 80 | private: 81 | 82 | #if defined(_UBTServo_) 83 | UBTServo _servo; 84 | #elif defined(_HLServo_) 85 | HLServo _servo; 86 | #endif 87 | }; 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /lib/RobotServo/baseServo.cpp: -------------------------------------------------------------------------------- 1 | #include "baseServo.h" 2 | 3 | baseServo::baseServo() { 4 | } 5 | 6 | baseServo::~baseServo() { 7 | 8 | } 9 | 10 | void baseServo::setEnableTxCalback(void (*enableTxCallback)(bool)) { 11 | _enableTxCallback = enableTxCallback; 12 | } 13 | 14 | void baseServo::enableTx(bool mode) { 15 | if (_enableTxCallback != NULL) _enableTxCallback(mode); 16 | } 17 | 18 | 19 | bool baseServo::begin(Stream *busPort, Stream *debugPort) { 20 | _bus = busPort; 21 | _dbg.setOutput(debugPort); 22 | _arrayReady = false; 23 | initBus(); 24 | return true; 25 | } 26 | 27 | bool baseServo::end() { 28 | return true; 29 | } 30 | 31 | bool baseServo::init(byte maxId, byte maxRetry) { 32 | if (!_arrayReady) { 33 | _maxId = maxId; 34 | _maxRetry = maxRetry; 35 | 36 | int arraySize = _maxId + 1; 37 | _servo = new bool[arraySize]; 38 | _led = new byte[arraySize]; 39 | _isLocked = new bool[arraySize]; 40 | _lastPos = new int16_t[arraySize]; 41 | _isServo = new bool[arraySize]; 42 | 43 | memset(_servo, false, arraySize); 44 | memset(_led, false, arraySize); 45 | memset(_isLocked, false, arraySize); 46 | for (int i = 0; i < arraySize; i++) _lastPos[i] = _INVALID_POS; 47 | memset(_isServo, false, arraySize); 48 | 49 | _servoCnt = 0; 50 | _arrayReady = true; 51 | } 52 | return true; 53 | } 54 | 55 | void baseServo::enableDebug(bool value) { 56 | return _dbg.enableDebug(value); 57 | } 58 | 59 | void baseServo::showInfo() { 60 | if (_dbg.isEnabled()) { 61 | if (_arrayReady) { 62 | _dbg.println("-- Setup is ready"); 63 | _dbg.printf("-- Max ID: %d\n-- Max Retry: %d\n", _maxId, _maxRetry); 64 | _dbg.printf("-- Position: %d - %d [%d] (%s)\n", _MIN_POS, _MAX_POS, _INVALID_POS, 65 | (_MULTI_SERVO_COMMAND ? "allow multiple sevo command" : "single servo command only")); 66 | _dbg.printf("-- %d servo detected: \n", _servoCnt); 67 | if (_servoCnt) { 68 | for (byte id = 1; id <= _maxId; id++) { 69 | if (_servo[id]) { 70 | _dbg.printf("id: %02d (", id); 71 | if (validPos(_lastPos[id])) { 72 | _dbg.print(_lastPos[id]); 73 | } else { 74 | _dbg.print("Unknown"); 75 | } 76 | _dbg.printf(") %s ; LED: %s \n", (_isLocked[id] ? "locked" : "unlocked"), (_led[id] ? "ON" : "OFF")); 77 | } 78 | } 79 | _dbg.println(); 80 | } 81 | } else { 82 | _dbg.println("-- Setup is not yet completed"); 83 | } 84 | } 85 | } 86 | 87 | 88 | bool baseServo::detectServo() { 89 | 90 | memset(_servo, 0, _maxId + 1); 91 | _servoCnt = 0; 92 | for (byte id = 1; id <= _maxId; id++) { 93 | if (getVersion(id)) { 94 | _servo[id] = true; 95 | _servoCnt++; 96 | initServo(id); 97 | getPos(id, true); 98 | } 99 | } 100 | return true; 101 | } 102 | 103 | bool baseServo::validId(byte id) { 104 | if (!_arrayReady) return false; 105 | if (id > _maxId) return false; 106 | return true; 107 | } 108 | 109 | bool baseServo::validPos(int16_t pos) { 110 | if (pos == _INVALID_POS) return false; 111 | if ((pos < _MIN_POS) || (pos > _MAX_POS)) return false; 112 | return true; 113 | } 114 | 115 | bool baseServo::isLocked(byte id) { 116 | if (!validId(id)) return false; 117 | if (_isLocked[id] && validPos(_lastPos[id])) return true; 118 | return false; 119 | } 120 | 121 | bool baseServo::lockAll() { 122 | bool success = true; 123 | for (byte id = 1; id <= _maxId; id++) { 124 | if (_servo[id]) { 125 | success &= lock(id); 126 | } 127 | } 128 | return success; 129 | } 130 | 131 | bool baseServo::unlockAll() { 132 | bool success = true; 133 | for (byte id = 1; id <= _maxId; id++) { 134 | if (_servo[id]) { 135 | success &= unlock(id); 136 | } 137 | } 138 | return success; 139 | } 140 | 141 | 142 | bool baseServo::moveX(byte cnt, byte *data) { 143 | if (cnt == 0) return true; 144 | int moveCnt = 0; 145 | BS_MOVEPARM *mp; 146 | for (int i = 0; i < cnt; i++) { 147 | mp = (BS_MOVEPARM *) (data + i * sizeof(BS_MOVEPARM)); 148 | if (_dbg.isEnabled()) _dbg.printf("%d: id: %d ; pos=%d ; time=%d \n", i, mp->id, mp->pos, mp->time); 149 | if (move(mp->id, mp->pos, mp->time)) moveCnt ++; 150 | } 151 | return (moveCnt == cnt); 152 | } 153 | -------------------------------------------------------------------------------- /lib/RobotServo/baseServo.h: -------------------------------------------------------------------------------- 1 | #ifndef _BASE_SERVO_H_ 2 | #define _BASE_SERVO_H_ 3 | 4 | #include 5 | #include "MyDebugger.h" 6 | 7 | #define BS_TYPE_UNKNOWN 0 8 | #define BS_TYPE_UBT 1 9 | #define BS_TYPE_HaiLzd 2 10 | 11 | 12 | struct BS_MOVEPARM { 13 | byte id; 14 | int16_t pos; 15 | uint16_t time; 16 | }; 17 | 18 | class baseServo { 19 | 20 | protected: 21 | 22 | Stream *_bus = NULL; 23 | MyDebugger _dbg; 24 | 25 | void (*_enableTxCallback)(bool); 26 | void enableTx(bool); 27 | 28 | // servo depended constant; to be setup by servo 29 | int16_t _MIN_POS = 0; 30 | int16_t _MAX_POS = 180; 31 | int16_t _MIN_ANGLE = 0; 32 | int16_t _MAX_ANGLE = 180; 33 | int16_t _INVALID_POS = 999; 34 | bool _MULTI_SERVO_COMMAND = false; 35 | 36 | bool _arrayReady = false; 37 | byte _maxId = 0; 38 | bool* _servo = NULL; 39 | byte* _led = NULL; 40 | bool* _isLocked = NULL; 41 | int16_t* _lastPos = NULL; 42 | bool* _isServo = NULL; 43 | byte _servoCnt = 0; 44 | 45 | byte _maxRetry = 0; 46 | 47 | public: 48 | void setEnableTxCalback(void (*enableTxCallback)(bool)); 49 | 50 | baseServo(); 51 | virtual ~baseServo(); 52 | bool begin(Stream *busPort, Stream *debugPort); 53 | 54 | bool init(byte maxId, byte maxRetry); 55 | bool setBaud(uint32_t baud); 56 | bool end(); 57 | void enableDebug(bool value); 58 | 59 | bool detectServo(); 60 | bool exists(byte id) { return (validId(id) ? _servo[id] : false); } 61 | 62 | int16_t minPos() { return _MIN_POS; } 63 | int16_t maxPos() { return _MAX_POS; } 64 | int16_t minAngle() { return _MIN_ANGLE; } 65 | int16_t maxAngle() { return _MAX_ANGLE; } 66 | int16_t servoCnt() { return _servoCnt; } 67 | 68 | 69 | // Method can be overrided 70 | virtual void initBus() { return; } 71 | virtual bool initServo(byte id) { return true; } 72 | virtual void showInfo(); 73 | 74 | virtual bool validId(byte id); 75 | virtual bool validPos(int16_t); 76 | 77 | virtual uint16_t angle2pos(uint16_t angle) { return map(angle, _MIN_ANGLE, _MAX_ANGLE, _MIN_POS, _MAX_POS); } 78 | virtual uint16_t pos2angle(uint16_t pos) { return map(pos, _MIN_POS, _MAX_POS, _MIN_ANGLE, _MAX_ANGLE); } 79 | 80 | virtual bool isLocked(byte id); 81 | virtual bool lockAll(); 82 | virtual bool unlockAll(); 83 | virtual bool moveX(byte cnt, byte *data); 84 | virtual bool goAngle(byte id, int16_t angle, uint16_t time) { return move(id, angle2pos(angle), time); } 85 | 86 | virtual uint16_t lastPos(byte id) { return (validId(id) ? _lastPos[id] : _INVALID_POS); } 87 | virtual uint16_t lastAngle(byte id) { return (validId(id) ? pos2angle(_lastPos[id]) : _INVALID_POS); } 88 | 89 | virtual uint16_t getAngle(byte id) { return (validId(id) ? pos2angle(getPos(id)) : _INVALID_POS); } 90 | virtual uint16_t getAngle(byte id, bool lockAfterGet) { return (validId(id) ? pos2angle(getPos(id, lockAfterGet)) : _INVALID_POS); } 91 | 92 | // Methods MUST be overrided 93 | virtual byte servoType() = 0; 94 | virtual bool reset() = 0; 95 | virtual uint32_t getVersion(byte id) = 0; 96 | 97 | 98 | virtual bool resetServo() = 0; 99 | virtual bool resetServo(byte id) = 0; 100 | 101 | virtual bool move(byte id, int16_t pos, uint16_t time) = 0; 102 | virtual bool setLED(byte id, bool mode) = 0; 103 | virtual bool setLED(bool mode) = 0; 104 | 105 | virtual int16_t getPos(byte id) = 0; 106 | virtual int16_t getPos(byte id, bool lockAfterGet) = 0; 107 | 108 | virtual bool lock(byte id) = 0; 109 | virtual bool unlock(byte id) = 0; 110 | 111 | virtual uint16_t getAdjAngle(byte id) = 0; 112 | virtual uint16_t setAdjAngle(byte id, uint16 adjValue) = 0; 113 | virtual byte servoCommand(byte* cmd) = 0; 114 | 115 | virtual byte setAngle(byte id, byte angle, byte minor) = 0; 116 | 117 | private: 118 | 119 | }; 120 | 121 | #endif 122 | -------------------------------------------------------------------------------- /lib/RobotServo/drivers/HaiLzd/HLServo.h: -------------------------------------------------------------------------------- 1 | #ifndef _HL_Servo_H_ 2 | #define _HL_Servo_H_ 3 | 4 | // Profile for Hailzd Servo 5 | // 6 | #include "baseServo.h" 7 | #define HL_RETURN_BUFFER_SIZE 64 8 | #define HL_COMMAND_WAIT_TIME 5 // 3ms is more than enough 9 | 10 | #define HL_SERVO_MODE 1 // 180 degree 11 | #define HL_POS_MAX 2500 // This is a limitation due to old version action file, if servo pos > POS_MAX, move it to POS_MAX 12 | #define HL_ANGLE_MAX 180 // This is a limitation due to old version action file, if servo angle > ANGLE_MAX, move it to ANGLE_MAX 13 | 14 | class HLServo : public baseServo 15 | { 16 | public: 17 | HLServo(); 18 | ~HLServo(); 19 | 20 | // Methos MUST be implemented 21 | // 22 | byte servoType() override { return BS_TYPE_HaiLzd; } 23 | 24 | bool reset() override; 25 | uint32_t getVersion(byte id) override; 26 | 27 | bool resetServo() override { return false; } 28 | bool resetServo(byte id) override; 29 | bool move(byte id, int16_t pos, uint16_t time) override; 30 | 31 | 32 | bool setLED(byte id, bool mode) override; 33 | bool setLED(bool mode) override; 34 | 35 | inline int16_t getPos(byte id) override { return getPos(id, _isLocked[id]); } 36 | int16_t getPos(byte id, bool lockAfterGet) override; 37 | 38 | bool lock(byte id) override; 39 | bool unlock(byte id) override; 40 | 41 | uint16_t getAdjAngle(byte id) override { return 0; } 42 | uint16_t setAdjAngle(byte id, uint16 adjValue) override { return 0; } 43 | byte servoCommand(byte *cmd) override; 44 | 45 | byte setAngle(byte id, byte angle, byte minor) override; 46 | 47 | // Methods can be overrided (optional) 48 | // 49 | void initBus() override; 50 | bool initServo(byte id) override; 51 | void showInfo() override; 52 | 53 | // bool validId(byte id); 54 | // bool validPos(int16_t pos); 55 | 56 | // bool isLocked(byte id) override; 57 | bool lockAll() override; 58 | bool unlockAll() override; 59 | 60 | bool moveX(byte cnt, byte *data) override; 61 | // bool goAngle(byte id, int16_t angle, uint16_t time) override; 62 | 63 | // uint16_t lastPos(byte id) override; 64 | // uint16_t lastAngle(byte id) override; 65 | 66 | // uint16_t getAngle(byte id) override; 67 | // uint16_t getAngle(byte id, bool lockAfterGet) override; 68 | 69 | 70 | private: 71 | 72 | String _buffer; 73 | byte _retBuf[HL_RETURN_BUFFER_SIZE]; 74 | byte _retCnt = 0; 75 | 76 | inline bool sendCommand() { return sendCommand(true); } 77 | bool sendCommand(bool expectReturn, unsigned long waitMs = HL_COMMAND_WAIT_TIME); 78 | bool sendUntilOK(unsigned long waitMs = HL_COMMAND_WAIT_TIME); 79 | 80 | void showCommand(); 81 | bool checkReturn(unsigned long waitMs = HL_COMMAND_WAIT_TIME); 82 | bool isReturnOK(); 83 | void resetReturnBuffer(); 84 | 85 | inline bool getRetNum(int16_t *data, byte start, byte len) { return getRetNum(data, start, len, len); } 86 | bool getRetNum(int16_t *data, byte start, byte len, byte minLen); 87 | 88 | byte getServoMode(int id); 89 | 90 | }; 91 | 92 | #endif 93 | -------------------------------------------------------------------------------- /lib/RobotServo/drivers/UBT/UBTServo.cpp: -------------------------------------------------------------------------------- 1 | #include "UBTServo.h" 2 | 3 | UBTServo::UBTServo() { 4 | _MIN_POS = 0; 5 | _MAX_POS = 240; 6 | _MIN_ANGLE = 0; 7 | _MAX_ANGLE = 240; 8 | _INVALID_POS = 999; 9 | _MULTI_SERVO_COMMAND = false; 10 | } 11 | 12 | UBTServo::~UBTServo() { 13 | 14 | } 15 | 16 | void UBTServo::showInfo() { 17 | if (_dbg.isEnabled()){ 18 | _dbg.println("I am UBT Servo"); 19 | baseServo::showInfo(); 20 | } 21 | } 22 | 23 | void UBTServo::resetCommandBuffer() { 24 | memcpy(_buf, UBT_SERVO_CMD, UBT_COMMAND_BUFFER_SIZE); 25 | } 26 | 27 | void UBTServo::resetReturnBuffer() { 28 | while (_bus->available()) _bus->read(); 29 | memset(_retBuf, 0, UBT_RETURN_BUFFER_SIZE); 30 | _retCnt = 0; 31 | } 32 | 33 | bool UBTServo::sendCommand(bool expectReturn) { 34 | byte sum = 0; 35 | for (int i = 2; i < 8; i++) { 36 | sum += _buf[i]; 37 | } 38 | _buf[8] = sum; 39 | if (_dbg.isEnabled()) showCommand(); 40 | 41 | // clear reutnr buffer before sending commands 42 | resetReturnBuffer(); 43 | _bus->flush(); 44 | enableTx(true); 45 | _bus->write(_buf, 10); 46 | enableTx(false); 47 | if (expectReturn) return checkReturn(); 48 | return true; 49 | } 50 | 51 | void UBTServo::showCommand() { 52 | if (!_dbg.isEnabled()) return; 53 | _dbg.printf("%08ld OUT>>", millis()); 54 | for (int i = 0; i < 10; i++) { 55 | _dbg.print( (_buf[i] < 0x10 ? " 0" : " ")); 56 | _dbg.print(_buf[i], HEX); 57 | } 58 | _dbg.println(); 59 | } 60 | 61 | bool UBTServo::checkReturn() { 62 | unsigned long endMs = millis() + UBT_COMMAND_WAIT_TIME; 63 | while ( (millis() < endMs) && (!_bus->available()) ) ; 64 | if (!_bus->available()) { 65 | return false; 66 | } 67 | byte ch; 68 | 69 | if (_dbg.isEnabled()) { 70 | _dbg.printf("%08ld IN>>>", millis()); 71 | } 72 | while (_bus->available()) { 73 | ch = (byte) _bus->read(); 74 | _retBuf[_retCnt++] = ch; 75 | if (_dbg.isEnabled()) { 76 | _dbg.printf(" %02X", ch); 77 | } 78 | // extra delay to make sure transaction completed 79 | // ToDo: check data end? 80 | // But what can i do if not ended, seems not logical as only few bytes returned. 81 | // 1ms is already more than enough. 82 | // if (!_bus->available()) delay(1); 83 | } 84 | // TODO: Think about any better solution to initiate the bus. 85 | // Special handling for missing frist byte. 86 | // In some situation, espeically right after reset, the first return byte will be missing. 87 | // This is a temporary solution to handle FC / FA return with missing byte. 88 | if ((_retCnt == 9) && (_retBuf[8]==0xED)) { 89 | byte b1 = 0; 90 | 91 | // Now, only handle those 0x?F 92 | if ((_retBuf[0] % 0x10) == 0x0F) { 93 | b1 = 0xF0 + (_retBuf[0] >> 4); 94 | } 95 | // add handling for other code if needed 96 | 97 | if (b1) { 98 | for (int i = _retCnt; i > 0; i--) { 99 | _retBuf[i] = _retBuf[i-1]; 100 | } 101 | _retBuf[0] = b1; 102 | _retCnt++; 103 | if (_dbg.isEnabled()) _dbg.printf(" **Missing byte added: [%02X]",b1); 104 | } 105 | } 106 | 107 | if (_dbg.isEnabled()) _dbg.println(); 108 | return true; 109 | } 110 | 111 | bool UBTServo::reset() { 112 | _dbg.msg("UBTServo: Reset not yet implemented."); 113 | return false; 114 | } 115 | 116 | uint32_t UBTServo::getVersion(byte id) { 117 | if (!validId(id)) return 0; 118 | int tryCnt = 0; 119 | while (tryCnt++ <= _maxRetry) { 120 | resetCommandBuffer(); 121 | _buf[0] = 0xFC; 122 | _buf[1] = 0xCF; 123 | _buf[2] = id; 124 | _buf[3] = 0x01; 125 | sendCommand(); 126 | 127 | if ((_retCnt > 0) && (_retBuf[2] == id) && (_retBuf[3] == 0xAA)) { 128 | uint32_t ver; 129 | byte *ptr1 = (byte *) &ver; 130 | byte *ptr2 = _retBuf + 4; 131 | memcpy(ptr1, ptr2, 4); 132 | return ver; 133 | } 134 | delay(1); // wait 1ms before retry 135 | } 136 | return 0; 137 | } 138 | 139 | bool UBTServo::move(byte id, int16_t pos, uint16_t time) { 140 | if (!validId(id)) return false; 141 | if (pos < 0) return false; 142 | int tryCnt = 0; 143 | while (tryCnt++ <= _maxRetry) { 144 | resetCommandBuffer(); 145 | _buf[2] = id; 146 | _buf[3] = 0x01; 147 | _buf[4] = (pos & 0xFF); 148 | _buf[5] = (time / 20); 149 | _buf[6] = 0x00; 150 | _buf[7] = (time / 20); 151 | sendCommand(); 152 | 153 | if ((_retCnt == 1) && (_retBuf[0] == (0xAA + id))) { 154 | _isLocked[id] = true; 155 | _lastPos[id] = pos; 156 | return true; 157 | } 158 | } 159 | return false; 160 | } 161 | 162 | bool UBTServo::setLED(byte id, bool mode) { 163 | if (!validId(id)) return 0; 164 | int tryCnt = 0; 165 | while (tryCnt++ <= _maxRetry) { 166 | resetCommandBuffer(); 167 | _buf[2] = id; 168 | _buf[3] = 0x04; 169 | _buf[4] = (mode ? 0 : 1); 170 | sendCommand(); 171 | 172 | if (id == 0) { 173 | // assume OK for all servo command and it cannot check return 174 | memset(_led, _maxId + 1, mode); 175 | return true; 176 | } 177 | 178 | if ((_retCnt == 1) && (_retBuf[0] == (0xAA + id))) { 179 | _led[id] = mode; 180 | return true; 181 | } 182 | } 183 | return false; 184 | } 185 | 186 | int16_t UBTServo::getPos(byte id) { 187 | if (!validId(id)) return _INVALID_POS; 188 | return getPos(id, _isLocked[id]); 189 | } 190 | 191 | 192 | int16_t UBTServo::getPos(byte id, bool lockAfterGet) { 193 | if (!validId(id)) return _INVALID_POS; 194 | if (isLocked(id) && lockAfterGet) { 195 | return _lastPos[id]; 196 | } 197 | 198 | int tryCnt = 0; 199 | while (tryCnt++ <= _maxRetry) { 200 | resetCommandBuffer(); 201 | _buf[2] = id; 202 | _buf[3] = 0x02; 203 | sendCommand(); 204 | if (_retCnt == 10) break; 205 | } 206 | if (_retCnt != 10) return _INVALID_POS; 207 | 208 | byte pos = _retBuf[7]; 209 | 210 | if (lockAfterGet) { 211 | move(id, pos, 0); 212 | } else { 213 | // servo will be unlocked after fire a get position command 214 | _isLocked[id] = false; 215 | _lastPos[id] = pos; 216 | } 217 | 218 | return (int16_t) pos; 219 | } 220 | 221 | bool UBTServo::lock(byte id) { 222 | if (!validId(id)) return false; 223 | if (isLocked(id)) return true; 224 | uint16_t pos = getPos(id, true); 225 | return (isLocked(id)); 226 | } 227 | 228 | bool UBTServo::unlock(byte id) { 229 | if (!validId(id)) return false; 230 | // since the system is default to unlock, so not checking and go ahead to unlock 231 | getPos(id, false); 232 | return (!_isLocked[id]); 233 | } 234 | 235 | uint16_t UBTServo::getAdjAngle(byte id) { 236 | if (!validId(id)) return 0x7F7F; 237 | int tryCnt = 0; 238 | while (tryCnt++ <= _maxRetry) { 239 | resetCommandBuffer(); 240 | _buf[2] = id; 241 | _buf[3] = 0xD4; 242 | sendCommand(); 243 | if (_retCnt == 10) break; 244 | } 245 | if (_retCnt != 10) { 246 | // What can I do if it has not return the position 247 | return 0x7F7F; 248 | } 249 | uint16_t _adjAngle = _retBuf[6] * 256 + _retBuf[7]; 250 | return _adjAngle; 251 | } 252 | 253 | uint16 UBTServo::setAdjAngle(byte id, uint16 adjValue) { 254 | if (!validId(id)) return 0x7F7F; 255 | int tryCnt = 0; 256 | while (tryCnt++ < _maxRetry) { 257 | resetCommandBuffer(); 258 | _buf[2] = id; 259 | _buf[3] = 0xD2; 260 | _buf[6] = adjValue / 256; 261 | _buf[7] = adjValue % 256; 262 | sendCommand(); 263 | if (_retCnt == 10) break; 264 | } 265 | if (_retCnt != 10) { 266 | // What can I do if it has not return the position 267 | return 0x7F7F; 268 | } 269 | return getAdjAngle(id); 270 | } 271 | -------------------------------------------------------------------------------- /lib/RobotServo/drivers/UBT/UBTServo.h: -------------------------------------------------------------------------------- 1 | #ifndef _UBT_Servo_H_ 2 | #define _UBT_Servo_H_ 3 | 4 | #include "baseServo.h" 5 | 6 | #define UBT_COMMAND_BUFFER_SIZE 10 7 | #define UBT_RETURN_BUFFER_SIZE 20 // Actually, 10 is enough, just for saftey 8 | #define UBT_COMMAND_WAIT_TIME 5 // 2ms is mroe than enough as it should be returned within 400us 9 | 10 | const byte UBT_SERVO_CMD[] = {0xFA, 0xAF,0x00,0x00,0x00, 0x00, 0x00, 0x00, 0x00, 0xED}; 11 | // const byte JIMU_VERSION[] = {0xFC, 0xCF,0x00,0xAA,0x41, 0x16, 0x51, 0x01, 0x00, 0xED}; 12 | 13 | 14 | class UBTServo : public baseServo 15 | { 16 | public: 17 | 18 | UBTServo(); 19 | ~UBTServo(); 20 | 21 | // Methos MUST be implemented 22 | // 23 | // Methods without default implementation, MUST be overrided 24 | byte servoType() override { return BS_TYPE_UBT; } 25 | 26 | bool reset() override; 27 | uint32_t getVersion(byte id) override; 28 | 29 | bool resetServo() override { return false; } 30 | bool resetServo(byte id) override { return false; } 31 | 32 | bool move(byte id, int16_t pos, uint16_t time) override; 33 | bool setLED(byte id, bool mode) override; 34 | bool setLED(bool mode) override { return setLED(0, mode); } 35 | 36 | int16_t getPos(byte id) override; 37 | int16_t getPos(byte id, bool lockAfterGet) override; 38 | 39 | // bool isLocked(byte id) override; 40 | bool lock(byte id) override; 41 | bool unlock(byte id) override; 42 | 43 | uint16_t getAdjAngle(byte id) override; 44 | uint16_t setAdjAngle(byte id, uint16 adjValue) override; 45 | byte servoCommand(byte *cmd) override { return 0; } 46 | 47 | byte setAngle(byte id, byte angle, byte minor) override { return 0; } 48 | 49 | // Methods can be overrided (optional) 50 | // 51 | // void initBus() override; 52 | void showInfo() override; 53 | 54 | // bool validId(byte id) override; 55 | // bool validPos(int16_t pos) override; 56 | 57 | // bool moveX(byte cnt, byte *data) override; 58 | 59 | // bool isLocked(byte id) override; 60 | // bool lockAll() override; 61 | // bool unlockAll() override; 62 | 63 | // bool moveX(byte cnt, byte *data) override; 64 | // bool goAngle(byte id, int16_t angle, uint16_t time) override; 65 | 66 | // uint16_t lastPos(byte id) override; 67 | // uint16_t lastAngle(byte id) override; 68 | 69 | // uint16_t getAngle(byte id) override; 70 | // uint16_t getAngle(byte id, bool lockAfterGet) override; 71 | 72 | 73 | private: 74 | 75 | byte _buf[UBT_COMMAND_BUFFER_SIZE]; 76 | byte _retBuf[UBT_RETURN_BUFFER_SIZE]; 77 | byte _retCnt = 0; 78 | 79 | inline bool sendCommand() { return sendCommand(true); } 80 | bool sendCommand(bool expectReturn); 81 | void showCommand(); 82 | bool checkReturn(); 83 | inline void resetCommandBuffer(); 84 | inline void resetReturnBuffer(); 85 | 86 | }; 87 | 88 | #endif 89 | -------------------------------------------------------------------------------- /lib/RobotServo/drivers/_Sample_Servo/_Sample_Servo.cpp: -------------------------------------------------------------------------------- 1 | #include "_Sample_Servo.h" 2 | 3 | SAMPLE_Servo::SAMPLE_Servo() { 4 | 5 | } 6 | 7 | SAMPLE_Servo::~SAMPLE_Servo() { 8 | 9 | } -------------------------------------------------------------------------------- /lib/RobotServo/drivers/_Sample_Servo/_Sample_Servo.h: -------------------------------------------------------------------------------- 1 | // This is a hasic sample header file for new servo 2 | 3 | #ifndef _SAMPLE_SERVO_H_ 4 | #define _SAMPLE_SERVO_H_ 5 | 6 | #include "baseServo.h" 7 | 8 | 9 | class SAMPLE_Servo : public baseServo 10 | { 11 | public: 12 | 13 | SAMPLE_Servo(); 14 | ~SAMPLE_Servo(); 15 | 16 | // Methos MUST be implemented 17 | // 18 | byte servoType() override { return BS_TYPE_UNKNOWN; } 19 | 20 | bool reset() override; 21 | uint32_t getVersion(byte id) override; 22 | 23 | 24 | bool resetServo() override; 25 | bool resetServo(byte id) override; 26 | 27 | bool move(byte id, int16_t pos, uint16_t time) override; 28 | bool setLED(byte id, bool mode) override; 29 | bool setLED(bool mode) override; 30 | 31 | int16_t getPos(byte id) override; 32 | int16_t getPos(byte id, bool lockAfterGet) override; 33 | 34 | bool lock(byte id) override; 35 | bool unlock(byte id) override; 36 | 37 | uint16_t getAdjAngle(byte id) override; 38 | uint16_t setAdjAngle(byte id, uint16 adjValue) override; 39 | byte servoCommand(byte *cmd) override; 40 | byte setAngle(byte id, byte angle, byte minor) override; 41 | 42 | // Methods can be overrided (optional) 43 | // 44 | // void initBus() override; 45 | // void showInfo() override; 46 | 47 | // bool validId(byte id) override; 48 | // bool validPos(int16_t) override; 49 | 50 | // bool isLocked(byte id) override; 51 | // bool lockAll() override; 52 | // bool unlockAll() override; 53 | 54 | // bool moveX(byte cnt, byte *data) override; 55 | // bool goAngle(byte id, int16_t angle, uint16_t time) override; 56 | 57 | // uint16_t lastPos(byte id) override; 58 | // uint16_t lastAngle(byte id) override; 59 | 60 | // uint16_t getAngle(byte id) override; 61 | // uint16_t getAngle(byte id, bool lockAfterGet) override; 62 | 63 | private: 64 | 65 | }; 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /lib/SSBoard/SSBoard.cpp: -------------------------------------------------------------------------------- 1 | #include "SSBoard.h" 2 | 3 | SSBoard::SSBoard() { 4 | } 5 | 6 | SSBoard::~SSBoard() { 7 | } 8 | 9 | void SSBoard::Begin(Stream *busPort, Stream *debugPort) { 10 | _bus = busPort; 11 | _dbg.setOutput(debugPort); 12 | _retBuf.init(SSB_BUFFER_SIZE); 13 | } 14 | 15 | void SSBoard::SetEnableTxCalback(void (*enableTxCallback)(bool)) { 16 | _enableTxCallback = enableTxCallback; 17 | } 18 | 19 | void SSBoard::EnableTx(bool mode) { 20 | if (_enableTxCallback != NULL) { 21 | _enableTxCallback(mode); 22 | delayMicroseconds(10); 23 | } 24 | } 25 | 26 | void SSBoard::ShowCommand() { 27 | if (!_enableDebug) return; 28 | _dbg.msgh("SSB OUT>>"); 29 | byte count = _buf[2] + 4; 30 | for (int i = 0; i < count; i++) { 31 | _dbg.msgf(" %02X", _buf[i]); 32 | } 33 | _dbg.msgf("\n"); 34 | } 35 | 36 | 37 | void SSBoard::ClearRxBuffer() { 38 | while (_bus->available() ) { 39 | _bus->read(); 40 | delay(2); 41 | } 42 | } 43 | 44 | bool SSBoard::IsReturnCompleted() { 45 | // Must start with A8 8A 46 | while ((_retBuf.available() > 1) && (_retBuf.peek() != 0xA8) && (_retBuf.peek(1) != 0x8A)) { 47 | _retBuf.skip(); 48 | } 49 | // At least 6 byte: A8 8A 02 {cmd} {sum} ED 50 | if (_retBuf.available() < 6) return false; 51 | byte count = _retBuf.peek(2); 52 | if (_retBuf.available() < count + 4) return false; 53 | if (_retBuf.peek(count + 3) != 0xED) { 54 | // Skip header and check again later 55 | _retBuf.skip(2); 56 | return false; 57 | } 58 | // checksume 59 | uint16_t sum = 0; 60 | for (int i = 0; i < count; i++) { 61 | sum += _retBuf.peek(2 + i); 62 | } 63 | byte checkSum = sum & 0xFF; 64 | if (checkSum != _retBuf.peek(count + 2)) { 65 | // _dbg.msg("Sum not match: %02X vs %02X", checkSum, _retBuf.peek(count + 2)); 66 | // Skip header and check again later 67 | _retBuf.skip(2); 68 | return false; 69 | } 70 | return true; 71 | } 72 | 73 | bool SSBoard::SendCommand(byte *cmd, bool expectReturn) { 74 | _buf[0] = 0xA8; 75 | _buf[1] = 0x8A; 76 | byte count = cmd[2]; 77 | _buf[2] = cmd[2]; 78 | uint16_t sum = 0; 79 | for (int i = 0; i < count; i++) { 80 | _buf[2 + i] = cmd[2 + i]; 81 | sum += _buf[2 + i]; 82 | } 83 | _buf[count+2] = sum; 84 | _buf[count+3] = 0xED; 85 | 86 | if (_enableDebug) ShowCommand(); 87 | 88 | ClearRxBuffer(); 89 | _bus->flush(); 90 | EnableTx(true); 91 | _bus->write(_buf, count + 4); 92 | EnableTx(false); 93 | if (expectReturn) return CheckReturn(); 94 | ClearRxBuffer(); 95 | return true; 96 | } 97 | 98 | bool SSBoard::CheckReturn() { 99 | // if (_enableDebug) _dbg.msg("SSB CheckReturn"); 100 | unsigned long startMs = millis(); 101 | ResetReturnBuffer(); 102 | byte ch; 103 | while ( ((millis() - startMs) < SSB_COMMAND_WAIT_TIME) && (!_bus->available()) ) delay(1); 104 | // unsigned long endMs = millis(); 105 | // if (_enableDebug) _dbg.msgf("SSB wait return from %d to %d\n", startMs, endMs); 106 | 107 | if (!_bus->available()) { 108 | if (_enableDebug) _dbg.msg("SSB no return after %d ms", SSB_COMMAND_WAIT_TIME); 109 | return false; 110 | } 111 | if (_enableDebug) { 112 | _dbg.msgh("SSB IN>>>"); 113 | } 114 | // 10 ms is almost good for 10byte data 115 | bool returnCompleted = false; 116 | delay(10); 117 | while (_bus->available()) { 118 | ch = (byte) _bus->read(); 119 | _retBuf.write(ch); 120 | if (_enableDebug) { 121 | _dbg.msgf(" %02X", ch); 122 | } 123 | if (IsReturnCompleted()) { 124 | returnCompleted = true; 125 | break; 126 | } 127 | // extra delay to make sure transaction completed 128 | // ToDo: check data end? 129 | // But what can i do if not ended, seems not logical as only few bytes returned. 130 | // 1ms is already more than enough. 131 | if (!_bus->available()) delay(2); 132 | } 133 | if (_enableDebug) _dbg.msgf("\n"); 134 | return returnCompleted; 135 | } 136 | -------------------------------------------------------------------------------- /lib/SSBoard/SSBoard.h: -------------------------------------------------------------------------------- 1 | #ifndef _SSBOARD_H_ 2 | #define _SSBOARD_H_ 3 | 4 | #include "MyDebugger.h" 5 | #include "Buffer.h" 6 | /* 7 | * Sub-system Board (SSB) 8 | * 9 | * Command: A8 8A {len} {cmd} {data...} {sum} ED 10 | * 11 | */ 12 | #define SSB_BUFFER_SIZE 64 13 | #define SSB_COMMAND_WAIT_TIME 20 14 | 15 | class SSBoard { 16 | public: 17 | SSBoard(); 18 | ~SSBoard(); 19 | 20 | void Begin(Stream *busPort, Stream *debugPort); 21 | void SetEnableTxCalback(void (*enableTxCallback)(bool)); 22 | 23 | bool SendCommand(byte *cmd, bool expectReturn); 24 | Buffer *ReturnBuffer() { return &_retBuf; } 25 | 26 | private: 27 | Stream *_bus = NULL; 28 | MyDebugger _dbg; 29 | bool _enableDebug = false; 30 | 31 | void (*_enableTxCallback)(bool) = NULL; 32 | void EnableTx(bool); 33 | 34 | byte _buf[SSB_BUFFER_SIZE]; 35 | Buffer _retBuf; 36 | 37 | inline void ResetCommandBuffer() { memset(_buf, 0, SSB_BUFFER_SIZE); } 38 | inline void ResetReturnBuffer() { _retBuf.reset(); } 39 | 40 | void ShowCommand(); 41 | void ClearRxBuffer(); 42 | bool CheckReturn(); 43 | bool IsReturnCompleted(); 44 | }; 45 | 46 | #endif -------------------------------------------------------------------------------- /lib/SimpleWiFiManager/SimpleWiFiManager.h: -------------------------------------------------------------------------------- 1 | #ifndef _SIMPLE_WIFI_MANAGER_H_ 2 | #define _SIMPLE_WIFI_MANAGER_H_ 3 | 4 | #define _ENABLE_TRACE_ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #define FILE_READ "r" 14 | #define FILE_WRITE "w" 15 | 16 | #include "myUtil.h" 17 | #include "MyDebugger.h" 18 | #include "MyData.h" 19 | #include "Buffer.h" 20 | 21 | #define SWFM_OFFSET_A9 0 22 | #define SWFM_OFFSET_9A 1 23 | #define SWFM_OFFSET_RECORD_SIZE 2 24 | #define SWFM_OFFSET_COMMAND 3 25 | #define SWFM_OFFSET_VER_MAJOR 4 26 | #define SWFM_OFFSET_VER_MINOR 5 27 | #define SWFM_OFFSET_ENABLE_ROUTER 8 28 | #define SWFM_OFFSET_SSID 10 29 | #define SWFM_OFFSET_PASSWORD 40 30 | #define SWFM_OFFSET_ROUTER_TIMEOUT 60 31 | #define SWFM_OFFSET_ENABLE_AP 62 32 | #define SWFM_OFFSET_AP_NAME 64 33 | #define SWFM_OFFSET_AP_KEY 84 34 | #define SWFM_OFFSET_ENABLE_SERVER 104 35 | #define SWFM_OFFSET_SERVER_PORT 106 36 | #define SWFM_OFFSET_ENABLE_UDP 108 37 | #define SWFM_OFFSET_UDP_RX_PORT 110 38 | #define SWFM_OFFSET_UDP_TX_PORT 112 39 | #define SWFM_OFFSET_CHECKSUM 118 40 | #define SWFM_OFFSET_END_BYTE 119 41 | 42 | #define SWFM_CONFIG_FILE_SIZE 120 43 | #define SWFM_FILE_BUFFER_SIZE 120 44 | #define SWFM_MD_SIZE 20 45 | 46 | #define SWFM_MODE_NONE 0 47 | #define SWFM_MODE_ROUTER 1 48 | #define SWFM_MODE_AP 2 49 | 50 | #define SWFM_CONFIG_FILE "/system/wifi.cfg" 51 | 52 | struct __field { 53 | int type; 54 | String label; 55 | String key; 56 | int64_t defInt; 57 | String defString; 58 | int64_t vInt; 59 | String vString; 60 | }; 61 | 62 | class SimpleWiFiManager { 63 | 64 | public: 65 | SimpleWiFiManager(); 66 | ~SimpleWiFiManager(); 67 | 68 | void setDebug(Stream *dbg, bool enableDebug = true); 69 | void enableDebug(bool value); 70 | 71 | void setWiFiServer(uint16_t port); 72 | bool begin(int preferMode = SWFM_MODE_NONE); 73 | void httpServerHandler(); 74 | void stopServer(); 75 | 76 | void dumpSettings(); 77 | 78 | uint8_t *getConfig(); 79 | bool setConfig(uint8_t *data); 80 | bool resetDefault(); 81 | 82 | bool updateRouter(bool enableRouter, String ssid, String password, uint32_t long routerTimeout, bool updateConfig = true); 83 | bool updateAP(bool enableAP, String APName, String APKey, bool updateConfig = true); 84 | bool updateServer(bool enableServer, uint16_t serverPort, bool updateConfig = true); 85 | bool updateUDP(bool enableUDP, uint16_t udpRxPort, uint16_t udpTxPort, bool updateConfig = true); 86 | bool updateSettings(bool enableRouter, String ssid, String password, uint32_t long routerTimeout, 87 | bool enableAP, String APName, String APKey, 88 | bool enableServer, uint16_t serverPort, 89 | bool enableUDP, uint16_t udpRxPort, uint16_t udpTxPort); 90 | 91 | 92 | bool isReady(); 93 | bool isServerRunning() { return _serverRunning; } 94 | uint8_t mode() { return _mode; } 95 | String ip(); 96 | 97 | bool enableRouter() { return _wifi.enableRouter.getBool(); } 98 | String ssid() { return _wifi.ssid.getString(); } 99 | String password() { return _wifi.password.getString(); } 100 | uint32_t routerTimeout() { return (uint32_t) _wifi.routerTimeout.getInt(); } 101 | bool enableAP() { return _wifi.enableAP.getBool(); } 102 | String apName() { return _wifi.apName.getString(); } 103 | String apKey() { return _wifi.apKey.getString(); } 104 | bool enableServer() { return _wifi.enableServer.getBool(); } 105 | uint16_t serverPort() { return (uint16_t) _wifi.serverPort.getInt(); } 106 | bool enableUDP() { return _wifi.enableUDP.getBool(); } 107 | uint16_t udpRxPort() { return (uint16_t) _wifi.udpRxPort.getInt(); } 108 | uint16_t udpTxPort() { return (uint16_t) _wifi.udpTxPort.getInt(); } 109 | 110 | bool wifiClientConnected() { return _wifiClientConnected; } 111 | size_t write(byte data); 112 | size_t write(byte *data, size_t cnt); 113 | size_t udpSendPacket(const char* target, const char *data); 114 | 115 | 116 | uint8_t checkData(); 117 | Buffer buffer() { return _buffer; } 118 | void resetBuffer() { _buffer.reset(); } 119 | uint16_t available() { return _buffer.available(); } 120 | byte peek() { return _buffer.peek(); } 121 | bool peek(byte *storage, uint16_t count) { return _buffer.peek(storage, count); } 122 | byte read() { return _buffer.read(); } 123 | bool read(byte *storage, uint16_t count) { return _buffer.read(storage, count); } 124 | bool skip(uint16_t count = 1) { return _buffer.skip(count); } 125 | 126 | private: 127 | 128 | String _chipId; 129 | 130 | // std::unique_ptr _buffer; 131 | Buffer _buffer; 132 | std::unique_ptr _httpServer; 133 | std::unique_ptr _wifiServer; 134 | 135 | // Status flag 136 | bool _configReady; 137 | uint8_t _mode; 138 | bool _serverRunning; 139 | 140 | // WiFi Server 141 | bool _wifiServerEnabled; 142 | uint16_t _wifiServerPort; 143 | bool _wifiServerRunning; 144 | WiFiClient _wifiClient; 145 | bool _wifiClientConnected; 146 | 147 | WiFiUDP _udpClient; 148 | bool _udpClientRunning; 149 | 150 | MyDebugger _dbg; 151 | 152 | struct WIFI_STRUCT { 153 | MyData varMajor; 154 | MyData varMinor; 155 | MyData enableRouter; 156 | MyData ssid; 157 | MyData password; 158 | MyData routerTimeout; 159 | MyData enableAP; 160 | MyData apName; 161 | MyData apKey; 162 | MyData enableServer; 163 | MyData serverPort; 164 | MyData enableUDP; 165 | MyData udpRxPort; 166 | MyData udpTxPort; 167 | }; 168 | 169 | WIFI_STRUCT _wifi = { 170 | MyData("Major Version", "major_version", (int64_t) 1, SWFM_OFFSET_VER_MAJOR, 1), 171 | MyData("Minor Version", "minor_version", (int64_t) 0, SWFM_OFFSET_VER_MINOR, 1), 172 | MyData("Enable Router", "enable_router", (bool) false, SWFM_OFFSET_ENABLE_ROUTER), 173 | MyData("SSID", "ssid", (String) "", SWFM_OFFSET_SSID, 30), 174 | MyData("PASSWORD", "password", (String) "", SWFM_OFFSET_PASSWORD, 20), 175 | MyData("Router Timeout", "router_timeout", (int64_t) 15, SWFM_OFFSET_ROUTER_TIMEOUT, 2), 176 | MyData("Enable AP", "enable_ap", (bool) true, SWFM_OFFSET_ENABLE_AP), 177 | MyData("AP Name", "ap_name", (String) "ESP", SWFM_OFFSET_AP_NAME, 20), 178 | MyData("AP Key", "ap_key", (String) "12345678", SWFM_OFFSET_AP_KEY, 20), 179 | MyData("Enable Server", "enable_server", (bool) true, SWFM_OFFSET_ENABLE_SERVER), 180 | MyData("Server Port", "server_port", (int64_t) 80, SWFM_OFFSET_SERVER_PORT, 2), 181 | MyData("Enable UDP", "enable_udp", (bool) true, SWFM_OFFSET_ENABLE_UDP), 182 | MyData("UDP Rx Port", "udp_rx_port", (int64_t) 9012, SWFM_OFFSET_UDP_RX_PORT, 2), 183 | MyData("UDP Tx Port", "udp_tx_port", (int64_t) 9020, SWFM_OFFSET_UDP_TX_PORT, 2) 184 | }; 185 | 186 | MyData* _md[SWFM_MD_SIZE]; 187 | uint8_t _mdCnt; 188 | 189 | struct WIFI_SETTING_STRUCT { 190 | uint8_t headerA9; // 0 191 | uint8_t header9A; // 1 192 | uint8_t size; // 2 : 120 - 4 = 116 = 0x74 193 | uint8_t command; // 3 194 | uint8_t ver_major; // 4 195 | uint8_t ver_minor; // 5 196 | uint8_t filler_01[2]; // 6 197 | bool enableRouter; // 8 198 | uint8_t filler_02[1]; // 9 199 | char ssid[30]; // 10 200 | char password[20]; // 40 201 | uint8_t routerTimeout; // 60 202 | uint8_t filler_03[1]; // 61 203 | bool enableAP; // 62 204 | uint8_t filler_04[1]; // 63 205 | char apName[20]; // 64 206 | char apKey[20]; // 84 207 | bool enableServer; // 104 208 | uint8_t filler_05[1]; // 105 209 | uint16_t serverPort; // 106 210 | bool enableUDP; // 108 211 | uint8_t filler_06[1]; // 109 212 | uint16_t udpRxPort; // 110 213 | uint16_t udpTxPort; // 112 214 | uint8_t filler_07[4]; // 114 215 | uint8_t sum; // 118 216 | uint8_t endEA; // 119 217 | }; 218 | 219 | union WIFI_SETTING { 220 | uint8_t buffer[SWFM_FILE_BUFFER_SIZE]; 221 | WIFI_SETTING_STRUCT data; 222 | }; 223 | 224 | WIFI_SETTING _file; 225 | 226 | void mdFromArray(); 227 | void mdToArray(); 228 | 229 | bool readConfig(); 230 | void setDefaultConfig(); 231 | String defaultAPName(); 232 | bool saveConfig(); 233 | 234 | void dumpConfig(); 235 | 236 | bool checkHttpArg(MyData *data); 237 | 238 | void httpTrField(String *s, MyData data); 239 | void httpTrCheckbox(String *s, MyData data, int colSpan = 1); 240 | 241 | void handleNotFound(); 242 | void handleRoot(); 243 | }; 244 | #endif 245 | -------------------------------------------------------------------------------- /lib/SimpleWiFiManager/SimpleWiFiManager_Handler.cpp: -------------------------------------------------------------------------------- 1 | #include "SimpleWiFiManager.h" 2 | 3 | 4 | void SimpleWiFiManager::handleNotFound() { 5 | _dbg.msg("Start handleNotFound"); 6 | String message = "File Not Found\n\n"; 7 | message += "URI: "; 8 | message += _httpServer->uri(); 9 | message += "\nMethod: "; 10 | message += (_httpServer->method() == HTTP_GET) ? "GET" : "POST"; 11 | message += "\nArguments: "; 12 | message += _httpServer->args(); 13 | message += "\n"; 14 | for (uint8_t i = 0; i < _httpServer->args(); i++) { 15 | message += " " + _httpServer->argName(i) + ": " + _httpServer->arg(i) + "\n"; 16 | } 17 | _httpServer->send(404, "text/plain", message); 18 | _dbg.msg("End handleNotFound"); 19 | } 20 | 21 | bool SimpleWiFiManager::checkHttpArg(MyData *data) { 22 | bool valChanged = false; 23 | String value = ""; 24 | if (_httpServer->hasArg(data->key().c_str())) { 25 | value = _httpServer->arg(data->key().c_str()); 26 | } 27 | _dbg.printf("Parm %s = %s\n", data->key().c_str(), value.c_str()); 28 | bool b_value; 29 | bool old_value; 30 | int64_t i64_value; 31 | 32 | switch (data->type()) { 33 | 34 | case MD_BOOL: 35 | b_value = false; 36 | old_value = data->getBool(); 37 | if (value.length() > 0) b_value = (value.compareTo("enable") == 0); 38 | if (b_value != old_value) { 39 | data->setBool(b_value); 40 | valChanged = true; 41 | } 42 | break; 43 | 44 | case MD_INTEGER: 45 | i64_value = atoll(value.c_str()); 46 | if ((i64_value > 0) && (i64_value != data->getInt())) { 47 | data->setInt(i64_value); 48 | valChanged = true; 49 | } 50 | break; 51 | 52 | case MD_STRING: 53 | if (data->getString().compareTo(value) != 0) { 54 | data->setString(value); 55 | valChanged = true; 56 | } 57 | break; 58 | 59 | } 60 | if (valChanged) { 61 | _dbg.printf("%s set to \"%s\"\n", data->label().c_str(), data->getPrintable().c_str()); 62 | } 63 | return valChanged; 64 | 65 | } 66 | 67 | 68 | void SimpleWiFiManager::handleRoot() { 69 | _dbg.msg("Start handleRoot"); 70 | 71 | // check for arguments, at leastt 2 arguments (key + any) 72 | if ( (_httpServer->args() > 1) && 73 | (_httpServer->hasArg("key")) && (_chipId == _httpServer->arg("key")) ) { 74 | bool changeSetting = false; 75 | for (int idx = 0; idx < _mdCnt; idx++) { 76 | if (checkHttpArg(_md[idx])) changeSetting = true; 77 | } 78 | if (changeSetting) { 79 | if (saveConfig()) { 80 | _dbg.printf("WiFi Setting updated.\n"); 81 | } 82 | } 83 | } 84 | 85 | 86 | String s = ""; 87 | s.concat("\n"); 88 | s.concat("\n"); 89 | s.concat("

myRobot - WiFi Settings

\n"); 90 | s.concat("

Server running at "); 91 | s.concat(ip()); 92 | s.concat("

\n"); 93 | s.concat("
"); 94 | s.concat(""); 95 | s.concat(""); 96 | 97 | s.concat(""); 98 | 99 | httpTrCheckbox(&s, _wifi.enableRouter, 2); 100 | httpTrField(&s, _wifi.ssid); 101 | httpTrField(&s, _wifi.password); 102 | httpTrField(&s, _wifi.routerTimeout); 103 | 104 | s.concat(""); 105 | 106 | s.concat(""); 107 | 108 | httpTrCheckbox(&s, _wifi.enableAP, 2); 109 | httpTrField(&s, _wifi.apName); 110 | httpTrField(&s, _wifi.apKey); 111 | 112 | s.concat(""); 113 | 114 | s.concat(""); 115 | 116 | httpTrCheckbox(&s, _wifi.enableServer, 2); 117 | httpTrField(&s, _wifi.serverPort); 118 | 119 | s.concat(""); 120 | 121 | s.concat(""); 122 | 123 | httpTrCheckbox(&s, _wifi.enableUDP, 2); 124 | httpTrField(&s, _wifi.udpRxPort); 125 | httpTrField(&s, _wifi.udpTxPort); 126 | 127 | s.concat(""); 128 | 129 | s.concat("\n"); 130 | s.concat("
Station Mode  
 
AP Mode  
 
Web Server Settings  
 
UDP Settings  
 
\n"); 131 | s.concat("\n"); 134 | s.concat("\n"); 135 | s.concat("
\n"); 136 | 137 | s.concat("\n"); 138 | 139 | _httpServer->send(200,"text/html", s); 140 | 141 | _dbg.msg("HTML page sent"); 142 | 143 | _dbg.msg("End handleRoot"); 144 | } -------------------------------------------------------------------------------- /lib/SimpleWiFiManager/SimpleWiFiManager_Receiver.cpp: -------------------------------------------------------------------------------- 1 | #include "SimpleWiFiManager.h" 2 | 3 | size_t SimpleWiFiManager::write(byte data) { 4 | if (!_wifiClientConnected) return 0; 5 | if (!_wifiClient.connected()) { 6 | _wifiClientConnected = false; 7 | return 0; 8 | } 9 | return _wifiClient.write(data); 10 | } 11 | 12 | size_t SimpleWiFiManager::write(byte *data, size_t cnt) { 13 | if (!_wifiClientConnected) return 0; 14 | if (!_wifiClient.connected()) { 15 | _wifiClientConnected = false; 16 | return 0; 17 | } 18 | return _wifiClient.write(data, cnt); 19 | } 20 | 21 | 22 | size_t SimpleWiFiManager::udpSendPacket(const char* target, const char *data) { 23 | _udpClient.beginPacket(target, udpTxPort()); 24 | size_t cnt = _udpClient.print(data); 25 | _udpClient.endPacket(); 26 | return cnt; 27 | } 28 | 29 | uint8_t SimpleWiFiManager::checkData() { 30 | if (_wifiServerRunning) { 31 | bool newConnection = true; 32 | if (_wifiClientConnected) { 33 | if (_wifiClient.connected()) { 34 | newConnection = false; 35 | }else { 36 | _wifiClientConnected = false; 37 | _dbg.msg("Wifi client disconnected."); 38 | } 39 | } 40 | if (!_wifiClientConnected) { 41 | _wifiClient = _wifiServer->available(); 42 | } 43 | if (_wifiClient) { 44 | if (_wifiClient.connected()) { 45 | _wifiClientConnected = true; 46 | if (newConnection) { 47 | _dbg.msg("Wifi client connected."); 48 | } 49 | while (_wifiClient.available()) { 50 | _buffer.write(_wifiClient.read()); 51 | // Add 1ms delay to make sure data transmission is completed. 52 | if (!_wifiClient.available()) delay(1); 53 | } 54 | } 55 | } 56 | } 57 | 58 | if (_udpClientRunning) { 59 | int packetSize = _udpClient.parsePacket(); 60 | if (packetSize) { 61 | byte packet[packetSize]; 62 | int udpRxCnt = _udpClient.read(packet, packetSize); 63 | for (int i = 0; i < udpRxCnt; i++) _buffer.write(packet[i]); 64 | } 65 | } 66 | 67 | return (_buffer.available()); 68 | } -------------------------------------------------------------------------------- /lib/SimpleWiFiManager/examples/WebServer/webserver.ino: -------------------------------------------------------------------------------- 1 | #include "SimpleWiFiManager.h" 2 | SimpleWiFiManager SWFM; 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | ESP8266WebServer server(80); 9 | #define DEBUG_PORT Serial1 10 | 11 | #include "MyDebugger.h" 12 | MyDebugger dbg; 13 | 14 | void setup() { 15 | Serial.begin(115200); 16 | while (!Serial) delay(1); 17 | Serial1.begin(115200); 18 | while (!Serial1) delay(1); 19 | DEBUG_PORT.println("\n\n\nLet's GO!\n\n"); 20 | 21 | dbg.setOutput(&DEBUG_PORT); 22 | 23 | SWFM.setDebug(&DEBUG_PORT); 24 | SWFM.begin(); 25 | 26 | if (WiFi.status() != WL_CONNECTED) { 27 | dbg.msg("Unexpected error, wifi not connected"); 28 | while (WiFi.status() != WL_CONNECTED) delay(1); 29 | } 30 | 31 | if (MDNS.begin("esp8266")) { 32 | dbg.msg("MDNS responder started"); 33 | } 34 | 35 | server.on("/", handleRoot); 36 | server.on("/update", handleUpdate); 37 | server.onNotFound(handleNotFound); 38 | 39 | server.begin(); 40 | dbg.msg("HTTP Server started"); 41 | 42 | 43 | } 44 | 45 | void loop() { 46 | server.handleClient(); 47 | } 48 | 49 | void handleRoot() { 50 | dbg.msg("Start handleRoot"); 51 | server.send(200, "text/plain", "hello from esp8266!"); 52 | dbg.msg("End handleRoot"); 53 | } 54 | 55 | void handleNotFound() { 56 | dbg.msg("Start handleNotFound"); 57 | String message = "File Not Found\n\n"; 58 | message += "URI: "; 59 | message += server.uri(); 60 | message += "\nMethod: "; 61 | message += (server.method() == HTTP_GET) ? "GET" : "POST"; 62 | message += "\nArguments: "; 63 | message += server.args(); 64 | message += "\n"; 65 | for (uint8_t i = 0; i < server.args(); i++) { 66 | message += " " + server.argName(i) + ": " + server.arg(i) + "\n"; 67 | } 68 | server.send(404, "text/plain", message); 69 | dbg.msg("End handleNotFound"); 70 | } 71 | 72 | void handleUpdate() { 73 | dbg.msg("Start handleUpdate"); 74 | String message = "Record update HTML page\n\n"; 75 | message += "URI: "; 76 | message += server.uri(); 77 | message += "\nMethod: "; 78 | message += (server.method() == HTTP_GET) ? "GET" : "POST"; 79 | message += "\nArguments: "; 80 | message += server.args(); 81 | message += "\n"; 82 | for (uint8_t i = 0; i < server.args(); i++) { 83 | message += " " + server.argName(i) + ": " + server.arg(i) + "\n"; 84 | } 85 | message += ""; 86 | server.send(404, "text/plain", message); 87 | dbg.msg("End handleUpdate"); 88 | } -------------------------------------------------------------------------------- /lib/SimpleWiFiManager/examples/WiFiServer/wifiserver.ino: -------------------------------------------------------------------------------- 1 | #include "SimpleWiFiManager.h" 2 | SimpleWiFiManager SWFM; 3 | 4 | 5 | #define DEBUG_PORT Serial1 6 | 7 | String ip; 8 | 9 | void setup() { 10 | Serial.begin(115200); 11 | while (!Serial) delay(1); 12 | Serial1.begin(115200); 13 | while (!Serial1) delay(1); 14 | DEBUG_PORT.println("\n\n\nLet's GO!\n\n"); 15 | 16 | SWFM.setWiFiServer(6169); 17 | SWFM.setDebug(&DEBUG_PORT); 18 | SWFM.begin(); 19 | ip = SWFM.ip(); 20 | } 21 | 22 | void loop() { 23 | SWFM.httpServerHandler(); 24 | uint8_t cnt = SWFM.checkData(); 25 | if (cnt) { 26 | DEBUG_PORT.printf("\nWiFi Data received: %d bytes\n", cnt); 27 | while (SWFM.available()) { 28 | DEBUG_PORT.printf("%02X ", SWFM.read()); 29 | } 30 | DEBUG_PORT.println(); 31 | SWFM.resetBuffer(); 32 | } 33 | } -------------------------------------------------------------------------------- /lib/SimpleWiFiManager/examples/basic/basic.ino: -------------------------------------------------------------------------------- 1 | #include "SimpleWiFiManager.h" 2 | SimpleWiFiManager SWFM; 3 | 4 | #include 5 | WiFiUDP udpClient; 6 | 7 | #define DEBUG_PORT Serial1 8 | 9 | bool isConnected = false; 10 | String ip; 11 | uint8_t networkMode; 12 | 13 | void setup() { 14 | Serial.begin(115200); 15 | while (!Serial) delay(1); 16 | Serial1.begin(115200); 17 | while (!Serial1) delay(1); 18 | DEBUG_PORT.println("\n\n\nLet's GO!\n\n"); 19 | 20 | SWFM.setDebug(&DEBUG_PORT); 21 | SWFM.begin(); 22 | if (SWFM.mode() == SWFM_MODE_ROUTER) { 23 | networkMode = SWFM_MODE_ROUTER; 24 | if (SWFM.enableUDP()) { 25 | udpClient.begin(SWFM.udpRxPort()); 26 | } 27 | } 28 | isConnected = true; 29 | ip = SWFM.ip(); 30 | } 31 | 32 | void loop() { 33 | SWFM.httpServerHandler(); 34 | } -------------------------------------------------------------------------------- /lib/SimpleWiFiManager/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "SimpleWiFiManager", 3 | "version": "1.0.0", 4 | "keywords": [ 5 | "wifi", "network" 6 | ], 7 | "description": "Simplified WiFi Manager for ESP8266.", 8 | "repository": 9 | { 10 | "type": "git", 11 | "url": "http://super169.asuscomm.com:30000/PlatformIO/SimpleWiFiManager" 12 | }, 13 | "frameworks": "arduino", 14 | "platforms": "espressif8266" 15 | } 16 | -------------------------------------------------------------------------------- /lib/SimpleWiFiManager/library.properties: -------------------------------------------------------------------------------- 1 | name=SimpleWiFiManager 2 | version=1.0 3 | author=Super169 4 | maintainer=Super169 5 | sentence=Simplified WiFi Manager for ESP8266. 6 | paragraph= 7 | category=WiFi 8 | url= 9 | architectures=esp8266 10 | -------------------------------------------------------------------------------- /lib/myUtil/myUtil.cpp: -------------------------------------------------------------------------------- 1 | #include "myUtil.h" 2 | 3 | uint32_t myUtil::getDeviceId() { 4 | uint32_t id; 5 | #ifdef ESP32 6 | id = (uint32_t) ESP.getEfuseMac(); 7 | #else 8 | id = ESP.getChipId(); 9 | #endif 10 | return id; 11 | } 12 | 13 | void myUtil::clearStreamBuffer(Stream *stream) { 14 | if (stream->available()) { 15 | while (stream->available()) { 16 | stream->read(); 17 | if (!stream->available()) delay(1); 18 | } 19 | } 20 | } 21 | 22 | bool myUtil::readSPIFFS(const char *fileName, String &data) { 23 | if (!SPIFFS.begin()) return false; 24 | bool success = false; 25 | File f = SPIFFS.open(fileName, FILE_READ); 26 | if (f) { 27 | data = f.readString(); 28 | success = true; 29 | } 30 | f.close(); 31 | SPIFFS.end(); 32 | return success; 33 | } 34 | 35 | 36 | bool myUtil::readSPIFFS(const char *fileName, char *buffer, uint16_t size) { 37 | if (!SPIFFS.begin()) return false; 38 | bool success = false; 39 | File f = SPIFFS.open(fileName, FILE_READ); 40 | if ((f) && (f.size() == size)) { 41 | f.readBytes(buffer, size); 42 | success = true; 43 | } 44 | f.close(); 45 | SPIFFS.end(); 46 | return success; 47 | } 48 | 49 | bool myUtil::writeSPIFFS(const char *fileName, const char *data) { 50 | if (!SPIFFS.begin()) return false; 51 | bool success = false; 52 | File f = SPIFFS.open(fileName, FILE_WRITE); 53 | if (f) { 54 | success = f.print(data); 55 | } 56 | f.close(); 57 | SPIFFS.end(); 58 | return success; 59 | } 60 | 61 | bool myUtil::writeSPIFFS(const char *fileName, uint8_t *buffer, uint16_t size) { 62 | if (!SPIFFS.begin()) return false; 63 | bool success = false; 64 | File f = SPIFFS.open(fileName, FILE_WRITE); 65 | if (f) { 66 | size_t wCnt = f.write(buffer, size); 67 | success = (wCnt == size); 68 | } 69 | f.close(); 70 | SPIFFS.end(); 71 | return success; 72 | } 73 | 74 | bool myUtil::isEmpty(String data) { 75 | // Will String be NULL? Seems not possible 76 | // This dummy function shoudl have the same result as 77 | // data.equals("") / data == "" / data == NULL 78 | // Just define a function here to centralize the checking, and change in single point if need. 79 | return (data.length() == 0); 80 | } 81 | 82 | String myUtil::getInt64String(int64_t data) { 83 | // can be converted to String directly 84 | if ((data <= 2147483647) && (data >= -2147483648)) { 85 | return String((int32_t) data); 86 | } 87 | // int64 has value from -9,223,372,036,854,775,808 to 9,223,372,036,854,775,807, up to 19 digits and 1 sign 88 | char buf[21]; 89 | uint64_t i64; 90 | memset(buf, 0, 21); 91 | i64 = (data < 0 ? -data : data); 92 | int64_t temp; 93 | int maxPos = 0; 94 | for (int i = 0; i < 19; i++) { 95 | temp = i64 % 10; 96 | i64 /= 10; 97 | buf[19-i] = temp; 98 | if (temp) maxPos = i; 99 | } 100 | 101 | char result[21]; 102 | memset(result, 0, 21); 103 | uint8_t idx = 0; 104 | if (data < 0) result[idx++] = '-'; 105 | 106 | for (int i = (19 - maxPos); i < 20; i++) { 107 | result[idx++] = '0' + buf[i]; 108 | } 109 | return String(result); 110 | } 111 | -------------------------------------------------------------------------------- /lib/myUtil/myUtil.h: -------------------------------------------------------------------------------- 1 | #ifndef _MY_UTIL_H_ 2 | #define _MY_UTIL_H_ 3 | 4 | #ifdef ESP32 5 | #include 6 | #include 7 | #include 8 | #else 9 | #include 10 | #include 11 | #define FILE_READ "r" 12 | #define FILE_WRITE "w" 13 | #endif 14 | 15 | class myUtil { 16 | public: 17 | static uint32_t getDeviceId(); 18 | static bool readSPIFFS(const char *filename, String &data); 19 | static bool readSPIFFS(const char *fileName, char *buffer, uint16_t size); 20 | static bool writeSPIFFS(const char *filename, const char *data); 21 | static bool writeSPIFFS(const char *fileName, uint8_t *buffer, uint16_t size); 22 | static bool isEmpty(String data); 23 | inline static void clearSerialBuffer() { return clearStreamBuffer(&Serial); } 24 | static void clearStreamBuffer(Stream *stream); 25 | static String getInt64String(int64_t data); 26 | private: 27 | 28 | }; 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /lib/readme.txt: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for the project specific (private) libraries. 3 | PlatformIO will compile them to static libraries and link to executable file. 4 | 5 | The source code of each library should be placed in separate directory, like 6 | "lib/private_lib/[here are source files]". 7 | 8 | For example, see how can be organized `Foo` and `Bar` libraries: 9 | 10 | |--lib 11 | | |--Bar 12 | | | |--docs 13 | | | |--examples 14 | | | |--src 15 | | | |- Bar.c 16 | | | |- Bar.h 17 | | |--Foo 18 | | | |- Foo.c 19 | | | |- Foo.h 20 | | |- readme.txt --> THIS FILE 21 | |- platformio.ini 22 | |--src 23 | |- main.c 24 | 25 | Then in `src/main.c` you should use: 26 | 27 | #include 28 | #include 29 | 30 | // rest H/C/CPP code 31 | 32 | PlatformIO will find your libraries automatically, configure preprocessor's 33 | include paths and build them. 34 | 35 | More information about PlatformIO Library Dependency Finder 36 | - http://docs.platformio.org/page/librarymanager/ldf.html 37 | -------------------------------------------------------------------------------- /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 | ; http://docs.platformio.org/page/projectconf.html 10 | 11 | [platformio] 12 | env_default = d1_mini 13 | 14 | [env:d1_mini] 15 | platform = espressif8266 16 | board = d1_mini 17 | framework = arduino 18 | monitor_speed = 115200 19 | upload_port=COM4 20 | build_flags = -Wno-comment -Wno-unknown-pragmas -Wno-unused-variable 21 | lib_ldf_mode = chain+ 22 | lib_deps = 23 | OLED12864=https://github.com/Super169/OLED12864.git 24 | 25 | [env:nodemcuv2] 26 | platform = espressif8266 27 | board = nodemcuv2 28 | framework = arduino 29 | monitor_speed = 115200 30 | build_flags = -Wno-comment -Wno-unknown-pragmas -Wno-unused-variable 31 | upload_port=COM3 32 | upload_speed=921600 33 | lib_ldf_mode = chain+ 34 | lib_deps = 35 | OLED12864=https://github.com/Super169/OLED12864.git 36 | ; OLED12864=https://github.com/Super169/OLED12864/archive/RobotControl_2.0.zip -------------------------------------------------------------------------------- /src/ActionData.h: -------------------------------------------------------------------------------- 1 | #ifndef _ACTION_DATA_H_ 2 | #define _ACTION_DATA_H_ 3 | 4 | #include 5 | #include 6 | #include "RESULT.h" 7 | 8 | #define AD_HEADER_SIZE 60 9 | #define AD_OFFSET_LEN 2 10 | #define AD_OFFSET_COMMAND 3 11 | #define AD_OFFSET_ID 4 12 | #define AD_OFFSET_NAME 6 13 | #define AD_NAME_SIZE 20 14 | #define AD_OFFSET_POSECNT_LOW 28 15 | #define AD_OFFSET_POSECNT_HIGH 29 16 | #define AD_OFFSET_AFFECTSERVO 34 17 | 18 | #define AD_COMMAND 0x61 19 | 20 | #define AD_POFFSET_ACTION 4 21 | #define AD_POFFSET_SEQ 5 22 | #define AD_POFFSET_ENABLE 6 23 | #define AD_POFFSET_STIME 7 24 | #define AD_POFFSET_WTIME 9 25 | #define AD_POFFSET_ANGLE 11 26 | #define AD_POFFSET_LED 43 27 | #define AD_POFFSET_HEAD 51 28 | #define AD_POFFSET_MP3_FOLDER 52 29 | #define AD_POFFSET_MP3_FILE 53 30 | #define AD_POFFSET_MP3_VOL 54 31 | #define AD_POFFSET_SEQ_HIGH 55 32 | 33 | #define AD_MP3_STOP_VOL 0xFE 34 | 35 | #define AD_POSE_SIZE 60 36 | #define AD_PBUFFER_COUNT 10 37 | #define AD_PBUFFER_SIZE 600 // Make sure AD_PBUFFER_SIZE = AD_PBUFFER_COUNT * AD_POSE_SIZE 38 | #define AD_MAX_POSE 65535 39 | 40 | 41 | 42 | // poseCnt is single byte, so max is 255. 43 | // But for safety, due to memory issue, only 12000 byte is used 44 | 45 | #define ACTION_PATH "/alpha/action" 46 | #define ACTION_FILE "/alpha/action/%03d.dat" 47 | #define ACTION_POS 14 48 | 49 | class ActionData { 50 | public: 51 | ActionData(); 52 | ~ActionData(); 53 | void InitObject(byte actionId); 54 | bool SetActionName(char *actionName, byte len); 55 | bool SetActionName(String actionName); 56 | byte UpdatePose(byte actionId, byte poseId, byte *data); 57 | byte ReadSPIFFS(byte actionId); 58 | byte WriteSPIFFS(); 59 | byte WriteHeader(); 60 | byte WritePoseData(); 61 | byte SpiffsWritePoseData(); 62 | byte DeleteActionFile(byte actionId); 63 | // bool DumpActionHeader(byte actionId); 64 | // bool DumpActionData(byte actionId); 65 | byte id() { return _id; } 66 | 67 | byte * Header() { return (byte *) _header; } 68 | byte * Data() { return (byte *) _data; } 69 | 70 | uint16_t PoseCnt() { return (_header[AD_OFFSET_POSECNT_HIGH] << 8 | _header[AD_OFFSET_POSECNT_LOW]); } 71 | uint16_t PoseOffset() { return _poseOffset; } 72 | uint16_t BufferEndPose() { return (_poseOffset + AD_PBUFFER_COUNT - 1);} 73 | void RefreshActionInfo(); 74 | bool IsPoseReady(uint16_t poseId); 75 | bool IsPoseReady(uint16_t poseId, uint16_t &offset); 76 | bool PoseOffsetInBuffer(uint16_t poseId, uint16_t &offset); 77 | 78 | void GenSample(byte actionId); 79 | 80 | 81 | private: 82 | 83 | byte ReadActionFile(int actionId); 84 | byte SpiffsReadActionFile(int actionId); 85 | byte ReadActionHeader(int actionId); 86 | byte SpiffsReadActionHeader(int actionId); 87 | byte ReadActionPose(); 88 | byte SpiffsReadActionPose(); 89 | 90 | byte _id; 91 | byte _len; 92 | char *_name; 93 | byte *_action; 94 | char _filename[25]; 95 | uint16_t _poseOffset; 96 | byte _header[AD_HEADER_SIZE]; 97 | byte _data[AD_PBUFFER_SIZE]; 98 | 99 | }; 100 | 101 | #endif -------------------------------------------------------------------------------- /src/ComboData.cpp: -------------------------------------------------------------------------------- 1 | #include "ComboData.h" 2 | 3 | ComboData::ComboData() { 4 | 5 | } 6 | 7 | ComboData::~ComboData() { 8 | } 9 | 10 | 11 | void ComboData::InitCombo(byte seq) { 12 | #ifdef DEBUG_ComboData 13 | Serial1.printf("ComboData::InitObject(%d)\n", seq); 14 | #endif 15 | _seq = seq; 16 | // To avoid duplicate memory required when saving record and simplify the action 17 | // use the same structe in memory & file 18 | memset(_data, 0, CD_COMBO_DATA_SIZE); 19 | _data[0] = 0xA9; // File identifier A9 9A 20 | _data[1] = 0x9A; 21 | _data[2] = CD_COMBO_DATA_SIZE - 4; 22 | _data[4] = seq; 23 | _data[CD_COMBO_DATA_SIZE - 1] = 0xED; 24 | } 25 | 26 | byte ComboData::ReadSPIFFS(byte seq) { 27 | #ifdef DEBUG_ComboData 28 | Serial1.printf("ComboData::ReadSPIFFS(%d)\n", seq); 29 | #endif 30 | if (!SPIFFS.begin()) return RESULT::ERR::SPIFFS; 31 | byte success = SpiffsReadComboFile(seq); 32 | SPIFFS.end(); 33 | return success; 34 | } 35 | 36 | byte ComboData::SpiffsReadComboFile(byte seq) { 37 | #ifdef DEBUG_ComboData 38 | Serial1.printf("ComboData::SpiffsReadActionHeader(%d)\n", seq); 39 | #endif 40 | InitCombo(seq); 41 | 42 | char fileName[25]; 43 | memset(fileName, 0, 25); 44 | sprintf(fileName, COMBO_FILE, seq); 45 | if (!SPIFFS.exists(fileName)) { 46 | return RESULT::ERR::FILE_NOT_FOUND; // File must checked before calling the function 47 | } 48 | 49 | File f = SPIFFS.open(fileName, "r"); 50 | if (!f) return RESULT::ERR::FILE_OPEN_READ; 51 | 52 | if (f.size() < CD_COMBO_DATA_SIZE) { 53 | return RESULT::ERR::FILE_SIZE; 54 | } 55 | 56 | byte *buffer; 57 | buffer = (byte *) malloc(CD_COMBO_DATA_SIZE); 58 | size_t bCnt = f.readBytes((char *) buffer, CD_COMBO_DATA_SIZE); 59 | byte result = RESULT::ERR::FILE_SIZE; 60 | 61 | if (bCnt == CD_COMBO_DATA_SIZE) { 62 | memcpy(_data, buffer, CD_COMBO_DATA_SIZE); 63 | result = RESULT::SUCCESS; 64 | } 65 | 66 | free(buffer); 67 | f.close(); 68 | return result; 69 | } 70 | -------------------------------------------------------------------------------- /src/ComboData.h: -------------------------------------------------------------------------------- 1 | #ifndef _COMBO_DATA_H_ 2 | #define _COMBO_DATA_H_ 3 | 4 | #include 5 | #include 6 | #include "RESULT.h" 7 | 8 | #define CD_MAX_COMBO 10 9 | #define CD_COMBO_DATA_SIZE 60 10 | #define CD_COMBO_SIZE 2 11 | 12 | #define COMBO_PATH "/alpha/combo" 13 | #define COMBO_FILE "/alpha/combo/%03d.dat" 14 | #define COMBO_POS 13 15 | 16 | #define CD_OFFSET_SEQ 4 17 | #define CD_OFFSET_DATA 10 18 | #define CD_COFFSET_ACTION 0 19 | #define CD_COFFSET_COUNT 1 20 | 21 | class ComboData { 22 | public: 23 | ComboData(); 24 | ~ComboData(); 25 | void InitCombo(byte seq); 26 | 27 | inline byte ReadSPIFFS() { return ReadSPIFFS(_seq); } 28 | byte ReadSPIFFS(byte seq); 29 | 30 | byte * Data() { return (byte *) _data; } 31 | 32 | private: 33 | byte SpiffsReadComboFile(byte seq); 34 | 35 | byte _seq; 36 | char _data[CD_COMBO_DATA_SIZE]; 37 | }; 38 | 39 | #endif -------------------------------------------------------------------------------- /src/Command_Generic.ino: -------------------------------------------------------------------------------- 1 | #include "robot.h" 2 | 3 | #define GENERIC_COMMAND_WAIT_TIME 200 4 | 5 | bool SendGenericCommand(byte *cmd, byte sendCnt) { 6 | 7 | // Clear robotPort buffer 8 | while (robotPort.available()) { 9 | char ch = robotPort.read(); 10 | if (!robotPort.available()) delay(1); 11 | } 12 | 13 | robotPort.enableTx(true); 14 | delayMicroseconds(10); 15 | robotPort.write(cmd, sendCnt); 16 | robotPort.enableTx(false); 17 | 18 | unsigned long endMs = millis() + GENERIC_COMMAND_WAIT_TIME; 19 | while ( (millis() < endMs) && (!robotPort.available()) ) delay(1); 20 | if (!robotPort.available()) { 21 | if (debug) DEBUG.printf("No return detected\n"); 22 | return false; 23 | } 24 | 25 | byte cnt = 0; 26 | byte buffer[64]; 27 | while (robotPort.available()) { 28 | buffer[cnt++] = robotPort.read(); 29 | if (cnt >= 64) break; 30 | // wait 1 ms for serial data to make sure all related result received 31 | if (!robotPort.available()) delay(1); 32 | } 33 | // Clear buffer 34 | while (robotPort.available()) { 35 | robotPort.read(); 36 | if (!robotPort.available()) delay(1); 37 | } 38 | 39 | String sender; 40 | if (cnt > 0) { 41 | if (SWFM.wifiClientConnected()) { 42 | SWFM.write(buffer, cnt); 43 | sender = "Network"; 44 | } else { 45 | Serial.write(buffer, cnt); 46 | sender = "Serial"; 47 | } 48 | if (debug) DEBUG.printf("Send result via %s\n", sender.c_str()); 49 | } 50 | 51 | return true; 52 | } -------------------------------------------------------------------------------- /src/EyeLed.h: -------------------------------------------------------------------------------- 1 | #ifndef _EYELED_H_ 2 | #define _EYELED_H_ 3 | 4 | // EyeLed.ino 5 | void ReserveEyeBlink(); 6 | void ReserveEyeBreath(); 7 | void EyeBlink(); 8 | void EyeBreath(); 9 | void EyeLedHandle(); 10 | 11 | #endif 12 | -------------------------------------------------------------------------------- /src/EyeLed.ino: -------------------------------------------------------------------------------- 1 | #include "robot.h" 2 | 3 | unsigned long eyeBlinkTime; 4 | boolean eyeStatus; 5 | boolean eyeBlinkStatus ,eyeBreathStatus; 6 | unsigned long eyeBreathTime; 7 | uint8_t breathPwm = 0; 8 | int8_t breathRatio = 3; 9 | 10 | void ReserveEyeBlink(){ 11 | eyeBlinkStatus = !eyeBlinkStatus; 12 | for(int i=0;i<4;i++){ 13 | digitalWrite(HEAD_LED_GPIO,i%2); 14 | delay(50); 15 | }; 16 | } 17 | 18 | void ReserveEyeBreath(){ 19 | eyeBreathStatus = !eyeBreathStatus; 20 | for(int i=0;i<4;i++){ 21 | digitalWrite(HEAD_LED_GPIO,i%2); 22 | delay(50); 23 | }; 24 | } 25 | 26 | void EyeBlink(){ 27 | if(millis()-eyeBlinkTime>300 && eyeBlinkStatus ){ 28 | digitalWrite(HEAD_LED_GPIO,eyeStatus); 29 | eyeStatus = !eyeStatus; 30 | eyeBlinkTime = millis(); 31 | } 32 | } 33 | 34 | void EyeBreath(){ 35 | if(millis()-eyeBreathTime>40 && eyeBreathStatus){ 36 | breathPwm += breathRatio; 37 | if(breathPwm>250){ 38 | breathRatio=-breathRatio; 39 | breathPwm=250; 40 | } 41 | else if(breathPwm<=0){ 42 | breathRatio= breathRatio; 43 | breathPwm=0; 44 | } 45 | analogWrite(HEAD_LED_GPIO,breathPwm); 46 | eyeBreathTime = millis(); 47 | } 48 | } 49 | 50 | void EyeLedHandle(){ 51 | EyeBreath(); 52 | EyeBlink(); 53 | } 54 | -------------------------------------------------------------------------------- /src/HAILZD_Command.ino: -------------------------------------------------------------------------------- 1 | /* 2 | UBT Bluetooth Coomunization Command 3 | 4 | */ 5 | #include "robot.h" 6 | 7 | bool HAILZD_Command() { 8 | 9 | if (debug) DEBUG.print(F("HAILZD Command detected\n")); 10 | 11 | // #?P???\r\d - at lease 8 bytes 12 | if (cmdBuffer.available() < 8) { 13 | return true; 14 | } 15 | 16 | // command ended with 0x0d 0x0a 17 | byte lastEndPos = cmdBuffer.available() - 1; 18 | byte endPos = 6; 19 | bool findEnd = false; 20 | 21 | while (endPos < lastEndPos) { 22 | findEnd = ((cmdBuffer.peek(endPos) == 0x0D) && (cmdBuffer.peek(endPos+1) == 0x0A)); 23 | if (findEnd) break; 24 | endPos++; 25 | } 26 | 27 | if (!findEnd) return true; 28 | 29 | byte cmd[endPos+2]; 30 | cmdBuffer.read(cmd, endPos+2); 31 | 32 | if (!enable_HAILZD) return true; 33 | 34 | if (debug) { 35 | DEBUG.printf("HaiLzd Servo Command sent (%d): ", endPos+2); 36 | for (int i = 0; i < endPos; i++) { 37 | DEBUG.print((char) cmd[i]); 38 | } 39 | DEBUG.println(); 40 | } 41 | SendGenericCommand(cmd, endPos + 2); 42 | return true; 43 | } -------------------------------------------------------------------------------- /src/OTA.h: -------------------------------------------------------------------------------- 1 | #ifndef _OTA_H_ 2 | #define _OTA_H_ 3 | 4 | // #define ENABLE_OTA 5 | 6 | #ifdef ENABLE_OTA 7 | //OTA Setting 8 | #include 9 | #include 10 | #include 11 | #include 12 | const char* ssid = "wuhulanren"; 13 | const char* password = "wuhulanren"; 14 | #define EN_OTA true 15 | 16 | // OTA.ino 17 | void ArduinoOTASetup(); 18 | void ArduinoOTAHandle(); 19 | 20 | #endif 21 | 22 | #endif -------------------------------------------------------------------------------- /src/OTA.ino: -------------------------------------------------------------------------------- 1 | #include "robot.h" 2 | 3 | #ifdef ENABLE_OTA 4 | 5 | void ArduinoOTASetup(){ 6 | Serial.begin(115200); 7 | Serial.println("Booting"); 8 | WiFi.mode(WIFI_STA); 9 | WiFi.begin(ssid, password); 10 | while (WiFi.waitForConnectResult() != WL_CONNECTED) { 11 | Serial.println("Connection Failed! Rebooting..."); 12 | delay(5000); 13 | ESP.restart(); 14 | ArduinoOTA.onStart([]() { 15 | Serial.println("Start"); 16 | }); 17 | ArduinoOTA.onEnd([]() { 18 | Serial.println("\nEnd"); 19 | }); 20 | ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) { 21 | Serial.printf("Progress: %u%%\r", (progress / (total / 100))); 22 | }); 23 | ArduinoOTA.onError([](ota_error_t error) { 24 | Serial.printf("Error[%u]: ", error); 25 | if (error == OTA_AUTH_ERROR) Serial.println("Auth Failed"); 26 | else if (error == OTA_BEGIN_ERROR) Serial.println("Begin Failed"); 27 | else if (error == OTA_CONNECT_ERROR) Serial.println("Connect Failed"); 28 | else if (error == OTA_RECEIVE_ERROR) Serial.println("Receive Failed"); 29 | else if (error == OTA_END_ERROR) Serial.println("End Failed"); 30 | }); 31 | ArduinoOTA.begin(); 32 | Serial.println("Ready"); 33 | Serial.print("IP address: "); 34 | Serial.println(WiFi.localIP()); 35 | } 36 | } 37 | 38 | void ArduinoOTAHandle(){ 39 | ArduinoOTA.handle(); 40 | } 41 | 42 | #endif -------------------------------------------------------------------------------- /src/RESULT.h: -------------------------------------------------------------------------------- 1 | #ifndef _RESULT_H_ 2 | #define _RESULT_H_ 3 | 4 | 5 | class RESULT { 6 | public: 7 | static const byte SUCCESS = 0; 8 | 9 | class ERR { 10 | public: 11 | // Generic error 12 | static const byte SPIFFS = 1; 13 | static const byte NOT_FOUND = 2; 14 | static const byte NOT_MATCH = 3; 15 | static const byte NOT_READY = 4; 16 | static const byte READ = 5; 17 | static const byte WRITE = 6; 18 | static const byte COPY = 7; 19 | static const byte DATA_OVERFLOW = 8; 20 | static const byte VERSION_NOT_MATCH = 9; 21 | 22 | 23 | static const byte PARM_SIZE = 11; 24 | static const byte PARM_AID_NOT_MATCH = 12; 25 | static const byte PARM_AD_NAME_SIZE = 13; 26 | static const byte PARM_PID_OUT_RANGE = 14; 27 | static const byte PARM_COMBO_OUT_RANGE = 15; 28 | static const byte PARM_INVALID = 16; 29 | static const byte CHECKSUM = 19; 30 | static const byte FILE_SPIFFS = 21; 31 | static const byte FILE_NOT_FOUND = 22; 32 | static const byte FILE_OPEN_READ = 23; 33 | static const byte FILE_OPEN_WRITE = 24; 34 | static const byte FILE_OPEN_APPEND = 25; 35 | static const byte FILE_SIZE = 26; 36 | static const byte FILE_READ_COUNT = 27; 37 | static const byte FILE_WRITE_COUNT = 28; 38 | static const byte FILE_SEEK = 29; 39 | static const byte FILE_REMOVE = 30; 40 | static const byte AD_HEADER_CONTENT = 31; 41 | static const byte AD_HEADER_CHECKSUM = 32; 42 | static const byte AD_POSE_NOT_READY = 35; 43 | static const byte AD_POSE_CHECKSUM = 36; 44 | static const byte UPDATE_CONDIG = 41; 45 | static const byte UNKNOWN = 255; 46 | }; 47 | }; 48 | 49 | 50 | #endif -------------------------------------------------------------------------------- /src/RobotEventHandler.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROBOTEVENTHANDLER_H_ 2 | #define _ROBOTEVENTHANDLER_H_ 3 | 4 | #include "EventData.h" 5 | #include "EventHandler.h" 6 | #include "EdsDrivers.h" 7 | 8 | #define EVENT_IDEL_FILE "/alpha/event/idle.event" 9 | #define EVENT_BUSY_FILE "/alpha/event/busy.event" 10 | 11 | #define EVENT_HANDLER_VERSION 1 12 | 13 | #define EH_OFFSET_MODE 4 14 | #define EH_OFFSET_VERSION 5 15 | #define EH_OFFSET_COUNT 6 16 | #define EH_OFFSET_ACTION 7 17 | 18 | #define ED_OFFSET_MODE 4 19 | #define ED_OFFSET_STARTIDX 5 20 | #define ED_OFFSET_COUNT 6 21 | 22 | 23 | #define EVENT_HANDLER_ELAPSE_MS 10 24 | 25 | bool eventHandlerSuspended = true; 26 | 27 | struct { 28 | uint8_t rx_pin = 14; 29 | uint8_t tx_pin = 14; 30 | unsigned long baud = 115200; 31 | bool inverse_logic = false; 32 | uint16_t buffer_size = SSB_BUFFER_SIZE; 33 | } ssbConfig; 34 | 35 | SoftwareSerial ssbPort(ssbConfig.rx_pin, ssbConfig.tx_pin, ssbConfig.inverse_logic, ssbConfig.buffer_size); 36 | 37 | unsigned long nextHandlerMs = 0; 38 | unsigned long nextShowMs = 0; 39 | bool lastPlaying = false; 40 | 41 | #define EDS_TOUCH_GPIO 13 42 | #define EDS_MPU6050_I2CADDR 0x68 // I2C address of the MPU-6050 43 | 44 | // Too bad, cannot put them here as it cannot reconized the _dbg in robot.h, so have to move to robot.h 45 | // After Eds moved to robot.h, eData also need to mov e to robot.h, so all moved to robot.h 46 | 47 | //EventData eData; 48 | //EventHandler eIdle(&eData); 49 | //EventHandler eBusy(&eData); 50 | //EventHandler eTemp(&eData); 51 | 52 | //SSBoard ssb; 53 | //EdsPsxButton edsPsxButton(&eData, _dbg); 54 | //EdsBattery edsBattery(&eData, _dbg); 55 | //EdsTouch edsTouch(&eData, _dbg); 56 | 57 | void InitEventHandler(); 58 | void RobotEventHandler(); 59 | void CheckPosition(); 60 | void CheckTouch(); 61 | void CheckVoltage() ; 62 | byte GetPower(uint16_t v); 63 | 64 | 65 | EventDataSource** eds; 66 | 67 | void EnableSsbTxCallBack(bool send); 68 | void USB_TTL(SoftwareSerial *ttl); 69 | void USER_TTL(SoftwareSerial *ttl); 70 | 71 | #endif -------------------------------------------------------------------------------- /src/RobotMaintence.ino: -------------------------------------------------------------------------------- 1 | #include "robot.h" 2 | 3 | void RobotMaintenanceMode() { 4 | unsigned long nextMs; 5 | bool keepGoing = true; 6 | while (keepGoing) { 7 | ClearBuffer(); 8 | nextMs = 0; 9 | while (!Serial.available()) { 10 | if (millis() > nextMs) { 11 | Serial.println("(L)ist, (D)elete, (G)enerate, E(x)it"); 12 | nextMs = millis() + 5000; 13 | } 14 | } 15 | ch = Serial.read(); 16 | switch (ch) { 17 | case 'L': 18 | case 'l': 19 | RM_ListFile(); 20 | break; 21 | case 'D': 22 | case 'd': 23 | RM_DelFile(); 24 | break; 25 | case 'G': 26 | case 'g': 27 | RM_GenFile(); 28 | break; 29 | case 'X': 30 | case 'x': 31 | keepGoing = false; 32 | break; 33 | } 34 | } 35 | } 36 | 37 | void ClearBuffer() { 38 | while (Serial.available()) { 39 | Serial.read(); 40 | delay(1); 41 | } 42 | } 43 | 44 | void WaitForInput() { 45 | ClearBuffer(); 46 | while (!Serial.available()); 47 | } 48 | 49 | void GetTypeId(char *action, int *id) { 50 | if (Serial.available()) { 51 | *action = Serial.read(); 52 | } else { 53 | *action = 0; 54 | } 55 | int aId = -1; 56 | while (Serial.available()) { 57 | char ch = Serial.read(); 58 | if ((ch >= '0') && (ch <= '9')) { 59 | if (aId == -1) { 60 | aId = ch - '0'; 61 | } else { 62 | aId = aId * 10 + ch - '0'; 63 | } 64 | } else { 65 | break; 66 | } 67 | } 68 | *id = aId; 69 | } 70 | 71 | void RM_ListFile() { 72 | char ch = 0; 73 | if (Serial.available()) ch = Serial.read(); 74 | Serial.print("\nList files in SPIFFS:\n"); 75 | SPIFFS.begin(); 76 | Dir dir; 77 | switch (ch) { 78 | case 'A': 79 | case 'a': 80 | Serial.println("\nList of action files: \n"); 81 | dir = SPIFFS.openDir("/alpha/action/"); 82 | break; 83 | default: 84 | Serial.println("\nFiles of all files in SPIFFS:\n"); 85 | dir = SPIFFS.openDir(""); 86 | break; 87 | } 88 | while (dir.next()) { 89 | Serial.print(dir.fileName()); 90 | 91 | File f = dir.openFile("r"); 92 | Serial.printf(" %d\n", f.size()); 93 | f.close(); 94 | } 95 | Serial.println(); 96 | SPIFFS.end(); 97 | } 98 | 99 | void RM_DelFile() { 100 | char ch = 0; 101 | if (Serial.available()) ch = Serial.read(); 102 | switch (ch) { 103 | case 'A': 104 | case 'a': 105 | RM_DelAction(); 106 | break; 107 | 108 | default: 109 | Serial.print("Invalid file type\n\n"); 110 | break; 111 | } 112 | } 113 | 114 | void RM_DelAction() { 115 | int aId = -1; 116 | while (Serial.available()) { 117 | char ch = Serial.read(); 118 | if ((ch >= '0') && (ch <= '9')) { 119 | if (aId == -1) { 120 | aId = ch - '0'; 121 | } else { 122 | aId = aId * 10 + ch - '0'; 123 | } 124 | } else { 125 | break; 126 | } 127 | } 128 | byte result = actionData.DeleteActionFile((byte) aId); 129 | 130 | char fileName[25]; 131 | memset(fileName, 0, 25); 132 | sprintf(fileName, ACTION_FILE, aId); 133 | switch (result) { 134 | case 0: 135 | Serial.printf("Action file delted: %s\n\n", fileName); 136 | break; 137 | case 1: 138 | Serial.printf("Action file not found: %s\n\n", fileName); 139 | break; 140 | case 2: 141 | Serial.printf("Fail to delete file: %s\n\n", fileName); 142 | break; 143 | default: 144 | Serial.printf("Fail to delete file: %s\n\n", fileName); 145 | break; 146 | } 147 | SPIFFS.end(); 148 | } 149 | 150 | 151 | void RM_GenFile() { 152 | char ch = 0; 153 | if (Serial.available()) ch = Serial.read(); 154 | switch (ch) { 155 | case 'A': 156 | case 'a': 157 | // RM_GenAction(); 158 | break; 159 | 160 | default: 161 | Serial.print("Invalid file type\n\n"); 162 | break; 163 | } 164 | } 165 | -------------------------------------------------------------------------------- /src/UBTBT_Command.ino: -------------------------------------------------------------------------------- 1 | /* 2 | UBT Bluetooth Coomunization Command 3 | 4 | */ 5 | #include "robot.h" 6 | 7 | bool UBTBT_Command() { 8 | if (!enable_UBTCB) return true; 9 | 10 | 11 | return true; 12 | } -------------------------------------------------------------------------------- /src/UBTCB_Command.ino: -------------------------------------------------------------------------------- 1 | /* 2 | UBT Control Board Coomunization Command 3 | 4 | */ 5 | #include "robot.h" 6 | 7 | bool UBTCB_Command() { 8 | if (!enable_UBTCB) return true; 9 | 10 | 11 | return true; 12 | } -------------------------------------------------------------------------------- /src/UBTSV_Command.ino: -------------------------------------------------------------------------------- 1 | /* 2 | UBT Servo Coomunization Command 3 | 4 | */ 5 | #include "robot.h" 6 | 7 | #pragma region "UBTech Command" 8 | bool UBTSV_Command() { 9 | 10 | if (debug) DEBUG.print(F("UBT Servo Command detected\n")); 11 | 12 | // if (cmdBuffer.available() < 10) return false; 13 | // It's not logicaly that 10 byte cannot be received within 1 ms, so consider as invalid input directly 14 | if (cmdBuffer.available() < 10) { 15 | if (debug) DEBUG.print(F("Invalid length\n")); 16 | return cmdSkip(true); 17 | } 18 | 19 | byte cmd[10]; 20 | byte result[10]; 21 | 22 | cmdBuffer.peek(cmd, 10); // Don't read command, just check the data first 23 | if (cmd[9] != 0xED) { 24 | if (debug) DEBUG.print(F("Invalid end code\n")); 25 | return cmdSkip(true); 26 | } 27 | 28 | if (((cmd[0] == 0xFA) && (cmd[1] != 0xAF)) || ((cmd[0] == 0xFC) && (cmd[1] != 0xCF))) { 29 | if (debug) DEBUG.print(F("Invalid start code\n")); 30 | return cmdSkip(true); 31 | } 32 | 33 | byte sum = CheckSum(cmd); 34 | if (cmd[8] != sum) { 35 | if (debug) { 36 | if (debug) DEBUG.print(F("Invalid checksum\n")); 37 | } 38 | return cmdSkip(true); 39 | } 40 | // Now a complete command received, clear data from buffer 41 | cmdBuffer.skip(10); 42 | 43 | if (!enable_UBTSV) return true; 44 | 45 | if (debug) { 46 | DEBUG.print(F("UBT Servo Command sent: ")); 47 | for (int i = 0; i < 10; i++) { 48 | DEBUG.printf("%02X ", cmd[i]); 49 | } 50 | DEBUG.println(); 51 | } 52 | 53 | SendGenericCommand(cmd, 10); 54 | return true; 55 | 56 | } 57 | #pragma endregion 58 | -------------------------------------------------------------------------------- /src/UBT_Common.ino: -------------------------------------------------------------------------------- 1 | // UBT command utility 2 | #include "robot.h" 3 | 4 | void UBT_GetServoAngle(byte *result) { 5 | // result must have at least 32 bytes 6 | 7 | for (int id = 1; id <= config.maxServo() ; id++) { 8 | int pos = 2 * (id - 1); 9 | if (rs.exists(id)) { 10 | if (rs.isLocked(id)) { 11 | result[pos] = rs.lastAngle(id); 12 | result[pos+1] = 1; 13 | } else { 14 | result[pos] = rs.getAngle(id); 15 | result[pos+1] = 0; 16 | } 17 | } else { 18 | result[pos] = 0xFF; 19 | result[pos+1] = 0; 20 | } 21 | } 22 | 23 | if (debug) { 24 | for (int i = 0; i < 2 * config.maxServo(); i++) { 25 | DEBUG.printf("%02X ", result[i]); 26 | } 27 | DEBUG.println(); 28 | } 29 | } 30 | 31 | void UBT_GetServoAdjAngle(byte *result) { 32 | for (int id = 1; id <= config.maxServo(); id++) { 33 | int pos =2 * (id - 1); 34 | if (rs.exists(id)) { 35 | uint16 adjAngle = rs.getAdjAngle(id); 36 | result[pos] = adjAngle / 256; 37 | result[pos+1] = adjAngle % 256; 38 | } else { 39 | result[pos] = 0x7F; 40 | result[pos+1] = 0x7F; 41 | } 42 | } 43 | } 44 | 45 | -------------------------------------------------------------------------------- /src/UTIL.h: -------------------------------------------------------------------------------- 1 | #ifndef _UTIL_H_ 2 | #define _UTIL_H_ 3 | 4 | void SetDebug(bool mode); 5 | void SetHeadLed(bool status); 6 | 7 | byte CheckSum(byte *cmd); 8 | byte CheckVarSum(byte *cmd); 9 | byte CheckFullSum(byte *cmd); 10 | 11 | void clearInputBuffer(); 12 | bool cmdSkip(bool flag); 13 | 14 | void DebugShowSkipByte(); 15 | 16 | #endif -------------------------------------------------------------------------------- /src/UTIL.ino: -------------------------------------------------------------------------------- 1 | #include "robot.h" 2 | 3 | void SetDebug(bool mode) { 4 | debug = mode; 5 | _dbg->enableDebug(mode); 6 | _dbg->setLogLevel((debug ? DEFAULT_LOG_LEVEL : 0)); 7 | rs.enableDebug(mode); 8 | mp3.setDebug(mode); 9 | config.setDebug(mode); 10 | SWFM.enableDebug(mode); 11 | } 12 | 13 | 14 | void SetHeadLed(bool status) { 15 | headLed = status; 16 | if (status) { 17 | #ifdef ROBOT_ARM_BOARD 18 | digitalWrite(HEAD_LED_GPIO, LOW); 19 | #else 20 | digitalWrite(HEAD_LED_GPIO, HIGH); 21 | #endif 22 | } else { 23 | #ifdef ROBOT_ARM_BOARD 24 | digitalWrite(HEAD_LED_GPIO, HIGH); 25 | #else 26 | digitalWrite(HEAD_LED_GPIO, LOW); 27 | #endif 28 | } 29 | } 30 | 31 | // XX XX {id} {cmd} xx xx xx xx {sum} ED 32 | // {sum} = cmd[8] = sum ( {id} {cmd} xx xx xx xx ) 33 | byte CheckSum(byte *cmd) { 34 | byte sum = 0; 35 | for (int i = 2; i < 8; i++) { 36 | sum += cmd[i]; 37 | } 38 | return sum; 39 | } 40 | 41 | // XX XX {len} xx .. xx {sum} ED 42 | // {len} = cmd[2] = count( {len} xx .. xx ) 43 | // {sum} = cmd[{len} + 2] = sum( {len} xx .. xx ) 44 | // e.g. F3 3F 45 | byte CheckVarSum(byte *cmd) { 46 | byte sum = 0; 47 | int len = cmd[2]; 48 | int endPos = len + 2; 49 | for (int i = 2; i < endPos; i++) { 50 | sum += cmd[i]; 51 | } 52 | return sum; 53 | } 54 | 55 | 56 | // XX XX {len} {cmd} xx .. xx {sum} ED 57 | // {len} = count( XX XX {len} {cmd} xx .. xx {sum} ) 58 | // {sum} = cmd[{len} - 1] = sum( {len} {cmd} xx .. xx ) 59 | // e.g. F3 3F 60 | byte CheckFullSum(byte *cmd) { 61 | byte sum = 0; 62 | int len = cmd[2]; 63 | int endPos = len - 1; 64 | for (int i = 2; i < endPos; i++) { 65 | sum += cmd[i]; 66 | } 67 | return sum; 68 | } 69 | 70 | void clearInputBuffer() { 71 | while (Serial.available()) { 72 | Serial.read(); 73 | if (!Serial.available()) delay(1); // make sure no more data coming, add 1ms delay when no data. 74 | } 75 | } 76 | 77 | bool cmdSkip(bool flag) { 78 | DebugShowSkipByte(); 79 | cmdBuffer.skip(); 80 | return flag; 81 | } 82 | 83 | void DebugShowSkipByte() { 84 | if (debug) { 85 | DEBUG.printf("Skip byte: %02X\n", cmdBuffer.peek()); 86 | } 87 | } -------------------------------------------------------------------------------- /src/V1_Command.ino: -------------------------------------------------------------------------------- 1 | #include "robot.h" 2 | 3 | // A - Angle : (V1_GetServoAngle, V1_GetServoAngleText) 4 | // B - Debug 5 | // C 6 | // D - Download Action Data (MCU -> PC) 7 | // E 8 | // F - Free Servo 9 | // G 10 | // H 11 | // I 12 | // J 13 | // K 14 | // L - Lock servo 15 | // M - Move 16 | // N 17 | // O 18 | // P - Play Action/Combo 19 | // Q 20 | // R - Read Action Data form SPIFFS 21 | // S - Stop Action 22 | // T - Detect Servo 23 | // U - Upload Action Data (PC -> MCU) 24 | // V 25 | // W - Write Action Data to SPIFFS 26 | // X 27 | // Y 28 | // Z - Reset Connection 29 | 30 | bool V1_Command() { 31 | byte cmd = cmdBuffer.read(); 32 | 33 | // a little bit danger as subsequent data may trrigger other commands 34 | // but unforunately, V1 command does not have start/sum/end structure, 35 | // it need to identify the command one by one until parameters read. 36 | // For to simplify the logic, skip the command directly here. 37 | // Unless the data part meet the start/sum/end of other command, it will not be triggered. 38 | if (!enable_V1) { 39 | if (debug) DEBUG.printf("V1 Command disabled: %c [0x%02X]\n", cmd, cmd); 40 | return true; 41 | } 42 | 43 | switch (cmd) { 44 | case 'A': 45 | V1_GetServoAngle(); 46 | break; 47 | case 'a': 48 | V1_GetServoAngleText(); 49 | break; 50 | case 'i': 51 | V1_CheckSD(&Serial); 52 | break; 53 | case 'I': 54 | V1_CheckSD(&Serial1); 55 | break; 56 | case 'P': 57 | V1_PlayAction(); 58 | break; 59 | case 'Z': 60 | V1_ResetConnection(); 61 | break; 62 | } 63 | return true; 64 | } 65 | 66 | #pragma region "Utilities" 67 | 68 | int getServoId() { 69 | if (!cmdBuffer.available()) return -1; 70 | int id = (byte) cmdBuffer.read(); 71 | if ((id >= '0') && (id <= '9')) { 72 | id -= '0'; 73 | } else if ((id >= 'A') && (id <= 'G')) { 74 | id = 10 + id - 'A'; 75 | } else if ((id >= 'a') && (id <= 'g')) { 76 | id = 10 + id - 'a'; 77 | } else { 78 | return -2; 79 | } 80 | return id; 81 | } 82 | 83 | #pragma endregion 84 | 85 | #pragma region "A,a - Get Servo Angle" 86 | 87 | void V1_GetServoAngle() { 88 | if (debug) DEBUG.println(F("[V1_GetServoAngle]")); 89 | byte outBuffer[32]; 90 | UBT_GetServoAngle(outBuffer); 91 | Serial.write(outBuffer, 32); 92 | } 93 | 94 | void V1_GetServoAngleText() { 95 | if (debug) DEBUG.println(F("[V1_GetServoAngleText]")); 96 | byte outBuffer[32]; 97 | UBT_GetServoAngle(outBuffer); 98 | Serial.println(F("\nServo Angle:\n")); 99 | for (int id = 1; id <= 16; id++) { 100 | int pos = 2 * (id - 1); 101 | Serial.printf("Servo %02d: ", id); 102 | if (outBuffer[pos] == 0xFF) { 103 | Serial.println("---"); 104 | } else { 105 | Serial.printf("%3d [0x%02X] %s\n", outBuffer[pos], outBuffer[pos], (outBuffer[pos+1] ? "Locked": "")); 106 | } 107 | } 108 | } 109 | 110 | #pragma endregion 111 | 112 | #pragma region "P - Play action" 113 | 114 | void V1_PlayAction() { 115 | if (debug) DEBUG.println(F("[V1_PlayAction]")); 116 | byte actionId = 0; 117 | if (cmdBuffer.available()) { 118 | byte ch = cmdBuffer.read(); 119 | if ((ch >= 'A') && (ch <= 'Z')) { 120 | actionId = ch - 'A'; 121 | } else { 122 | if (debug) DEBUG.printf("Invalid action: %02X\n", ch); 123 | return; // Return for invalid action 124 | } 125 | 126 | // Convert V1 Play Action to V2 Play Action 127 | byte cmd[] = {0x9A, 0x9A, 0x03, 0x41, actionId , 0x00 ,0xED}; 128 | 129 | V2_GoAction(actionId, false, cmd); 130 | } 131 | 132 | 133 | } 134 | #pragma endregion 135 | 136 | #pragma region "Z - Reset Connection" 137 | 138 | void V1_ResetConnection() { 139 | if (debug) DEBUG.println(F("[V1_ResetConnection]")); 140 | // bus control is not under RobotServo, should be done separately 141 | robotPort.end(); 142 | delay(100); 143 | robotPort.begin(busConfig.baud); 144 | delay(100); 145 | 146 | byte showAngle = 0; 147 | if (cmdBuffer.available()) showAngle = cmdBuffer.read(); 148 | if ((showAngle) && (showAngle != '0')) V1_GetServoAngle(); 149 | } 150 | 151 | #pragma endregion 152 | 153 | void V1_CheckSD(HardwareSerial *ss) { 154 | if (debug) DEBUG.println(F("[V1_CheckSD]")); 155 | 156 | ss->println("**** SPIFFS List"); 157 | SPIFFS.begin(); 158 | Dir dir; 159 | ss->println("\nFiles in root: "); 160 | dir = SPIFFS.openDir(""); 161 | while (dir.next()) { 162 | ss->print(dir.fileName()); 163 | ss->print(" "); 164 | File f = dir.openFile("r"); 165 | ss->println(f.size()); 166 | } 167 | 168 | SPIFFS.end();; 169 | } 170 | -------------------------------------------------------------------------------- /src/V2_CheckAction.ino: -------------------------------------------------------------------------------- 1 | #include "robot.h" 2 | #include 3 | #include "V2_Command.h" 4 | 5 | 6 | void V2_ResetAction() { 7 | V2_ActionPlaying = false; 8 | V2_ActionCombo = 0; 9 | V2_NextAction = 0; 10 | V2_NextPose = 0; 11 | V2_GlobalTimeMs = 0; 12 | V2_NextPlayMs = 0; 13 | V2_ActionPlayCount = 0; 14 | } 15 | 16 | bool V2_IsEndPose() { 17 | return (V2_NextPose >= actionData.PoseCnt()); 18 | } 19 | 20 | void V2_CheckAction() { 21 | 22 | if (!V2_ActionPlaying) return; 23 | if (millis() < V2_NextPlayMs) return; 24 | 25 | if (debug) DEBUG.printf("%08ld V2_CheckAction: %d - %d - %d\n", millis(), V2_ActionCombo, V2_NextAction, V2_NextPose); 26 | 27 | 28 | if (actionData.id() != V2_NextAction) { 29 | if (V2_NextPose) { 30 | // For new action, should always be started at 0 31 | // TODO: should quit or just reset to 0 ???? 32 | if (debug) DEBUG.printf("Invalid poseId %d for new action. Action stopped!\n", V2_NextPose); 33 | V2_ResetAction(); 34 | return; 35 | } 36 | if (debug) DEBUG.printf("Load Action: %d -> %d\n", actionData.id(), V2_NextAction); 37 | actionData.ReadSPIFFS(V2_NextAction); 38 | } 39 | // Should already checked in previous pose, just check again for safety 40 | if (V2_IsEndPose()) { 41 | if (V2_ActionPlayCount < 2) { 42 | if (debug) DEBUG.printf("Action Completed\n"); 43 | // TODO: go next action in Combo when Combo is ready 44 | V2_ResetAction(); 45 | return; 46 | } 47 | // Set count to 0xFF for endless loop 48 | if (V2_ActionPlayCount != 0xFF) V2_ActionPlayCount--; 49 | V2_NextPose = 0; 50 | if (debug) DEBUG.printf("Action finish, continue with last %d times\n", V2_ActionPlayCount); 51 | } 52 | 53 | 54 | uint16_t offset = 0; 55 | if (!actionData.IsPoseReady(V2_NextPose, offset)) { 56 | // Fail loading pose data 57 | if (debug) DEBUG.printf("Fail loading pose data: %d - %d\n", actionData.id(), V2_NextAction); 58 | V2_ResetAction(); 59 | return; 60 | } 61 | 62 | 63 | byte *pose = actionData.Data(); 64 | // pose = pose + AD_POSE_SIZE * V2_NextPose; 65 | pose = pose + offset; 66 | 67 | if (debug && deepDebug) { 68 | DEBUG.println("\nPOSE Data: "); 69 | for (int i = 0; i < AD_POSE_SIZE; i++) DEBUG.printf("%02X ", pose[i]); 70 | DEBUG.println("\n"); 71 | } 72 | 73 | 74 | // safetu check: running corrupted data may damage the servo 75 | if ((pose[0] != 0xA9) || (pose[1] != 0x9A) || (pose[AD_POSE_SIZE - 1] != 0xED)) { 76 | if (debug) DEBUG.printf("Data file corrupted, action STOP!\n"); 77 | V2_ResetAction(); 78 | return; 79 | } 80 | 81 | 82 | // Play current pose here 83 | // Should use float for rounding here? // or just let it truncated. 84 | float fServoTimeMs = (pose[AD_POFFSET_STIME] << 8) | pose[AD_POFFSET_STIME+1]; 85 | // int iServoTime = round(fServoTimeMs / V2_ServoTimeRatio); 86 | // byte servoTime = (byte) iServoTime; 87 | int iServoTime = fServoTimeMs; // For new servo library, use MS as servo time when calling move command 88 | uint16_t servoTime = (uint16_t) iServoTime; 89 | servoTime = CalAdjMs(servoTime); 90 | 91 | if (debug && deepDebug) DEBUG.printf("Servo Time: %f (%f) -> %d\n", fServoTimeMs, actionTimeFactor, servoTime); 92 | 93 | byte ledChange = 0; 94 | for (int i = 0; i < 8; i++) { 95 | ledChange |= pose[AD_POFFSET_LED + i]; 96 | } 97 | 98 | if (debug) DEBUG.printf("%08ld -- Start servo command\n", millis()); 99 | 100 | if (debug && deepDebug) DEBUG.printf("LED changed: %s\n", (ledChange ? "YES" : "NO")); 101 | 102 | 103 | // Move servo only if servo time > 0 104 | if ((servoTime > 0) || (ledChange)) { 105 | for (int id = 1; id <= config.maxServo(); id++) { 106 | if (rs.exists(id)) { 107 | if (ledChange) { 108 | int h = (id - 1) / 4; 109 | int l = 2 * ( 3 - ((id -1) % 4)); 110 | byte led = (pose[AD_POFFSET_LED + h] >> l) & 0b11; 111 | if ((led == 0b10) || (led == 0b11)) { 112 | byte newMode = (led & 1); 113 | rs.setLED(id, !newMode); 114 | } 115 | } 116 | if (servoTime > 0) { 117 | byte angle = pose[AD_POFFSET_ANGLE + id - 1]; 118 | // Check for dummy action to reduce commands 119 | if ((angle <= 0xF0) && ((V2_NextPose) || (rs.lastAngle(id) != angle))) { 120 | rs.goAngle(id, angle, servoTime); 121 | delay(1); 122 | } 123 | } 124 | } 125 | } 126 | } 127 | 128 | if (debug) DEBUG.printf("%08ld -- End servo command\n", millis()); 129 | 130 | // Check HeadLED, follow servo status 0 - on, 1 - off 131 | byte headLight = pose[AD_POFFSET_HEAD]; 132 | if (headLight == 0b10) { 133 | if (debug && deepDebug) DEBUG.printf("HeadLED: %d -> %d\n", headLight, 1); 134 | if (headLed != 1) SetHeadLed(1); 135 | } else if (headLight == 0b11) { 136 | if (debug && deepDebug) DEBUG.printf("HeadLED: %d -> %d\n", headLight, 0); 137 | if (headLed != 0) SetHeadLed(0); 138 | } 139 | 140 | // Chagne MP3 141 | byte mp3Folder = pose[AD_POFFSET_MP3_FOLDER]; 142 | byte mp3File = pose[AD_POFFSET_MP3_FILE]; 143 | byte mp3Vol = pose[AD_POFFSET_MP3_VOL]; 144 | 145 | if (debug && deepDebug) DEBUG.printf("MP3: %d %d %d\n", mp3Folder, mp3File, mp3Vol); 146 | 147 | if (mp3Vol == AD_MP3_STOP_VOL) { 148 | mp3.begin(); 149 | mp3.stop(); 150 | mp3.end(); 151 | } else { 152 | if (mp3File != 0xFF) { 153 | mp3.begin(); 154 | if (mp3Folder == 0xff) { 155 | mp3.playFile(mp3File); 156 | } else { 157 | mp3.playFolderFile(mp3Folder, mp3File); 158 | } 159 | mp3.end(); 160 | } 161 | } 162 | 163 | uint16_t waitTimeMs = (pose[AD_POFFSET_WTIME] << 8) | pose[AD_POFFSET_WTIME+1]; 164 | waitTimeMs = CalAdjMs(waitTimeMs); 165 | 166 | if (V2_UseGlobalTime) { 167 | V2_GlobalTimeMs += waitTimeMs; 168 | V2_NextPlayMs = V2_GlobalTimeMs; 169 | } else { 170 | V2_NextPlayMs = millis() + waitTimeMs; 171 | } 172 | 173 | 174 | if (debug) DEBUG.printf("Wait Time: %d -> %ld\n", waitTimeMs, V2_NextPlayMs); 175 | 176 | V2_NextPose++; 177 | 178 | if (V2_IsEndPose()) { 179 | if (V2_ActionPlayCount < 2) { 180 | // TODO: go next action in Combo when Combo is ready 181 | if (debug) DEBUG.printf("Action Completed\n"); 182 | V2_ResetAction(); 183 | return; 184 | } 185 | // Set count to 0xFF for endless loop 186 | if (V2_ActionPlayCount != 0xFF) V2_ActionPlayCount--; 187 | V2_NextPose = 0; 188 | if (debug) DEBUG.printf("Action finished, continue with last %d times\n", V2_ActionPlayCount); 189 | } 190 | 191 | // Try to preLoad next pose by checking is 192 | actionData.IsPoseReady(V2_NextPose); 193 | 194 | } 195 | 196 | 197 | uint16_t CalAdjMs(uint16_t ms) { 198 | // DEBUG.printf("CalAdjMs %d Factor: %f \n", ms, actionTimeFactor); 199 | if ((ms == 0) || (actionTimeFactor == 1.0f)) return ms; 200 | uint32_t adjMs = (uint32_t) (actionTimeFactor * ms); 201 | if (adjMs > 0xFFFF) return 0xFFFF; 202 | // DEBUG.printf("CalAdjMs %d Factor: %f => %d \n", ms, actionTimeFactor, adjMs); 203 | return (uint16_t) adjMs; 204 | } -------------------------------------------------------------------------------- /src/V2_Command.h: -------------------------------------------------------------------------------- 1 | #ifndef _V2_COMMAND_H_ 2 | #define _V2_COMMAND_H_ 3 | 4 | #include "robot.h" 5 | #include "math.h" 6 | 7 | bool V2_ActionPlaying = false; 8 | byte V2_ActionCombo = 0; // combo = 0 reserved for single action play 9 | byte V2_NextAction = 0; 10 | uint16_t V2_NextPose = 0; 11 | unsigned long V2_GlobalTimeMs = 0; 12 | unsigned long V2_NextPlayMs = 0; 13 | byte V2_ActionPlayCount = 0; 14 | 15 | int V2_ServoTimeRatio = 20; 16 | bool V2_UseGlobalTime = true; 17 | 18 | 19 | #define V2_CMD_RESET 0x01 20 | #define V2_CMD_DEBUG 0x02 21 | #define V2_CMD_DEVMODE 0x03 22 | #define V2_CMD_GETCONFIG 0x04 23 | #define V2_CMD_SETCONFIG 0x05 24 | #define V2_CMD_DEFAULTCONFIG 0x06 25 | #define V2_CMD_USB_TTL 0x07 26 | #define V2_CMD_GET_EH_MODE 0x08 27 | #define V2_CMD_SET_EH_MODE 0x09 28 | 29 | #define V2_CMD_ENABLE 0x0A 30 | #define V2_CMD_CHECK_BATTERY 0x0B 31 | #define V2_CMD_GET_NETWORK 0x0C 32 | #define V2_CMD_GET_WIFI_CONFIG 0x0D 33 | #define V2_CMD_SET_WIFI_CONFIG 0x0E 34 | #define V2_CMD_PARTIAL_CONFIG 0x0F 35 | 36 | #define V2_CMD_SERVOTYPE 0x10 37 | #define V2_CMD_SERVOANGLE 0x11 38 | #define V2_CMD_ONEANGLE 0x12 39 | #define V2_CMD_SERVOADJANGLE 0x13 40 | #define V2_CMD_ONEADJANGLE 0x14 41 | #define V2_CMD_SETADJANGLE 0x15 42 | #define V2_CMD_SERVOCMD 0x16 43 | #define V2_CMD_SETANGLE 0x18 44 | 45 | #define V2_CMD_LOCKSERVO 0x21 46 | #define V2_CMD_UNLOCKSERVO 0x22 47 | 48 | #define V2_CMD_SERVOMOVE 0x23 49 | #define V2_CMD_LED 0x24 50 | 51 | #define V2_CMD_SET_HEADLED 0x31 52 | #define V2_CMD_MP3_STOP 0x32 53 | #define V2_CMD_MP3_PLAYFILE 0x33 54 | #define V2_CMD_MP3_PLAYMP3 0x34 55 | #define V2_CMD_MP3_PLAYADVERT 0x35 56 | #define V2_CMD_MP3_SETVOLUME 0x36 57 | #define V2_CMD_MP3_COMMAND 0x37 58 | 59 | #define V2_CMD_PLAYACTION 0x41 60 | #define V2_CMD_PLAYCOMBO 0x42 61 | #define V2_CMD_SET_ACTIONSPEED 0x43 62 | #define V2_CMD_STOPPLAY 0x4F 63 | 64 | #define V2_CMD_GET_ADLIST 0x60 65 | #define V2_CMD_GET_ADHEADER 0x61 66 | #define V2_CMD_GET_ADPOSE 0x62 67 | 68 | #define V2_CMD_GET_COMBO 0x68 69 | #define V2_CMD_UPD_COMBO 0x69 70 | 71 | 72 | #define V2_CMD_UPD_ADHEADER 0x71 73 | #define V2_CMD_UPD_ADPOSE 0x72 74 | #define V2_CMD_UPD_ADNAME 0x74 75 | #define V2_CMD_DEL_ACTION 0x75 76 | 77 | #define V2_CMD_CHK_MPU 0x81 78 | #define V2_CMD_GET_MPU_DATA 0x82 79 | 80 | #define V2_CMD_GET_EVENT_HEADER 0x91 81 | #define V2_CMD_GET_EVENT_DATA 0x92 82 | #define V2_CMD_SAVE_EVENT_HEADER 0x93 83 | #define V2_CMD_SAVE_EVENT_DATA 0x94 84 | 85 | /* 86 | #define V2_CMD_READSPIFFS 0xF1 87 | #define V2_CMD_WRITESPIFFS 0xF2 88 | */ 89 | 90 | #define EVENT_HEADER_RESULT_SIZE 16 91 | #define EVENT_DATA_RESULT_SIZE 128 92 | #define EVENT_DATA_BATCH_SIZE 8 93 | 94 | 95 | #define V2_CMD_GET_VERSION 0xFF 96 | 97 | bool deepDebug = true; 98 | 99 | void ActionSetHeadLed(byte mode); 100 | void ActionStopPlay(); 101 | void ActionPlayAction(byte actionId); 102 | void ActionMp3Stop(); 103 | void ActionMp3PlayMp3(byte fileSeq); 104 | void ActionMp3PlayFile(byte folderSeq, byte fileSeq); 105 | void ActionServo(uint8_t servoId, int8_t angle, uint8_t time); 106 | void ActionSonic(uint8_t status); 107 | 108 | #endif -------------------------------------------------------------------------------- /src/message.h: -------------------------------------------------------------------------------- 1 | #ifndef _message_h_ 2 | #define _message_h_ 3 | 4 | #define __PROG_TYPES_COMPAT__ 5 | 6 | #if defined( ESP8266 ) 7 | #include 8 | #else 9 | #include 10 | #endif 11 | 12 | const char msg_welcomeHeader[] PROGMEM = "Robot Control v2"; 13 | const char msg_author[] PROGMEM = "by Super169"; 14 | const char msg_welcomeMsg[] PROGMEM = "Starting robot......."; 15 | 16 | #endif -------------------------------------------------------------------------------- /src/robot.h: -------------------------------------------------------------------------------- 1 | #ifndef _robot_h_ 2 | #define _robot_h_ 3 | 4 | 5 | #define DISABLE_BATTERY_CHECK 6 | 7 | 8 | #include "ESP8266WiFi.h" 9 | #include "WiFiClient.h" 10 | #include 11 | #include "SimpleWiFiManager.h" 12 | SimpleWiFiManager SWFM; 13 | 14 | #include 15 | #include "OLED12864.h" 16 | 17 | #include "FS.h" 18 | #include "Buffer.h" 19 | #include "ComboData.h" 20 | #include "ActionData.h" 21 | #include "MP3TF16P.h" 22 | #include "RobotConfig.h" 23 | 24 | #include "MyDebugger.h" 25 | #include "UTIL.h" 26 | 27 | #include "message.h" 28 | #include "RESULT.h" 29 | 30 | 31 | #include "RobotServo.h" 32 | #include "V2_Command.h" 33 | 34 | #include "RobotEventHandler.h" 35 | // #include "Event_Touch.h" 36 | // #include "Event_Mpu6050.h" 37 | 38 | #include "EyeLed.h" 39 | 40 | #include "OTA.h" 41 | 42 | // Version 2.2 - New version for event handler added 43 | #define VERSION_MAJOR 2 44 | #define VERSION_MINOR 3 45 | #define VERSION_SUB 0 46 | #define VERSION_FIX 0 47 | 48 | 49 | // Level 0 - all 50 | // Level 100 - most important only 51 | #define DEFAULT_LOG_LEVEL 110 52 | 53 | // Start a TCP Server on port 6169 54 | uint16_t port = 6169; 55 | String localSegment; 56 | byte localSegmentStart; 57 | // WiFiServer server(port); 58 | // WiFiClient client; 59 | bool isConnected = false; 60 | 61 | // UDP related settings 62 | uint16_t udpReceiveport = 9012; 63 | uint16_t udpSendport = 9020; 64 | WiFiUDP udpClient; 65 | 66 | 67 | #define NETWORK_NONE 0 68 | #define NETWORK_ROUTER 1 69 | #define NETWORK_AP 2 70 | uint8_t NetworkMode = 0; 71 | 72 | char *AP_Name = (char *) "Alpha 1S"; 73 | char *AP_Password = (char *) "12345678"; 74 | 75 | 76 | #define DEBUG Serial1 77 | 78 | MyDebugger debugger; 79 | MyDebugger *_dbg = &debugger; 80 | 81 | // Event Handling 82 | 83 | EventData eData; 84 | EventHandler eIdle(&eData); 85 | EventHandler eBusy(&eData); 86 | EventHandler eTemp(&eData); 87 | 88 | SSBoard ssb; 89 | EdsMpu6050* edsMpu6050[ED_COUNT_MPU]; 90 | EdsTouch* edsTouch[ED_COUNT_TOUCH]; 91 | EdsPsxButton* edsPsxButton[ED_COUNT_PSXBUTTON]; 92 | EdsBattery* edsBattery[ED_COUNT_BATTERY]; 93 | EdsSonic* edsSonic[ED_COUNT_SONIC]; 94 | EdsMaze* edsMaze[ED_COUNT_MAZE]; 95 | 96 | #define CMD_BUFFER_SIZE 160 97 | 98 | Buffer cmdBuffer(CMD_BUFFER_SIZE); 99 | 100 | RobotConfig config(&DEBUG); 101 | 102 | 103 | // OLED Settings 104 | 105 | boolean ENABLE_OLED_BUFFER = true; 106 | boolean ENABLE_OLED_DIRECTDRAW = false; 107 | 108 | // OLED_1306i2c - 0.96" OLED 109 | // OLED_1106i2c - 1.3" OLED 110 | OLED12864 myOLED(OLED_1306i2c, ENABLE_OLED_BUFFER, ENABLE_OLED_DIRECTDRAW); 111 | 112 | bool enable_V1 = true; 113 | bool enable_V2 = true; 114 | bool enable_UBTBT = true; 115 | bool enable_UBTCB = true; 116 | bool enable_UBTSV = true; 117 | bool enable_HAILZD = true; 118 | 119 | ComboData comboTable[CD_MAX_COMBO]; 120 | ActionData actionData; 121 | 122 | float actionTimeFactor = 1.0f; 123 | 124 | #define MAX_ACTION 26 125 | #define MAX_POSES 30 126 | // #define MAX_POSES_SIZE 40 127 | #define MAX_POSES_SIZE 20 128 | // PoseInfo (20 bytes) 129 | // 0 : enabled 130 | // 1~16 : 16 servo angle, 0xFF = no change 131 | // 17 : Time in servo command 132 | // 18~19 : waiting time in millis 133 | 134 | // 26 * 30 * 20 = 15600 135 | #define ACTION_TABLE_SIZE 15600 136 | 137 | #define ENABLE_FLAG 0 138 | #define ID_OFFSET 0 139 | #define EXECUTE_TIME 17 140 | #define WAIT_TIME_HIGH 18 141 | #define WAIT_TIME_LOW 19 142 | 143 | #define MAX_COMBO 10 144 | #define MAX_COMBO_SIZE 100 145 | 146 | #pragma region "Global variables" 147 | 148 | char* actionDataFile = (char *) "/robot.dat"; 149 | 150 | 151 | struct { 152 | uint8_t rx_pin = 12; 153 | uint8_t tx_pin = 12; 154 | unsigned long baud = 115200; 155 | bool inverse_logic = false; 156 | uint16_t buffer_size = 64; 157 | } busConfig; 158 | 159 | SoftwareSerial robotPort(busConfig.rx_pin, busConfig.tx_pin, busConfig.inverse_logic, busConfig.buffer_size); 160 | RobotServo rs; 161 | 162 | // SoftwareSerial ubt_ss(12, 12, false, 256); 163 | // UBTech servo(&ubt_ss, &DEBUG); // Debug on Serial1 164 | 165 | int servoCnt = 0; 166 | // byte *retBuffer; 167 | 168 | byte ledMode = 0; 169 | 170 | byte ch, cmd; 171 | 172 | long lastCmdMs = 0; 173 | 174 | bool debug = true; 175 | bool devMode = false; 176 | 177 | //BoardDefine 178 | 179 | //#define ROBOT_ARM_BOARD 180 | 181 | // #define MP3_RXD_GPIO 14 182 | // #define MP3_TXD_GPIO 16 183 | #define HEAD_LED_GPIO 15 184 | 185 | bool headLed = false; 186 | 187 | // Disable RX pin by passing -1 as rx_pin 188 | // Do use single wire mode, it seems not work with MP3 module. 189 | // 190 | struct { 191 | int8_t rx_pin = -1; 192 | int8_t tx_pin = 16; 193 | unsigned long baud = 9600; 194 | bool inverse_logic = false; 195 | uint16_t buffer_size = 64; 196 | } mp3Config; 197 | 198 | SoftwareSerial mp3_ss(mp3Config.rx_pin, mp3Config.tx_pin, mp3Config.inverse_logic, mp3Config.buffer_size); 199 | MP3TF16P mp3(&mp3_ss, &DEBUG); 200 | 201 | uint8_t mp3_Vol = 0xFF; 202 | 203 | #pragma endregion 204 | 205 | #pragma region "Local Functions" 206 | 207 | byte GetPower(uint16_t v); 208 | void cmd_ReadSPIFFS(); 209 | void ReadSPIFFS(bool sendResult); 210 | 211 | bool UBTBT_Command(); 212 | bool UBTCB_Command(); 213 | bool UBTSV_Command(); 214 | 215 | bool V1_Command(); // To be obsolete 216 | bool V2_Command(); 217 | 218 | #pragma endregion 219 | 220 | 221 | // UBT_Command.ino 222 | void UBT_GetServoAngle(byte *result); 223 | void UBT_GetServoAdjAngle(byte *result); 224 | 225 | // V2_CheckAction.ino 226 | void V2_ResetAction(); 227 | 228 | void UBT_ReadSPIFFS(byte cmdCode); 229 | void UBT_WriteSPIFFS(byte cmdCode); 230 | 231 | void V1_UBT_ReadSPIFFS(byte cmdCode); 232 | void V2_CheckAction(); 233 | void V2_GoAction(byte actionId, bool v2, byte *cmd); 234 | 235 | // Command_Generic 236 | bool SendGenericCommand(byte *cmd, byte sendCnt); 237 | 238 | // HILZD 239 | bool HAILZD_Command(); 240 | 241 | 242 | // System Action 243 | void ActionPlaySystemAction(byte systemActionId); 244 | 245 | #endif 246 | --------------------------------------------------------------------------------