├── .gitignore ├── pics ├── gait_trot.png ├── schematic.png ├── pca9658_mod.jpg ├── simulation.gif ├── gait_pattern.gif ├── quadruped_side.png └── quadruped_top.png ├── quadruped_code ├── data │ ├── kaoffs.cfg │ ├── kastep.cfg │ ├── smgyro.cfg │ ├── smoffs.cfg │ ├── smstep.cfg │ └── edit.html ├── .gitignore ├── .vscode │ ├── settings.json │ └── extensions.json ├── test │ └── README ├── lib │ ├── msp │ │ ├── CommSerial.cpp │ │ ├── common.h │ │ ├── CommInterface.h │ │ ├── CommSerialBT.cpp │ │ ├── Protocol.h │ │ └── Protocol.cpp │ ├── README │ ├── PIDController │ │ ├── PIDController.h │ │ └── PIDController.cpp │ └── ControlStick │ │ ├── ControlStick.h │ │ └── ControlStick.cpp ├── src │ ├── utils.h │ ├── utils.cpp │ ├── GyroProc.h │ ├── Gait.cpp │ ├── TimedMove.h │ ├── VecRot.h │ ├── hardware.h │ ├── GyroDev.h │ ├── OTA.h │ ├── StatusLed.h │ ├── QuadRuped.h │ ├── StatusLed.cpp │ ├── config.h │ ├── GyroProc.cpp │ ├── CommandHandler.h │ ├── GyroDev.cpp │ ├── QuadRuped.cpp │ ├── Gait.h │ ├── FSBrowser.h │ ├── hardware.cpp │ └── main.cpp ├── platformio.ini └── include │ └── README ├── processing ├── gait_pattern │ ├── VecRot.pde │ ├── gait_pattern.pde │ └── Gait.pde └── simulation │ ├── VecRot.pde │ ├── simulation.pde │ ├── Gait.pde │ └── QuadRuped.pde └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | ~$*.* 2 | .ref 3 | -------------------------------------------------------------------------------- /pics/gait_trot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PingguSoft/esp32_quadruped/HEAD/pics/gait_trot.png -------------------------------------------------------------------------------- /pics/schematic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PingguSoft/esp32_quadruped/HEAD/pics/schematic.png -------------------------------------------------------------------------------- /pics/pca9658_mod.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PingguSoft/esp32_quadruped/HEAD/pics/pca9658_mod.jpg -------------------------------------------------------------------------------- /pics/simulation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PingguSoft/esp32_quadruped/HEAD/pics/simulation.gif -------------------------------------------------------------------------------- /pics/gait_pattern.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PingguSoft/esp32_quadruped/HEAD/pics/gait_pattern.gif -------------------------------------------------------------------------------- /pics/quadruped_side.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PingguSoft/esp32_quadruped/HEAD/pics/quadruped_side.png -------------------------------------------------------------------------------- /pics/quadruped_top.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PingguSoft/esp32_quadruped/HEAD/pics/quadruped_top.png -------------------------------------------------------------------------------- /quadruped_code/data/kaoffs.cfg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PingguSoft/esp32_quadruped/HEAD/quadruped_code/data/kaoffs.cfg -------------------------------------------------------------------------------- /quadruped_code/data/kastep.cfg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PingguSoft/esp32_quadruped/HEAD/quadruped_code/data/kastep.cfg -------------------------------------------------------------------------------- /quadruped_code/data/smgyro.cfg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PingguSoft/esp32_quadruped/HEAD/quadruped_code/data/smgyro.cfg -------------------------------------------------------------------------------- /quadruped_code/data/smoffs.cfg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PingguSoft/esp32_quadruped/HEAD/quadruped_code/data/smoffs.cfg -------------------------------------------------------------------------------- /quadruped_code/data/smstep.cfg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PingguSoft/esp32_quadruped/HEAD/quadruped_code/data/smstep.cfg -------------------------------------------------------------------------------- /quadruped_code/.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/.browse.c_cpp.db* 3 | .vscode/c_cpp_properties.json 4 | .vscode/launch.json 5 | .vscode/ipch 6 | .ref/ -------------------------------------------------------------------------------- /quadruped_code/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "cmath": "cpp", 4 | "functional": "cpp", 5 | "*.tcc": "cpp", 6 | "fstream": "cpp", 7 | "esp_attr.h": "c" 8 | } 9 | } -------------------------------------------------------------------------------- /quadruped_code/.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 | "unwantedRecommendations": [ 8 | "ms-vscode.cpptools-extension-pack" 9 | ] 10 | } 11 | -------------------------------------------------------------------------------- /quadruped_code/test/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for PlatformIO Unit Testing and project tests. 3 | 4 | Unit Testing is a software testing method by which individual units of 5 | source code, sets of one or more MCU program modules together with associated 6 | control data, usage procedures, and operating procedures, are tested to 7 | determine whether they are fit for use. Unit testing finds problems early 8 | in the development cycle. 9 | 10 | More information about PlatformIO Unit Testing: 11 | - https://docs.platformio.org/page/plus/unit-testing.html 12 | -------------------------------------------------------------------------------- /quadruped_code/lib/msp/CommSerial.cpp: -------------------------------------------------------------------------------- 1 | #include "common.h" 2 | #include "CommInterface.h" 3 | 4 | class SerialDev : public ProtocolDevice { 5 | public: 6 | SerialDev() { 7 | } 8 | 9 | 10 | SerialDev(CommSerial* x):m_pParent(x) { 11 | } 12 | 13 | virtual ~SerialDev() { 14 | } 15 | 16 | virtual void write(u8 *pBuf, u8 size) { 17 | Serial.write(pBuf, size); 18 | } 19 | 20 | virtual int read(void) { 21 | return Serial.read(); 22 | } 23 | 24 | virtual int available(void) { 25 | return Serial.available(); 26 | } 27 | 28 | private: 29 | CommSerial *m_pParent; 30 | }; 31 | 32 | int CommSerial::loop() { 33 | getProtocol()->processRx(); 34 | return 0; 35 | } 36 | 37 | CommSerial::CommSerial(int arg) : CommInterface(arg) { 38 | getProtocol()->setDevice(new SerialDev(this)); 39 | setConnected(TRUE); 40 | } 41 | 42 | CommSerial::~CommSerial() { 43 | } 44 | 45 | -------------------------------------------------------------------------------- /quadruped_code/src/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef _UTILS_H_ 2 | #define _UTILS_H_ 3 | 4 | #include "config.h" 5 | 6 | /* 7 | ***************************************************************************************** 8 | * CONSTANTS 9 | ***************************************************************************************** 10 | */ 11 | 12 | /* 13 | ***************************************************************************************** 14 | * MACROS & STRUCTURES 15 | ***************************************************************************************** 16 | */ 17 | #define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0])) 18 | #define IS_ELAPSED(ts, last, duration) ((ts) - (last) > (duration)) 19 | 20 | #define LOG(...) printf(__VA_ARGS__) //Utils.log(__VA_ARGS__) 21 | #define DUMP(name, data, cnt) Utils::dump(name, data, cnt) 22 | 23 | class Utils { 24 | public: 25 | static void dump(String name, uint8_t* data, uint16_t cnt); 26 | static void log(...); 27 | static float mapf(float x, float in_min, float in_max, float out_min, float out_max); 28 | }; 29 | 30 | #endif -------------------------------------------------------------------------------- /quadruped_code/src/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "utils.h" 2 | 3 | void Utils::dump(String name, uint8_t* data, uint16_t cnt) { 4 | uint8_t i; 5 | uint8_t b; 6 | uint16_t addr = 0; 7 | 8 | LOG(PSTR("-- %s buf size : %d -- \n"), name.c_str(), cnt); 9 | while (cnt) { 10 | LOG(PSTR("%08x - "), addr); 11 | 12 | for (i = 0; (i < 16) && (i < cnt); i++) { 13 | b = *(data + i); 14 | LOG(PSTR("%02x "), b); 15 | } 16 | 17 | LOG(PSTR(" : ")); 18 | for (i = 0; (i < 16) && (i < cnt); i++) { 19 | b = *(data + i); 20 | if ((b > 0x1f) && (b < 0x7f)) 21 | LOG(PSTR("%c"), b); 22 | else 23 | LOG(PSTR(".")); 24 | } 25 | LOG(PSTR("\n")); 26 | data += i; 27 | addr += i; 28 | cnt -= i; 29 | } 30 | } 31 | 32 | void Utils::log(...) { 33 | } 34 | 35 | float Utils::mapf(float x, float in_min, float in_max, float out_min, float out_max) { 36 | const float dividend = out_max - out_min; 37 | const float divisor = in_max - in_min; 38 | const float delta = x - in_min; 39 | 40 | return (delta * dividend) / divisor + out_min; 41 | } 42 | -------------------------------------------------------------------------------- /quadruped_code/lib/msp/common.h: -------------------------------------------------------------------------------- 1 | /* 2 | This project is free software: you can redistribute it and/or modify 3 | it under the terms of the GNU General Public License as published by 4 | the Free Software Foundation, either version 3 of the License, or 5 | (at your option) any later version. 6 | 7 | This program is distributed in the hope that it will be useful, 8 | but WITHOUT ANY WARRANTY; without even the implied warranty of 9 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 10 | GNU General Public License for more details. 11 | see 12 | */ 13 | 14 | 15 | #ifndef _COMMON_H_ 16 | #define _COMMON_H_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | typedef int8_t s8; 24 | typedef int16_t s16; 25 | typedef int32_t s32; 26 | #ifndef __USBAPI__ 27 | typedef uint8_t u8; 28 | typedef uint16_t u16; 29 | typedef uint32_t u32; 30 | #endif 31 | typedef uint64_t u64; 32 | 33 | #ifndef NULL 34 | #define NULL 0 35 | #endif 36 | 37 | #ifndef TRUE 38 | #define TRUE 1 39 | #endif 40 | 41 | #ifndef FALSE 42 | #define FALSE 0 43 | #endif 44 | 45 | #define BV(bit) (1 << (bit)) 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /quadruped_code/lib/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project specific (private) libraries. 3 | PlatformIO will compile them to static libraries and link into executable file. 4 | 5 | The source code of each library should be placed in a an own separate directory 6 | ("lib/your_library_name/[here are source files]"). 7 | 8 | For example, see a structure of the following two libraries `Foo` and `Bar`: 9 | 10 | |--lib 11 | | | 12 | | |--Bar 13 | | | |--docs 14 | | | |--examples 15 | | | |--src 16 | | | |- Bar.c 17 | | | |- Bar.h 18 | | | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html 19 | | | 20 | | |--Foo 21 | | | |- Foo.c 22 | | | |- Foo.h 23 | | | 24 | | |- README --> THIS FILE 25 | | 26 | |- platformio.ini 27 | |--src 28 | |- main.c 29 | 30 | and a contents of `src/main.c`: 31 | ``` 32 | #include 33 | #include 34 | 35 | int main (void) 36 | { 37 | ... 38 | } 39 | 40 | ``` 41 | 42 | PlatformIO Library Dependency Finder will find automatically dependent 43 | libraries scanning project source files. 44 | 45 | More information about PlatformIO Library Dependency Finder 46 | - https://docs.platformio.org/page/librarymanager/ldf.html 47 | -------------------------------------------------------------------------------- /quadruped_code/platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | 12 | [env] 13 | platform = espressif32 14 | board = esp32dev 15 | board_build.partitions = min_spiffs.csv 16 | 17 | monitor_speed = 115200 18 | framework = arduino 19 | lib_deps = 20 | adafruit/Adafruit PWM Servo Driver Library@^2.4.0 21 | martymacgyver/ESP32 Digital RGB LED Drivers@^1.5.4 22 | electroniccats/MPU6050@^1.4.0 23 | h2zero/NimBLE-Arduino@^1.2.0 24 | madhephaestus/ESP32Servo@^0.9.0 25 | contrem/arduino-timer@^2.3.0 26 | 27 | ;lib_dir = ./lib/ControlStick 28 | ;./lib/msp 29 | 30 | [env:spotmicro] 31 | ;upload_protocol = espota 32 | ;upload_port = HexaPod.local ;192.168.0.72 33 | upload_speed = 512000 34 | upload_protocol = esptool 35 | build_flags = -DCONFIG_BODY=1 36 | 37 | [env:kangal] 38 | ;upload_protocol = espota 39 | ;upload_port = HexaPod.local ;192.168.0.72 40 | upload_speed = 512000 41 | upload_protocol = esptool 42 | build_flags = -DCONFIG_BODY=2 -------------------------------------------------------------------------------- /quadruped_code/include/README: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /quadruped_code/src/GyroProc.h: -------------------------------------------------------------------------------- 1 | #ifndef _GYRO_PROC_H_ 2 | #define _GYRO_PROC_H_ 3 | 4 | #include "config.h" 5 | #include "VecRot.h" 6 | #include "GyroDev.h" 7 | #include "PIDController.h" 8 | #include "utils.h" 9 | 10 | /* 11 | ***************************************************************************************** 12 | * CONSTANTS 13 | ***************************************************************************************** 14 | */ 15 | 16 | /* 17 | ***************************************************************************************** 18 | * MACROS & STRUCTURES 19 | ***************************************************************************************** 20 | */ 21 | 22 | class GyroProc { 23 | public: 24 | GyroProc(int hz); 25 | 26 | void setup(); 27 | bool calibrate(void); 28 | bool isValid(float angle); 29 | Rotator process(unsigned long ts, Rotator rot); 30 | void togglePID(); 31 | void incP(); 32 | void decP(); 33 | void incI(); 34 | void decI(); 35 | void incD(); 36 | void decD(); 37 | void reset(); 38 | void setTarget(float roll, float pit); 39 | 40 | private: 41 | void setPID(); 42 | float fixAngle(float fTgt, float fSrc); 43 | 44 | GyroDev _gyro; 45 | 46 | PIDController _pidPit; 47 | PIDController _pidRoll; 48 | PIDController *_pPID; 49 | 50 | Rotator _rotTgt; 51 | 52 | int _iInterval; 53 | float _p, _i, _d; 54 | int _iSel; 55 | int16_t _offsets[6]; 56 | }; 57 | 58 | #endif -------------------------------------------------------------------------------- /quadruped_code/lib/msp/CommInterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | This project is free software: you can redistribute it and/or modify 3 | it under the terms of the GNU General Public License as published by 4 | the Free Software Foundation, either version 3 of the License, or 5 | (at your option) any later version. 6 | 7 | This program is distributed in the hope that it will be useful, 8 | but WITHOUT ANY WARRANTY; without even the implied warranty of 9 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 10 | GNU General Public License for more details. 11 | see 12 | */ 13 | 14 | 15 | #ifndef _COMM_INTERFACE_H_ 16 | #define _COMM_INTERFACE_H_ 17 | 18 | #include 19 | #include 20 | #include "common.h" 21 | #include "Protocol.h" 22 | 23 | class CommInterface { 24 | 25 | public: 26 | CommInterface(u8 size) { m_pProtocol = new Protocol(size); m_isConnected = FALSE; } 27 | 28 | virtual ~CommInterface() { if (m_pProtocol) delete m_pProtocol; } 29 | 30 | void setConnected(bool conn) { m_isConnected = conn; } 31 | 32 | bool isConnected() { return m_isConnected; } 33 | 34 | Protocol *getProtocol() { return m_pProtocol; } 35 | 36 | virtual int loop(void) = 0; 37 | 38 | private: 39 | Protocol *m_pProtocol; 40 | bool m_isConnected; 41 | }; 42 | 43 | 44 | class CommSerial : public CommInterface { 45 | 46 | public: 47 | CommSerial(int arg); 48 | 49 | ~CommSerial(); 50 | 51 | virtual int loop(void); 52 | }; 53 | 54 | 55 | class CommSerialBT : public CommInterface { 56 | 57 | public: 58 | CommSerialBT(int arg); 59 | 60 | CommSerialBT(char *name, int bufSize); 61 | 62 | ~CommSerialBT(); 63 | 64 | virtual int loop(void); 65 | }; 66 | 67 | 68 | #endif 69 | 70 | -------------------------------------------------------------------------------- /quadruped_code/lib/PIDController/PIDController.h: -------------------------------------------------------------------------------- 1 | #ifndef _PID_CONTROLLER_H_ 2 | #define _PID_CONTROLLER_H_ 3 | 4 | #include 5 | 6 | /* 7 | ***************************************************************************************** 8 | * CONSTANTS 9 | ***************************************************************************************** 10 | */ 11 | 12 | class PIDController { 13 | private: 14 | String _strName; 15 | float _p; 16 | float _i; 17 | float _d; 18 | unsigned long _lLastTS; 19 | float _fTarget; 20 | float _fLastInput; 21 | float _fLastError; 22 | float _fIntegral; 23 | float _fIntegralLimit; 24 | bool _isCircular; 25 | bool _isStableCheck; 26 | float _fIntegStableCheck; 27 | float _fDurationStableCheck; 28 | 29 | public: 30 | PIDController(String name, bool circular, float p, float i, float d); 31 | PIDController(String name, float p, float i, float d); 32 | PIDController(String name, float p, float i, float d, float iLimit); 33 | PIDController(String name, bool circular, float p, float i, float d, float iLimit); 34 | 35 | void reset(); 36 | String getName(); 37 | void set(float p, float i, float d); 38 | float getP(); 39 | float getI(); 40 | float getD(); 41 | void setTarget(float target); 42 | float getTarget(); 43 | float compute(unsigned long now, float input); 44 | float compute(unsigned long now, float input, float limit); 45 | bool isStable(float duration, float v); 46 | private: 47 | float getLastInput(); 48 | float calcError(float a, float b); 49 | float computeWithDelta(float input, float delta); 50 | float computeWithDelta(float input, float limit, float delta); 51 | float fConstrain(float v, float min, float max, float deadzone); 52 | }; 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /processing/gait_pattern/VecRot.pde: -------------------------------------------------------------------------------- 1 | 2 | 3 | public class Vector { 4 | public float x; 5 | public float y; 6 | public float z; 7 | 8 | public Vector() { 9 | set(0, 0, 0); 10 | } 11 | 12 | public Vector(float x, float y) { 13 | set(x, y, 0); 14 | } 15 | 16 | public Vector(float x, float y, float z) { 17 | set(x, y, z); 18 | } 19 | 20 | public void set(Vector v) { 21 | set(v.x, v.y, v.z); 22 | } 23 | 24 | public void set(float x, float y, float z) { 25 | this.x = x; 26 | this.y = y; 27 | this.z = z; 28 | } 29 | 30 | public void zero() { 31 | set(0, 0, 0); 32 | } 33 | 34 | public boolean equals(Vector v) { 35 | return (x == v.x && y == v.y && z == v.z); 36 | } 37 | 38 | void dump(String name) { 39 | print(String.format("[%4s] x:%7.3f, y:%7.3f, z:%7.3f\n", name, x, y, z)); 40 | } 41 | }; 42 | 43 | public class Rotator { 44 | public float yaw; 45 | public float pitch; 46 | public float roll; 47 | 48 | public Rotator() { 49 | set(0, 0, 0); 50 | } 51 | 52 | public Rotator(Rotator r) { 53 | set(r.yaw, r.pitch, r.roll); 54 | } 55 | 56 | public Rotator(float yaw, float pitch, float roll) { 57 | set(yaw, pitch, roll); 58 | } 59 | 60 | public void set(Rotator r) { 61 | set(r.yaw, r.pitch, r.roll); 62 | } 63 | 64 | public void set(float yaw, float pitch, float roll) { 65 | this.yaw = yaw; 66 | this.pitch = pitch; 67 | this.roll = roll; 68 | } 69 | 70 | public void zero() { 71 | set(0, 0, 0); 72 | } 73 | 74 | public boolean equals(Rotator r) { 75 | return (yaw == r.yaw && pitch == r.pitch && roll == r.roll); 76 | } 77 | 78 | void dump(String name) { 79 | print(String.format("[%4s] y:%7.3f, p:%7.3f, r:%7.3f\n", name, yaw, pitch, roll)); 80 | } 81 | }; 82 | -------------------------------------------------------------------------------- /processing/simulation/VecRot.pde: -------------------------------------------------------------------------------- 1 | 2 | 3 | public class Vector { 4 | public float x; 5 | public float y; 6 | public float z; 7 | 8 | public Vector() { 9 | set(0, 0, 0); 10 | } 11 | 12 | public Vector(Vector v) { 13 | set(v.x, v.y, v.z); 14 | } 15 | 16 | public Vector(float x, float y) { 17 | set(x, y, 0); 18 | } 19 | 20 | public Vector(float x, float y, float z) { 21 | set(x, y, z); 22 | } 23 | 24 | public void set(Vector v) { 25 | set(v.x, v.y, v.z); 26 | } 27 | 28 | public void set(float x, float y, float z) { 29 | this.x = x; 30 | this.y = y; 31 | this.z = z; 32 | } 33 | 34 | public void zero() { 35 | set(0, 0, 0); 36 | } 37 | 38 | public boolean equals(Vector v) { 39 | return (x == v.x && y == v.y && z == v.z); 40 | } 41 | 42 | void dump(String name) { 43 | print(String.format("[%4s] x:%7.3f, y:%7.3f, z:%7.3f\n", name, x, y, z)); 44 | } 45 | }; 46 | 47 | public class Rotator { 48 | public float yaw; 49 | public float pitch; 50 | public float roll; 51 | 52 | public Rotator() { 53 | set(0, 0, 0); 54 | } 55 | 56 | public Rotator(Rotator r) { 57 | set(r.yaw, r.pitch, r.roll); 58 | } 59 | 60 | public Rotator(float yaw, float pitch, float roll) { 61 | set(yaw, pitch, roll); 62 | } 63 | 64 | public void set(Rotator r) { 65 | set(r.yaw, r.pitch, r.roll); 66 | } 67 | 68 | public void set(float yaw, float pitch, float roll) { 69 | this.yaw = yaw; 70 | this.pitch = pitch; 71 | this.roll = roll; 72 | } 73 | 74 | public void zero() { 75 | set(0, 0, 0); 76 | } 77 | 78 | public boolean equals(Rotator r) { 79 | return (yaw == r.yaw && pitch == r.pitch && roll == r.roll); 80 | } 81 | 82 | void dump(String name) { 83 | print(String.format("[%4s] y:%7.3f, p:%7.3f, r:%7.3f\n", name, yaw, pitch, roll)); 84 | } 85 | }; 86 | -------------------------------------------------------------------------------- /quadruped_code/src/Gait.cpp: -------------------------------------------------------------------------------- 1 | #include "Gait.h" 2 | 3 | Vector Gait::_vecStep(30, 30, 20); 4 | float Gait::_stepsPerSec = 2.0f; 5 | 6 | //: direction ratio calculation function 7 | Vector Gait::calcMoveRatio(Vector move) { 8 | float maxV = max(abs(move.x), abs(move.y)); 9 | 10 | return { move.x / maxV, move.y / maxV, move.z }; 11 | } 12 | 13 | void Gait::setStepsPerSec(float steps) { 14 | for (int i = 0; i < BODY_NUM_LEGS; i++) { 15 | _paramLegs[i].reset(); 16 | } 17 | _stepsPerSec = steps; 18 | } 19 | 20 | void Gait::setSwingOffsets(const float offsets[]) { 21 | for (int i = 0; i < BODY_NUM_LEGS; i++) { 22 | _paramLegs[i].setSwingOffset(offsets[i]); 23 | } 24 | } 25 | 26 | void Gait::doStep(int leg, Vector *move, Rotator *rot, bool enLog) { 27 | if (abs(move->x) == PRECISION && abs(move->y) == PRECISION) { 28 | _paramLegs[leg].reset(); 29 | move->set(0, 0, move->z); 30 | return; 31 | } 32 | 33 | _paramLegs[leg].tick(*move, _vecStep, _fFreq, _stepsPerSec); 34 | Vector r = calcMoveRatio(*move); 35 | Vector c(r.x * _paramLegs[leg].getAmplitude(), r.y * _paramLegs[leg].getAmplitude(), move->z); 36 | 37 | if (_paramLegs[leg].isSwingState()) { 38 | float w0 = _vecStep.x * UNIT_MM * 4 * move->x; 39 | float l0 = _vecStep.y * UNIT_MM / 2 * move->y; 40 | float h0 = _vecStep.z * UNIT_MM; 41 | 42 | _iSwingLeg = leg; 43 | _fSwingAmplitude = sqrt(abs((1 - sq(c.x / w0) - sq(c.y / l0)) * sq(h0))); 44 | c.z = c.z - _fSwingAmplitude; 45 | } else { 46 | // if (_isComp && _iSwingLeg >= 0) { 47 | // float pct = abs(_fSwingAmplitude / _vecStep.z); 48 | // float roll = (IS_RIGHT_LEG(_iSwingLeg) ? -pct : pct) * 2.0f; 49 | // float pitch = (IS_FRONT_LEG(_iSwingLeg) ? -pct : pct) * 2.0f; 50 | 51 | // rot->set(rot->yaw, pitch, roll); 52 | // } 53 | c.set(-c.x, -c.y, c.z); 54 | } 55 | 56 | if (enLog) { 57 | LOG("tick:%3d, amplitude:%6.1f, swing:%d, (%6.1f, %6.1f, %6.1f)\n", _paramLegs[leg].getTick(), 58 | _paramLegs[leg].getAmplitude(), int(_paramLegs[leg].isSwingState()), c.x, c.y, c.z); 59 | } 60 | 61 | move->set(c.x / UNIT_MM, c.y / UNIT_MM, c.z / UNIT_MM); 62 | } 63 | -------------------------------------------------------------------------------- /quadruped_code/lib/msp/CommSerialBT.cpp: -------------------------------------------------------------------------------- 1 | #include "CommInterface.h" 2 | 3 | #define LOG(...) printf(__VA_ARGS__) 4 | 5 | class SerialBTDev : public ProtocolDevice { 6 | public: 7 | void init(char *name) { 8 | m_pSerialBT = new BluetoothSerial(); 9 | m_pSerialBT->register_callback([](esp_spp_cb_event_t event, esp_spp_cb_param_t *param) { 10 | if (event == ESP_SPP_SRV_OPEN_EVT) { 11 | LOG("BT:CONNECTED\n"); 12 | } else if (event == ESP_SPP_CLOSE_EVT) { 13 | LOG("BT:CLOSED\n"); 14 | } 15 | }); 16 | bool ret = m_pSerialBT->begin(name); 17 | if (!ret) { 18 | LOG("bluetooth begin failure\n"); 19 | } 20 | } 21 | 22 | SerialBTDev() { 23 | init((char*)"ESP32-SPP"); 24 | } 25 | 26 | SerialBTDev(char *name) { 27 | init(name); 28 | } 29 | 30 | SerialBTDev(CommSerialBT* x):m_pParent(x) { 31 | init((char*)"ESP32-SPP"); 32 | } 33 | 34 | SerialBTDev(char *name, CommSerialBT* x):m_pParent(x) { 35 | init(name); 36 | } 37 | 38 | virtual ~SerialBTDev() { 39 | } 40 | 41 | virtual void write(u8 *pBuf, u8 size) { 42 | if (m_pSerialBT) 43 | m_pSerialBT->write(pBuf, size); 44 | } 45 | 46 | virtual int read(void) { 47 | return (m_pSerialBT ? m_pSerialBT->read() : -1); 48 | } 49 | 50 | virtual int available(void) { 51 | return (m_pSerialBT ? m_pSerialBT->available() : 0); 52 | } 53 | 54 | private: 55 | CommSerialBT *m_pParent; 56 | BluetoothSerial *m_pSerialBT; 57 | }; 58 | 59 | int CommSerialBT::loop() { 60 | getProtocol()->processRx(); 61 | return 0; 62 | } 63 | 64 | CommSerialBT::CommSerialBT(int bufSize) : CommInterface(bufSize) { 65 | getProtocol()->setDevice(new SerialBTDev(this)); 66 | setConnected(TRUE); 67 | } 68 | 69 | CommSerialBT::CommSerialBT(char *name, int bufSize) : CommInterface(bufSize) { 70 | getProtocol()->setDevice(new SerialBTDev(name, this)); 71 | setConnected(TRUE); 72 | } 73 | 74 | CommSerialBT::~CommSerialBT() { 75 | } 76 | -------------------------------------------------------------------------------- /quadruped_code/src/TimedMove.h: -------------------------------------------------------------------------------- 1 | #ifndef _TIMED_MOVE_H_ 2 | #define _TIMED_MOVE_H_ 3 | 4 | #include "config.h" 5 | #include "VecRot.h" 6 | 7 | class TimedMove { 8 | private: 9 | Vector _vecDirInc; 10 | Rotator _rotDirInc; 11 | Vector _vecDirTgt; 12 | Rotator _rotDirTgt; 13 | float _msToGo; 14 | unsigned long _lLastTS; 15 | Vector *_pDestVec; 16 | Rotator *_pDestRot; 17 | 18 | public: 19 | TimedMove() { 20 | } 21 | 22 | void setup(Vector *pVec, Rotator *pRot) { 23 | _pDestVec = pVec; 24 | _pDestRot = pRot; 25 | } 26 | 27 | void go(Vector vecTgt, Rotator rotTgt, float ms) { 28 | float x = (vecTgt.x - _pDestVec->x) / ms; 29 | float y = (vecTgt.y - _pDestVec->y) / ms; 30 | float z = (vecTgt.z - _pDestVec->z) / ms; 31 | _vecDirInc.set(x, y, z); 32 | _vecDirTgt.set(vecTgt); 33 | vecTgt.dump("target"); 34 | _vecDirInc.dump("dir_inc"); 35 | 36 | x = (rotTgt.yaw - _pDestRot->yaw) / ms; 37 | y = (rotTgt.pitch - _pDestRot->pitch) / ms; 38 | z = (rotTgt.roll - _pDestRot->roll) / ms; 39 | _rotDirInc.set(x, y, z); 40 | _rotDirTgt.set(rotTgt); 41 | _msToGo = ms; 42 | _lLastTS = millis(); 43 | } 44 | 45 | void handle(unsigned long ts) { 46 | int diff = int(ts - _lLastTS); 47 | 48 | if (diff > 100) { 49 | diff = 20; 50 | } 51 | 52 | // every 20ms 53 | if (diff >= 20) { 54 | _lLastTS = ts; 55 | 56 | if (_msToGo > 0) { 57 | _msToGo -= diff; 58 | if (_msToGo <= 0) { 59 | *_pDestVec = _vecDirTgt; 60 | *_pDestRot = _rotDirTgt; 61 | } else { 62 | _pDestVec->x += (_vecDirInc.x * diff); 63 | _pDestVec->y += (_vecDirInc.y * diff); 64 | _pDestVec->z += (_vecDirInc.z * diff); 65 | 66 | _pDestRot->yaw += (_rotDirInc.yaw * diff); 67 | _pDestRot->pitch += (_rotDirInc.pitch * diff); 68 | _pDestRot->roll += (_rotDirInc.roll * diff); 69 | } 70 | } 71 | } 72 | } 73 | }; 74 | #endif -------------------------------------------------------------------------------- /processing/gait_pattern/gait_pattern.pde: -------------------------------------------------------------------------------- 1 | 2 | final color kCOLOR_1 = color(50, 150, 220); 3 | final color kCOLOR_2 = color(255, 0, 0); 4 | final boolean kIS_ANIMATE_KEY = false; 5 | final int kFREQ = 60; 6 | 7 | Vector _mov = new Vector(Gait.kPRECISION + 1.5f, Gait.kPRECISION, 50); 8 | Rotator _rot = new Rotator(0.0f, 0, 0); 9 | boolean _done = false; 10 | int _ctr = 0; 11 | LegGraph _graph[] = new LegGraph[4]; 12 | boolean _start = true; 13 | Gait _gait = new GaitPace(kFREQ); 14 | 15 | void setup() { 16 | hint(ENABLE_KEY_REPEAT); 17 | size(1280, 720); 18 | frameRate(30); 19 | 20 | _gait.setStepsPerSec(2); 21 | _graph[0] = new LegGraph(0, width / 2 + 50, 400); //<>// 22 | _graph[1] = new LegGraph(1, width / 2 - 300, 400); 23 | _graph[2] = new LegGraph(2, width / 2 - 300, 200); 24 | _graph[3] = new LegGraph(3, width / 2 + 50, 200); 25 | 26 | background(0); 27 | } 28 | 29 | void draw() { 30 | if (_start) { 31 | background(0); 32 | _start = false; 33 | } else { 34 | if (!kIS_ANIMATE_KEY) { 35 | for (int i = 0; i < _graph.length; i++) { 36 | _graph[i].draw(); 37 | } 38 | } 39 | } 40 | } 41 | 42 | class LegGraph { 43 | int _pos; 44 | int _x; 45 | int _y; 46 | boolean _cidx; 47 | short _last; 48 | int _oldPx; 49 | int _oldPy; 50 | 51 | LegGraph(int leg, int x, int y) { 52 | _pos = leg; 53 | _x = x; 54 | _y = y; 55 | _last = -1; 56 | _oldPx = Integer.MAX_VALUE; 57 | _oldPy = Integer.MAX_VALUE; 58 | } 59 | 60 | void draw() { 61 | strokeWeight(10); 62 | stroke(kCOLOR_1); 63 | noFill(); 64 | rect(_x - 100, _y, 300, 200); 65 | 66 | Vector vec; 67 | vec = _gait.doStep(_pos, _mov, _rot); 68 | 69 | if (_oldPx != Integer.MAX_VALUE && _oldPx != Integer.MAX_VALUE) { 70 | stroke(kCOLOR_1); 71 | point(_x + 50 + _oldPx, _y - 70 + _oldPy); 72 | } 73 | 74 | stroke(kCOLOR_2); 75 | point(_x + 50 + int(vec.x * 5), _y - 70 + int(vec.z * 5)); 76 | 77 | _oldPx = int(vec.x * 5); 78 | _oldPy = int(vec.z * 5); 79 | } 80 | } 81 | 82 | 83 | 84 | void keyPressed() { 85 | if (kIS_ANIMATE_KEY) { 86 | if (key == ' ') { 87 | for (int i = 0; i < _graph.length; i++) { 88 | _graph[i].draw(); 89 | } 90 | } 91 | } 92 | } 93 | -------------------------------------------------------------------------------- /quadruped_code/src/VecRot.h: -------------------------------------------------------------------------------- 1 | #ifndef _VECROT_H_ 2 | #define _VECROT_H_ 3 | 4 | #include "config.h" 5 | #include "utils.h" 6 | 7 | /* 8 | ***************************************************************************************** 9 | * CONSTANTS 10 | ***************************************************************************************** 11 | */ 12 | 13 | /* 14 | ***************************************************************************************** 15 | * MACROS & STRUCTURES 16 | ***************************************************************************************** 17 | */ 18 | 19 | 20 | /* 21 | ***************************************************************************************** 22 | * CLASS 23 | ***************************************************************************************** 24 | */ 25 | class Vector { 26 | public: 27 | float x; 28 | float y; 29 | float z; 30 | 31 | Vector() { 32 | set(0, 0, 0); 33 | } 34 | 35 | Vector(float x, float y, float z) { 36 | set(x, y, z); 37 | } 38 | 39 | void set(Vector v) { 40 | set(v.x, v.y, v.z); 41 | } 42 | 43 | void set(float x, float y, float z) { 44 | this->x = x; 45 | this->y = y; 46 | this->z = z; 47 | } 48 | 49 | void zero() { 50 | set(0, 0, 0); 51 | } 52 | 53 | bool equals(Vector v) { 54 | return (x == v.x && y == v.y && z == v.z); 55 | } 56 | 57 | void dump(String name) { 58 | LOG("[%4s] x:%7.3f, y:%7.3f, z:%7.3f\n", name.c_str(), x, y, z); 59 | } 60 | }; 61 | 62 | class Rotator { 63 | public: 64 | float yaw; 65 | float pitch; 66 | float roll; 67 | 68 | Rotator() { 69 | set(0, 0, 0); 70 | } 71 | 72 | Rotator(Rotator *r) { 73 | set(r->yaw, r->pitch, r->roll); 74 | } 75 | 76 | Rotator(float yaw, float pitch, float roll) { 77 | set(yaw, pitch, roll); 78 | } 79 | 80 | void set(Rotator r) { 81 | set(r.yaw, r.pitch, r.roll); 82 | } 83 | 84 | void set(float yaw, float pitch, float roll) { 85 | this->yaw = yaw; 86 | this->pitch = pitch; 87 | this->roll = roll; 88 | } 89 | 90 | void zero() { 91 | set(0, 0, 0); 92 | } 93 | 94 | bool equals(Rotator r) { 95 | return (yaw == r.yaw && pitch == r.pitch && roll == r.roll); 96 | } 97 | 98 | void dump(String name) { 99 | LOG("[%4s] y:%7.3f, p:%7.3f, r:%7.3f\n", name.c_str(), yaw, pitch, roll); 100 | } 101 | }; 102 | 103 | #endif -------------------------------------------------------------------------------- /quadruped_code/src/hardware.h: -------------------------------------------------------------------------------- 1 | #ifndef _HARDWARE_H_ 2 | #define _HARDWARE_H_ 3 | 4 | //#define ENABLE_DEBUG_OUTPUT 5 | #include 6 | #include "config.h" 7 | #include "VecRot.h" 8 | #include "utils.h" 9 | 10 | /* 11 | ***************************************************************************************** 12 | * CONSTANTS 13 | ***************************************************************************************** 14 | */ 15 | 16 | /* 17 | ***************************************************************************************** 18 | * MACROS & STRUCTURES 19 | ***************************************************************************************** 20 | */ 21 | 22 | /* 23 | ***************************************************************************************** 24 | * CLASS 25 | ***************************************************************************************** 26 | */ 27 | class Hardware { 28 | struct _joint { 29 | int min; // min angle 30 | int max; // max angle 31 | int offset; // offset angle 32 | }; 33 | 34 | struct _servo_cfg { 35 | uint8_t pin; // pin 36 | int8_t sign; // direction sign (1:normal, -1:inverted) 37 | int angle; // last angle 38 | int uS; // micro second 39 | }; 40 | 41 | public: 42 | Hardware(); 43 | void setup(void); 44 | void setJoint(int leg, int joint, int deg10); 45 | void setLeg(int leg, int a10, int b10, int c10); 46 | void calibrateLegs(int key); 47 | void dump(); 48 | int checkBattery(); 49 | int getServoFreq() { return HW_SERVO_UPDATE_FREQ; } 50 | #if CONFIG_ENABLE_CAM_PAN_TILT 51 | void setPanTilt(int pan, int tilt); 52 | #endif 53 | 54 | private: 55 | void loadConfig(void); 56 | void saveConfig(void); 57 | uint16_t analogReadAvg(uint16_t pin); 58 | #if CONFIG_ENABLE_CAM_PAN_TILT 59 | void setPanTiltInternal(int ch, int deg10); 60 | #endif 61 | 62 | int _calAngles[BODY_NUM_LEGS][BODY_NUM_JOINTS]; 63 | Adafruit_PWMServoDriver _servo; 64 | 65 | static const char *_kLegNames[BODY_NUM_LEGS]; 66 | static const struct _joint _kJointRanges[BODY_NUM_JOINTS]; 67 | static const float _kParentDir[BODY_NUM_LEGS][BODY_NUM_JOINTS - 1]; 68 | 69 | static struct _servo_cfg _cfgServos[BODY_NUM_LEGS][BODY_NUM_JOINTS]; 70 | 71 | #if CONFIG_ENABLE_CAM_PAN_TILT 72 | static struct _servo_cfg _cfgPanTiltServos[2]; 73 | int _calAnglesPanTilt[2]; 74 | #endif 75 | }; 76 | 77 | #endif -------------------------------------------------------------------------------- /quadruped_code/src/GyroDev.h: -------------------------------------------------------------------------------- 1 | /* 2 | This project is free software: you can redistribute it and/or modify 3 | it under the terms of the GNU General Public License as published by 4 | the Free Software Foundation, either version 3 of the License, or 5 | (at your option) any later version. 6 | 7 | This program is derived from deviationTx project for Arduino. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | see 14 | */ 15 | 16 | #ifndef _GYRO_DEV_H_ 17 | #define _GYRO_DEV_H_ 18 | #include 19 | #include 20 | #include "config.h" 21 | #include "I2Cdev.h" 22 | #include "utils.h" 23 | 24 | /* 25 | ***************************************************************************************** 26 | * MACROS 27 | ***************************************************************************************** 28 | */ 29 | typedef struct { 30 | float psi; 31 | float theta; 32 | float phi; 33 | } euler_t; 34 | 35 | typedef struct { 36 | float yaw; 37 | float pitch; 38 | float roll; 39 | } ypr_t; 40 | 41 | typedef struct { 42 | int16_t x; 43 | int16_t y; 44 | int16_t z; 45 | } vec_t; 46 | 47 | class GyroDev 48 | { 49 | public: 50 | void setup(int16_t *offsets); 51 | uint8_t loop(void); 52 | bool calibrate(int16_t *offsets); 53 | bool isEnabled(void) { return _isEnable; } 54 | euler_t getEuler(void) { return _euler; } 55 | ypr_t getYPR(void) { return _ypr; } 56 | vec_t getGyro(void) { return _gyro; } 57 | vec_t getAccel(void) { return _accel; } 58 | 59 | private: 60 | void sleep(void); 61 | int8_t cut(int val); 62 | float delta(float val); 63 | 64 | // 65 | // MPU control/status vars 66 | bool _isDmpReady = false; // set true if DMP init was successful 67 | uint8_t _mpuIntStatus; // holds actual interrupt status byte from MPU 68 | uint8_t _devStatus; // return status after each device operation (0 = success, !0 = error) 69 | uint16_t _packetSize; // expected DMP packet size (default is 42 bytes) 70 | uint16_t _fifoCount; // count of all bytes currently in FIFO 71 | uint8_t _fifoBuffer[64]; // FIFO storage buffer 72 | // 73 | bool _isEnable; 74 | ypr_t _ypr; 75 | euler_t _euler; 76 | vec_t _gyro; 77 | vec_t _accel; 78 | }; 79 | 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /quadruped_code/src/OTA.h: -------------------------------------------------------------------------------- 1 | #ifndef _OTA_H_ 2 | #define _OTA_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "config.h" 9 | #include "utils.h" 10 | 11 | class OTA { 12 | private: 13 | String _hostname; 14 | 15 | public: 16 | OTA(String hostname) { 17 | _hostname = hostname; 18 | } 19 | 20 | void setup(char *ssid, char *password) { 21 | LOG("OTA !!!!!!\n"); 22 | WiFi.mode(WIFI_STA); 23 | WiFi.setHostname(_hostname.c_str()); 24 | WiFi.begin(ssid, password); 25 | while (WiFi.waitForConnectResult() != WL_CONNECTED) { 26 | LOG("Connection Failed! Rebooting...\n"); 27 | delay(5000); 28 | ESP.restart(); 29 | } 30 | 31 | // Port defaults to 3232 32 | // ArduinoOTA.setPort(3232); 33 | 34 | // Hostname defaults to esp3232-[MAC] 35 | ArduinoOTA.setHostname(_hostname.c_str()); 36 | 37 | // No authentication by default 38 | // ArduinoOTA.setPassword("admin"); 39 | 40 | // Password can be set with it's md5 value as well 41 | // MD5(admin) = 21232f297a57a5a743894a0e4a801fc3 42 | // ArduinoOTA.setPasswordHash("21232f297a57a5a743894a0e4a801fc3"); 43 | 44 | ArduinoOTA 45 | .onStart([]() { 46 | String type; 47 | if (ArduinoOTA.getCommand() == U_FLASH) { 48 | type = "sketch"; 49 | } else { // U_SPIFFS 50 | type = "filesystem"; 51 | } 52 | // NOTE: if updating SPIFFS this would be the place to unmount SPIFFS using SPIFFS.end() 53 | LOG("Start updating : %s\n", type.c_str()); 54 | }) 55 | .onEnd([]() { 56 | LOG("\nEnd\n"); 57 | }) 58 | .onProgress([](unsigned int progress, unsigned int total) { 59 | LOG("Progress: %u%%\r", (progress / (total / 100))); 60 | }) 61 | .onError([](ota_error_t error) { 62 | LOG("Error[%u]: ", error); 63 | if (error == OTA_AUTH_ERROR) LOG("Auth Failed\n"); 64 | else if (error == OTA_BEGIN_ERROR) LOG("Begin Failed\n"); 65 | else if (error == OTA_CONNECT_ERROR) LOG("Connect Failed\n"); 66 | else if (error == OTA_RECEIVE_ERROR) LOG("Receive Failed\n"); 67 | else if (error == OTA_END_ERROR) LOG("End Failed\n"); 68 | }); 69 | 70 | ArduinoOTA.begin(); 71 | LOG("Ready\n"); 72 | LOG("IP address: %s\n", WiFi.localIP().toString().c_str()); 73 | LOG("hostname : %s\n", ArduinoOTA.getHostname().c_str()); 74 | } 75 | 76 | void loop() { 77 | ArduinoOTA.handle(); 78 | } 79 | 80 | }; 81 | #endif -------------------------------------------------------------------------------- /quadruped_code/src/StatusLed.h: -------------------------------------------------------------------------------- 1 | #ifndef _STATUS_LED_H_ 2 | #define _STATUS_LED_H_ 3 | #include 4 | #include "config.h" 5 | #include "utils.h" 6 | 7 | /* 8 | ***************************************************************************************** 9 | * CONSTANTS 10 | ***************************************************************************************** 11 | */ 12 | #define HUE(h) (65535L * (h) / 360) 13 | #define SAT(s) (255L * (s) / 100) 14 | #define VAL(v) (255L * (v) / 100) 15 | 16 | /* 17 | ***************************************************************************************** 18 | * CLASS 19 | ***************************************************************************************** 20 | */ 21 | class StatusLed { 22 | public: 23 | const static int ALL_POS = 0xff; 24 | 25 | typedef enum { 26 | COLOR_BLACK = 0, 27 | COLOR_PINK, 28 | COLOR_PURPLE, 29 | COLOR_BLUE, 30 | COLOR_LIGHTBLUE, 31 | COLOR_CYAN, 32 | COLOR_GREEN, 33 | COLOR_YELLOW, 34 | COLOR_ORANGE, 35 | COLOR_RED, 36 | COLOR_WHITE 37 | } color_idx_t; 38 | 39 | StatusLed() : 40 | _ledStrip { 0, PIN_LED_STRIP, LED_WS2812B_V2, 64, 2 }, 41 | _pStrands { &_ledStrip }, 42 | _tblColor { 43 | pixelFromRGB(0, 0, 0), // black 44 | pixelFromRGB(255, 192, 203), // pink 45 | pixelFromRGB(128, 0, 128), // purple 46 | pixelFromRGB(0, 0, 255), // blue 47 | pixelFromRGB(173, 216, 230), // lightblue 48 | pixelFromRGB(0, 255, 255), // cyan 49 | pixelFromRGB(0, 255, 0), // green 50 | pixelFromRGB(255, 255, 0), // yellow 51 | pixelFromRGB(255, 165, 0), // orange 52 | pixelFromRGB(255, 0, 0), // red 53 | pixelFromRGB(255, 255, 255) // white 54 | } 55 | { 56 | } 57 | 58 | pixelColor_t getColor(uint8_t idx) { 59 | if (idx < ARRAY_SIZE(_tblColor)) 60 | return _tblColor[idx]; 61 | return _tblColor[0]; 62 | } 63 | 64 | void setup(void); 65 | void setBrightness(uint8_t brightness); 66 | void set(int pos, color_idx_t idx, uint16_t msBlink = 0, color_idx_t offColor = StatusLed::COLOR_BLACK, bool update = true); 67 | void set(int pos, uint8_t r, uint8_t g, uint8_t b, uint16_t msBlink = 0, pixelColor_t offColor = pixelFromRGB(0, 0, 0), bool update = true); 68 | void set(int pos, pixelColor_t color, uint16_t msBlink = 0, pixelColor_t offColor = pixelFromRGB(0, 0, 0), bool update = true); 69 | void update(); 70 | void loop(unsigned long ts); 71 | 72 | private: 73 | static const int kCtrStrands = 1; 74 | 75 | strand_t _ledStrip; 76 | strand_t *_pStrands[kCtrStrands]; 77 | pixelColor_t _tblColor[11]; 78 | unsigned long _lLastBlinkTS[2]; 79 | uint16_t _wBlinkPeriod[2]; 80 | pixelColor_t _colorBlinkOn[2]; 81 | pixelColor_t _colorBlinkOff[2]; 82 | bool _isAllSame; 83 | }; 84 | 85 | #endif -------------------------------------------------------------------------------- /quadruped_code/src/QuadRuped.h: -------------------------------------------------------------------------------- 1 | #ifndef _QUAD_RUPED_H 2 | #define _QUAD_RUPED_H 3 | 4 | #include "config.h" 5 | #include "VecRot.h" 6 | #include "hardware.h" 7 | #include "Gait.h" 8 | #include "utils.h" 9 | 10 | /* 11 | ***************************************************************************************** 12 | * CONSTANTS 13 | ***************************************************************************************** 14 | */ 15 | 16 | /* 17 | ***************************************************************************************** 18 | * MACROS & STRUCTURES 19 | ***************************************************************************************** 20 | */ 21 | class JointAngle { 22 | public: 23 | JointAngle() { 24 | _coxa = 0.0f; 25 | _femur = 0.0f; 26 | _tibia = 0.0f; 27 | } 28 | 29 | void set(float coxa, float femur, float tibia) { 30 | _coxa = coxa; 31 | _femur = femur; 32 | _tibia = tibia; 33 | } 34 | 35 | float getCoxa() { return _coxa; } 36 | float getFemur() { return _femur; } 37 | float getTibia() { return _tibia; } 38 | 39 | private: 40 | float _coxa; 41 | float _femur; 42 | float _tibia; 43 | }; 44 | 45 | /* 46 | ***************************************************************************************** 47 | * CLASS 48 | ***************************************************************************************** 49 | */ 50 | class Leg { 51 | public: 52 | Leg() { 53 | _isDebug = false; 54 | }; 55 | 56 | void set(int pos, float bodyFrontWidth, float bodyHeight, float bodyMiddleWidth, 57 | float coxaLength, float femurLength, float tibiaLength); 58 | 59 | void bodyIK(Vector *pMov, Rotator *pRot, Vector *pTgt); 60 | void legIK(Vector *pTgt, JointAngle *pJA); 61 | void move(Vector *pMov, Rotator *pRot); 62 | void enableDebug(bool en) { _isDebug = en; } 63 | JointAngle *getJointPtr() { return &_jointAngle; } 64 | static Vector *getTipOffset(void) { return &_vTipOffset; } 65 | 66 | private: 67 | float fixAngle(float angle); 68 | float roundUp(float v); 69 | 70 | int _pos; 71 | bool _isDebug; 72 | float _coxaLength; 73 | float _femurLength; 74 | float _tibiaLength; 75 | float _coxaOffsetZ; 76 | 77 | Vector _vOffFromCenter; 78 | Vector _vInitPos; 79 | JointAngle _jointAngle; 80 | 81 | static Vector _vTipOffset; 82 | }; 83 | 84 | class QuadRuped { 85 | public: 86 | QuadRuped(float bodyWidth, float bodyHeight, float coxaLen, float coxaOffsetZ, float femurLen, float tibiaLen); 87 | 88 | void setup(Hardware *pHW); 89 | Vector *getTipOffset() { return Leg::getTipOffset(); } 90 | void setGait(Gait *gait) { _pGait = gait; } 91 | Gait *getGait() { return _pGait; } 92 | int getDebugMask() { return _debugLegMask; } 93 | 94 | void setDebugMask(int mask); 95 | void update(unsigned long ts, boolean isWalk, Vector *pMov, Rotator *pRot); 96 | 97 | private: 98 | Leg _legs[BODY_NUM_LEGS]; 99 | Vector _vBodyPos; 100 | Gait *_pGait; 101 | Hardware *_pHW; 102 | int _iInterval; // (unit:ms) 103 | unsigned long _lLastChcked; 104 | int _debugLegMask; 105 | }; 106 | 107 | #endif -------------------------------------------------------------------------------- /quadruped_code/src/StatusLed.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "StatusLed.h" 3 | 4 | 5 | /* 6 | ***************************************************************************************** 7 | * VARIABLES 8 | ***************************************************************************************** 9 | */ 10 | 11 | /* 12 | ***************************************************************************************** 13 | * Functions 14 | ***************************************************************************************** 15 | */ 16 | void StatusLed::setup(void) { 17 | digitalLeds_initDriver(); 18 | digitalLeds_addStrands((strand_t **)_pStrands, kCtrStrands); 19 | digitalLeds_resetPixels((strand_t **)_pStrands, kCtrStrands); 20 | } 21 | 22 | void StatusLed::setBrightness(uint8_t brightness) { 23 | _ledStrip.brightLimit = brightness; 24 | } 25 | 26 | void StatusLed::set(int pos, color_idx_t idx, uint16_t msBlink, color_idx_t offColor, bool update) { 27 | set(pos, getColor(idx), msBlink, getColor(offColor), update); 28 | } 29 | 30 | void StatusLed::set(int pos, uint8_t r, uint8_t g, uint8_t b, uint16_t msBlink, pixelColor_t offColor, bool update) { 31 | strand_t *pStrand = (strand_t *)_pStrands[0]; 32 | pixelColor_t color = pixelFromRGB(r * pStrand->brightLimit / 255, g * pStrand->brightLimit / 255, 33 | b * pStrand->brightLimit / 255); 34 | 35 | set(pos, color, msBlink, offColor, update); 36 | } 37 | 38 | void StatusLed::set(int pos, pixelColor_t color, uint16_t msBlink, pixelColor_t offColor, bool update) { 39 | strand_t *pStrand = (strand_t *)_pStrands[0]; 40 | pixelColor_t c = color; 41 | 42 | c.r = c.r * pStrand->brightLimit / 255; 43 | c.g = c.g * pStrand->brightLimit / 255; 44 | c.b = c.b * pStrand->brightLimit / 255; 45 | if (pos < pStrand->numPixels) { 46 | pStrand->pixels[pos] = c; 47 | _isAllSame = false; 48 | } else { 49 | for (uint16_t i = 0; i < pStrand->numPixels; i++) { 50 | pStrand->pixels[i] = c; 51 | } 52 | _isAllSame = true; 53 | pos = 0; 54 | } 55 | 56 | if (msBlink > 0) { 57 | _lLastBlinkTS[pos] = millis(); 58 | _colorBlinkOn[pos] = c; 59 | _colorBlinkOff[pos] = offColor; 60 | } 61 | _wBlinkPeriod[pos] = msBlink; 62 | 63 | if (update) { 64 | digitalLeds_drawPixels((strand_t **)_pStrands, kCtrStrands); 65 | } 66 | } 67 | 68 | void StatusLed::update() { 69 | digitalLeds_drawPixels((strand_t **)_pStrands, kCtrStrands); 70 | } 71 | 72 | void StatusLed::loop(unsigned long ts) { 73 | strand_t *pStrand = (strand_t *)_pStrands[0]; 74 | bool isUpdate = false; 75 | 76 | for (int i = 0; i < _ledStrip.numPixels; i++) { 77 | if (_wBlinkPeriod[i] > 0) { 78 | if (ts - _lLastBlinkTS[i] > _wBlinkPeriod[i]) { 79 | pixelColor_t c = (pStrand->pixels[i].num == _colorBlinkOff[i].num) ? _colorBlinkOn[i] : _colorBlinkOff[i]; 80 | _lLastBlinkTS[i] = ts; 81 | isUpdate = true; 82 | 83 | if (_isAllSame) { 84 | for (int j = 0; j < _ledStrip.numPixels; j++) { 85 | pStrand->pixels[j] = c; 86 | } 87 | break; 88 | } else { 89 | pStrand->pixels[i] = c; 90 | } 91 | } 92 | } 93 | } 94 | 95 | if (isUpdate) { 96 | update(); 97 | } 98 | } 99 | -------------------------------------------------------------------------------- /quadruped_code/lib/ControlStick/ControlStick.h: -------------------------------------------------------------------------------- 1 | #ifndef _CONTROLSTICK_H_ 2 | #define _CONTROLSTICK_H_ 3 | 4 | #include 5 | #include 6 | 7 | /* 8 | ***************************************************************************************** 9 | * CONSTANTS 10 | ***************************************************************************************** 11 | */ 12 | 13 | /* 14 | ***************************************************************************************** 15 | * MACROS & STRUCTURES 16 | ***************************************************************************************** 17 | */ 18 | 19 | /* 20 | ***************************************************************************************** 21 | * CLASS 22 | ***************************************************************************************** 23 | */ 24 | class StickCallback { 25 | public: 26 | StickCallback() {} 27 | virtual void onConnect() {} 28 | virtual void onDisconnect() {} 29 | virtual void onStickChanged(int axisX, int axisY, int axisZ, int axisRZ, int axisLT, int axisRT, int dpad, int btns) {} 30 | }; 31 | 32 | 33 | class AdvertisedDeviceCallbacks; 34 | class BLEJoyClientCallback; 35 | 36 | class ControlStick { 37 | public: 38 | enum { 39 | BTN_A = 0, 40 | BTN_B, 41 | BTN_MENU, 42 | BTN_X, 43 | BTN_Y, 44 | BTN_RSV1, 45 | BTN_L1, 46 | BTN_R1, 47 | 48 | BTN_L2 = 8, 49 | BTN_R2, 50 | BTN_SEL, 51 | BTN_START, 52 | BTN_POWER, 53 | BTN_LTHUMB, 54 | BTN_RTHUMB, 55 | BTN_RSV3, 56 | 57 | BTN_DPAD_UP = 16, 58 | BTN_DPAD_RIGHT, 59 | BTN_DPAD_DOWN, 60 | BTN_DPAD_LEFT 61 | } BTN_T; 62 | 63 | ControlStick(); 64 | virtual ~ControlStick() { } 65 | 66 | void add(BLEAddress* addr, BLEUUID uuidService, BLEUUID uuidCharacteristic, notify_callback cb); 67 | void begin(); 68 | bool connect(); 69 | bool isConnecting() { return _isConnecting; } 70 | bool isConnected() { return _isConnected; } 71 | void rescan(); 72 | void stop(); 73 | 74 | void addSupportedDevices(); 75 | void setStickCallback(StickCallback *callback) { _pStickCallback = callback; } 76 | 77 | friend class AdvertisedDeviceCallbacks; 78 | friend class BLEJoyClientCallback; 79 | 80 | protected: 81 | std::vector _reqDevAddrs; 82 | std::vector _uuidServices; 83 | std::vector _uuidCharacteristics; 84 | std::vector _notifyCallbacks; 85 | 86 | BLEAddress* _pServerAddress; 87 | boolean _isConnecting; 88 | boolean _isConnected; 89 | BLERemoteCharacteristic* _pRemoteCharacteristic; 90 | 91 | StickCallback* _pStickCallback; 92 | 93 | void cbControlStickBLE(NimBLERemoteCharacteristic* pBLERemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify); 94 | void cbFlyPadBLE(NimBLERemoteCharacteristic* pBLERemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify); 95 | void cbGameSirT1DBLE(NimBLERemoteCharacteristic* pBLERemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify); 96 | 97 | 98 | private: 99 | int getBit(int val, int bit) { return ((val >> bit) & 1); } 100 | int setBit(int val, int bit) { return (val << bit); } 101 | 102 | BLEUUID _uuidFoundService; 103 | int _nFoundIdx; 104 | int _nConnTryCtr; 105 | NimBLEClient* _pClient; 106 | }; 107 | 108 | #if 0 109 | class JoystickBLE : public ControlStick { 110 | public: 111 | // this is the service UUID of the VR Control handheld mouse/joystick device (HID) 112 | //"00001812-0000-1000-8000-00805f9b34fb" 113 | 114 | // this characteristic UUID works for joystick & triggers (report) 115 | //"00002A4D-0000-1000-8000-00805f9b34fb" 116 | JoystickBLE(BLEUUID uuidService = BLEUUID("1812"), BLEUUID uuidCharacteristic = BLEUUID("2A4D")) : ControlStick(uuidService, uuidCharacteristic) { 117 | } 118 | 119 | ~JoystickBLE() { 120 | } 121 | }; 122 | 123 | class FlyPadBLE : public ControlStick { 124 | public: 125 | FlyPadBLE(BLEUUID uuidService = BLEUUID("9e35fa00-4344-44d4-a2e2-0c7f6046878b"), BLEUUID uuidCharacteristic = BLEUUID("9e35fa01-4344-44d4-a2e2-0c7f6046878b")) : ControlStick(uuidService, uuidCharacteristic) { 126 | } 127 | 128 | ~FlyPadBLE() { 129 | } 130 | }; 131 | #endif 132 | 133 | #endif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ESP32 QuadRuped 2 | **An open-source 3D-printed quadrupedal robots with ESP32. (SpotMicro and Kangal)** 3 | - **Gait pattern generator using Processing** 4 | - **QuadRuped simulation using Processing** 5 | - **Balancing using IMU** 6 | - **FlyPad, GameSirT1D and Bluetooth LE joystick supported** 7 | 8 | ## 1.Schematic 9 |
10 | - Essential parts 11 | | part name | description | unit | 12 | |---------------------|--------------------------------------|-----------| 13 | | ESP32 LOLIN32 | cpu | 1 | 14 | | PCA9658 module | servo motor controller | 1 | 15 | | SPT5430HV-180W 8.4V | HV servo motor | 12 | 16 | | DC-DC buck converter| 7V-28V to 5V 3A | 1 | 17 | | 1S3P 18650 battery | lithium ion battery | 2 | 18 | | WS2812 RGB LED | RGB LED for eyes | 2 | 19 | | Round Rocker Switch | Switch with LED | 1 | 20 | 21 | - Optional parts 22 | | part name | description | unit | 23 | |---------------------|--------------------------------------|-----------| 24 | | HX-2S-JH20 | lithium ion 2S charger | 1 | 25 | | PDC004-PD 9V | usb-c PD decoy module | 1 | 26 | | STPS2045C | power shottky rectifier | 1 | 27 | | battery indicator | Lithium Battery Capacity Indicator | 1 | 28 |

29 | 30 | ## 2.Hardware Configuration 31 | 32 | 33 | - ### Kangal 34 |
35 | 36 |
37 | 38 | *click to youtube video* 39 | 40 | - ### SpotMicro 41 |
42 | 43 |
44 | 45 | *click to youtube video* 46 |

47 | 48 | ## 3.Simulation 49 | - pose and walking simulation with [processing](https://processing.org/). 50 | 51 | 52 | 53 | **moving keys** 54 | | key | camera | key | quadruped control | 55 | |-----------|---------------|-----------|-------------------| 56 | | **1** | Top View | **j / l** | roll- / roll+ | 57 | | **2** | Right View | **k / i** | pitch- / roll+ | 58 | | **3** | Left View | **u / o** | yaw- / yaw+ | 59 | | **4** | Front View | **p** | reset | 60 | | **up** | camera up | **q / e** | z- / z+ | 61 | | **down** | camera down | **w / s** | y- / y+ | 62 | | **left** | camera left | **a / d** | x- / x+ | 63 | | **right** | camera right | **w / s** | y- / y+ | 64 | | **home** | camera z up | **- / =** | step- / step+ | 65 | | **down** | camera z down | **space** | walk / stop | 66 | 67 | - gait pattern generation with [processing](https://processing.org/) 68 | 69 | 70 |

71 | 72 | ## 4.robot control with BLE(bluetooth le) joystick 73 | | key | control | key | control | 74 | |----------- |---------------|-----------|-------------------| 75 | | **A** | walk / stop | **L2+L1** | step interval- | 76 | | **B** | gait change | **L2+R1** | step interval+ | 77 | | **X** | balance mode | **L2+X** | step height- | 78 | | **Y** | flash ledstrip| **L2+Y** | step height+ | 79 | | **THUMB-L**| save settings | **L2+A** | step dist- | 80 | | **THUMB-R**| load settings | **L2+B** | step dist+ | 81 | | **DPAD-L** | toe offset- | **R2+L1** | toe offset- | 82 | | **DPAD-R** | toe offset+ | **R2+R1** | toe offset+ | 83 | 84 | ---- 85 | # Reference 86 | - IK model
87 | [12-DOF Quadrupedal Robot: InverseKinematics by Adham Elarabawy](https://www.adham-e.dev/pdf/IK_Model.pdf) 88 | - Kangal cad files
89 | [Diy quadruped robot by Baris ALP](https://grabcad.com/library/diy-quadruped-robot-1) 90 | - SpotMicro cad files
91 | [User Fran Ferri models](https://gitlab.com/public-open-source/spotmicroai/3dprinting/-/tree/master/User%20Fran%20Ferri%20models) 92 | -------------------------------------------------------------------------------- /quadruped_code/src/config.h: -------------------------------------------------------------------------------- 1 | #ifndef _CONFIG_H_ 2 | #define _CONFIG_H_ 3 | 4 | #include 5 | 6 | /* 7 | ***************************************************************************************** 8 | * FEATURES 9 | ***************************************************************************************** 10 | */ 11 | #define CONFIG_BODY_SPOTMICRO 1 12 | #define CONFIG_BODY_KANGAL 2 13 | #ifndef CONFIG_BODY 14 | #define CONFIG_BODY CONFIG_BODY_SPOTMICRO 15 | #endif 16 | 17 | #define CONFIG_CONTROL_BTPADS 1 18 | #define CONFIG_CONTROL_BT_UART 2 19 | #define CONFIG_CONTROL CONFIG_CONTROL_BTPADS 20 | 21 | #define CONFIG_ENABLE_GYRO 1 22 | #define CONFIG_ENABLE_CAM_PAN_TILT 1 23 | 24 | /* 25 | ***************************************************************************************** 26 | * CONSTANTS 27 | ***************************************************************************************** 28 | */ 29 | #define UNIT_MM 1.0f // (unit:mm) 30 | #define PRECISION 0.1f // (unit:mm) 31 | #define MOTION_SMOOTH_GAIN 0.9f 32 | 33 | #define BODY_NUM_LEGS 4 34 | #define BODY_NUM_JOINTS 3 35 | 36 | #if (CONFIG_BODY == CONFIG_BODY_SPOTMICRO) 37 | #define BODY_WIDTH 110 // (unit:mm) 38 | #define BODY_HEIGHT 300 // (unit:mm) 39 | #define BODY_COXA_OFFSET_Z 1 // (unit:mm) 40 | #define BODY_COXA_LENGTH 60 // (unit:mm) 41 | #define BODY_FEMUR_LENGTH 115 // (unit:mm) 42 | #define BODY_TIBIA_LENGTH 135 // (unit:mm) 43 | #define BODY_MIN_Z -70 44 | #define BODY_MAX_Z 70 45 | #define BODY_IDLE_Z -120 // (unit:mm) 46 | 47 | #define SERVER_NAME "SpotMicro" 48 | #define FILE_SERVO_CFG "/smoffs.cfg" 49 | #define FILE_STEP_CFG "/smstep.cfg" 50 | #define FILE_GYRO_CFG "/smgyro.cfg" 51 | #elif (CONFIG_BODY == CONFIG_BODY_KANGAL) 52 | #define BODY_WIDTH 95 // (unit:mm) 53 | #define BODY_HEIGHT 200 // (unit:mm) 54 | #define BODY_COXA_OFFSET_Z 0 // (unit:mm) 55 | #define BODY_COXA_LENGTH 40 // (unit:mm) 56 | #define BODY_FEMUR_LENGTH 100 // (unit:mm) 57 | #define BODY_TIBIA_LENGTH 115 // (unit:mm) 58 | #define BODY_MIN_Z -45 59 | #define BODY_MAX_Z 45 60 | #define BODY_IDLE_Z -45 // (unit:mm) 61 | 62 | #define SERVER_NAME "Kangal" 63 | #define FILE_SERVO_CFG "/kaoffs.cfg" 64 | #define FILE_STEP_CFG "/kastep.cfg" 65 | #define FILE_GYRO_CFG "/kagyro.cfg" 66 | #endif 67 | 68 | #define BODY_MAX_YAW 30.0f // (unit:degree) 69 | #define BODY_MAX_PITCH 30.0f // (unit:degree) 70 | #define BODY_MAX_ROLL 30.0f // (unit:degree) 71 | #define BODY_MAX_MOVE_MULT 3.0f 72 | 73 | #define WIFI_SSID "yourssid" 74 | #define WIFI_PASSWORD "yourpass" 75 | 76 | /* 77 | ***************************************************************************************** 78 | * H/W CONSTANTS (PINS) 79 | ***************************************************************************************** 80 | */ 81 | #if (CONFIG_BODY == CONFIG_BODY_SPOTMICRO) 82 | #define PIN_SCL0 22 83 | #define PIN_SDA0 21 84 | 85 | #define PIN_LED_STRIP 14 86 | #define PIN_PWR_LED 13 87 | 88 | #define PIN_PWR_ADC A0 89 | #define PIN_CAL_SW 12 90 | #elif (CONFIG_BODY == CONFIG_BODY_KANGAL) 91 | #define PIN_SCL0 22 92 | #define PIN_SDA0 21 93 | 94 | #define PIN_LED_STRIP 33 95 | #define PIN_PWR_LED 32 96 | 97 | #define PIN_PWR_ADC A0 98 | #define PIN_CAL_SW 12 99 | #endif 100 | 101 | #define PIN_LED 5 102 | 103 | // UART2 104 | #define PIN_RXD2 13 105 | #define PIN_TXD2 14 // 12 should be LOW during boot so TXD2 is changed to 14 106 | 107 | // ADC 108 | #define PIN_AMP A3 109 | 110 | // H/W CONFIGURATION 111 | #define HW_SERVO_UPDATE_FREQ 100 112 | 113 | /* 114 | ***************************************************************************************** 115 | * MACROS & STRUCTURES 116 | ***************************************************************************************** 117 | */ 118 | /* 119 | Z TOP VIEW 120 | | -Y 121 | | 122 | --------- 123 | | 2 3 | H 124 | -X -------| + |-------- +X E 125 | | 1 0 | A 126 | --------- D 127 | | 128 | | 129 | | +Y 130 | */ 131 | #define IS_FRONT_LEG(leg) (leg == 0 || leg == 3) 132 | #define IS_RIGHT_LEG(leg) (leg < 2) 133 | 134 | #endif -------------------------------------------------------------------------------- /quadruped_code/lib/PIDController/PIDController.cpp: -------------------------------------------------------------------------------- 1 | #include "PIDController.h" 2 | 3 | 4 | /* 5 | ***************************************************************************************** 6 | * CONSTANTS 7 | ***************************************************************************************** 8 | */ 9 | PIDController::PIDController(String name, bool circular, float p, float i, float d) { 10 | _strName = name; 11 | set(p, i, d); 12 | _fTarget = 0; 13 | _fIntegralLimit = 0; 14 | _isCircular = circular; 15 | } 16 | 17 | PIDController::PIDController(String name, float p, float i, float d) : PIDController(name, false, p, i, d) { 18 | } 19 | 20 | PIDController::PIDController(String name, float p, float i, float d, float iLimit) : PIDController(name, false, p, i, d) { 21 | _fIntegralLimit = iLimit; 22 | } 23 | 24 | PIDController::PIDController(String name, bool circular, float p, float i, float d, float iLimit) : PIDController(name, circular, p, i, d) { 25 | _fIntegralLimit = iLimit; 26 | } 27 | 28 | void PIDController::reset() { 29 | _fIntegral = 0; 30 | _lLastTS = 0; 31 | _fLastInput = 0; 32 | _fIntegStableCheck = 0; 33 | _fDurationStableCheck = 0; 34 | _isStableCheck = false; 35 | } 36 | 37 | String PIDController::getName() { 38 | return _strName; 39 | } 40 | 41 | void PIDController::set(float p, float i, float d) { 42 | _p = p; 43 | _i = i; 44 | _d = d; 45 | reset(); 46 | } 47 | 48 | float PIDController::getP() { 49 | return _p; 50 | } 51 | 52 | float PIDController::getI() { 53 | return _i; 54 | } 55 | 56 | float PIDController::getD() { 57 | return _d; 58 | } 59 | 60 | void PIDController::setTarget(float target) { 61 | _fTarget = target; 62 | } 63 | 64 | float PIDController::getTarget() { 65 | return _fTarget; 66 | } 67 | 68 | float PIDController::getLastInput() { 69 | return _fLastInput; 70 | } 71 | 72 | float PIDController::calcError(float a, float b) { 73 | float error; 74 | 75 | error = a - b; 76 | if (_isCircular) { 77 | if (error > 180) 78 | error = error -360; 79 | else if (error < -180) 80 | error = error + 360; 81 | } 82 | return error; 83 | } 84 | 85 | float PIDController::computeWithDelta(float input, float delta) { 86 | float error; 87 | float dError; 88 | float output; 89 | float outP; 90 | float outI; 91 | float outD; 92 | 93 | error = calcError(_fTarget, input); 94 | dError = calcError(error, _fLastError); 95 | 96 | if (_isStableCheck) { 97 | _fDurationStableCheck += delta; 98 | _fIntegStableCheck += (error * delta); 99 | } 100 | 101 | _fIntegral += (error * delta); 102 | if (_fIntegralLimit != 0) { 103 | _fIntegral = fConstrain(_fIntegral, -_fIntegralLimit, _fIntegralLimit, 0); 104 | } 105 | 106 | outP = _p * error; 107 | outI = _i * _fIntegral; 108 | outD = _d * (dError / delta); 109 | output = outP + outI + outD; 110 | //LOG("t:%5.2f, i:%5.2f, e:%5.2f, d:%5.2f, P:%5.2f, i:%5.2f, oD:%5.2f, out:%5.2f\n", _fTarget, input, error, delta, outP, _fIntegral, outD, output); 111 | 112 | _fLastInput = input; 113 | _fLastError = error; 114 | // clamp integral sum if the sign is changed 115 | // if ((dError < 0 && output > 0) || (dError > 0 || output < 0)) { 116 | // _fIntegral = 0; 117 | // } 118 | return output; 119 | } 120 | 121 | float PIDController::compute(unsigned long now, float input) { 122 | float delta = (_lLastTS == 0 || (now == _lLastTS)) ? 0.02f : ((now - _lLastTS) / 1000.0f); 123 | 124 | _lLastTS = now; 125 | return computeWithDelta(input, delta); 126 | } 127 | 128 | float PIDController::compute(unsigned long now, float input, float limit) { 129 | float out = compute(now, input); 130 | return fConstrain(out, -limit, limit, 0.0); 131 | } 132 | 133 | float PIDController::computeWithDelta(float input, float limit, float delta) { 134 | float out = computeWithDelta(input, delta); 135 | return fConstrain(out, -limit, limit, 0.0); 136 | } 137 | 138 | bool PIDController::isStable(float duration, float v) { 139 | bool ret = false; 140 | 141 | if (!_isStableCheck) { 142 | _isStableCheck = true; 143 | return ret; 144 | } 145 | 146 | if (_fDurationStableCheck > duration) { 147 | float avg = abs(_fIntegStableCheck / _fDurationStableCheck); 148 | //LOG("duration:%f, integ:%f, avg:%f", _fDurationStableCheck, _fIntegStableCheck, avg); 149 | 150 | if (avg > v) { 151 | // reset stable check window 152 | _fDurationStableCheck = 0; 153 | _fIntegStableCheck = 0; 154 | } else { 155 | ret = true; 156 | } 157 | } 158 | return ret; 159 | } 160 | 161 | float PIDController::fConstrain(float v, float min, float max, float deadzone) { 162 | if (v < min) 163 | v = min; 164 | else if (v > max) 165 | v = max; 166 | 167 | if (-deadzone < v && v < deadzone) 168 | v = 0; 169 | 170 | return v; 171 | } 172 | -------------------------------------------------------------------------------- /quadruped_code/src/GyroProc.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "GyroProc.h" 4 | 5 | /* 6 | ***************************************************************************************** 7 | * CONSTANTS 8 | ***************************************************************************************** 9 | */ 10 | 11 | 12 | /* 13 | ***************************************************************************************** 14 | * MACROS & STRUCTURES 15 | ***************************************************************************************** 16 | */ 17 | 18 | 19 | GyroProc::GyroProc(int hz) : 20 | _pidPit ("Pitch PID", true, 0.95, 0.35, 0), 21 | _pidRoll("Roll PID", true, 0.95, 0.35, 0) 22 | { 23 | _pidPit.setTarget(0.0f); 24 | _pidRoll.setTarget(0.0f); 25 | _iSel = 0; 26 | _iInterval = 1000 / hz; 27 | _pPID = &_pidPit; 28 | _p = _pPID->getP(); 29 | _i = _pPID->getI(); 30 | _d = _pPID->getD(); 31 | } 32 | 33 | void GyroProc::setup() { 34 | memset(_offsets, 0, sizeof(_offsets)); 35 | 36 | File file = SPIFFS.open(FILE_GYRO_CFG); 37 | if (file) { 38 | if (file.read((uint8_t*)_offsets, sizeof(_offsets)) > 0) { 39 | LOG("gyro cal data loaded !!!\n"); 40 | } 41 | file.close(); 42 | _gyro.setup(_offsets); 43 | } else { 44 | LOG("gyrocal.dat not found, start calibration !!\n"); 45 | calibrate(); 46 | } 47 | } 48 | 49 | bool GyroProc::calibrate(void) { 50 | bool ret; 51 | 52 | File file = SPIFFS.open(FILE_GYRO_CFG, FILE_WRITE); 53 | if (!file) { 54 | LOG("file write open error\n"); 55 | return false; 56 | } 57 | ret = _gyro.calibrate(_offsets); 58 | file.write((uint8_t*)_offsets, sizeof(_offsets)); 59 | file.close(); 60 | _gyro.setup(_offsets); 61 | 62 | // LOG("cal offsets\n"); 63 | // for (int i = 0; i < ARRAY_SIZE(_offsets); i++) { 64 | // LOG("%6d, ", _offsets[i]); 65 | // } 66 | // LOG("\n"); 67 | 68 | return ret; 69 | } 70 | 71 | float GyroProc::fixAngle(float fTgt, float fSrc) { 72 | if (abs(fTgt - fSrc) > 180) { 73 | if (fSrc > 0) { 74 | fTgt += 360; 75 | } else if (fSrc < 0) { 76 | fTgt -= 360; 77 | } 78 | } 79 | return fTgt; 80 | } 81 | 82 | bool GyroProc::isValid(float angle) { 83 | return !isnan(angle) && (-180 <= angle && angle <= 180); 84 | } 85 | 86 | void GyroProc::setTarget(float roll, float pit) { 87 | _pidRoll.setTarget(roll); 88 | _pidPit.setTarget(pit); 89 | } 90 | 91 | Rotator GyroProc::process(unsigned long ts, Rotator rot) { 92 | static Rotator ret; 93 | static int ctr = 0; 94 | ypr_t ypr; 95 | 96 | if (!_gyro.loop()) 97 | return ret; 98 | 99 | ypr = _gyro.getYPR(); 100 | if (!isValid(ypr.yaw) || !isValid(ypr.pitch) || !isValid(ypr.roll)) { 101 | return ret; 102 | } 103 | 104 | // spotmicro gyro sensor output kangal gyro sensor output robot orientation 105 | // yaw (left:-, right:+) yaw (left:-, right:+) yaw (left:-, right:+) 106 | // HEAD 107 | // pitch- pitch- pitch+ 108 | // | | | 109 | // -roll ---+--- roll+ +roll ---+--- roll- -roll ---+--- roll+ 110 | // | | | 111 | // pitch+ pitch+ pitch- 112 | // 113 | 114 | #if (CONFIG_BODY == CONFIG_BODY_SPOTMICRO) 115 | ypr.pitch = -ypr.pitch; 116 | #elif (CONFIG_BODY == CONFIG_BODY_KANGAL) 117 | ypr.roll = -ypr.roll; 118 | ypr.pitch = -ypr.pitch; 119 | #endif 120 | 121 | ret.roll = _pidRoll.compute(ts, ypr.roll, BODY_MAX_ROLL); 122 | ret.pitch = _pidPit.compute(ts, ypr.pitch, BODY_MAX_PITCH); 123 | ret.yaw = rot.yaw; 124 | 125 | if (ctr == 0) { 126 | LOG("%10ld %6.2f, RP(%6.2f, %6.2f) OUT(%6.2f, %6.2f)\n", ts, ypr.yaw, ypr.roll, ypr.pitch, 127 | ret.roll, ret.pitch); 128 | } 129 | ctr = (ctr + 1) % 20; 130 | 131 | return ret; 132 | } 133 | 134 | void GyroProc::togglePID() { 135 | _iSel = (_iSel + 1) % 2; 136 | _pPID = (_iSel == 0) ? &_pidPit : &_pidRoll; 137 | _p = _pPID->getP(); 138 | _i = _pPID->getI(); 139 | _d = _pPID->getD(); 140 | LOG("%s P:%6.2f, I:%6.2f, D:%6.2f\n", _pPID->getName().c_str(), _p, _i, _d); 141 | } 142 | 143 | void GyroProc::setPID() { 144 | _pPID->set(_p, _i, _d); 145 | LOG("%s P:%6.2f, I:%6.2f, D:%6.2f\n", _pPID->getName().c_str(), _pPID->getP(), _pPID->getI(), _pPID->getD()); 146 | } 147 | 148 | void GyroProc::incP() { 149 | _p += 0.05f; 150 | setPID(); 151 | } 152 | 153 | void GyroProc::decP() { 154 | _p -= 0.05f; 155 | setPID(); 156 | } 157 | 158 | void GyroProc::incI() { 159 | _i += 0.05f; 160 | setPID(); 161 | } 162 | 163 | void GyroProc::decI() { 164 | _i -= 0.05f; 165 | setPID(); 166 | } 167 | 168 | void GyroProc::incD() { 169 | _d += 0.05f; 170 | setPID(); 171 | } 172 | 173 | void GyroProc::decD() { 174 | _d -= 0.05f; 175 | setPID(); 176 | } 177 | 178 | void GyroProc::reset() { 179 | _pidPit.reset(); 180 | _pidRoll.reset(); 181 | } 182 | -------------------------------------------------------------------------------- /quadruped_code/src/CommandHandler.h: -------------------------------------------------------------------------------- 1 | #ifndef _COMMAND_HANDLER_H_ 2 | #define _COMMAND_HANDLER_H_ 3 | 4 | #include "config.h" 5 | #include "CommInterface.h" 6 | #include "QuadRuped.h" 7 | #include "StatusLed.h" 8 | #include "ControlStick.h" 9 | #include "TimedMove.h" 10 | #include "utils.h" 11 | #include "GyroProc.h" 12 | 13 | /* 14 | ***************************************************************************************** 15 | * CONSTANTS 16 | ***************************************************************************************** 17 | */ 18 | typedef enum { 19 | CONTROLLER_PROTOCOL = 0, 20 | CONTROLLER_BTPAD, 21 | CONTROLLER_MAX, 22 | } controller_t; 23 | 24 | #define BTN_SHIFT_L _BV(ControlStick::BTN_L2) 25 | #define BTN_SHIFT_R _BV(ControlStick::BTN_R2) 26 | 27 | #define BTN_WALK _BV(ControlStick::BTN_A) 28 | #define BTN_GAIT _BV(ControlStick::BTN_B) 29 | #define BTN_BALANCE _BV(ControlStick::BTN_X) 30 | #define BTN_FLASH _BV(ControlStick::BTN_Y) 31 | #define BTN_STAND _BV(ControlStick::BTN_START) 32 | #define BTN_OFFSET_DEC _BV(ControlStick::BTN_DPAD_LEFT) 33 | #define BTN_OFFSET_INC _BV(ControlStick::BTN_DPAD_RIGHT) 34 | #define BTN_HEIGHT_DEC (_BV(ControlStick::BTN_L1) | _BV(ControlStick::BTN_DPAD_DOWN)) 35 | #define BTN_HEIGHT_INC (_BV(ControlStick::BTN_R1) | _BV(ControlStick::BTN_DPAD_UP)) 36 | #define BTN_SAVE (_BV(ControlStick::BTN_LTHUMB) | _BV(ControlStick::BTN_MENU)) 37 | #define BTN_LOAD (_BV(ControlStick::BTN_RTHUMB) | _BV(ControlStick::BTN_POWER)) 38 | 39 | // with SHIFT_L 40 | #define BTN_STEP_DEC (BTN_SHIFT_L | _BV(ControlStick::BTN_L1)) 41 | #define BTN_STEP_INC (BTN_SHIFT_L | _BV(ControlStick::BTN_R1)) 42 | #define BTN_STEP_Z_DEC (BTN_SHIFT_L | _BV(ControlStick::BTN_X)) 43 | #define BTN_STEP_Z_INC (BTN_SHIFT_L | _BV(ControlStick::BTN_Y)) 44 | #define BTN_STEP_XY_DEC (BTN_SHIFT_L | _BV(ControlStick::BTN_A)) 45 | #define BTN_STEP_XY_INC (BTN_SHIFT_L | _BV(ControlStick::BTN_B)) 46 | 47 | // with SHIFT_R 48 | #define BTN_OFFSET_DEC2 (BTN_SHIFT_R | _BV(ControlStick::BTN_L1)) 49 | #define BTN_OFFSET_INC2 (BTN_SHIFT_R | _BV(ControlStick::BTN_R1)) 50 | #define BTN_CAMERA_MOVE (BTN_SHIFT_R | _BV(ControlStick::BTN_A)) 51 | 52 | // with SHIFT_L and SHIFT_R 53 | #define BTN_CAMERA_HOME (BTN_SHIFT_L | BTN_SHIFT_R | _BV(ControlStick::BTN_A)) 54 | 55 | /* 56 | ***************************************************************************************** 57 | * MACROS & STRUCTURES 58 | ***************************************************************************************** 59 | */ 60 | 61 | // 62 | // CommSerial2 63 | // 64 | class CommSerial2 : public CommInterface { 65 | private: 66 | class Serial2Dev : public ProtocolDevice { 67 | public: 68 | Serial2Dev() { 69 | // rpi txd --- esp rxd 70 | // rpi rxd --- esp txd 71 | Serial2.begin(115200, SERIAL_8N1, PIN_RXD2, PIN_TXD2); 72 | } 73 | 74 | virtual ~Serial2Dev() { 75 | } 76 | 77 | virtual void write(u8 *pBuf, u8 size) { 78 | Serial2.write(pBuf, size); 79 | } 80 | 81 | virtual int read(void) { 82 | return Serial2.read(); 83 | } 84 | 85 | virtual int available(void) { 86 | return Serial2.available(); 87 | } 88 | 89 | private: 90 | }; 91 | 92 | public: 93 | CommSerial2(int arg) : CommInterface(arg) { 94 | _pDev = new Serial2Dev(); 95 | getProtocol()->setDevice(_pDev); 96 | setConnected(TRUE); 97 | } 98 | 99 | virtual int loop(void) { 100 | getProtocol()->processRx(); 101 | return 1; 102 | } 103 | 104 | private: 105 | Serial2Dev *_pDev; 106 | uint8_t _ctr = 0; 107 | uint8_t _buf[100]; 108 | }; 109 | 110 | class CommandHandler : public ProtocolCallback { 111 | public: 112 | CommandHandler(CommInterface* comm = NULL); 113 | ~CommandHandler(); 114 | 115 | Hardware *getHW() { return &_hw; } 116 | StatusLed *getStatusLed() { return &_statusLed; } 117 | QuadRuped *getSpot() { return &_quadruped; } 118 | GyroProc *getGyroProc() { return &_gyroProc; } 119 | 120 | void setup(void); 121 | void setup(CommInterface* comm); 122 | void loop(); 123 | 124 | // joystick related functions 125 | void setConnected(controller_t con, bool connect) { _isConnected[con] = connect; }; 126 | bool isConnected(controller_t con) { return _isConnected[con]; } 127 | void setSwitches(int btns); 128 | void setButtons(int btns, bool toggle = false); 129 | void toggleButtons(int btns) { setButtons(btns, true); } 130 | 131 | // virtual functions 132 | virtual void onRC(struct param_rc* rc); 133 | virtual void onAttitude(struct param_att* att); 134 | virtual uint8_t onBattDV(void); 135 | virtual int8_t onOthers(uint8_t cmd, uint8_t* pData, uint8_t size, uint8_t* pRes); 136 | 137 | private: 138 | bool isSet(int shift, int toggledOn, int checkBtn); 139 | uint16_t filterDeadZone(uint16_t val); 140 | uint16_t aux2Switches(struct param_rc* rc); 141 | uint16_t aux2CombBits(uint16_t val, uint8_t bits); 142 | 143 | void saveUserData(); 144 | void loadUserData(); 145 | void updateStatusLeds(); 146 | 147 | float roundUp(float v) { return round(v * 10.0f) / 10.0f; } 148 | 149 | private: 150 | CommInterface* _pComm; 151 | 152 | int _nGait; 153 | Vector _vecMove; 154 | Vector _vecOldMove; 155 | Rotator _rotMove; 156 | Rotator _rotOldMove; 157 | 158 | static const float _fCurGain; 159 | static const float _fPrevGain; 160 | 161 | bool _isConnected[CONTROLLER_MAX]; 162 | bool _isStand; 163 | bool _isWalk; 164 | bool _isFlash; 165 | bool _isBalance; 166 | bool _isCamMove; 167 | 168 | struct param_rc _rc; 169 | 170 | Hardware _hw; 171 | Gait *_pGait; 172 | GaitMan _gaitMan; 173 | QuadRuped _quadruped; 174 | TimedMove _timedMove; 175 | 176 | StatusLed _statusLed; 177 | GyroProc _gyroProc; 178 | 179 | float _fCamPan; 180 | float _fOldCamPan; 181 | float _fCamTilt; 182 | float _fOldCamTilt; 183 | }; 184 | 185 | #endif -------------------------------------------------------------------------------- /quadruped_code/lib/msp/Protocol.h: -------------------------------------------------------------------------------- 1 | #ifndef _PROTOCOL_H_ 2 | #define _PROTOCOL_H_ 3 | #include "common.h" 4 | 5 | /* 6 | ***************************************************************************************** 7 | * CONSTANTS 8 | ***************************************************************************************** 9 | */ 10 | 11 | // 12 | // MSP VERSION 13 | // 14 | #define VER_MSP_MASTER_MASK 0x00 15 | #define VER_MSP_SLAVE_MASK 0x10 16 | #define VER_MSP_VERSION_MASK 0x0F 17 | // 18 | #define VER_MSP 0x01 19 | #define VER_EXT 0x02 20 | #define VER_MSP_MASTER (VER_MSP_MASTER_MASK | VER_MSP) 21 | #define VER_EXT_MASTER (VER_MSP_MASTER_MASK | VER_EXT) 22 | #define VER_MSP_SLAVE (VER_MSP_SLAVE_MASK | VER_MSP) 23 | #define VER_EXT_SLAVE (VER_MSP_SLAVE_MASK | VER_EXT) 24 | 25 | // 26 | // MULTIMEDIA COMMANDS 27 | // 28 | #define CMD_ACK_REQ 0x80 29 | // camera video 30 | #define CMD_CAM_INFO (CMD_ACK_REQ | 0x01) 31 | #define CMD_CAM_START (CMD_ACK_REQ | 0x02) 32 | #define CMD_CAM_STOP (CMD_ACK_REQ | 0x03) 33 | #define CMD_CAM_REQ_IFRAME 0x04 34 | #define CMD_CAM_SETTINGS (CMD_ACK_REQ | 0x05) 35 | // video, audio data 36 | #define CMD_VIDEO 0x20 37 | #define CMD_AUDIO 0x21 38 | 39 | // 40 | // AUX ASSIGNMENT 41 | // 42 | #define AUX_LIGHT1 0 43 | #define AUX_LIGHT2 1 44 | #define AUX_LIGHT3 2 45 | #define AUX_LIGHT4 3 46 | #define AUX_TRANSFORM 4 47 | #define AUX_FIRE 5 48 | #define AUX_GIMBAL_LOCK 6 49 | #define AUX_GIMBAL_ADJ 7 50 | 51 | // 52 | // CAPABILITY FLAG 53 | // 54 | #define CAP_VIDEO BV(31) 55 | #define CAP_AUDIO BV(30) 56 | 57 | 58 | /* 59 | ***************************************************************************************** 60 | * MACROS & STRUCTURES 61 | ***************************************************************************************** 62 | */ 63 | typedef enum { 64 | // MSP 65 | MSP_IDENT = 100, 66 | MSP_STATUS = 101, 67 | MSP_ATTITUDE = 108, 68 | MSP_ALTITUDE = 109, 69 | MSP_ANALOG = 110, 70 | MSP_MISC = 114, 71 | MSP_SET_RAW_RC_TINY = 150, 72 | MSP_SET_RAW_RC = 200, 73 | MSP_SET_MISC = 207, 74 | 75 | // DEFINED 76 | MSP_SET_SW_BUTTON = 51, 77 | 78 | MSP_SET_MOTOR_LED = 70, 79 | MSP_SET_GIMBAL_SPEED = 71, 80 | MSP_SET_TRANSFORM_SPEED = 72, 81 | MSP_SET_GIMBAL_HOMING = 73, 82 | MSP_GET_GIMBAL_POSITION = 74, 83 | 84 | MSP_SET_ONLINE = 80, 85 | MSP_SET_OFFLINE = 81, 86 | } MSP_T; 87 | 88 | 89 | struct param_id { 90 | u8 version; 91 | u8 type; 92 | u8 msp_version; 93 | u32 capability; 94 | } __attribute__((packed)); 95 | 96 | struct param_status { 97 | u16 cycles; 98 | u16 ctrErr; 99 | u16 maskSensors; 100 | u32 flags; 101 | u8 curSet; 102 | } __attribute__((packed)); 103 | 104 | struct param_att { 105 | s16 angleX; // [-1800;1800] (unit: 1/10 degree) 106 | s16 angleY; // [-900;900] (unit: 1/10 degree) 107 | s16 heading; // [-180;180] (unit: degree) 108 | } __attribute__((packed)); 109 | 110 | struct param_alt { 111 | u32 cmAltitude; // cm 112 | u16 speed; // cm/s 113 | } __attribute__((packed)); 114 | 115 | struct param_analog { 116 | u8 deciVolt; 117 | u16 sumPowerMeter; 118 | s16 rssi; 119 | u16 amperage; 120 | } __attribute__((packed)); 121 | 122 | struct param_misc { 123 | u16 powTrig; 124 | u16 minThr; 125 | u16 maxThr; 126 | u16 minCmdThr; 127 | u16 fsThr; 128 | u16 ctrArm; 129 | u32 lifeTime; 130 | u16 magDec; 131 | u8 battScale; 132 | u8 battWarn1; 133 | u8 battWarn2; 134 | u8 battCrit; 135 | } __attribute__((packed)); 136 | 137 | struct param_rc { 138 | s16 roll; 139 | s16 pitch; 140 | s16 yaw; 141 | s16 throttle; 142 | s16 aux[10]; 143 | u8 flag; 144 | } __attribute__((packed)); 145 | 146 | struct param_rc_short { 147 | u8 roll; 148 | u8 pitch; 149 | u8 yaw; 150 | u8 throttle; 151 | u8 switches; 152 | } __attribute__((packed)); 153 | 154 | struct param_motor_led { 155 | s16 motor[5]; 156 | u8 leds; 157 | } __attribute__((packed)); 158 | 159 | struct param_gimbal_speed { 160 | s16 yaw; 161 | s16 pitch; 162 | u8 move; 163 | } __attribute__((packed)); 164 | 165 | struct param_gimbal_pos { 166 | s16 yaw; 167 | s16 pitch; 168 | } __attribute__((packed)); 169 | 170 | 171 | /* 172 | ***************************************************************************************** 173 | * Class 174 | ***************************************************************************************** 175 | */ 176 | class ProtocolCallback { 177 | public: 178 | virtual ~ProtocolCallback() { } 179 | virtual void onRC(struct param_rc *rc) { } 180 | virtual u32 onCapInfo(void) { return 0; } 181 | virtual void onAttitude(struct param_att *att) { memset(att, 0, sizeof(struct param_att)); } 182 | virtual void onAltitude(struct param_alt *alt) { memset(alt, 0, sizeof(struct param_alt)); } 183 | virtual u8 onBattDV(void) { return 0; } 184 | virtual s8 onOthers(u8 cmd, u8 *pData, u8 size, u8 *pRes) { return -1; } 185 | }; 186 | 187 | class ProtocolDevice { 188 | public: 189 | virtual ~ProtocolDevice() { } 190 | virtual void write(u8 *pBuf, u8 size) = 0; 191 | virtual int read(void) = 0; 192 | virtual int available(void) = 0; 193 | }; 194 | 195 | class Protocol { 196 | public: 197 | Protocol(u8 m_wPacketSize); 198 | ~Protocol(); 199 | 200 | void setDevice(ProtocolDevice *pInterface); 201 | void setCallback(ProtocolCallback *pCallback); 202 | void processRx(void); 203 | void setSwButtonCmd(u8 cmd = MSP_SET_SW_BUTTON) { m_ucSwBtnCmd = cmd; } 204 | 205 | void send(bool resp, u8 cmd, u8 *pData, u8 size); 206 | void send(bool resp, ProtocolDevice *pDev, u8 cmd, u8 *pData, u8 size); 207 | 208 | private: 209 | void _onCommand(u8 cmd, u8 *pData, u8 size, u8 *pRes); 210 | 211 | typedef enum { 212 | STATE_IDLE, 213 | STATE_HEADER_START, 214 | STATE_HEADER_M, 215 | STATE_HEADER_ARROW, 216 | STATE_HEADER_SIZE, 217 | STATE_HEADER_CMD 218 | } STATE_T; 219 | 220 | u8 *m_pRxPackets; 221 | u8 *m_pResultPackets; 222 | u8 m_ucPacketSize; 223 | 224 | STATE_T m_stateRx; 225 | u8 m_ucOffset; 226 | u8 m_ucDataSize; 227 | u8 m_ucCheckSum; 228 | u8 m_ucCmd; 229 | u8 m_ucSwBtnCmd; 230 | u8 m_ucSwBtnVal; 231 | ProtocolCallback *m_pCallback; 232 | ProtocolDevice *m_pDevice; 233 | }; 234 | 235 | #endif 236 | 237 | -------------------------------------------------------------------------------- /quadruped_code/lib/msp/Protocol.cpp: -------------------------------------------------------------------------------- 1 | #include "Protocol.h" 2 | 3 | Protocol::Protocol(u8 ucPacketSize) { 4 | m_stateRx = STATE_IDLE; 5 | m_ucOffset = 0; 6 | m_ucDataSize = 0; 7 | m_ucCheckSum = 0; 8 | m_ucPacketSize = ucPacketSize; 9 | m_pRxPackets = new u8[m_ucPacketSize]; 10 | m_pResultPackets = new u8[m_ucPacketSize]; 11 | m_ucSwBtnCmd = 0; 12 | } 13 | 14 | Protocol::~Protocol() { 15 | delete m_pRxPackets; 16 | delete m_pResultPackets; 17 | delete m_pDevice; 18 | } 19 | 20 | void Protocol::setDevice(ProtocolDevice *pInterface) { 21 | m_pDevice = pInterface; 22 | } 23 | 24 | void Protocol::setCallback(ProtocolCallback *pCallback) { 25 | m_pCallback = pCallback; 26 | } 27 | 28 | void Protocol::send(bool resp, u8 cmd, u8 *pData, u8 size) { 29 | send(resp, m_pDevice, cmd, pData, size); 30 | } 31 | 32 | void Protocol::send(bool resp, ProtocolDevice *pDev, u8 cmd, u8 *pData, u8 size) { 33 | u8 header[5]; 34 | u8 checkSum = 0; 35 | 36 | header[0] = '$'; 37 | header[1] = 'M'; 38 | header[2] = resp ? '>' : '<'; 39 | header[3] = size; 40 | header[4] = cmd; 41 | 42 | for (u8 i = 3; i < sizeof(header); i++) { 43 | checkSum ^= header[i]; 44 | } 45 | 46 | for (u8 i = 0; i < size; i++) { 47 | u8 b = pData[i]; 48 | checkSum = checkSum ^ b; 49 | } 50 | 51 | if (pDev) { 52 | pDev->write(header, sizeof(header)); 53 | pDev->write(pData, size); 54 | pDev->write(&checkSum, sizeof(checkSum)); 55 | } 56 | } 57 | 58 | void Protocol::_onCommand(u8 cmd, u8 *pData, u8 size, u8 *pRes) { 59 | s8 ret = -1; 60 | 61 | switch (cmd) { 62 | case MSP_IDENT: { 63 | struct param_id *id = (struct param_id *)pRes; 64 | 65 | ret = sizeof(struct param_id); 66 | id->version = 240; 67 | id->type = 3; 68 | 69 | if (m_pCallback) 70 | id->capability = m_pCallback->onCapInfo(); 71 | } 72 | break; 73 | 74 | case MSP_STATUS: { 75 | static u16 ctr = 0; 76 | struct param_status *stat = (struct param_status *)pRes; 77 | 78 | ret = sizeof(struct param_status); 79 | memset(stat, 0, ret); 80 | stat->cycles = ctr++; 81 | } 82 | break; 83 | 84 | case MSP_ATTITUDE: { 85 | struct param_att *att = (struct param_att *)pRes; 86 | 87 | ret = sizeof(struct param_att); 88 | 89 | if (m_pCallback) 90 | m_pCallback->onAttitude(att); 91 | else 92 | memset(att, 0, ret); 93 | } 94 | break; 95 | 96 | case MSP_ALTITUDE: { 97 | struct param_alt *alt = (struct param_alt *)pRes; 98 | 99 | ret = sizeof(struct param_alt); 100 | 101 | if (m_pCallback) 102 | m_pCallback->onAltitude(alt); 103 | else 104 | memset(alt, 0, ret); 105 | } 106 | break; 107 | 108 | case MSP_ANALOG: { 109 | struct param_analog *anal = (struct param_analog *)pRes; 110 | 111 | ret = sizeof(struct param_analog); 112 | memset(anal, 0, ret); 113 | if (m_pCallback) 114 | anal->deciVolt = m_pCallback->onBattDV(); 115 | } 116 | break; 117 | 118 | case MSP_MISC: { 119 | struct param_misc *misc = (struct param_misc *)pRes; 120 | 121 | ret = sizeof(struct param_misc); 122 | memset(misc, 0, ret); 123 | misc->minThr = 1000; 124 | misc->maxThr = 2000; 125 | misc->minCmdThr = 1000; 126 | misc->fsThr = 1200; 127 | } 128 | break; 129 | 130 | case MSP_SET_RAW_RC: { 131 | struct param_rc *rc = (struct param_rc *)pData; 132 | 133 | rc->flag = 0; 134 | if (m_ucSwBtnCmd > 0) { 135 | for (u8 i = 0; i < 6; i++) { 136 | rc->aux[4 + i] = (m_ucSwBtnVal & BV(i)) ? 2000 : 1000; 137 | } 138 | } 139 | if (m_pCallback) 140 | m_pCallback->onRC(rc); 141 | } 142 | break; 143 | 144 | case MSP_SET_RAW_RC_TINY: { 145 | struct param_rc_short *rc = (struct param_rc_short *)pData; 146 | struct param_rc rcl; 147 | 148 | if (m_pCallback) { 149 | rcl.throttle = 1000 + rc->throttle * 4; 150 | rcl.yaw = 1000 + rc->yaw * 4; 151 | rcl.roll = 1000 + rc->roll * 4; 152 | rcl.pitch = 1000 + rc->pitch * 4; 153 | 154 | for (u8 i = 0; i < 8; i++) { 155 | rcl.aux[i] = (rc->switches & BV(i)) ? 2000 : 1000; 156 | } 157 | m_pCallback->onRC(&rcl); 158 | } 159 | } 160 | break; 161 | 162 | default: 163 | if (m_ucSwBtnCmd > 0 && cmd == m_ucSwBtnCmd) { 164 | m_ucSwBtnVal = *pData; 165 | } else if (m_pCallback) { 166 | ret = m_pCallback->onOthers(cmd, pData, size, pRes); 167 | } 168 | break; 169 | } 170 | 171 | if (ret >= 0) { 172 | send(true, m_pDevice, cmd, pRes, ret); 173 | } 174 | } 175 | 176 | void Protocol::processRx(void) { 177 | if (!m_pDevice) 178 | return; 179 | 180 | int size = m_pDevice->available(); 181 | 182 | for (int i = 0; i < size; i++) { 183 | u8 ch = m_pDevice->read(); 184 | switch (m_stateRx) { 185 | case STATE_IDLE: 186 | if (ch == '$') 187 | m_stateRx = STATE_HEADER_START; 188 | break; 189 | 190 | case STATE_HEADER_START: 191 | m_stateRx = (ch == 'M') ? STATE_HEADER_M : STATE_IDLE; 192 | break; 193 | 194 | case STATE_HEADER_M: 195 | m_stateRx = (ch == '<') ? STATE_HEADER_ARROW : STATE_IDLE; 196 | break; 197 | 198 | case STATE_HEADER_ARROW: 199 | if (ch > m_ucPacketSize) { // now we are expecting the payload size 200 | m_stateRx = STATE_IDLE; 201 | continue; 202 | } 203 | m_ucDataSize = ch; 204 | m_ucCheckSum = ch; 205 | m_ucOffset = 0; 206 | m_stateRx = STATE_HEADER_SIZE; 207 | break; 208 | 209 | case STATE_HEADER_SIZE: 210 | m_ucCmd = ch; 211 | m_ucCheckSum ^= ch; 212 | m_stateRx = STATE_HEADER_CMD; 213 | break; 214 | 215 | case STATE_HEADER_CMD: 216 | if (m_ucOffset < m_ucDataSize) { 217 | m_ucCheckSum ^= ch; 218 | m_pRxPackets[m_ucOffset++] = ch; 219 | } else { 220 | if (m_ucCheckSum == ch) { 221 | _onCommand(m_ucCmd, m_pRxPackets, m_ucDataSize, m_pResultPackets); 222 | } 223 | m_stateRx = STATE_IDLE; 224 | } 225 | break; 226 | } 227 | } 228 | } 229 | -------------------------------------------------------------------------------- /quadruped_code/data/edit.html: -------------------------------------------------------------------------------- 1 | 2 | ESP Editor 3 | 4 | 5 | 8 | 9 | 10 | 11 |
12 |
13 |
14 | 15 | 16 | -------------------------------------------------------------------------------- /quadruped_code/src/GyroDev.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "GyroDev.h" 3 | #include "MPU6050_6Axis_MotionApps20.h" 4 | 5 | /* 6 | ***************************************************************************************** 7 | * CONSTANTS 8 | ***************************************************************************************** 9 | */ 10 | 11 | 12 | /* 13 | ***************************************************************************************** 14 | * MACROS 15 | ***************************************************************************************** 16 | */ 17 | 18 | 19 | /* 20 | ***************************************************************************************** 21 | * TYPES 22 | ***************************************************************************************** 23 | */ 24 | 25 | 26 | /* 27 | ***************************************************************************************** 28 | * CONSTS 29 | ***************************************************************************************** 30 | */ 31 | 32 | 33 | /* 34 | ***************************************************************************************** 35 | * VARIABLES 36 | ***************************************************************************************** 37 | */ 38 | // 39 | // below types can't be referenced in the header files due to library error 40 | // 41 | static MPU6050 mpu; 42 | // orientation/motion vars 43 | static Quaternion q; // [w, x, y, z] quaternion container 44 | static VectorFloat gravity; // [x, y, z] gravity vector 45 | 46 | /* 47 | ***************************************************************************************** 48 | * 49 | ***************************************************************************************** 50 | */ 51 | int8_t GyroDev::cut(int val) { 52 | int8_t ret; 53 | 54 | ret = (val < -127) ? -127 : ((val > 127) ? 127 : val); 55 | return ret; 56 | } 57 | 58 | float GyroDev::delta(float val) { 59 | if (val > 180) { 60 | return val - 360; 61 | } else if (val < -180) { 62 | return val + 360; 63 | } 64 | return val; 65 | } 66 | 67 | /* 68 | ***************************************************************************************** 69 | * 70 | ***************************************************************************************** 71 | */ 72 | bool GyroDev::calibrate(int16_t *offsets) { 73 | _isDmpReady = false; 74 | 75 | mpu.initialize(); 76 | _devStatus = mpu.dmpInitialize(); 77 | 78 | mpu.setXAccelOffset(0); 79 | mpu.setYAccelOffset(0); 80 | mpu.setZAccelOffset(0); 81 | mpu.setXGyroOffset(0); 82 | mpu.setYGyroOffset(0); 83 | mpu.setZGyroOffset(0); 84 | 85 | if (_devStatus == 0) { 86 | // Calibration Time: generate offsets and calibrate our MPU6050 87 | mpu.CalibrateAccel(6); 88 | mpu.CalibrateGyro(6); 89 | 90 | offsets[0] = mpu.getXAccelOffset(); 91 | offsets[1] = mpu.getYAccelOffset(); 92 | offsets[2] = mpu.getZAccelOffset(); 93 | 94 | offsets[3] = mpu.getXGyroOffset(); 95 | offsets[4] = mpu.getYGyroOffset(); 96 | offsets[5] = mpu.getZGyroOffset(); 97 | 98 | mpu.PrintActiveOffsets(); 99 | 100 | _isDmpReady = true; 101 | } 102 | 103 | return _isDmpReady; 104 | } 105 | 106 | /* 107 | ***************************************************************************************** 108 | * 109 | ***************************************************************************************** 110 | */ 111 | void GyroDev::sleep(void) { 112 | mpu.setDMPEnabled(false); 113 | mpu.setSleepEnabled(false); 114 | } 115 | 116 | 117 | /* 118 | ***************************************************************************************** 119 | * SETUP 120 | ***************************************************************************************** 121 | */ 122 | void GyroDev::setup(int16_t *offsets) { 123 | _isDmpReady = false; 124 | 125 | mpu.initialize(); 126 | if (!mpu.testConnection()) { 127 | LOG("MPU6050 connection failed\n"); 128 | return; 129 | } 130 | 131 | // load and configure the DMP 132 | LOG("Initializing DMP...\n"); 133 | _devStatus = mpu.dmpInitialize(); 134 | 135 | // supply your own gyro offsets here, scaled for min sensitivity 136 | if (offsets != NULL) { 137 | mpu.setXAccelOffset(offsets[0]); 138 | mpu.setYAccelOffset(offsets[1]); 139 | mpu.setZAccelOffset(offsets[2]); 140 | mpu.setXGyroOffset(offsets[3]); 141 | mpu.setYGyroOffset(offsets[4]); 142 | mpu.setZGyroOffset(offsets[5]); 143 | // X Accel Y Accel Z Accel X Gyro Y Gyro Z Gyro 144 | //OFFSETS -2962, -121, 4536, 131, 30, -11 145 | } 146 | 147 | if (_devStatus == 0) { 148 | // Calibration Time: generate offsets and calibrate our MPU6050 149 | // mpu.CalibrateAccel(6); 150 | // mpu.CalibrateGyro(6); 151 | // mpu.PrintActiveOffsets(); 152 | 153 | // turn on the DMP, now that it's ready 154 | LOG("Enabling DMP...\n"); 155 | mpu.setDMPEnabled(true); 156 | 157 | //attachInterrupt(digitalPinToInterrupt(PIN_IRQ), &GyroDev::dmpDataReady, RISING); 158 | _mpuIntStatus = mpu.getIntStatus(); 159 | _isDmpReady = true; 160 | 161 | // get expected DMP packet size for later comparison 162 | _packetSize = mpu.dmpGetFIFOPacketSize(); 163 | } else { 164 | // ERROR! 165 | // 1 = initial memory load failed 166 | // 2 = DMP configuration updates failed 167 | // (if it's going to break, usually the code will be 1) 168 | LOG("DMP Initialization failed (code %d)\n", _devStatus); 169 | } 170 | 171 | _isEnable = true; 172 | } 173 | 174 | /* 175 | ***************************************************************************************** 176 | * LOOP 177 | ***************************************************************************************** 178 | */ 179 | uint8_t GyroDev::loop(void) { 180 | uint8_t ret = 0; 181 | float buf[3]; 182 | VectorInt16 result; 183 | VectorInt16 raw; 184 | 185 | if (!_isEnable || !_isDmpReady) { 186 | return ret; 187 | } 188 | 189 | _fifoCount = mpu.getFIFOCount(); 190 | if (_fifoCount < _packetSize) { 191 | return ret; 192 | } 193 | 194 | _mpuIntStatus = mpu.getIntStatus(); 195 | if ((_mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || _fifoCount >= 1024) { 196 | mpu.resetFIFO(); 197 | LOG("FIFO overflow!\n"); 198 | } else if (_mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) { 199 | while (_fifoCount >= _packetSize) { 200 | mpu.getFIFOBytes(_fifoBuffer, _packetSize); 201 | _fifoCount -= _packetSize; 202 | } 203 | 204 | // Euler angles in degrees 205 | mpu.dmpGetQuaternion(&q, _fifoBuffer); 206 | mpu.dmpGetGravity(&gravity, &q); 207 | 208 | #if 0 209 | mpu.dmpGetGyro((int16_t*)&_gyro, _fifoBuffer); 210 | mpu.dmpGetAccel(&raw, _fifoBuffer); 211 | mpu.dmpGetLinearAccel(&result, &raw, &gravity); 212 | //mpu.dmpGetLinearAccelInWorld(&result, &raw, &q); 213 | _accel.x = result.x; 214 | _accel.y = result.y; 215 | _accel.z = result.z; 216 | 217 | mpu.dmpGetEuler(buf, &q); 218 | _euler.psi = buf[0] * 180 / M_PI; 219 | _euler.theta = buf[1] * 180 / M_PI; 220 | _euler.phi = buf[2] * 180 / M_PI; 221 | #endif 222 | // 223 | // mpu6050 224 | // y,-r,p 225 | // 226 | mpu.dmpGetYawPitchRoll(buf, &q, &gravity); 227 | _ypr.yaw = degrees(buf[0]); 228 | _ypr.pitch = degrees(buf[2]); 229 | _ypr.roll = degrees(buf[1]); 230 | 231 | ret = 1; 232 | } 233 | return ret; 234 | } 235 | 236 | -------------------------------------------------------------------------------- /quadruped_code/src/QuadRuped.cpp: -------------------------------------------------------------------------------- 1 | #include "QuadRuped.h" 2 | 3 | /* 4 | ***************************************************************************************** 5 | * CONSTANTS 6 | ***************************************************************************************** 7 | */ 8 | 9 | /* 10 | Z TOP VIEW 11 | | -Y 12 | | 13 | --------- 14 | | 2 3 | H 15 | -X -------| + |-------- +X E 16 | | 1 0 | A 17 | --------- D 18 | | 19 | | 20 | | +Y 21 | */ 22 | 23 | /* 24 | ***************************************************************************************** 25 | * MACROS 26 | ***************************************************************************************** 27 | */ 28 | 29 | /* 30 | ***************************************************************************************** 31 | * LEG Class 32 | ***************************************************************************************** 33 | */ 34 | Vector Leg::_vTipOffset(-20, 0, 0); 35 | 36 | float Leg::fixAngle(float angle) { 37 | if (angle < -180) { 38 | angle += 360; 39 | } else if (angle >= 180) { 40 | angle -= 360; 41 | } 42 | return angle; 43 | } 44 | 45 | float Leg::roundUp(float v) { 46 | return round(v * 100.0f) / 100.0f; 47 | } 48 | 49 | void Leg::set(int pos, float bodyWidth, float bodyHeight, float coxaLength, float coxaOffsetZ, float femurLength, float tibiaLength) { 50 | _pos = pos; 51 | _coxaLength = coxaLength; 52 | _femurLength = femurLength; 53 | _tibiaLength = tibiaLength; 54 | _coxaOffsetZ = coxaOffsetZ; 55 | 56 | int signY; 57 | int signX; 58 | float offsetX; 59 | float offsetY; 60 | 61 | offsetX = bodyHeight / 2; 62 | offsetY = bodyWidth / 2; 63 | signY = IS_RIGHT_LEG(pos) ? 1 : -1; 64 | signX = IS_FRONT_LEG(pos) ? 1 : -1; 65 | 66 | _vOffFromCenter.x = signX * offsetX; 67 | _vOffFromCenter.y = signY * offsetY; 68 | _vOffFromCenter.z = tibiaLength; 69 | 70 | _vInitPos.x = signX * ((_tibiaLength * sin(radians(45))) - (_femurLength * sin(radians(45)))); 71 | _vInitPos.y = signY * coxaLength; 72 | _vInitPos.z = coxaOffsetZ + roundUp(sqrt(sq(_femurLength) + sq(_tibiaLength))); // angle between femur and tibia = 90 degree 73 | 74 | _vOffFromCenter.z += _vInitPos.z; 75 | if (_isDebug) { 76 | LOG("init leg:%d, off from center (%6.1f, %6.1f)\n", pos, 77 | _vOffFromCenter.x, _vOffFromCenter.y); 78 | LOG("init leg:%d, %6.1f, %6.1f, %6.1f\n", pos, 79 | _vInitPos.x, _vInitPos.y, _vInitPos.z); 80 | } 81 | } 82 | 83 | void Leg::bodyIK(Vector *pMov, Rotator *pRot, Vector *pTgt) { 84 | float mx = pMov->x + _vTipOffset.x; 85 | float my = pMov->y + _vTipOffset.y; 86 | 87 | // body ik 88 | float totX = _vInitPos.x + _vOffFromCenter.x + mx; 89 | float totY = _vInitPos.y + _vOffFromCenter.y + my; 90 | 91 | // yaw 92 | float alpha_0 = atan2(totX, totY); 93 | float alpha_1 = alpha_0 + radians(pRot->yaw); 94 | 95 | float dist = sqrt(sq(totX) + sq(totY)); 96 | float bodyIKX = roundUp(sin(alpha_1) * dist - totX); 97 | float bodyIKY = roundUp(cos(alpha_1) * dist - totY); 98 | 99 | // pitch, roll 100 | float pitchZ = tan(radians(pRot->pitch)) * totX; 101 | float rollZ = tan(radians(pRot->roll)) * totY; 102 | float bodyIKZ = roundUp(rollZ + pitchZ); 103 | 104 | pTgt->set(_vInitPos.x + mx + bodyIKX, _vInitPos.y + my + bodyIKY, _vInitPos.z + pMov->z - bodyIKZ); 105 | if (_isDebug) { 106 | LOG("newpos leg:%d, %6.1f, %6.1f, %6.1f\n", _pos, pTgt->x, pTgt->y, pTgt->z); 107 | } 108 | } 109 | 110 | // 111 | // https://www.adham-e.dev/pdf/IK_Model.pdf 112 | // 113 | void Leg::legIK(Vector *pTgt, JointAngle *pJA) { 114 | float h1 = sqrt(sq(_coxaLength) + sq(_coxaOffsetZ)); 115 | float h2 = sqrt(sq(pTgt->z) + sq(pTgt->y)); 116 | float a0 = atan(abs(pTgt->y) / pTgt->z); 117 | float a1 = atan(_coxaLength / _coxaOffsetZ); 118 | float a2 = atan(_coxaOffsetZ / _coxaLength); 119 | float a3 = asin(h1 * sin(a2 + radians(90)) / h2); 120 | float a4 = radians(180) - (a3 + a2 + radians(90)); 121 | float a5 = a1 - a4; 122 | float th = a0 - a5; 123 | 124 | float r0 = h1 * sin(a4) / sin(a3); 125 | float h = sqrt(sq(r0) + sq(abs(pTgt->x))); 126 | 127 | float phi= asin(pTgt->x / h); 128 | float ts = acos((sq(h) + sq(_femurLength) - sq(_tibiaLength)) / (2 * h * _femurLength)) - phi; 129 | float tw = acos((sq(_tibiaLength) + sq(_femurLength) - sq(h)) / (2 * _femurLength * _tibiaLength)); 130 | 131 | if (isnan(th) || isnan(ts) || isnan(tw)) { 132 | LOG("ERROR leg:%d, %6.1f, %6.1f, %6.1f\n", _pos, th, ts, tw); 133 | return; 134 | } 135 | 136 | float ac = degrees(th); // -90 ~ 90 137 | float af = -degrees(ts); // -90 ~ 90 138 | float at = degrees(tw) - 90; // -90 ~ 90 139 | 140 | #if (CONFIG_BODY == CONFIG_BODY_SPOTMICRO) 141 | ac = IS_RIGHT_LEG(_pos) ? ac : -ac; 142 | #elif (CONFIG_BODY == CONFIG_BODY_KANGAL) // coxa rotation reversed 143 | ac = IS_RIGHT_LEG(_pos) ? -ac : ac; 144 | #endif 145 | // zero base 146 | pJA->set(fixAngle(ac), fixAngle(af), fixAngle(at)); 147 | if (_isDebug) { 148 | LOG("angle leg:%d, %6.1f, %6.1f, %6.1f\n", _pos, pJA->getCoxa(), pJA->getFemur(), pJA->getTibia()); 149 | } 150 | } 151 | 152 | void Leg::move(Vector *pMov, Rotator *pRot) { 153 | Vector tgt; 154 | 155 | bodyIK(pMov, pRot, &tgt); 156 | legIK(&tgt, &_jointAngle); 157 | } 158 | 159 | 160 | /* 161 | ***************************************************************************************** 162 | * QuadRuped Class 163 | ***************************************************************************************** 164 | */ 165 | QuadRuped::QuadRuped(float bodyWidth, float bodyHeight, float coxaLen, float coxaOffsetZ, float femurLen, float tibiaLen) { 166 | _debugLegMask = 0x00; 167 | _lLastChcked = 0; 168 | _iInterval = 1000 / 50; // default update interval (50Hz) 169 | 170 | Vector v; // default vector (0, 0, 0) 171 | Rotator r; // default rotator (0, 0, 0) 172 | 173 | for (int i = 0; i < BODY_NUM_LEGS; i++) { 174 | _legs[i].enableDebug(_debugLegMask & _BV(i)); 175 | _legs[i].set(i, bodyWidth, bodyHeight, coxaLen, coxaOffsetZ, femurLen, tibiaLen); 176 | _legs[i].move(&v, &r); 177 | } 178 | LOG("-----------------------\n"); 179 | } 180 | 181 | void QuadRuped::setup(Hardware *pHW) { 182 | _pHW = pHW; 183 | _iInterval = 1000 / _pHW->getServoFreq(); 184 | } 185 | 186 | void QuadRuped::setDebugMask(int mask) { 187 | _debugLegMask = mask; 188 | for (int i = 0; i < BODY_NUM_LEGS; i++) { 189 | _legs[i].enableDebug(_debugLegMask & _BV(i)); 190 | } 191 | } 192 | 193 | void QuadRuped::update(unsigned long ts, boolean isWalk, Vector *pMov, Rotator *pRot) { 194 | if (_lLastChcked != 0 && (ts - _lLastChcked) < _iInterval) 195 | return; 196 | 197 | if (isWalk) { 198 | Vector move; 199 | Rotator rot; 200 | 201 | move.set(*pMov); 202 | rot.set(*pRot); 203 | for (int i = 0; i < BODY_NUM_LEGS; i++) { 204 | _pGait->doStep(i, &move, &rot, int(_debugLegMask & _BV(i))); 205 | _legs[i].move(&move, &rot); 206 | _pHW->setLeg(i, _legs[i].getJointPtr()->getCoxa() * 10.0f, _legs[i].getJointPtr()->getFemur() * 10.0f, _legs[i].getJointPtr()->getTibia() * 10.0f); 207 | } 208 | } else { 209 | for (int i = 0; i < BODY_NUM_LEGS; i++) { 210 | _legs[i].move(pMov, pRot); 211 | _pHW->setLeg(i, _legs[i].getJointPtr()->getCoxa() * 10.0f, _legs[i].getJointPtr()->getFemur() * 10.0f, _legs[i].getJointPtr()->getTibia() * 10.0f); 212 | } 213 | } 214 | 215 | if (_debugLegMask) { 216 | LOG("-----------------------\n"); 217 | } 218 | 219 | _lLastChcked = ts; 220 | } 221 | -------------------------------------------------------------------------------- /processing/simulation/simulation.pde: -------------------------------------------------------------------------------- 1 | boolean _isCamLog = false; 2 | Vector _origin; 3 | QuadRuped _quad; 4 | 5 | class CamCtrl { 6 | int _mode; 7 | Vector _eye; 8 | Vector _center; 9 | Vector _up; 10 | Vector _rot; 11 | 12 | Vector _tblCamFix[][] = { 13 | { new Vector( 0, 0, 700), new Vector( 0, 0, 0), new Vector(0, 0, 0) }, // up 14 | { new Vector(-300, 700, 0), new Vector(10, 0, 0), new Vector(-90, 0, 90) }, // right 15 | { new Vector(-300, 700, 0), new Vector(10, 0, 0), new Vector(-90, 0, -90) }, // left 16 | { new Vector(-300, 600, 0), new Vector(10, -600, 0), new Vector(-90, 0, 0) }, // front 17 | }; 18 | 19 | CamCtrl() { 20 | _mode = 0; 21 | _eye = new Vector(); 22 | _center = new Vector(0, 0, 0); 23 | _up = new Vector(0, 1, 0); 24 | _rot = new Vector(0, 0, 0); 25 | } 26 | 27 | void init() { 28 | _eye.set(0, 0, (height / 2.0) / tan(radians(30))); 29 | move(3); 30 | } 31 | 32 | void move(int idx) { 33 | if (idx < _tblCamFix.length) { 34 | _eye.set(_tblCamFix[idx][0]); 35 | _center.set(_tblCamFix[idx][1]); 36 | _rot.set(_tblCamFix[idx][2]); 37 | } 38 | } 39 | 40 | void apply() { 41 | beginCamera(); 42 | camera(_eye.x, _eye.y, _eye.z, _center.x, _center.y, _center.z, _up.x, _up.y, _up.z); 43 | endCamera(); 44 | 45 | rotateX(radians(_rot.y)); 46 | rotateY(radians(_rot.x)); 47 | rotateZ(radians(_rot.z)); 48 | } 49 | 50 | boolean process(int key) { 51 | boolean ret = true; 52 | Vector v = _eye; 53 | 54 | //print(String.format("keyCode:%d, key:%d\n", keyCode, key)); 55 | 56 | if (_mode == 1) 57 | v = _center; 58 | else if (_mode == 2) 59 | v = _rot; 60 | 61 | switch (keyCode) { 62 | case UP: 63 | if (_mode == 2) 64 | v.y = (v.y > -360) ? (v.y - 10) : -360; 65 | else 66 | v.y -= 10; 67 | break; 68 | 69 | case DOWN: 70 | if (_mode == 2) 71 | v.y = (v.y < 360) ? (v.y + 10) : 360; 72 | else 73 | v.y += 10; 74 | break; 75 | 76 | case LEFT: 77 | if (_mode == 2) 78 | v.x = (v.x > -360) ? (v.x - 10) : -360; 79 | else 80 | v.x -= 10; 81 | break; 82 | 83 | case RIGHT: 84 | if (_mode == 2) 85 | v.x = (v.x < 360) ? (v.x + 10) : 360; 86 | else 87 | v.x += 10; 88 | break; 89 | 90 | case 2: // home 91 | if (_mode == 2) 92 | v.z = (v.z > -360) ? (v.z - 10) : -360; 93 | else 94 | v.z = (v.z > 10) ? (v.z - 10) : 0; 95 | break; 96 | 97 | case 3: // end 98 | if (_mode == 2) 99 | v.z = (v.z < 360) ? (v.z + 10) : 360; 100 | else 101 | v.z += 10; 102 | break; 103 | 104 | case 147: // delete 105 | _mode = (_mode + 1) % 3; 106 | print(String.format("MODE:%d\n", _mode)); 107 | break; 108 | 109 | case 11: // pgdn 110 | break; 111 | 112 | case 26: // insert 113 | break; 114 | 115 | case 16: // pgup 116 | break; 117 | 118 | default: 119 | switch (key) { 120 | case '1': 121 | case '2': 122 | case '3': 123 | case '4': 124 | move(key - '1'); 125 | break; 126 | 127 | default: 128 | ret = false; 129 | break; 130 | }; 131 | break; 132 | } 133 | 134 | if (ret) { 135 | print(String.format("CAM : eye(%5.1f %5.1f %5.1f), center(%5.1f %5.1f %5.1f), rot(%5.1f %5.1f %5.1f)\n", 136 | _eye.x, _eye.y, _eye.z, _center.x, _center.y, _center.z, _rot.x, _rot.y, _rot.z)); 137 | } 138 | 139 | return ret; 140 | } 141 | } 142 | 143 | CamCtrl _cam = new CamCtrl(); 144 | boolean _isWalk = false; 145 | Object _mutex = new Object(); 146 | 147 | //static final float kBodyWidth = 110; 148 | //static final float kBodyHeight = 300; 149 | //static final float kCoxaLength = 60; 150 | //static final float kCoxaOffsetZ = 1; 151 | //static final float kFemurLength = 115; 152 | //static final float kTibiaLength = 135; 153 | 154 | static final float kBodyWidth = 95; 155 | static final float kBodyHeight = 200; 156 | static final float kCoxaLength = 40; 157 | static final float kCoxaOffsetZ = 0; 158 | static final float kFemurLength = 100; 159 | static final float kTibiaLength = 115; 160 | 161 | void setup() { 162 | hint(ENABLE_KEY_REPEAT); 163 | size(1280, 720, P3D); 164 | lights(); 165 | //noCursor(); 166 | frameRate(60); 167 | //fullScreen(P3D); 168 | com.jogamp.newt.opengl.GLWindow window = (com.jogamp.newt.opengl.GLWindow)(surface.getNative()); 169 | window.setResizable(true); 170 | window.setMaximized(true, true); 171 | 172 | _cam.init(); 173 | _origin = new Vector(width / 2, height / 2, 0); 174 | _quad = new QuadRuped(kBodyWidth, kBodyHeight, kCoxaLength, kCoxaOffsetZ, kFemurLength, kTibiaLength); 175 | } 176 | 177 | private final color kCOLOR_PLANE = color(100, 100, 100); 178 | 179 | void drawPlane(float x, float y, float z, float w, float l) { 180 | pushMatrix(); 181 | translate(x, y, z); 182 | fill(kCOLOR_PLANE); 183 | box(w, l, 1); 184 | popMatrix(); 185 | } 186 | 187 | void draw() { 188 | _cam.apply(); 189 | background(0); 190 | drawPlane(_origin.x, _origin.y, _origin.z, 5000, 5000); 191 | _quad.update(_isWalk); 192 | } 193 | 194 | void keyPressed() { 195 | 196 | if (_cam.process(key)) 197 | return; 198 | 199 | switch (key) { 200 | case 'j': 201 | _quad.getRot().roll--; 202 | break; 203 | 204 | case 'l': 205 | _quad.getRot().roll++; 206 | break; 207 | 208 | case 'i': 209 | _quad.getRot().pitch++; 210 | break; 211 | 212 | case 'k': 213 | _quad.getRot().pitch--; 214 | break; 215 | 216 | case 'u': 217 | _quad.getRot().yaw--; 218 | break; 219 | 220 | case 'o': 221 | _quad.getRot().yaw++; 222 | break; 223 | 224 | case 'p': 225 | _quad.getRot().zero(); 226 | _quad.getPos().zero(); 227 | break; 228 | 229 | case '0': 230 | _isCamLog = !_isCamLog; 231 | break; 232 | 233 | case 'q': 234 | _quad.getPos().z--; 235 | break; 236 | 237 | case 'e': 238 | _quad.getPos().z++; 239 | break; 240 | 241 | case 'w': 242 | _quad.getPos().y++; 243 | break; 244 | 245 | case 's': 246 | _quad.getPos().y--; 247 | break; 248 | 249 | case 'a': 250 | _quad.getPos().x--; 251 | break; 252 | 253 | case 'd': 254 | _quad.getPos().x++; 255 | break; 256 | 257 | case ' ': 258 | _isWalk = !_isWalk; 259 | print(String.format("Waling mode:%d\n", int(_isWalk))); 260 | break; 261 | 262 | case '=': 263 | _quad._gait.setStepsPerSec(_quad._gait.getStepsPerSec() + 1); 264 | print(String.format("Steps Per Sec:%f\n", _quad._gait.getStepsPerSec())); 265 | break; 266 | 267 | case '-': 268 | if (_quad._gait.getStepsPerSec() > 1) { 269 | _quad._gait.setStepsPerSec(_quad._gait.getStepsPerSec() - 1); 270 | print(String.format("Steps Per Sec:%f\n", _quad._gait.getStepsPerSec())); 271 | } 272 | 273 | break; 274 | } 275 | //_quad.update(_isWalk); 276 | //print(String.format("P%s R%s\n", _quad.getPos().toString(), _quad.getRot().toString())); 277 | } 278 | -------------------------------------------------------------------------------- /quadruped_code/src/Gait.h: -------------------------------------------------------------------------------- 1 | #ifndef _GAIT_H_ 2 | #define _GAIT_H_ 3 | 4 | #include "config.h" 5 | #include "VecRot.h" 6 | #include "hardware.h" 7 | #include "utils.h" 8 | 9 | /* 10 | ***************************************************************************************** 11 | * CONSTANTS 12 | ***************************************************************************************** 13 | */ 14 | /* 15 | Z TOP VIEW 16 | | -Y 17 | | 18 | --------- 19 | | 2 3 | H 20 | -X -------| + |-------- +X E 21 | | 1 0 | A 22 | --------- D 23 | | 24 | | 25 | | +Y 26 | */ 27 | // Leg order : RF, RH, LH, LF 28 | 29 | /* 30 | ***************************************************************************************** 31 | * MACROS & STRUCTURES 32 | ***************************************************************************************** 33 | */ 34 | 35 | /* 36 | ***************************************************************************************** 37 | * GaitParam 38 | ***************************************************************************************** 39 | */ 40 | class GaitParam { 41 | private: 42 | float _fAmplitude; 43 | short _tick; 44 | 45 | boolean _isSwing; 46 | float _fCurMult; 47 | float _fSwingMult; 48 | float _fStanceMult; 49 | float _fSwingOffset; 50 | 51 | public: 52 | GaitParam(float fSwingMult = 1.0f, float fStanceMult = 1.0f) { 53 | _tick = 0; 54 | _fSwingMult = fSwingMult; 55 | _fStanceMult = fStanceMult; 56 | setSwingState(false); 57 | } 58 | 59 | void setSwingOffset(float fSwingOffset) { 60 | _fSwingOffset = fSwingOffset; 61 | 62 | if (fSwingOffset == 0) { 63 | setSwingState(true); 64 | } else { 65 | _fCurMult = fSwingOffset; 66 | _isSwing = false; 67 | } 68 | } 69 | 70 | void setSwingState(boolean en) { 71 | _isSwing = en; 72 | if (en) { 73 | _fCurMult = _fSwingMult; 74 | } else { 75 | _fCurMult = _fStanceMult; 76 | } 77 | } 78 | 79 | void tick(Vector move, Vector step, float freq, float stepsPerSec) { 80 | float full = round(freq * _fCurMult / stepsPerSec); 81 | float w0 = step.x * UNIT_MM / (2 / max(abs(move.x), abs(move.y))); 82 | float a0 = (w0 * 2) * (float(_tick) / full) - w0; 83 | 84 | _tick++; 85 | _fAmplitude = a0; 86 | if (_tick > full) { 87 | setSwingState(!_isSwing); 88 | _fAmplitude = -w0; 89 | _tick = 1; 90 | } 91 | } 92 | 93 | void reset() { 94 | _tick = 0; 95 | setSwingOffset(_fSwingOffset); 96 | } 97 | 98 | void setMult(float fSwingMult = 1.0f, float fStanceMult = 1.0f) { 99 | _fSwingMult = fSwingMult; 100 | _fStanceMult = fStanceMult; 101 | } 102 | 103 | boolean isSwingState() { return _isSwing; } 104 | float getAmplitude() { return _fAmplitude; } 105 | int getTick() { return _tick; } 106 | }; 107 | 108 | /* 109 | ***************************************************************************************** 110 | * Gait 111 | ***************************************************************************************** 112 | */ 113 | class Gait { 114 | public: 115 | Gait(float fSwingMult = 1.0f, float fStanceMult = 1.0f, float freq = HW_SERVO_UPDATE_FREQ) { 116 | _fFreq = freq; 117 | _isComp = false; 118 | _iSwingLeg = -1; 119 | 120 | for (int i = 0; i < BODY_NUM_LEGS; i++) { 121 | _paramLegs[i].setMult(fSwingMult, fStanceMult); 122 | } 123 | } 124 | 125 | virtual ~Gait() {} 126 | 127 | void setComp(boolean en) { _isComp = en; } 128 | void setStep(Vector vecStep) { _vecStep = vecStep; } 129 | Vector *getStep(void) { return &_vecStep; } 130 | float getStepsPerSec(void) { return _stepsPerSec; } 131 | void setStepsPerSec(float steps); 132 | 133 | virtual String getName(void) = 0; 134 | virtual void doStep(int leg, Vector *move, Rotator *rot, bool enLog); 135 | 136 | protected: 137 | Vector calcMoveRatio(Vector move); 138 | void setSwingOffsets(const float offsets[]); 139 | 140 | float _fFreq; 141 | GaitParam _paramLegs[BODY_NUM_LEGS]; 142 | float _fSwingAmplitude; 143 | int _iSwingLeg; 144 | bool _isComp; 145 | 146 | static Vector _vecStep; 147 | static float _stepsPerSec; 148 | 149 | }; 150 | 151 | #if 0 152 | /* 153 | ***************************************************************************************** 154 | * GaitCrawl 155 | ***************************************************************************************** 156 | */ 157 | class GaitCrawl : public Gait { 158 | private: 159 | const float _offsets[BODY_NUM_LEGS] = { 0, 3.0f, 1.0f, 2.0f }; // RF, RH, LH, LF 160 | 161 | public: 162 | GaitCrawl(float freq) : Gait(1.0f, 3.0f, freq) { 163 | setSwingOffsets(_offsets); 164 | setComp(true); 165 | } 166 | 167 | String getName(void) { return "Crawl"; } 168 | }; 169 | #endif 170 | 171 | /* 172 | ***************************************************************************************** 173 | * GaitPace 174 | ***************************************************************************************** 175 | */ 176 | class GaitPace : public Gait { 177 | private: 178 | const float _offsets[BODY_NUM_LEGS] = { 0f, 0f, 1.0f, 1.0f }; 179 | // leg order: RF, RH, LH, LF 180 | // sequence : RF, RH -> LF, LH 181 | 182 | public: 183 | GaitPace(float freq) : Gait(1.0f, 1.0f, freq) { 184 | setSwingOffsets(_offsets); 185 | setComp(true); 186 | } 187 | 188 | String getName(void) { return "Pace"; } 189 | }; 190 | 191 | 192 | /* 193 | ***************************************************************************************** 194 | * GaitTrot 195 | ***************************************************************************************** 196 | */ 197 | class GaitTrot : public Gait { 198 | private: 199 | const float _offsets[BODY_NUM_LEGS] = { 0, 1, 0, 1 }; 200 | // leg order: RF, RH, LH, LF 201 | // sequence : RF, LH -> RH, LF 202 | 203 | public: 204 | GaitTrot(float freq) : Gait(1.0f, 1.0f, freq) { 205 | setSwingOffsets(_offsets); 206 | } 207 | virtual ~GaitTrot() {} 208 | 209 | String getName(void) { return "Trot"; } 210 | }; 211 | 212 | /* 213 | ***************************************************************************************** 214 | * GaitLateral 215 | ***************************************************************************************** 216 | */ 217 | class GaitLateral : public Gait { 218 | private: 219 | const float _offsets[BODY_NUM_LEGS] = { 0.0f, 1.5f, 0.5f, 1.0f }; 220 | // leg order: RF, RH, LH, LF 221 | // sequence : RF -> LH -> LF -> RH 222 | 223 | public: 224 | GaitLateral(float freq) : Gait(0.5f, 1.5f, freq) { 225 | setSwingOffsets(_offsets); 226 | setComp(true); 227 | } 228 | 229 | String getName(void) { return "Lateral"; } 230 | }; 231 | 232 | /* 233 | ***************************************************************************************** 234 | * GaitDiagonal 235 | ***************************************************************************************** 236 | */ 237 | class GaitDiagonal : public Gait { 238 | private: 239 | const float _offsets[BODY_NUM_LEGS] = { 0.0f, 0.5f, 1.5f, 1.0f }; 240 | // leg order: RF, RH, LH, LF 241 | // sequence : RF -> RH -> LF -> LH 242 | 243 | public: 244 | GaitDiagonal(float freq) : Gait(0.5f, 1.5f, freq) { 245 | setSwingOffsets(_offsets); 246 | setComp(true); 247 | } 248 | 249 | String getName(void) { return "Diagonal"; } 250 | }; 251 | 252 | /* 253 | ***************************************************************************************** 254 | * CLASS 255 | ***************************************************************************************** 256 | */ 257 | #define GAIT_CTR 3 258 | 259 | class GaitMan { 260 | private: 261 | 262 | public: 263 | GaitMan() { 264 | _pGaits[0] = new GaitTrot(HW_SERVO_UPDATE_FREQ); 265 | _pGaits[1] = new GaitLateral(HW_SERVO_UPDATE_FREQ); 266 | _pGaits[2] = new GaitDiagonal(HW_SERVO_UPDATE_FREQ); 267 | } 268 | 269 | Gait *get(int idx) { 270 | if (idx < GAIT_CTR) 271 | return _pGaits[idx]; 272 | return NULL; 273 | } 274 | 275 | int getGaitCnt() { return GAIT_CTR; } 276 | 277 | private: 278 | Gait *_pGaits[GAIT_CTR]; 279 | }; 280 | 281 | #endif 282 | -------------------------------------------------------------------------------- /processing/gait_pattern/Gait.pde: -------------------------------------------------------------------------------- 1 | public abstract class Gait { 2 | public final static float kUNIT_MM = 1f; 3 | public final static float kPRECISION = 0.1f; 4 | 5 | protected float _fFreq; 6 | protected GaitParam _paramLegs[] = new GaitParam[4]; 7 | protected float _fSwingAmplitude; 8 | protected int _iSwingLeg; 9 | protected boolean _isComp; 10 | protected Vector _vecStep; 11 | protected float _stepsPerSec; 12 | 13 | // Leg order : RF, RH, LH, LF 14 | boolean IS_FRONT_LEG(int leg) { 15 | return (leg == 0 || leg == 3); 16 | } 17 | 18 | boolean IS_RIGHT_LEG(int leg) { 19 | return (leg < 2); 20 | } 21 | 22 | public Gait(float fSwingMult, float fStanceMult, float freq) { 23 | _fFreq = freq; 24 | _isComp = false; 25 | _iSwingLeg = -1; 26 | 27 | _vecStep = new Vector(20, 20, 15); 28 | _stepsPerSec = 1f; 29 | for (int i = 0; i < _paramLegs.length; i++) { 30 | _paramLegs[i] = new GaitParam(fSwingMult, fStanceMult); 31 | //_paramLegs[i].setMult(fSwingMult, fStanceMult); 32 | } 33 | } 34 | 35 | public void setStep(Vector vecStep) { _vecStep = vecStep; } 36 | public Vector getStep() { return _vecStep; } 37 | public void setStepsPerSec(float steps) { _stepsPerSec = steps; } 38 | public float getStepsPerSec() { return _stepsPerSec; } 39 | 40 | protected void setSwingOffsets(float[] offsets) { 41 | for (int i = 0; i < _paramLegs.length; i++) { 42 | _paramLegs[i].setSwingOffset(offsets[i]); 43 | } 44 | } 45 | 46 | protected Vector calcMoveRatio(Vector mov) { 47 | float maxV = max(abs(mov.x), abs(mov.y)); 48 | return new Vector(mov.x / maxV, mov.y / maxV); 49 | } 50 | 51 | public Vector doStep(int leg, Vector mov, Rotator rot) { 52 | if (abs(mov.x) == kPRECISION && abs(mov.y) == kPRECISION) { 53 | _paramLegs[leg].reset(); 54 | return new Vector(0.0f, 0.0f, mov.z); 55 | } 56 | 57 | _paramLegs[leg].tick(mov, _vecStep, _fFreq, _stepsPerSec); 58 | Vector r = calcMoveRatio(mov); 59 | Vector c = new Vector(r.x * _paramLegs[leg].getAmplitude(), r.y * _paramLegs[leg].getAmplitude(), mov.z); 60 | 61 | if (_paramLegs[leg].isSwingState()) { 62 | float w0 = _vecStep.x * Gait.kUNIT_MM / 2 * mov.x; 63 | float l0 = _vecStep.y * Gait.kUNIT_MM * 4 * mov.y; 64 | float h0 = _vecStep.z * Gait.kUNIT_MM; 65 | 66 | _iSwingLeg = leg; 67 | _fSwingAmplitude = sqrt(abs((1 - sq(c.x / w0) - sq(c.y / l0)) * sq(h0))); 68 | c.z = c.z - _fSwingAmplitude; 69 | } else { 70 | if (_isComp && _iSwingLeg >= 0) { 71 | float pct = abs(_fSwingAmplitude / _vecStep.z); 72 | float roll = (IS_RIGHT_LEG(_iSwingLeg) ? -pct : pct) * 2.0f; 73 | float pitch = (IS_FRONT_LEG(_iSwingLeg) ? -pct : pct) * 2.0f; 74 | 75 | rot.set(rot.yaw, pitch, roll); 76 | } 77 | c.set(-c.x, -c.y, c.z); 78 | } 79 | 80 | print(String.format("tick:%3d, leg:%d, amplitude:%6.1f, swing:%d, (%6.1f, %6.1f)\n", _paramLegs[leg].getTick(), 81 | leg, _paramLegs[leg].getAmplitude(), int(_paramLegs[leg].isSwingState()), c.x, c.z)); 82 | 83 | return new Vector(c.x / Gait.kUNIT_MM, c.y / Gait.kUNIT_MM, c.z / Gait.kUNIT_MM); 84 | } 85 | 86 | abstract public String getName(); 87 | }; 88 | 89 | /* 90 | *************************************************************************************************** 91 | * GaitParam 92 | *************************************************************************************************** 93 | */ 94 | public class GaitParam { 95 | private float _fAmplitude; 96 | private short _tick; 97 | 98 | private boolean _isSwing; 99 | private float _fCurMult; 100 | private float _fSwingMult; 101 | private float _fStanceMult; 102 | private float _fSwingOffset; 103 | 104 | 105 | public GaitParam(float fSwingMult, float fStanceMult) { 106 | _tick = 0; 107 | _fSwingMult = fSwingMult; 108 | _fStanceMult = fStanceMult; 109 | setSwingState(false); 110 | } 111 | 112 | public void setSwingOffset(float fSwingOffset) { 113 | _fSwingOffset = fSwingOffset; 114 | 115 | if (fSwingOffset == 0) { 116 | setSwingState(true); 117 | } else { 118 | _fCurMult = fSwingOffset; 119 | _isSwing = false; 120 | } 121 | } 122 | 123 | public void setSwingState(boolean en) { 124 | _isSwing = en; 125 | if (en) { 126 | _fCurMult = _fSwingMult; 127 | } else { 128 | _fCurMult = _fStanceMult; 129 | } 130 | } 131 | 132 | protected void tick(Vector mov, Vector step, float freq, float stepsPerSec) { 133 | float full = round(freq * _fCurMult / stepsPerSec); 134 | float w0 = step.x * Gait.kUNIT_MM / (2 / max(abs(mov.x), abs(mov.y))); 135 | float a0 = (w0 * 2) * (float(_tick) / full) - w0; 136 | 137 | _tick++; 138 | _fAmplitude = a0; 139 | if (_tick > full) { 140 | setSwingState(!isSwingState()); 141 | _fAmplitude = -w0; 142 | _tick = 1; 143 | } 144 | } 145 | 146 | void reset() { 147 | _tick = 0; 148 | setSwingOffset(_fSwingOffset); 149 | } 150 | 151 | void setMult(float fSwingMult, float fStanceMult) { 152 | _fSwingMult = fSwingMult; 153 | _fStanceMult = fStanceMult; 154 | } 155 | 156 | public boolean isSwingState() { return _isSwing; } 157 | public float getAmplitude() { return _fAmplitude; } 158 | public int getTick() { return _tick; } 159 | } 160 | 161 | /* 162 | *************************************************************************************************** 163 | * GaitPace 164 | *************************************************************************************************** 165 | */ 166 | public class GaitPace extends Gait { 167 | public GaitPace(float freq) { 168 | super(1.0f, 1.0f, freq); // leg order: RF, RH, LH, LF 169 | setSwingOffsets(new float[] { 0, 0, 1, 1}); // sequence : RF, RH -> LF, LH 170 | } 171 | 172 | public String getName() { return "Trot"; } 173 | }; 174 | 175 | 176 | /* 177 | *************************************************************************************************** 178 | * GaitTrot 179 | *************************************************************************************************** 180 | */ 181 | public class GaitTrot extends Gait { 182 | public GaitTrot(float freq) { 183 | super(1.0f, 1.0f, freq); // leg order: RF, RH, LH, LF 184 | setSwingOffsets(new float[] { 0, 1, 0, 1}); // sequence : RF, LH -> RH, LF 185 | } 186 | 187 | public String getName() { return "Trot"; } 188 | }; 189 | 190 | /* 191 | *************************************************************************************************** 192 | * GaitLateral 193 | *************************************************************************************************** 194 | */ 195 | public class GaitLateral extends Gait { 196 | public GaitLateral(float freq) { 197 | super(0.5f, 1.5f, freq); // leg order: RF, RH, LH, LF 198 | setSwingOffsets(new float[] { 0.0f, 1.5f, 0.5f, 1.0f }); // sequence : RF -> LH -> LF -> RH 199 | } 200 | 201 | public String getName() { return "Lateral"; } 202 | }; 203 | 204 | /* 205 | *************************************************************************************************** 206 | * GaitDiagonal 207 | *************************************************************************************************** 208 | */ 209 | public class GaitDiagonal extends Gait { 210 | public GaitDiagonal(float freq) { 211 | super(0.5f, 1.5f, freq); // leg order: RF, RH, LH, LF //<>// 212 | setSwingOffsets(new float[] { 0.0f, 0.5f, 1.5f, 1.0f }); // sequence : RF -> RH -> LF -> LH 213 | } 214 | 215 | public String getName() { return "Diagonal"; } 216 | }; 217 | 218 | 219 | /* 220 | *************************************************************************************************** 221 | * GaitTripod - hexapod 222 | *************************************************************************************************** 223 | */ 224 | public class GaitTripod extends Gait { 225 | public GaitTripod(float freq) { 226 | super(1.0f, 1.0f, freq); //<>// 227 | setSwingOffsets(new float[] { 0, 1, 0, 1, 0, 1}); 228 | } 229 | 230 | public String getName() { return "Tripod"; } 231 | }; 232 | 233 | 234 | /* 235 | *************************************************************************************************** 236 | * GaitWave - hexapod 237 | *************************************************************************************************** 238 | */ 239 | public class GaitWave extends Gait { 240 | public GaitWave(float freq) { 241 | super(5.0f, 1.0f, freq); 242 | setSwingOffsets(new float[] { 2, 1, 0, 3, 4, 5}); 243 | } 244 | 245 | public String getName() { return "Wave"; } 246 | 247 | }; 248 | 249 | 250 | /* 251 | *************************************************************************************************** 252 | * GaitRipple - hexapod 253 | *************************************************************************************************** 254 | */ 255 | public class GaitRipple extends Gait { 256 | public GaitRipple(float freq) { 257 | super(2.0f, 1.0f, freq); 258 | // 0 1 2 3 4 5 259 | setSwingOffsets(new float[] { 2, 0, 1, 2, 1, 0}); 260 | } 261 | 262 | public String getName() { return "Wave"; } 263 | 264 | }; 265 | -------------------------------------------------------------------------------- /processing/simulation/Gait.pde: -------------------------------------------------------------------------------- 1 | public abstract class Gait { //<>// 2 | public final static float kUNIT_MM = 1f; 3 | public final static float kPRECISION = 0.1f; 4 | 5 | protected float _fFreq; 6 | protected GaitParam _paramLegs[] = new GaitParam[4]; 7 | protected float _fSwingAmplitude; 8 | protected int _iSwingLeg; 9 | protected boolean _isComp; 10 | protected Vector _vecStep; 11 | protected float _stepsPerSec; 12 | 13 | // Leg order : RF, RH, LH, LF 14 | boolean IS_FRONT_LEG(int leg) { 15 | return (leg == 0 || leg == 3); 16 | } 17 | 18 | boolean IS_RIGHT_LEG(int leg) { 19 | return (leg < 2); 20 | } 21 | 22 | public Gait(float fSwingMult, float fStanceMult, float freq) { 23 | _fFreq = freq; 24 | _isComp = false; 25 | _iSwingLeg = -1; 26 | 27 | _vecStep = new Vector(20, 20, 20); 28 | _stepsPerSec = 1f; 29 | for (int i = 0; i < _paramLegs.length; i++) { 30 | _paramLegs[i] = new GaitParam(fSwingMult, fStanceMult); 31 | //_paramLegs[i].setMult(fSwingMult, fStanceMult); 32 | } 33 | } 34 | 35 | public void setStep(Vector vecStep) { _vecStep = vecStep; } 36 | public Vector getStep() { return _vecStep; } 37 | public void setStepsPerSec(float steps) { _stepsPerSec = steps; } 38 | public float getStepsPerSec() { return _stepsPerSec; } 39 | 40 | public boolean isSwingState(int leg) { 41 | return _paramLegs[leg].isSwingState(); 42 | } 43 | 44 | protected void setSwingOffsets(float[] offsets) { 45 | for (int i = 0; i < _paramLegs.length; i++) { 46 | _paramLegs[i].setSwingOffset(offsets[i]); 47 | } 48 | } 49 | 50 | protected Vector calcMoveRatio(Vector mov) { 51 | float maxV = max(abs(mov.x), abs(mov.y)); 52 | return new Vector(mov.x / maxV, mov.y / maxV); 53 | } 54 | 55 | public Vector doStep(int leg, Vector mov, Rotator rot) { 56 | if (abs(mov.x) <= kPRECISION && abs(mov.y) <= kPRECISION) { 57 | _paramLegs[leg].reset(); 58 | return new Vector(0.0f, 0.0f, mov.z); 59 | } 60 | 61 | _paramLegs[leg].tick(mov, _vecStep, _fFreq, _stepsPerSec); 62 | Vector r = calcMoveRatio(mov); 63 | Vector c = new Vector(r.x * _paramLegs[leg].getAmplitude(), r.y * _paramLegs[leg].getAmplitude(), mov.z); 64 | 65 | if (_paramLegs[leg].isSwingState()) { 66 | float w0 = _vecStep.x * Gait.kUNIT_MM / 2 * mov.x; 67 | float l0 = _vecStep.y * Gait.kUNIT_MM * 4 * mov.y; 68 | float h0 = _vecStep.z * Gait.kUNIT_MM; 69 | 70 | _iSwingLeg = leg; 71 | _fSwingAmplitude = sqrt(abs((1 - sq(c.x / w0) - sq(c.y / l0)) * sq(h0))); 72 | c.z = c.z - _fSwingAmplitude; 73 | } else { 74 | c.x = -c.x; 75 | c.y = -c.y; 76 | } 77 | 78 | // rotation with yaw 79 | float theta = radians(rot.yaw); 80 | theta = (leg == 1 || leg == 3) ? -theta : theta; 81 | float sintheta = sin(theta); 82 | float costheta = cos(theta); 83 | c.set(c.x * costheta - c.y * sintheta, c.x * sintheta + c.y * costheta, c.z); 84 | 85 | print(String.format("tick:%3d, leg:%d, amplitude:%6.1f, swing:%d, (%6.1f, %6.1f)\n", _paramLegs[leg].getTick(), 86 | leg, _paramLegs[leg].getAmplitude(), int(_paramLegs[leg].isSwingState()), c.x, c.z)); 87 | 88 | return new Vector(c.x / Gait.kUNIT_MM, c.y / Gait.kUNIT_MM, c.z / Gait.kUNIT_MM); 89 | } 90 | 91 | abstract public String getName(); 92 | }; 93 | 94 | /* 95 | *************************************************************************************************** 96 | * GaitParam 97 | *************************************************************************************************** 98 | */ 99 | public class GaitParam { 100 | private float _fAmplitude; 101 | private short _tick; 102 | 103 | private boolean _isSwing; 104 | private float _fCurMult; 105 | private float _fSwingMult; 106 | private float _fStanceMult; 107 | private float _fSwingOffset; 108 | 109 | 110 | public GaitParam(float fSwingMult, float fStanceMult) { 111 | _tick = 0; 112 | _fSwingMult = fSwingMult; 113 | _fStanceMult = fStanceMult; 114 | setSwingState(false); 115 | } 116 | 117 | public void setSwingOffset(float fSwingOffset) { 118 | _fSwingOffset = fSwingOffset; 119 | 120 | if (fSwingOffset == 0) { 121 | setSwingState(true); 122 | } else { 123 | _fCurMult = fSwingOffset; 124 | _isSwing = false; 125 | } 126 | } 127 | 128 | public void setSwingState(boolean en) { 129 | _isSwing = en; 130 | if (en) { 131 | _fCurMult = _fSwingMult; 132 | } else { 133 | _fCurMult = _fStanceMult; 134 | } 135 | } 136 | 137 | protected void tick(Vector mov, Vector step, float freq, float stepsPerSec) { 138 | float full = round(freq * _fCurMult / stepsPerSec); 139 | float w0 = step.x * Gait.kUNIT_MM / (2 / max(abs(mov.x), abs(mov.y))); 140 | float a0 = (w0 * 2) * (float(_tick) / full) - w0; 141 | 142 | _tick++; 143 | _fAmplitude = a0; 144 | if (_tick > full) { 145 | setSwingState(!isSwingState()); 146 | _fAmplitude = -w0; 147 | _tick = 1; 148 | } 149 | } 150 | 151 | void reset() { 152 | _tick = 0; 153 | setSwingOffset(_fSwingOffset); 154 | } 155 | 156 | void setMult(float fSwingMult, float fStanceMult) { 157 | _fSwingMult = fSwingMult; 158 | _fStanceMult = fStanceMult; 159 | } 160 | 161 | public boolean isSwingState() { return _isSwing; } 162 | public float getAmplitude() { return _fAmplitude; } 163 | public int getTick() { return _tick; } 164 | } 165 | 166 | /* 167 | *************************************************************************************************** 168 | * GaitPace 169 | *************************************************************************************************** 170 | */ 171 | public class GaitPace extends Gait { 172 | public GaitPace(float freq) { 173 | super(1.0f, 1.0f, freq); // leg order: RF, RH, LH, LF 174 | setSwingOffsets(new float[] { 0, 0, 1, 1}); // sequence : RF, RH -> LF, LH 175 | } 176 | 177 | public String getName() { return "Trot"; } 178 | }; 179 | 180 | 181 | /* 182 | *************************************************************************************************** 183 | * GaitTrot 184 | *************************************************************************************************** 185 | */ 186 | public class GaitTrot extends Gait { 187 | public GaitTrot(float freq) { 188 | super(1.0f, 1.0f, freq); // leg order: RF, RH, LH, LF 189 | setSwingOffsets(new float[] { 0, 1, 0, 1}); // sequence : RF, LH -> RH, LF 190 | } 191 | 192 | public String getName() { return "Trot"; } 193 | }; 194 | 195 | /* 196 | *************************************************************************************************** 197 | * GaitLateral 198 | *************************************************************************************************** 199 | */ 200 | public class GaitLateral extends Gait { 201 | public GaitLateral(float freq) { 202 | super(0.5f, 1.5f, freq); // leg order: RF, RH, LH, LF 203 | setSwingOffsets(new float[] { 0.0f, 1.5f, 0.5f, 1.0f }); // sequence : RF -> LH -> LF -> RH 204 | } 205 | 206 | public String getName() { return "Lateral"; } 207 | }; 208 | 209 | /* 210 | *************************************************************************************************** 211 | * GaitDiagonal 212 | *************************************************************************************************** 213 | */ 214 | public class GaitDiagonal extends Gait { 215 | public GaitDiagonal(float freq) { 216 | super(0.5f, 1.5f, freq); // leg order: RF, RH, LH, LF 217 | setSwingOffsets(new float[] { 0.0f, 0.5f, 1.5f, 1.0f }); // sequence : RF -> RH -> LF -> LH 218 | } 219 | 220 | public String getName() { return "Diagonal"; } 221 | }; 222 | 223 | 224 | /* 225 | *************************************************************************************************** 226 | * GaitTripod - hexapod 227 | *************************************************************************************************** 228 | */ 229 | public class GaitTripod extends Gait { 230 | public GaitTripod(float freq) { 231 | super(1.0f, 1.0f, freq); //<>// 232 | setSwingOffsets(new float[] { 0, 1, 0, 1, 0, 1}); 233 | } 234 | 235 | public String getName() { return "Tripod"; } 236 | }; 237 | 238 | 239 | /* 240 | *************************************************************************************************** 241 | * GaitWave - hexapod 242 | *************************************************************************************************** 243 | */ 244 | public class GaitWave extends Gait { 245 | public GaitWave(float freq) { 246 | super(5.0f, 1.0f, freq); 247 | setSwingOffsets(new float[] { 2, 1, 0, 3, 4, 5}); 248 | } 249 | 250 | public String getName() { return "Wave"; } 251 | 252 | }; 253 | 254 | 255 | /* 256 | *************************************************************************************************** 257 | * GaitRipple - hexapod 258 | *************************************************************************************************** 259 | */ 260 | public class GaitRipple extends Gait { 261 | public GaitRipple(float freq) { 262 | super(2.0f, 1.0f, freq); 263 | // 0 1 2 3 4 5 264 | setSwingOffsets(new float[] { 2, 0, 1, 2, 1, 0}); 265 | } 266 | 267 | public String getName() { return "Wave"; } 268 | 269 | }; 270 | -------------------------------------------------------------------------------- /quadruped_code/src/FSBrowser.h: -------------------------------------------------------------------------------- 1 | #ifndef _FS_BROWSER_H_ 2 | #define _FS_BROWSER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "config.h" 9 | #include "utils.h" 10 | 11 | #define FILESYSTEM SPIFFS 12 | #define FORMAT_FILESYSTEM false // You only need to format the filesystem once 13 | 14 | #if FILESYSTEM == FFat 15 | #include 16 | #endif 17 | 18 | #if FILESYSTEM == SPIFFS 19 | #include 20 | #endif 21 | 22 | class FSBrowser; 23 | 24 | static FSBrowser *_pThis; 25 | 26 | class FSBrowser { 27 | private: 28 | WebServer _server; 29 | File _fsUploadFile; //holds the current upload 30 | String _hostname; 31 | 32 | public: 33 | //format bytes 34 | String formatBytes(size_t bytes) { 35 | if (bytes < 1024) { 36 | return String(bytes) + "B"; 37 | } else if (bytes < (1024 * 1024)) { 38 | return String(bytes / 1024.0) + "KB"; 39 | } else if (bytes < (1024 * 1024 * 1024)) { 40 | return String(bytes / 1024.0 / 1024.0) + "MB"; 41 | } else { 42 | return String(bytes / 1024.0 / 1024.0 / 1024.0) + "GB"; 43 | } 44 | } 45 | 46 | String getContentType(String filename) { 47 | if (_server.hasArg("download")) { 48 | return "application/octet-stream"; 49 | } else if (filename.endsWith(".htm")) { 50 | return "text/html"; 51 | } else if (filename.endsWith(".html")) { 52 | return "text/html"; 53 | } else if (filename.endsWith(".css")) { 54 | return "text/css"; 55 | } else if (filename.endsWith(".js")) { 56 | return "application/javascript"; 57 | } else if (filename.endsWith(".png")) { 58 | return "image/png"; 59 | } else if (filename.endsWith(".gif")) { 60 | return "image/gif"; 61 | } else if (filename.endsWith(".jpg")) { 62 | return "image/jpeg"; 63 | } else if (filename.endsWith(".ico")) { 64 | return "image/x-icon"; 65 | } else if (filename.endsWith(".xml")) { 66 | return "text/xml"; 67 | } else if (filename.endsWith(".pdf")) { 68 | return "application/x-pdf"; 69 | } else if (filename.endsWith(".zip")) { 70 | return "application/x-zip"; 71 | } else if (filename.endsWith(".gz")) { 72 | return "application/x-gzip"; 73 | } 74 | return "text/plain"; 75 | } 76 | 77 | bool exists(String path) { 78 | bool yes = false; 79 | File file = FILESYSTEM.open(path, "r"); 80 | if (!file.isDirectory()) { 81 | yes = true; 82 | } 83 | file.close(); 84 | return yes; 85 | } 86 | 87 | bool handleFileRead(String path) { 88 | LOG("handleFileRead: %s\n", path.c_str()); 89 | if (path.endsWith("/")) { 90 | path += "index.html"; 91 | } 92 | String contentType = getContentType(path); 93 | String pathWithGz = path + ".gz"; 94 | if (exists(pathWithGz) || exists(path)) { 95 | if (exists(pathWithGz)) { 96 | path += ".gz"; 97 | } 98 | File file = FILESYSTEM.open(path, "r"); 99 | _server.streamFile(file, contentType); 100 | file.close(); 101 | return true; 102 | } 103 | return false; 104 | } 105 | 106 | void handleFileUpload() { 107 | if (_server.uri() != "/edit") { 108 | return; 109 | } 110 | HTTPUpload& upload = _server.upload(); 111 | if (upload.status == UPLOAD_FILE_START) { 112 | String filename = upload.filename; 113 | if (!filename.startsWith("/")) { 114 | filename = "/" + filename; 115 | } 116 | LOG("handleFileUpload Name: %s\n", filename.c_str()); 117 | _fsUploadFile = FILESYSTEM.open(filename, "w"); 118 | filename = String(); 119 | } else if (upload.status == UPLOAD_FILE_WRITE) { 120 | //LOG("handleFileUpload Data: %ld\n", upload.currentSize); 121 | if (_fsUploadFile) { 122 | _fsUploadFile.write(upload.buf, upload.currentSize); 123 | } 124 | } else if (upload.status == UPLOAD_FILE_END) { 125 | if (_fsUploadFile) { 126 | _fsUploadFile.close(); 127 | } 128 | LOG("handleFileUpload Size: :%d\n", upload.totalSize); 129 | } 130 | } 131 | 132 | void handleFileDelete() { 133 | if (_server.args() == 0) { 134 | return _server.send(500, "text/plain", "BAD ARGS"); 135 | } 136 | String path = _server.arg(0); 137 | LOG("handleFileDelete: %s\n", path.c_str()); 138 | if (path == "/") { 139 | return _server.send(500, "text/plain", "BAD PATH"); 140 | } 141 | if (!exists(path)) { 142 | return _server.send(404, "text/plain", "FileNotFound"); 143 | } 144 | FILESYSTEM.remove(path); 145 | _server.send(200, "text/plain", ""); 146 | path = String(); 147 | } 148 | 149 | void handleFileCreate() { 150 | if (_server.args() == 0) { 151 | return _server.send(500, "text/plain", "BAD ARGS"); 152 | } 153 | String path = _server.arg(0); 154 | LOG("handleFileCreate: %s\n", path.c_str()); 155 | if (path == "/") { 156 | return _server.send(500, "text/plain", "BAD PATH"); 157 | } 158 | if (exists(path)) { 159 | return _server.send(500, "text/plain", "FILE EXISTS"); 160 | } 161 | File file = FILESYSTEM.open(path, "w"); 162 | if (file) { 163 | file.close(); 164 | } else { 165 | return _server.send(500, "text/plain", "CREATE FAILED"); 166 | } 167 | _server.send(200, "text/plain", ""); 168 | path = String(); 169 | } 170 | 171 | void handleFileList() { 172 | if (!_server.hasArg("dir")) { 173 | _server.send(500, "text/plain", "BAD ARGS"); 174 | return; 175 | } 176 | 177 | String path = _server.arg("dir"); 178 | LOG("handleFileList: %s\n", path.c_str()); 179 | 180 | File root = FILESYSTEM.open(path); 181 | path = String(); 182 | 183 | String output = "["; 184 | if (root.isDirectory()) { 185 | File file = root.openNextFile(); 186 | while (file) { 187 | if (output != "[") { 188 | output += ','; 189 | } 190 | output += "{\"type\":\""; 191 | output += (file.isDirectory()) ? "dir" : "file"; 192 | output += "\",\"name\":\""; 193 | output += String(file.name()).substring(1); 194 | output += "\"}"; 195 | file = root.openNextFile(); 196 | } 197 | } 198 | output += "]"; 199 | _server.send(200, "text/json", output); 200 | } 201 | 202 | FSBrowser(String hostname) { 203 | _hostname = hostname; 204 | _pThis = this; 205 | } 206 | 207 | void setup(char *ssid, char *password) { 208 | //if (FORMAT_FILESYSTEM) FILESYSTEM.format(); 209 | FILESYSTEM.begin(); 210 | { 211 | File root = FILESYSTEM.open("/"); 212 | File file = root.openNextFile(); 213 | while (file) { 214 | String fileName = file.name(); 215 | size_t fileSize = file.size(); 216 | LOG("FS File: %s, size: %s\n", fileName.c_str(), formatBytes(fileSize).c_str()); 217 | file = root.openNextFile(); 218 | } 219 | LOG("\n"); 220 | } 221 | 222 | //WIFI INIT 223 | LOG("Connecting to %s\n", ssid); 224 | if (String(WiFi.SSID()) != String(ssid)) { 225 | WiFi.mode(WIFI_STA); 226 | WiFi.setHostname(_hostname.c_str()); 227 | WiFi.begin(ssid, password); 228 | } 229 | 230 | while (WiFi.status() != WL_CONNECTED) { 231 | delay(500); 232 | LOG("."); 233 | } 234 | LOG("\nConnected! IP address: %s\n", WiFi.localIP().toString().c_str()); 235 | 236 | MDNS.begin(_hostname.c_str()); 237 | MDNS.addService("http", "tcp", 80); 238 | LOG("Open http://%s.local/edit to see the file browser\n", _hostname.c_str()); 239 | 240 | //SERVER INIT 241 | //list directory 242 | _server.on("/list", HTTP_GET, []() { 243 | _pThis->handleFileList(); 244 | }); 245 | 246 | //load editor 247 | _server.on("/edit", HTTP_GET, []() { 248 | if (!_pThis->handleFileRead("/edit.html")) { 249 | _pThis->_server.send(404, "text/plain", "FileNotFound"); 250 | } 251 | }); 252 | 253 | //create file 254 | _server.on("/edit", HTTP_PUT, []() { 255 | _pThis->handleFileCreate(); 256 | }); 257 | 258 | //delete file 259 | _server.on("/edit", HTTP_DELETE, []() { 260 | _pThis->handleFileDelete(); 261 | }); 262 | //first callback is called after the request has ended with all parsed arguments 263 | //second callback handles file uploads at that location 264 | 265 | _server.on("/edit", HTTP_POST, []() { 266 | _pThis->_server.send(200, "text/plain", ""); 267 | }, []() { 268 | _pThis->handleFileUpload(); 269 | }); 270 | 271 | //called when the url is not defined here 272 | //use it to load content from FILESYSTEM 273 | _server.onNotFound([]() { 274 | if (!_pThis->handleFileRead(_pThis->_server.uri())) { 275 | _pThis->_server.send(404, "text/plain", "FileNotFound"); 276 | } 277 | }); 278 | 279 | //get heap status, analog input value and all GPIO statuses in one json call 280 | _server.on("/all", HTTP_GET, []() { 281 | String json = "{"; 282 | json += "\"heap\":" + String(ESP.getFreeHeap()); 283 | json += ", \"analog\":" + String(analogRead(A0)); 284 | json += ", \"gpio\":" + String((uint32_t)(0)); 285 | json += "}"; 286 | _pThis->_server.send(200, "text/json", json); 287 | json = String(); 288 | }); 289 | 290 | _server.begin(); 291 | LOG("HTTP server started !!\n"); 292 | } 293 | 294 | void loop(void) { 295 | _server.handleClient(); 296 | } 297 | 298 | void close(void) { 299 | MDNS.end(); 300 | _server.close(); 301 | WiFi.disconnect(true, true); 302 | LOG("HTTP server closed !!\n"); 303 | } 304 | }; 305 | 306 | #endif -------------------------------------------------------------------------------- /processing/simulation/QuadRuped.pde: -------------------------------------------------------------------------------- 1 | final color kCOLOR_BONE = color(40, 200, 110); 2 | final color kCOLOR_JOINT = color(255, 255, 255); 3 | final color kCOLOR_JOINT2 = color(255, 255, 0); 4 | final color kCOLOR_BODY = color(50, 150, 220); 5 | final color kCOLOR_HEAD = color(255, 0, 0); 6 | final float kLEG_WIDTH = 5.0f; 7 | 8 | 9 | /* 10 | Z TOP VIEW 11 | | -Y 12 | | 13 | --------- 14 | | 2 3 | H 15 | -X -------| + |-------- +X E 16 | | 1 0 | A 17 | --------- D 18 | | 19 | | 20 | | +Y 21 | */ 22 | 23 | class Leg { 24 | float _coxaLength; 25 | float _femurLength; 26 | float _tibiaLength; 27 | float _coxaOffsetZ; 28 | 29 | Vector _vOffFromCenter = new Vector(); 30 | Vector _vInitPos = new Vector(); 31 | Vector _vPos = new Vector(); 32 | Vector _vTipOffset = new Vector(-30, 0, 0); 33 | 34 | float _angleCoxa; 35 | float _angleFemur; 36 | float _angleTibia; 37 | 38 | int _pos; 39 | boolean _isDebug; 40 | 41 | float fixAngle(float angle) { 42 | if (angle < -180) { 43 | angle += 360; 44 | } else if (angle > 180) { 45 | angle -= 360; 46 | } 47 | return angle; 48 | } 49 | 50 | float roundUp(float v) { 51 | return round(v * 100.0f) / 100.0f; 52 | } 53 | 54 | void enableDebug(boolean en) { 55 | _isDebug = en; 56 | } 57 | 58 | boolean isDebugEnabled() { 59 | return _isDebug; 60 | } 61 | 62 | Leg(int pos, float bodyWidth, float bodyHeight, float coxaLength, float coxaOffsetZ, float femurLength, float tibiaLength) { 63 | _pos = pos; 64 | _isDebug = false; 65 | _coxaLength = coxaLength; 66 | _femurLength = femurLength; 67 | _tibiaLength = tibiaLength; 68 | _coxaOffsetZ = coxaOffsetZ; 69 | 70 | int signY; 71 | int signX; 72 | float offsetX; 73 | float offsetY; 74 | 75 | offsetX = bodyHeight / 2; 76 | offsetY = bodyWidth / 2; 77 | signY = IS_RIGHT_LEG(pos) ? 1 : -1; 78 | signX = IS_FRONT_LEG(pos) ? 1 : -1; 79 | 80 | _vOffFromCenter.x = signX * offsetX; 81 | _vOffFromCenter.y = signY * offsetY; 82 | _vOffFromCenter.z = tibiaLength; 83 | 84 | _vInitPos.x = signX * ((_tibiaLength * sin(radians(45))) - (_femurLength * sin(radians(45)))); 85 | _vInitPos.y = signY * coxaLength; 86 | _vInitPos.z = coxaOffsetZ + roundUp(sqrt(sq(_femurLength) + sq(_tibiaLength))); // angle between femur and tibia = 90 degree 87 | 88 | _vOffFromCenter.z += _vInitPos.z; 89 | 90 | _vPos.set(_vInitPos); 91 | if (true) { //_isDebug) { 92 | println(String.format("init leg:%d, off from center (%6.1f, %6.1f)", pos, 93 | _vOffFromCenter.x, _vOffFromCenter.y)); 94 | println(String.format("init leg:%d, %6.1f, %6.1f, %6.1f", pos, 95 | _vPos.x, _vPos.y, _vPos.z)); 96 | } 97 | } 98 | 99 | Vector bodyIK(Vector vMov, Rotator rRot) { 100 | float mx = vMov.x + _vTipOffset.x; 101 | float my = vMov.y + _vTipOffset.y; 102 | 103 | // body ik 104 | float totX = _vInitPos.x + _vOffFromCenter.x + mx; 105 | float totY = _vInitPos.y + _vOffFromCenter.y + my; 106 | 107 | // yaw 108 | float alpha_0 = atan2(totX, totY); 109 | float alpha_1 = alpha_0 + radians(rRot.yaw); 110 | 111 | float dist = sqrt(sq(totX) + sq(totY)); 112 | float bodyIKX = roundUp(sin(alpha_1) * dist - totX); 113 | float bodyIKY = roundUp(cos(alpha_1) * dist - totY); 114 | 115 | // pitch, roll 116 | float pitchZ = tan(radians(rRot.pitch)) * totX; 117 | float rollZ = tan(radians(rRot.roll)) * totY; 118 | float bodyIKZ = roundUp(rollZ + pitchZ); 119 | 120 | _vPos.set(_vInitPos.x + mx + bodyIKX, _vInitPos.y + my + bodyIKY, _vInitPos.z + vMov.z - bodyIKZ); 121 | 122 | if (_isDebug) { 123 | println(String.format("newpos leg:%d, %6.1f, %6.1f, %6.1f [%6.1f, %6.1f]", _pos, _vPos.x, _vPos.y, _vPos.z, 124 | bodyIKX, bodyIKY)); 125 | } 126 | 127 | return _vPos; 128 | } 129 | 130 | void legIK(Vector tgt) { 131 | float h1 = sqrt(sq(_coxaLength) + sq(_coxaOffsetZ)); 132 | float h2 = sqrt(sq(tgt.z) + sq(tgt.y)); 133 | float a0 = atan(abs(tgt.y) / tgt.z); 134 | float a1 = atan(_coxaLength / _coxaOffsetZ); 135 | float a2 = atan(_coxaOffsetZ / _coxaLength); 136 | float a3 = asin(h1 * sin(a2 + radians(90)) / h2); 137 | float a4 = radians(180) - (a3 + a2 + radians(90)); 138 | float a5 = a1 - a4; 139 | float th = a0 - a5; 140 | 141 | float r0 = h1 * sin(a4) / sin(a3); 142 | float h = sqrt(sq(r0) + sq(abs(tgt.x))); 143 | 144 | float phi= asin(tgt.x / h); 145 | float ts = acos((sq(h) + sq(_femurLength) - sq(_tibiaLength)) / (2 * h * _femurLength)) - phi; 146 | float tw = acos((sq(_tibiaLength) + sq(_femurLength) - sq(h)) / (2 * _femurLength * _tibiaLength)); 147 | 148 | if (Float.isNaN(th) || Float.isNaN(ts) || Float.isNaN(tw)) { 149 | println(String.format("ERROR leg:%d, %6.1f, %6.1f, %6.1f", _pos, th, ts, tw)); 150 | return; 151 | } 152 | 153 | float ac = degrees(th); 154 | float af = 90 - degrees(ts); 155 | float at = degrees(tw); 156 | 157 | _angleCoxa = fixAngle(ac); // zero base 158 | _angleFemur = fixAngle(af); // zero base 0 - 180 159 | _angleTibia = fixAngle(at); // zero base 0 - 180 160 | 161 | if (_isDebug) { 162 | println(String.format("angle leg:%d, %6.1f, %6.1f, %6.1f", _pos, _angleCoxa, _angleFemur, _angleTibia)); 163 | } 164 | } 165 | 166 | void move(Vector vMov, Rotator rRot) { 167 | Vector tgt = bodyIK(vMov, rRot); 168 | legIK(tgt); 169 | } 170 | 171 | private void jointX(float x, float y, float z, float a, color c) { 172 | translate(x, y, z); 173 | rotateX(radians(a)); 174 | fill(c); 175 | sphere(kLEG_WIDTH); //> draws a sphere 176 | } 177 | 178 | private void jointZ(float x, float y, float z, float a, color c) { 179 | translate(x, y, z); 180 | rotateZ(radians(a)); 181 | fill(c); 182 | sphere(kLEG_WIDTH); 183 | } 184 | 185 | private void jointY(float x, float y, float z, float a, color c) { 186 | translate(x, y, z); 187 | rotateY(radians(a)); 188 | fill(c); 189 | sphere(kLEG_WIDTH); 190 | } 191 | 192 | void draw(boolean isSwing) { 193 | pushMatrix(); 194 | 195 | // 196 | // y is inverted in the screen '-' 197 | // 198 | 199 | // hip 200 | int sign = IS_RIGHT_LEG(_pos) ? 1 : -1; 201 | jointX(_vOffFromCenter.x, _vOffFromCenter.y, _vOffFromCenter.z, _angleCoxa, 202 | (_pos == 0) ? kCOLOR_HEAD : kCOLOR_JOINT); 203 | translate(0, sign * _coxaLength / 2, 0); 204 | fill(kCOLOR_BONE); 205 | box(kLEG_WIDTH, _coxaLength, kLEG_WIDTH); 206 | 207 | // femur 208 | jointY(0, sign * _coxaLength / 2, 0, - 90 - _angleFemur, kCOLOR_JOINT); 209 | translate(0, 0, _femurLength / 2); 210 | fill(kCOLOR_BONE); 211 | box(kLEG_WIDTH, kLEG_WIDTH, _femurLength); 212 | 213 | // tibia 214 | jointY(0, 0, _femurLength / 2, _angleTibia - 180, kCOLOR_JOINT); 215 | translate(0, 0, _tibiaLength / 2); 216 | fill(kCOLOR_BONE); 217 | box(kLEG_WIDTH, kLEG_WIDTH, _tibiaLength); 218 | 219 | int col = isSwing ? kCOLOR_JOINT : kCOLOR_JOINT2; 220 | jointY(0, 0, _tibiaLength / 2, 0, col); 221 | 222 | popMatrix(); 223 | } 224 | } 225 | 226 | boolean IS_FRONT_LEG(int leg) { 227 | return (leg == 0 || leg == 3); 228 | } 229 | 230 | boolean IS_RIGHT_LEG(int leg) { 231 | return (leg < 2); 232 | } 233 | 234 | class QuadRuped { 235 | Leg _legs[]; 236 | Vector _vMov; 237 | Rotator _rRot; // x-> pitch, y->roll, z->yaw 238 | Gait _gait; 239 | boolean _isWalk; 240 | int _debugLegMask = 0x1; 241 | 242 | Vector _vMovOld; 243 | Rotator _rRotOld; // x-> pitch, y->roll, z->yaw 244 | 245 | QuadRuped(float bodyWidth, float bodyHeight, float coxaLen, float coxaOffsetZ, float femurLen, float tibiaLen) { 246 | _legs = new Leg[4]; 247 | _vMov = new Vector(0, 0, 0); 248 | _rRot = new Rotator(0, 0, 0); 249 | _vMovOld = new Vector(0, 0, 0); 250 | _rRotOld = new Rotator(0, 0, 0); 251 | 252 | int mask; 253 | for (int i = 0; i < _legs.length; i++) { 254 | mask = 1 << i; 255 | _legs[i] = new Leg(i, bodyWidth, bodyHeight, coxaLen, coxaOffsetZ, femurLen, tibiaLen); 256 | _legs[i].enableDebug((_debugLegMask & mask) == mask); 257 | _legs[i].move(_vMov, _rRot); 258 | } 259 | _gait = new GaitTrot(60); 260 | println("-----------------------"); 261 | } 262 | 263 | void draw() { 264 | // body 265 | pushMatrix(); 266 | translate(0, 0, _vMov.z); 267 | rotateY(radians(_rRot.pitch)); 268 | rotateX(radians(-_rRot.roll)); 269 | if (!_isWalk) { 270 | rotateZ(radians(_rRot.yaw)); 271 | } 272 | strokeWeight(15); 273 | stroke(kCOLOR_BODY); 274 | noFill(); 275 | beginShape(); 276 | for (int i = 0; i <= _legs.length; i++) { 277 | vertex(_legs[i % _legs.length]._vOffFromCenter.x, _legs[i % _legs.length]._vOffFromCenter.y, 278 | _legs[i % _legs.length]._vOffFromCenter.z); 279 | } 280 | endShape(); 281 | noStroke(); 282 | 283 | // legs 284 | for (int i = 0; i < _legs.length; i++) { 285 | _legs[i].draw(_gait.isSwingState(i)); 286 | } 287 | popMatrix(); 288 | } 289 | 290 | Vector getPos() { 291 | return _vMov; 292 | } 293 | 294 | Rotator getRot() { 295 | return _rRot; 296 | } 297 | 298 | void update(boolean isWalk) { 299 | //_vMov.dump("_vMov"); 300 | //_rRot.dump("_rRot"); 301 | 302 | _isWalk = isWalk; 303 | 304 | Vector move = new Vector(_vMov); 305 | Rotator rot = new Rotator(_rRot); 306 | 307 | if (isWalk) { 308 | for (int i = 0; i < 4; i++) { 309 | move.set(Gait.kPRECISION + _vMov.x, Gait.kPRECISION + _vMov.y, _vMov.z); 310 | 311 | if (_legs[i].isDebugEnabled()) { 312 | println(String.format("move leg:%d, %6.1f, %6.1f, %6.1f", i, move.x, move.y, move.z)); 313 | } 314 | 315 | rot.set(_rRot); 316 | move = _gait.doStep(i, move, rot); 317 | if (_legs[i].isDebugEnabled()) { 318 | println(String.format("movpos leg:%d, (%6.1f, %6.1f, %6.1f), (Y:%6.1f, P:%6.1f, R:%6.1f)", i, move.x, move.y, move.z, rot.yaw, rot.pitch, rot.roll)); 319 | } 320 | rot.zero(); 321 | _legs[i].move(move, rot); 322 | println("-----------------------"); 323 | } 324 | } else { 325 | if (!_vMov.equals(_vMovOld) || !_rRot.equals(_rRotOld)) { 326 | _vMov.dump("_vMov"); 327 | _rRot.dump("_rRot"); 328 | 329 | for (int i = 0; i < _legs.length; i++) { 330 | rot.set(_rRot); 331 | if (IS_RIGHT_LEG(i)) { 332 | rot.yaw = -rot.yaw; 333 | } 334 | _legs[i].move(_vMov, rot); 335 | } 336 | 337 | _vMovOld.set(_vMov); 338 | _rRotOld.set(_rRot); 339 | println("-----------------------"); 340 | } 341 | } 342 | draw(); 343 | } 344 | } 345 | -------------------------------------------------------------------------------- /quadruped_code/src/hardware.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "hardware.h" 5 | 6 | /* 7 | ***************************************************************************************** 8 | * CONSTANTS 9 | ***************************************************************************************** 10 | */ 11 | #if (CONFIG_BODY == CONFIG_BODY_SPOTMICRO) 12 | static const int kSERVO_PERIOD_MIN = 650; 13 | static const int kSERVO_PERIOD_MAX = 2350; 14 | #elif (CONFIG_BODY == CONFIG_BODY_KANGAL) 15 | //SPT5430HV-180W 16 | //max -90 0 90 min 17 | //2300 2200 1370 520 380 18 | static const int kSERVO_PERIOD_MIN = 520; 19 | static const int kSERVO_PERIOD_MAX = 2220; 20 | #endif 21 | 22 | static const int kDEG10_MIN = 0; 23 | static const int kDEG10_MAX = 1800; 24 | 25 | /* 26 | ***************************************************************************************** 27 | * MACROS & STRUCTURES 28 | ***************************************************************************************** 29 | */ 30 | /* 31 | Z TOP VIEW 32 | | 33 | | --------- 34 | | | 2 3 | 35 | | | + | head 36 | | | 1 0 | 37 | | --------- 38 | | 39 | ----+---------------------> X 40 | | 41 | | 42 | */ 43 | 44 | 45 | /* 46 | ***************************************************************************************** 47 | * VARIABLES 48 | ***************************************************************************************** 49 | */ 50 | const struct Hardware::_joint Hardware::_kJointRanges[BODY_NUM_JOINTS] = { 51 | // min, max, default offset 52 | {-900, 900, 900}, // coxa 53 | {-900, 900, 900}, // femur 54 | {-900, 900, 450} // tibia (angle - 45) 55 | }; 56 | 57 | // top view 58 | #if (CONFIG_BODY == CONFIG_BODY_SPOTMICRO) 59 | struct Hardware::_servo_cfg Hardware::_cfgServos[BODY_NUM_LEGS][BODY_NUM_JOINTS] = { 60 | // coxa femur tibia leg no. 61 | { { 12, 1, 0 }, { 13, 1, 0 }, { 14, -1, 0 } }, // right front 62 | { { 8, -1, 0 }, { 9, 1, 0 }, { 10, -1, 0 } }, // right rear 63 | { { 4, -1, 0 }, { 5, -1, 0 }, { 6, 1, 0 } }, // left rear 64 | { { 0, 1, 0 }, { 1, -1, 0 }, { 2, 1, 0 } } // left front 65 | }; 66 | #elif (CONFIG_BODY == CONFIG_BODY_KANGAL) 67 | // Please note that: The movement of the femur is effects to the tibia. 68 | // So it works dependent on it. Shortly; new tibia angle = tibia angle + femur angle 69 | struct Hardware::_servo_cfg Hardware::_cfgServos[BODY_NUM_LEGS][BODY_NUM_JOINTS] = { 70 | // coxa femur tibia leg no. 71 | { { 2, 1, 0 }, { 1, 1, 0 }, { 0, -1, 0 } }, // right front 72 | { { 12, -1, 0 }, { 14, 1, 0 }, { 13, -1, 0 } }, // right rear 73 | { { 8, -1, 0 }, { 10, -1, 0 }, { 9, 1, 0 } }, // left rear 74 | { { 6, 1, 0 }, { 5, -1, 0 }, { 4, 1, 0 } } // left front 75 | }; 76 | #endif 77 | 78 | #if CONFIG_ENABLE_CAM_PAN_TILT 79 | struct Hardware::_servo_cfg Hardware::_cfgPanTiltServos[2] = { 80 | { 7, 1, 0}, { 3, 1, 0}, 81 | // { 11, 1, 0}, { 15, 1, 0}, 82 | }; 83 | #endif 84 | 85 | /* 86 | ***************************************************************************************** 87 | * FUNCTIONS 88 | ***************************************************************************************** 89 | */ 90 | Hardware::Hardware() { 91 | memset(&_calAngles, 0, sizeof(_calAngles)); 92 | memset(&_calAnglesPanTilt, 0, sizeof(_calAnglesPanTilt)); 93 | } 94 | 95 | void Hardware::setup(void) { 96 | _servo = Adafruit_PWMServoDriver(PCA9685_I2C_ADDRESS, Wire); 97 | loadConfig(); 98 | 99 | _servo.begin(); 100 | _servo.setOscillatorFrequency(27000000); 101 | _servo.setPWMFreq(getServoFreq()); 102 | Wire.setClock(400000); 103 | 104 | setPanTilt(900, 900); 105 | } 106 | 107 | void Hardware::loadConfig(void) { 108 | File file = SPIFFS.open(FILE_SERVO_CFG); 109 | if (!file) { 110 | LOG("file not found\n"); 111 | return; 112 | } 113 | file.read((uint8_t*)&_calAngles, sizeof(_calAngles)); 114 | #if CONFIG_ENABLE_CAM_PAN_TILT 115 | file.read((uint8_t*)&_calAnglesPanTilt, sizeof(_calAnglesPanTilt)); 116 | #endif 117 | file.close(); 118 | } 119 | 120 | void Hardware::saveConfig(void) { 121 | File file = SPIFFS.open(FILE_SERVO_CFG, FILE_WRITE); 122 | if (!file) { 123 | LOG("file not found\n"); 124 | return; 125 | } 126 | file.write((uint8_t*)&_calAngles, sizeof(_calAngles)); 127 | #if CONFIG_ENABLE_CAM_PAN_TILT 128 | file.write((uint8_t*)&_calAnglesPanTilt, sizeof(_calAnglesPanTilt)); 129 | #endif 130 | file.close(); 131 | } 132 | 133 | /* 134 | ***************************************************************************************** 135 | * FUNCTIONS 136 | ***************************************************************************************** 137 | */ 138 | void Hardware::dump() { 139 | LOG("---------- leg dump ----------\n"); 140 | for (int i = 0; i < BODY_NUM_LEGS; i++) { 141 | LOG("leg:%d ", i + 1); 142 | for (int j = 0; j < 3; j++) { 143 | LOG("(%5.1f, %5d, %4d), ", _cfgServos[i][j].angle / 10.0f, _cfgServos[i][j].uS, _calAngles[i][j]); 144 | } 145 | LOG("\n"); 146 | } 147 | 148 | #if CONFIG_ENABLE_CAM_PAN_TILT 149 | for (int j = 0; j < 2; j++) { 150 | LOG("(%5.1f, %5d, %4d), ", _cfgPanTiltServos[j].angle / 10.0f, _cfgPanTiltServos[j].uS, _calAnglesPanTilt[j]); 151 | } 152 | LOG("\n"); 153 | #endif 154 | 155 | LOG("------------------------------\n"); 156 | } 157 | 158 | #if CONFIG_ENABLE_CAM_PAN_TILT 159 | void Hardware::setPanTiltInternal(int ch, int deg10) { 160 | int uS; 161 | int adjDeg; 162 | 163 | adjDeg = constrain(deg10, kDEG10_MIN, kDEG10_MAX); 164 | adjDeg = adjDeg + _calAnglesPanTilt[ch]; 165 | uS = (_cfgPanTiltServos[ch].sign == 1) ? 166 | map(adjDeg, kDEG10_MIN, kDEG10_MAX, kSERVO_PERIOD_MIN, kSERVO_PERIOD_MAX) : 167 | map(adjDeg, kDEG10_MIN, kDEG10_MAX, kSERVO_PERIOD_MAX, kSERVO_PERIOD_MIN); 168 | 169 | _cfgPanTiltServos[ch].angle = deg10; 170 | _cfgPanTiltServos[ch].uS = uS; 171 | _servo.writeMicroseconds(_cfgPanTiltServos[ch].pin, uS); 172 | } 173 | 174 | void Hardware::setPanTilt(int pan, int tilt) { 175 | setPanTiltInternal(0, pan); 176 | setPanTiltInternal(1, tilt); 177 | } 178 | #endif 179 | 180 | void Hardware::setJoint(int leg, int joint, int deg10) { 181 | int uS; 182 | int adjDeg; 183 | 184 | adjDeg = constrain(deg10, _kJointRanges[joint].min, _kJointRanges[joint].max); 185 | adjDeg = adjDeg + _kJointRanges[joint].offset + _calAngles[leg][joint]; 186 | adjDeg = constrain(adjDeg, kDEG10_MIN, kDEG10_MAX); 187 | 188 | uS = (_cfgServos[leg][joint].sign == 1) ? 189 | map(adjDeg, kDEG10_MIN, kDEG10_MAX, kSERVO_PERIOD_MIN, kSERVO_PERIOD_MAX) : 190 | map(adjDeg, kDEG10_MIN, kDEG10_MAX, kSERVO_PERIOD_MAX, kSERVO_PERIOD_MIN); 191 | 192 | //LOG("leg:%d, joint:%d, sign:%2d, angle:%5.1f => %5.1f, cal:%5.1f, uS:%5d\n", leg + 1, joint, _cfgServos[leg][joint].sign, deg10 / 10.0f, adjDeg / 10.0f, _calAngles[leg][joint] / 10.0f, uS); 193 | 194 | _cfgServos[leg][joint].angle = deg10; 195 | _cfgServos[leg][joint].uS = uS; 196 | 197 | _servo.writeMicroseconds(_cfgServos[leg][joint].pin, uS); 198 | } 199 | 200 | void Hardware::setLeg(int leg, int a10, int b10, int c10) { 201 | setJoint(leg, 0, a10); 202 | setJoint(leg, 1, b10); 203 | setJoint(leg, 2, c10); 204 | } 205 | 206 | void Hardware::calibrateLegs(int key) { 207 | static int leg = 0; 208 | static int joint = 0; 209 | 210 | switch (key) { 211 | case 'q': 212 | LOG("IDLE POSE (ALL 0 degree) !\n"); 213 | for (int i = 0; i < BODY_NUM_LEGS; i++) 214 | setLeg(i, 0, 0, 0); 215 | break; 216 | 217 | case 'w': 218 | LOG("READY POSE !\n"); 219 | for (int i = 0; i < BODY_NUM_LEGS; i++) 220 | setLeg(i, 0, -450, 0); 221 | return; 222 | 223 | #if (CONFIG_BODY == CONFIG_BODY_SPOTMICRO) 224 | case 'e': 225 | LOG("STRAIGHT POSE !\n"); 226 | for (int i = 0; i < BODY_NUM_LEGS; i++) 227 | setLeg(i, 0, 0, 900); 228 | return; 229 | #endif 230 | case 'c': 231 | LOG("calibration cleared !\n"); 232 | memset(&_calAngles, 0, sizeof(_calAngles)); 233 | return; 234 | 235 | case 's': 236 | saveConfig(); 237 | LOG("calibration saved !\n"); 238 | return; 239 | 240 | case 'l': 241 | loadConfig(); 242 | LOG("calibration loaded !\n"); 243 | 244 | case 'p': 245 | LOG("---------- cal ----------\n"); 246 | for (int i = 0; i < BODY_NUM_LEGS; i++) { 247 | LOG("leg:%d, %4d, %4d, %4d\n", i + 1, _calAngles[i][0], _calAngles[i][1], _calAngles[i][2]); 248 | } 249 | #if CONFIG_ENABLE_CAM_PAN_TILT 250 | LOG("p&t:5, %4d, %4d\n", _calAnglesPanTilt[0], _calAnglesPanTilt[1]); 251 | #endif 252 | LOG("-------------------------\n"); 253 | return; 254 | 255 | // leg selection 256 | case '1': 257 | case '2': 258 | case '3': 259 | case '4': 260 | leg = key - '1'; 261 | joint = 0; 262 | break; 263 | 264 | #if CONFIG_ENABLE_CAM_PAN_TILT 265 | case '5': 266 | leg = key - '1'; 267 | joint = 0; 268 | break; 269 | #endif 270 | 271 | // joint selection 272 | case '7': 273 | case '8': 274 | case '9': 275 | joint = key - '7'; 276 | break; 277 | 278 | // cal +/- 279 | case '=': 280 | case '+': 281 | if (leg < BODY_NUM_LEGS) { 282 | _calAngles[leg][joint] += 5; // 0.5 degree 283 | } else { 284 | #if CONFIG_ENABLE_CAM_PAN_TILT 285 | _calAnglesPanTilt[joint] += 5; 286 | #endif 287 | } 288 | break; 289 | 290 | case '-': 291 | if (leg < BODY_NUM_LEGS) { 292 | _calAngles[leg][joint] -= 5; // 0.5 degree 293 | } else { 294 | #if CONFIG_ENABLE_CAM_PAN_TILT 295 | if (joint < 2) 296 | _calAnglesPanTilt[joint] -= 5; 297 | #endif 298 | } 299 | break; 300 | 301 | // angle +/- for test 302 | case ']': 303 | if (leg < BODY_NUM_LEGS) { 304 | _cfgServos[leg][joint].angle += 5; 305 | } else { 306 | #if CONFIG_ENABLE_CAM_PAN_TILT 307 | if (joint < 2) 308 | _cfgPanTiltServos[joint].angle += 5; 309 | #endif 310 | } 311 | break; 312 | 313 | case '[': 314 | if (leg < BODY_NUM_LEGS) { 315 | _cfgServos[leg][joint].angle -= 5; 316 | } else { 317 | #if CONFIG_ENABLE_CAM_PAN_TILT 318 | if (joint < 2) 319 | _cfgPanTiltServos[joint].angle -= 5; 320 | #endif 321 | } 322 | break; 323 | } 324 | 325 | if (key > 0) { 326 | if (0 <= leg && leg < BODY_NUM_LEGS && joint >= 0) { 327 | setJoint(leg, joint, _cfgServos[leg][joint].angle); 328 | LOG("leg:%d, joint:%d, sign:%2d, angle:%5.1f, cal:%5.1f, uS:%5d\n", leg + 1, joint, _cfgServos[leg][joint].sign, 329 | _cfgServos[leg][joint].angle / 10.0f, _calAngles[leg][joint] / 10.0f, _cfgServos[leg][joint].uS); 330 | } 331 | 332 | #if CONFIG_ENABLE_CAM_PAN_TILT 333 | if (leg == BODY_NUM_LEGS && joint < 2) { 334 | setPanTiltInternal(joint, _cfgPanTiltServos[joint].angle); 335 | LOG("leg:%d, joint:%d, sign:%2d, angle:%5.1f, cal:%5.1f, uS:%5d\n", leg + 1, joint, _cfgPanTiltServos[joint].sign, 336 | _cfgPanTiltServos[joint].angle / 10.0f, _calAnglesPanTilt[joint] / 10.0f, _cfgPanTiltServos[joint].uS); 337 | } 338 | #endif 339 | } 340 | } 341 | 342 | #define ANALOG_RETRY_COUNT 5 343 | 344 | uint16_t Hardware::analogReadAvg(uint16_t pin) { 345 | uint16_t v; 346 | uint32_t sum = 0; 347 | uint16_t values[ANALOG_RETRY_COUNT]; 348 | 349 | for (uint8_t i = 0; i < ANALOG_RETRY_COUNT; i++) { 350 | v = analogRead(pin); 351 | values[i] = v; 352 | sum += v; 353 | } 354 | 355 | uint16_t avg = sum / ANALOG_RETRY_COUNT; 356 | uint8_t cnt = ANALOG_RETRY_COUNT; 357 | for (uint8_t i = 0; i < ANALOG_RETRY_COUNT; i++) { 358 | v = values[i]; 359 | if (abs(v - avg) > 100) { 360 | sum -= v; 361 | cnt --; 362 | } 363 | } 364 | return (cnt > 0) ? (sum / cnt) : 0; 365 | } 366 | 367 | /* 368 | R1 = 44000, R2 = 22000 369 | Vadc = Vbatt * (R2 / R1 + R2) 370 | ----------------------------- 371 | adc Vbatt level 372 | ----------------------------- 373 | 280 under: no battery -1 374 | 3330 above: 8.4V 375 | 3230 : 8.2V 4 376 | 3130 : 8.0V 377 | 3030 : 7.8V 378 | 2930 : 7.6V 3 379 | 2870 : 7.4V 380 | 2780 : 7.2V 381 | 2700 : 7.0V 2 382 | 2610 : 6.8V 383 | 2530 : 6.6V 384 | 2450 : 6.4V 1 385 | under : 0 386 | ----------------------------- 387 | */ 388 | 389 | int Hardware::checkBattery() { 390 | int level; 391 | uint16_t adc = analogReadAvg(PIN_PWR_ADC); 392 | 393 | //LOG("BATT:%5d\n", adc); 394 | if (adc > 3230) { 395 | level = 4; 396 | } else if (adc > 2930) { 397 | level = 3; 398 | } else if (adc > 2700) { 399 | level = 2; 400 | } else if (adc > 2450) { 401 | level = 1; 402 | } else if (adc > 2000) { 403 | level = 0; 404 | } else { 405 | level = -1; 406 | } 407 | return level; 408 | } 409 | -------------------------------------------------------------------------------- /quadruped_code/lib/ControlStick/ControlStick.cpp: -------------------------------------------------------------------------------- 1 | #include "ControlStick.h" 2 | 3 | using namespace std::placeholders; 4 | 5 | #ifdef LOG 6 | #undef LOG 7 | #endif 8 | 9 | #define LOG(...) //printf(__VA_ARGS__) 10 | 11 | /* 12 | ***************************************************************************************** 13 | * CONSTANTS 14 | ***************************************************************************************** 15 | */ 16 | 17 | /* 18 | ***************************************************************************************** 19 | * MACROS & STRUCTURES 20 | ***************************************************************************************** 21 | */ 22 | 23 | 24 | /* 25 | ***************************************************************************************** 26 | * VARIABLES 27 | ***************************************************************************************** 28 | */ 29 | 30 | 31 | /* 32 | ***************************************************************************************** 33 | * FUNCTIONS 34 | ***************************************************************************************** 35 | */ 36 | ControlStick::ControlStick() { 37 | _pStickCallback = nullptr; 38 | } 39 | 40 | class AdvertisedDeviceCallbacks : public NimBLEAdvertisedDeviceCallbacks { 41 | ControlStick* _pJoy; 42 | 43 | public: 44 | AdvertisedDeviceCallbacks(ControlStick* pJoy) : NimBLEAdvertisedDeviceCallbacks() { 45 | _pJoy = pJoy; 46 | } 47 | 48 | void onResult(NimBLEAdvertisedDevice* advertisedDevice) { 49 | //Found a device, check if the service is contained and optional if address fits requested address 50 | LOG("advertised device: %s\n", advertisedDevice->toString().c_str()); 51 | 52 | for (int i = 0; i < _pJoy->_uuidServices.size(); i++) { 53 | if (advertisedDevice->haveServiceUUID() && advertisedDevice->getServiceUUID().equals(_pJoy->_uuidServices[i])) { 54 | if (_pJoy->_reqDevAddrs[i] == nullptr || (_pJoy->_reqDevAddrs[i] && advertisedDevice->getAddress().equals(*_pJoy->_reqDevAddrs[i]))) { 55 | LOG("found : %s\n", advertisedDevice->getName().c_str()); 56 | _pJoy->_nFoundIdx = i; 57 | _pJoy->_nConnTryCtr = 0; 58 | _pJoy->_uuidFoundService = advertisedDevice->getServiceUUID(); 59 | _pJoy->_pServerAddress = new BLEAddress(advertisedDevice->getAddress()); 60 | _pJoy->_isConnecting = true; 61 | advertisedDevice->getScan()->stop(); 62 | break; 63 | } 64 | } 65 | } 66 | } 67 | }; 68 | 69 | class BLEJoyClientCallback : public NimBLEClientCallbacks { 70 | ControlStick* _pJoy; 71 | 72 | public: 73 | BLEJoyClientCallback(ControlStick* pJoy) : NimBLEClientCallbacks() { 74 | _pJoy = pJoy; 75 | } 76 | 77 | void onDisconnect(BLEClient* bleClient) { 78 | _pJoy->_isConnecting = false; 79 | _pJoy->_isConnected = false; 80 | LOG("disconnected client\n"); 81 | if (_pJoy && _pJoy->_pStickCallback) 82 | _pJoy->_pStickCallback->onDisconnect(); 83 | 84 | if (_pJoy->_pServerAddress) { 85 | delete _pJoy->_pServerAddress; 86 | _pJoy->_pServerAddress = nullptr; 87 | } 88 | 89 | if (_pJoy->_pClient) { 90 | //NimBLEDevice::deleteClient(_pJoy->_pClient); 91 | //_pJoy->_pClient = nullptr; 92 | } 93 | _pJoy->rescan(); 94 | } 95 | }; 96 | 97 | bool ControlStick::connect() { 98 | if (_pServerAddress == nullptr) { 99 | return false; 100 | } 101 | 102 | BLEAddress pAddress = *_pServerAddress; 103 | _nConnTryCtr++; 104 | LOG("TRY:%2d - number of ble clients: %d\n", _nConnTryCtr, NimBLEDevice::getClientListSize()); 105 | 106 | /** Check if we have a client we should reuse first **/ 107 | if (NimBLEDevice::getClientListSize()) { 108 | /** Special case when we already know this device, we send false as the 109 | * second argument in connect() to prevent refreshing the service database. 110 | * This saves considerable time and power. 111 | */ 112 | _pClient = NimBLEDevice::getClientByPeerAddress(pAddress); 113 | if (!_pClient) { 114 | /** We don't already have a client that knows this device, 115 | * we will check for a client that is disconnected that we can use. 116 | */ 117 | _pClient = NimBLEDevice::getDisconnectedClient(); 118 | } 119 | } 120 | 121 | if (!_pClient) { 122 | if (NimBLEDevice::getClientListSize() >= NIMBLE_MAX_CONNECTIONS) { 123 | LOG("max clients reached - no more connections available: %d", NimBLEDevice::getClientListSize()); 124 | } else { 125 | /** No client to reuse? Create a new one. */ 126 | _pClient = NimBLEDevice::createClient(); 127 | } 128 | } 129 | 130 | bool ret = false; 131 | if (_pClient) { 132 | ret = _pClient->isConnected(); 133 | if (!ret) { 134 | ret = _pClient->connect(pAddress, true); 135 | } 136 | } 137 | 138 | if (ret) { 139 | // add callback instance to get notified if a disconnect event appears 140 | _pClient->setClientCallbacks(new BLEJoyClientCallback(this)); 141 | 142 | std::vector *pSvcs = _pClient->getServices(true); 143 | std::vector svcs = *pSvcs; 144 | LOG("services :%d\n", svcs.size()); 145 | for(NimBLERemoteService *svc: svcs) { 146 | LOG("services:%s\n", svc->toString().c_str()); 147 | } 148 | 149 | LOG("connected to: %s, RSSI: %d, uuid:%s\n", _pClient->getPeerAddress().toString().c_str(), _pClient->getRssi(), _uuidFoundService.toString().c_str()); 150 | BLERemoteService* pRemoteService = _pClient->getService(_uuidFoundService); 151 | 152 | if (pRemoteService) { 153 | std::vector *pChars = pRemoteService->getCharacteristics(_nConnTryCtr > 2); 154 | if (pChars) { 155 | std::vector chars = *pChars; 156 | 157 | // logs 158 | LOG("get service success chars:%d\n", chars.size()); 159 | if (_nConnTryCtr > 2) { 160 | for(NimBLERemoteCharacteristic *it: chars) { 161 | LOG("chars:%s\n", it->getUUID().toString().c_str()); 162 | } 163 | } 164 | 165 | _pRemoteCharacteristic = pRemoteService->getCharacteristic(_uuidCharacteristics[_nFoundIdx]); 166 | if (_pRemoteCharacteristic) { 167 | if (_pRemoteCharacteristic->canNotify()) { 168 | LOG("notifiable.. subscribed!! : %s\n", _pRemoteCharacteristic->getUUID().toString().c_str()); 169 | _pRemoteCharacteristic->subscribe(true, _notifyCallbacks[_nFoundIdx], true); 170 | } 171 | 172 | if (_pStickCallback) 173 | _pStickCallback->onConnect(); 174 | 175 | _isConnected = true; 176 | _isConnecting = false; 177 | ret = true; 178 | } else { 179 | LOG("failed to get ble service\n"); 180 | } 181 | } else { 182 | LOG("no characteristics found\n"); 183 | } 184 | } else { 185 | LOG("no remote service found\n"); 186 | } 187 | } else { 188 | LOG("failed to connect\n"); 189 | } 190 | return ret; 191 | } 192 | 193 | static ControlStick *_pParent; 194 | static void scanEndedCallback(NimBLEScanResults results) { 195 | /* 196 | LOG("Number of devices: %d\n", results.getCount()); 197 | for (int i = 0; i < results.getCount(); i++) { 198 | LOG("device[%d]: %s\n", i, results.getDevice(i).toString().c_str()); 199 | } 200 | */ 201 | LOG("SCAN ENDED\n"); 202 | _pParent->rescan(); 203 | } 204 | 205 | void ControlStick::rescan() { 206 | BLEScan* pBLEScan = BLEDevice::getScan(); 207 | if (!isConnecting() && !isConnected()) { 208 | LOG("SCAN AGAIN !!!\n"); 209 | pBLEScan->setActiveScan(true); 210 | pBLEScan->start(5, scanEndedCallback); 211 | } 212 | } 213 | 214 | void ControlStick::begin() { 215 | BLEDevice::init(""); 216 | BLEScan* pBLEScan = BLEDevice::getScan(); 217 | 218 | pBLEScan->setAdvertisedDeviceCallbacks(new AdvertisedDeviceCallbacks(this)); 219 | pBLEScan->setActiveScan(true); 220 | _pParent = this; 221 | pBLEScan->start(5, scanEndedCallback); 222 | } 223 | 224 | void ControlStick::stop() { 225 | if (!BLEDevice::getInitialized()) 226 | return; 227 | 228 | BLEScan* pBLEScan = BLEDevice::getScan(); 229 | if (pBLEScan) { 230 | pBLEScan->stop(); 231 | } 232 | BLEDevice::deinit(true); 233 | } 234 | 235 | void ControlStick::add(BLEAddress* addr, BLEUUID uuidService, BLEUUID uuidCharacteristic, notify_callback cb) { 236 | _reqDevAddrs.push_back(addr); 237 | _uuidServices.push_back(uuidService); 238 | _uuidCharacteristics.push_back(uuidCharacteristic); 239 | _notifyCallbacks.push_back(cb); 240 | } 241 | 242 | /* 243 | *************************************************************************************************** 244 | * Callbacks 245 | *************************************************************************************************** 246 | */ 247 | void ControlStick::addSupportedDevices() { 248 | // PG-9167 249 | add(nullptr, BLEUUID("1812"), BLEUUID("2A4D"), std::bind(&ControlStick::cbControlStickBLE, this, _1, _2, _3, _4)); 250 | 251 | // flypad 252 | add(nullptr, BLEUUID("9e35fa00-4344-44d4-a2e2-0c7f6046878b"), 253 | BLEUUID("9e35fa01-4344-44d4-a2e2-0c7f6046878b"), std::bind(&ControlStick::cbFlyPadBLE, this, _1, _2, _3, _4)); 254 | 255 | // gamesir-t1d 256 | add(nullptr, BLEUUID("00008650-0000-1000-8000-00805f9b34fb"), 257 | BLEUUID("00008651-0000-1000-8000-00805f9b34fb"), std::bind(&ControlStick::cbGameSirT1DBLE, this, _1, _2, _3, _4)); 258 | } 259 | 260 | void ControlStick::cbControlStickBLE(NimBLERemoteCharacteristic* pBLERemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify) { 261 | // 0 1 2 3 4 5 6 7 8 9 262 | // 00000000 - 80 80 80 80 ff 00 00 00 00 e2 263 | // LA LA RA RA dp B1 B2 264 | // LR UD LR UD 265 | // 266 | // 7 6 5 4 3 2 1 0 267 | // B1 bits: R1 L1 Y X B A 268 | // B2 bits: RTH LTH START SEL R2 L2 269 | // dp val: none-0xff up-0, r-1, d-2, l-3 270 | 271 | int btns = (int(pData[6]) << 8) | int(pData[5]); 272 | int dpad = pData[4] + 1; 273 | 274 | // 8bits -> 10bits 275 | if (_pStickCallback) 276 | _pStickCallback->onStickChanged(int(pData[0]) << 2, int(pData[1]) << 2, int(pData[2]) << 2, int(pData[3]) << 2, 0, 0, dpad, btns); 277 | } 278 | 279 | void ControlStick::cbFlyPadBLE(NimBLERemoteCharacteristic* pBLERemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify) { 280 | // 0 1 2 3 4 5 6 281 | // 00000000 - 50 00 00 80 80 80 80 282 | // B1 B2 RA RA LA LA 283 | // LR UP LR UP 284 | // 285 | // 7 6 5 4 3 2 1 0 286 | // B1 bits: L1 R2 R1 A B 2 1 TAKEOFF 287 | // B2 bits: RTH LTH L2 288 | int btns = (int(pData[2]) << 8) | (pData[1] & 0xff); 289 | btns = setBit(getBit(btns, 0), BTN_START) | 290 | setBit(getBit(btns, 1), BTN_X) | 291 | setBit(getBit(btns, 2), BTN_Y) | 292 | setBit(getBit(btns, 3), BTN_B) | 293 | setBit(getBit(btns, 4), BTN_A) | 294 | setBit(getBit(btns, 5), BTN_R1) | 295 | setBit(getBit(btns, 6), BTN_R2) | 296 | setBit(getBit(btns, 7), BTN_L1) | 297 | setBit(getBit(btns, 8), BTN_L2) | 298 | setBit(getBit(btns, 9), BTN_LTHUMB) | 299 | setBit(getBit(btns, 10), BTN_RTHUMB); 300 | 301 | // 8bits -> 10bits 302 | if (_pStickCallback) 303 | _pStickCallback->onStickChanged(int(pData[5]) << 2, int(pData[6]) << 2, int(pData[3]) << 2, int(pData[4]) << 2, 0, 0, 0, btns); 304 | } 305 | 306 | void ControlStick::cbGameSirT1DBLE(NimBLERemoteCharacteristic* pBLERemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify) { 307 | // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 308 | // a1 c5 80 20 08 02 00 00 00 00 00 00 00 00 00 00 00 00 00 9b 309 | // L2 R2 B1 B2 DP 310 | // A A 311 | // L2 Anal : XX 8bit 312 | // R2 Anal : XX 8bit 313 | // L-LR : XX X 10bit 314 | // L-UPDOWN: XX X 10bit 315 | // R-LR : XX X 10bit 316 | // R-UPDOWN: XX X 10bit 317 | // 318 | // 7 6 5 4 3 2 1 0 319 | // B1 bits : R1 L1 Y X menu B A 320 | // B2 bits : RTH LTH PWR C2 C1 R2 L2 321 | // DP val : none-0, up-1, ur-2, r-3, rd-4, d-5, dl-6, l-7, ul-8 322 | 323 | if (length < 12 || pData[0] != (byte)0xa1 || pData[1] != (byte)0xc5) { 324 | return; 325 | } 326 | 327 | // 10 bits * 4 axis 328 | int llr = (int(pData[2] & 0xff) << 2) | ((pData[3] & 0xc0) >> 6); 329 | int lud = (int(pData[3] & 0x3f) << 4) | ((pData[4] & 0xf0) >> 4); 330 | int rlr = (int(pData[4] & 0x0f) << 6) | ((pData[5] & 0xf3) >> 2); 331 | int rud = (int(pData[5] & 0x03) << 8) | ((pData[6] & 0xff) >> 0); 332 | 333 | // 8 bits-> 10 bits 334 | int alt = int(pData[7]) << 2; 335 | int art = int(pData[8]) << 2; 336 | 337 | int btns = (int(pData[10]) << 8) | int(pData[9]); 338 | int dpad = int(pData[11]); 339 | 340 | if (_pStickCallback) 341 | _pStickCallback->onStickChanged(llr, lud, rlr, rud, alt, art, dpad, btns); 342 | } 343 | -------------------------------------------------------------------------------- /quadruped_code/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "CommInterface.h" 4 | #include "ControlStick.h" 5 | #include "OTA.h" 6 | #include "FSBrowser.h" 7 | #include "CommandHandler.h" 8 | #include "utils.h" 9 | 10 | using namespace std::placeholders; 11 | 12 | /* 13 | ***************************************************************************************** 14 | * CONSTANTS 15 | ***************************************************************************************** 16 | */ 17 | 18 | /* 19 | ***************************************************************************************** 20 | * VARIABLES 21 | ***************************************************************************************** 22 | */ 23 | #if (CONFIG_CONTROL == CONFIG_CONTROL_BTPADS) 24 | static ControlStick _joy; 25 | #endif 26 | 27 | static CommInterface* _pComm = NULL; 28 | static CommandHandler _cmdHandler; 29 | 30 | static OTA _ota(SERVER_NAME); 31 | static FSBrowser _fsbrowser(SERVER_NAME); 32 | static Timer<> _timer = timer_create_default(); 33 | 34 | static bool _isCal = false; 35 | 36 | /* 37 | *************************************************************************************************** 38 | * PROTOCOL 39 | *************************************************************************************************** 40 | */ 41 | 42 | 43 | /* 44 | ***************************************************************************************** 45 | * FUNCTIONS 46 | ***************************************************************************************** 47 | */ 48 | int loopOTA(unsigned long ts) { 49 | static int state = 0; 50 | 51 | switch (state) { 52 | case 0: 53 | #if CONFIG_ENABLE_BTPADS 54 | _joy.stop(); 55 | #endif 56 | _ota.setup((char*)WIFI_SSID, (char*)WIFI_PASSWORD); 57 | _cmdHandler.getStatusLed()->set(StatusLed::ALL_POS, StatusLed::COLOR_RED, 1000); 58 | state = 1; 59 | return -1; 60 | 61 | case 1: 62 | _ota.loop(); 63 | _cmdHandler.getStatusLed()->loop(ts); 64 | return -1; 65 | } 66 | return 0; 67 | } 68 | 69 | int loopFS(unsigned long ts, bool isClose = false) { 70 | static int state = 0; 71 | 72 | if (isClose) { 73 | state = 2; 74 | } 75 | 76 | switch (state) { 77 | case 0: 78 | state = 1; 79 | _fsbrowser.setup((char*)WIFI_SSID, (char*)WIFI_PASSWORD); 80 | _cmdHandler.getStatusLed()->set(StatusLed::ALL_POS, StatusLed::COLOR_YELLOW, 1000); 81 | return -1; 82 | 83 | case 1: 84 | _fsbrowser.loop(); 85 | _cmdHandler.getStatusLed()->loop(ts); 86 | return -1; 87 | 88 | case 2: 89 | _fsbrowser.close(); 90 | state = 0; 91 | return -1; 92 | 93 | } 94 | return 0; 95 | } 96 | 97 | /* 98 | *************************************************************************************************** 99 | * Bluetooth Stick callback 100 | *************************************************************************************************** 101 | */ 102 | class StickCB : public StickCallback { 103 | private: 104 | struct param_rc _rc; 105 | const uint8_t _kTblDpadMap[9]; 106 | 107 | public: 108 | StickCB() : 109 | _rc { 110 | 1500, 1500, 1500, 1500, 111 | 1000, 1000, 1000, 1000, 1000, 112 | 1000, 1000, 1000, 1000, 1000, 113 | 1 114 | }, 115 | _kTblDpadMap { 116 | 0x00, 117 | 0x01, // up 118 | 0x03, 119 | 0x02, // right 120 | 0x06, 121 | 0x04, // down 122 | 0x0c, 123 | 0x08, // left 124 | 0x09 125 | } 126 | { 127 | } 128 | 129 | void onConnect() { 130 | _cmdHandler.setConnected(CONTROLLER_BTPAD, true); 131 | } 132 | 133 | void onDisconnect() { 134 | _cmdHandler.setConnected(CONTROLLER_BTPAD, false); 135 | } 136 | 137 | void onStickChanged(int axisX, int axisY, int axisZ, int axisRZ, int axisLT, int axisRT, int dpad, int btns) { 138 | _rc.yaw = map(axisX, 0, 1024, 1000, 2000); 139 | _rc.throttle = map(axisY, 0, 1024, 2000, 1000); 140 | _rc.roll = map(axisZ, 0, 1024, 1000, 2000); 141 | _rc.pitch = map(axisRZ, 0, 1024, 2000, 1000); 142 | _cmdHandler.onRC(&_rc); 143 | 144 | uint8_t dp = (dpad > 8) ? 0 : _kTblDpadMap[dpad]; 145 | uint32_t btn = (int(dp) << 16) | btns; 146 | _cmdHandler.setButtons(btn); 147 | } 148 | }; 149 | 150 | /* 151 | *************************************************************************************************** 152 | * Serial Command 153 | *************************************************************************************************** 154 | */ 155 | static int handleSerialCommand(unsigned long ts, int key) { 156 | static int iMode = 0; 157 | static struct param_rc rc = { 1500, 1500, 1500, 1500, 158 | 1000, 1000, 1000, 1000, 1000, 159 | 1000, 1000, 1000, 1000, 1000, 160 | 1 }; 161 | 162 | switch (key) { 163 | case 'm': 164 | if (iMode == 0 || iMode == 1) { 165 | iMode = (iMode == 0) ? 1 : 0; 166 | if (iMode > 0) { 167 | LOG("\n\n>>> calibration mode !!!\n"); 168 | } else { 169 | LOG("\n\n>>> normal mode !!!\n"); 170 | } 171 | } 172 | break; 173 | 174 | case '&': 175 | if (iMode == 0 || iMode == 2) { 176 | iMode = (iMode == 0) ? 2 : 0; 177 | if (iMode > 0) { 178 | LOG("\n\n>>> fs browser mode !!!\n"); 179 | } else { 180 | LOG("\n\n>>> normal mode !!!\n"); 181 | loopFS(ts, true); 182 | } 183 | } 184 | break; 185 | 186 | case '*': 187 | if (iMode == 0 || iMode == 3) { 188 | iMode = (iMode == 0) ? 3 : 0; 189 | if (iMode > 0) { 190 | LOG("\n\n>>> OTA mode !!!\n"); 191 | } else { 192 | LOG("\n\n>>> normal mode !!!\n"); 193 | } 194 | } 195 | break; 196 | } 197 | 198 | if (iMode > 0) { 199 | switch (iMode) { 200 | case 1: 201 | _cmdHandler.getHW()->calibrateLegs(key); 202 | return -100; 203 | 204 | case 2: 205 | loopFS(ts); 206 | return -100; 207 | 208 | case 3: 209 | loopOTA(ts); 210 | return -100; 211 | } 212 | } 213 | 214 | // 215 | // robot control 216 | // 217 | switch (key) { 218 | case 'w': 219 | if (rc.pitch < 2000) { 220 | rc.pitch++; 221 | _cmdHandler.onRC(&rc); 222 | } 223 | break; 224 | 225 | case 's': 226 | if (rc.pitch > 1000) { 227 | rc.pitch--; 228 | _cmdHandler.onRC(&rc); 229 | } 230 | break; 231 | 232 | case 'a': 233 | if (rc.roll > 1000) { 234 | rc.roll--; 235 | _cmdHandler.onRC(&rc); 236 | } 237 | break; 238 | 239 | case 'd': 240 | if (rc.roll < 2000) { 241 | rc.roll++; 242 | _cmdHandler.onRC(&rc); 243 | } 244 | break; 245 | 246 | case 'z': 247 | _cmdHandler.toggleButtons(BTN_HEIGHT_DEC); 248 | break; 249 | 250 | case 'c': 251 | _cmdHandler.toggleButtons(BTN_HEIGHT_INC); 252 | break; 253 | 254 | case 'e': 255 | if (rc.yaw < 2000) { 256 | rc.yaw += 10; 257 | _cmdHandler.onRC(&rc); 258 | } 259 | break; 260 | 261 | case 'q': 262 | if (rc.yaw > 1000) { 263 | rc.yaw -= 10; 264 | _cmdHandler.onRC(&rc); 265 | } 266 | break; 267 | 268 | case '1': 269 | _cmdHandler.toggleButtons(BTN_WALK); 270 | break; 271 | 272 | case '2': 273 | _cmdHandler.toggleButtons(BTN_GAIT); 274 | break; 275 | 276 | case '3': 277 | _cmdHandler.toggleButtons(BTN_BALANCE); 278 | //rc.roll = rc.pitch = rc.throttle = rc.yaw = 1500; 279 | break; 280 | 281 | case '4': 282 | _cmdHandler.toggleButtons(BTN_FLASH); 283 | //rc.roll = rc.pitch = rc.throttle = rc.yaw = 1500; 284 | break; 285 | 286 | case '9': 287 | _cmdHandler.toggleButtons(BTN_SAVE); 288 | break; 289 | 290 | case '0': 291 | _cmdHandler.toggleButtons(BTN_LOAD); 292 | break; 293 | 294 | case '!': 295 | _cmdHandler.getSpot()->setDebugMask(_cmdHandler.getSpot()->getDebugMask() ^ BV(0)); 296 | break; 297 | 298 | case '@': 299 | _cmdHandler.getSpot()->setDebugMask(_cmdHandler.getSpot()->getDebugMask() ^ BV(1)); 300 | break; 301 | 302 | case '#': 303 | _cmdHandler.getSpot()->setDebugMask(_cmdHandler.getSpot()->getDebugMask() ^ BV(2)); 304 | break; 305 | 306 | case '$': 307 | _cmdHandler.getSpot()->setDebugMask(_cmdHandler.getSpot()->getDebugMask() ^ BV(3)); 308 | break; 309 | 310 | case '=': 311 | _cmdHandler.toggleButtons(BTN_STEP_DEC); 312 | break; 313 | 314 | case '-': 315 | _cmdHandler.toggleButtons(BTN_STEP_INC); 316 | break; 317 | 318 | case '[': 319 | _cmdHandler.toggleButtons(BTN_STEP_XY_DEC); 320 | break; 321 | 322 | case ']': 323 | _cmdHandler.toggleButtons(BTN_STEP_XY_INC); 324 | break; 325 | 326 | case ';': 327 | _cmdHandler.toggleButtons(BTN_STEP_Z_DEC); 328 | break; 329 | 330 | case '\'': 331 | _cmdHandler.toggleButtons(BTN_STEP_Z_INC); 332 | break; 333 | 334 | case ' ': 335 | _cmdHandler.toggleButtons(BTN_STAND); 336 | break; 337 | 338 | case ',': 339 | _cmdHandler.toggleButtons(BTN_OFFSET_DEC); 340 | break; 341 | 342 | case '.': 343 | _cmdHandler.toggleButtons(BTN_OFFSET_INC); 344 | break; 345 | 346 | case 'h': 347 | _cmdHandler.getHW()->dump(); 348 | break; 349 | 350 | // 351 | // gyro proc 352 | // 353 | case 'p': 354 | _cmdHandler.getGyroProc()->decP(); 355 | break; 356 | 357 | case 'P': 358 | _cmdHandler.getGyroProc()->incP(); 359 | break; 360 | 361 | case 'i': 362 | _cmdHandler.getGyroProc()->decI(); 363 | break; 364 | 365 | case 'I': 366 | _cmdHandler.getGyroProc()->incI(); 367 | break; 368 | 369 | case 'o': 370 | _cmdHandler.getGyroProc()->decD(); 371 | break; 372 | 373 | case 'O': 374 | _cmdHandler.getGyroProc()->incD(); 375 | break; 376 | 377 | case 'u': 378 | _cmdHandler.getGyroProc()->togglePID(); 379 | break; 380 | 381 | case 'U': 382 | _cmdHandler.getGyroProc()->calibrate(); 383 | break; 384 | } 385 | return key; 386 | } 387 | 388 | /* 389 | *************************************************************************************************** 390 | * Setup 391 | *************************************************************************************************** 392 | */ 393 | static bool batteryWarning(void* param) { 394 | if (int(param) >= 0) { 395 | LOG("battery warning : %d\n", int(param)); 396 | } 397 | int level = digitalRead(PIN_PWR_LED); 398 | digitalWrite(PIN_PWR_LED, !level); 399 | 400 | return true; 401 | } 402 | 403 | static bool batteryCheck(void* param) { 404 | static int lastLevel = 5; 405 | static uintptr_t taskBattLed = 0; 406 | static const int tblDelays[] = { 407 | 2000, // -1 : no batt 408 | 200, // 0 409 | 500 // 1 410 | }; 411 | 412 | int level = _cmdHandler.getHW()->checkBattery(); 413 | if (lastLevel != level) { 414 | if (taskBattLed) { 415 | _timer.cancel(taskBattLed); 416 | } 417 | if (level < 2) { 418 | taskBattLed = _timer.every(tblDelays[level + 1], batteryWarning, (void*)level); 419 | } else { 420 | digitalWrite(PIN_PWR_LED, HIGH); 421 | } 422 | lastLevel = level; 423 | } 424 | 425 | return true; 426 | } 427 | 428 | void setup() { 429 | setCpuFrequencyMhz(80); 430 | Serial.begin(115200); 431 | LOG("setup start !!! : heap:%d, psram:%d\n", ESP.getFreeHeap(), ESP.getPsramSize()); 432 | 433 | pinMode(PIN_LED, OUTPUT); 434 | pinMode(PIN_PWR_LED, OUTPUT); 435 | digitalWrite(PIN_PWR_LED, HIGH); 436 | 437 | #ifdef PIN_CAL_SW 438 | pinMode(PIN_CAL_SW, INPUT_PULLUP); 439 | _isCal = (digitalRead(PIN_CAL_SW) == LOW); 440 | #endif 441 | 442 | pinMode(PIN_SCL0, INPUT_PULLUP); 443 | pinMode(PIN_SDA0, INPUT_PULLUP); 444 | Wire.setPins(PIN_SDA0, PIN_SCL0); 445 | Wire.begin(); 446 | Wire.setClock(400000); 447 | 448 | if (!SPIFFS.begin(true)) { 449 | LOG("SPIFFS Mount Failed\n"); 450 | } 451 | _timer.every(5000, batteryCheck); 452 | 453 | if (!_isCal) { 454 | #if (CONFIG_CONTROL == CONFIG_CONTROL_BTPADS) 455 | _joy.setStickCallback(new StickCB()); 456 | _joy.addSupportedDevices(); 457 | _joy.begin(); 458 | _pComm = new CommSerial2(128); 459 | #elif (CONFIG_CONTROL == CONFIG_CONTROL_BT_UART) 460 | _pComm = new CommSerialBT((char*)SERVER_NAME, 128); 461 | #endif 462 | } 463 | 464 | if (_pComm) { 465 | _pComm->getProtocol()->setCallback(&_cmdHandler); 466 | //_pComm->getProtocol()->setSwButtonCmd(); 467 | } 468 | _cmdHandler.setup(_pComm); 469 | for (int i = 0; i < 3; i++) { 470 | digitalWrite(PIN_PWR_LED, LOW); 471 | delay(300); 472 | digitalWrite(PIN_PWR_LED, HIGH); 473 | delay(300); 474 | } 475 | LOG("setup finished !!!\n"); 476 | } 477 | 478 | /* 479 | *************************************************************************************************** 480 | * loop 481 | *************************************************************************************************** 482 | */ 483 | void loop() { 484 | unsigned long ts = millis(); 485 | 486 | // 487 | // main loop 488 | // 489 | int key = Serial.read(); 490 | 491 | if (_isCal) { 492 | _cmdHandler.getHW()->calibrateLegs(key); 493 | } else { 494 | key = handleSerialCommand(ts, key); 495 | if (key != -100) { 496 | _cmdHandler.loop(); 497 | } 498 | #if (CONFIG_CONTROL == CONFIG_CONTROL_BTPADS) 499 | if (_joy.isConnecting()) { 500 | _joy.connect(); 501 | } 502 | if (!_joy.isConnected()) 503 | #endif 504 | _pComm->loop(); 505 | } 506 | 507 | _timer.tick(); 508 | } 509 | --------------------------------------------------------------------------------