diff --git a/LICENSE.txt b/LICENSE.txt new file mode 100644 index 0000000..20fb9c7 --- /dev/null +++ b/LICENSE.txt @@ -0,0 +1,458 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 2.1, February 1999 + + Copyright (C) 1991, 1999 Free Software Foundation, Inc. + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + +[This is the first released version of the Lesser GPL. It also counts + as the successor of the GNU Library Public License, version 2, hence + the version number 2.1.] + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +Licenses are intended to guarantee your freedom to share and change +free software--to make sure the software is free for all its users. + + This license, the Lesser General Public License, applies to some +specially designated software packages--typically libraries--of the +Free Software Foundation and other authors who decide to use it. You +can use it too, but we suggest you first think carefully about whether +this license or the ordinary General Public License is the better +strategy to use in any particular case, based on the explanations below. + + When we speak of free software, we are referring to freedom of use, +not price. Our General Public Licenses are designed to make sure that +you have the freedom to distribute copies of free software (and charge +for this service if you wish); that you receive source code or can get +it if you want it; that you can change the software and use pieces of +it in new free programs; and that you are informed that you can do +these things. + + To protect your rights, we need to make restrictions that forbid +distributors to deny you these rights or to ask you to surrender these +rights. These restrictions translate to certain responsibilities for +you if you distribute copies of the library or if you modify it. + + For example, if you distribute copies of the library, whether gratis +or for a fee, you must give the recipients all the rights that we gave +you. You must make sure that they, too, receive or can get the source +code. If you link other code with the library, you must provide +complete object files to the recipients, so that they can relink them +with the library after making changes to the library and recompiling +it. And you must show them these terms so they know their rights. + + We protect your rights with a two-step method: (1) we copyright the +library, and (2) we offer you this license, which gives you legal +permission to copy, distribute and/or modify the library. + + To protect each distributor, we want to make it very clear that +there is no warranty for the free library. Also, if the library is +modified by someone else and passed on, the recipients should know +that what they have is not the original version, so that the original +author's reputation will not be affected by problems that might be +introduced by others. + + Finally, software patents pose a constant threat to the existence of +any free program. We wish to make sure that a company cannot +effectively restrict the users of a free program by obtaining a +restrictive license from a patent holder. Therefore, we insist that +any patent license obtained for a version of the library must be +consistent with the full freedom of use specified in this license. + + Most GNU software, including some libraries, is covered by the +ordinary GNU General Public License. This license, the GNU Lesser +General Public License, applies to certain designated libraries, and +is quite different from the ordinary General Public License. We use +this license for certain libraries in order to permit linking those +libraries into non-free programs. + + When a program is linked with a library, whether statically or using +a shared library, the combination of the two is legally speaking a +combined work, a derivative of the original library. The ordinary +General Public License therefore permits such linking only if the +entire combination fits its criteria of freedom. The Lesser General +Public License permits more lax criteria for linking other code with +the library. + + We call this license the "Lesser" General Public License because it +does Less to protect the user's freedom than the ordinary General +Public License. It also provides other free software developers Less +of an advantage over competing non-free programs. These disadvantages +are the reason we use the ordinary General Public License for many +libraries. However, the Lesser license provides advantages in certain +special circumstances. + + For example, on rare occasions, there may be a special need to +encourage the widest possible use of a certain library, so that it becomes +a de-facto standard. To achieve this, non-free programs must be +allowed to use the library. A more frequent case is that a free +library does the same job as widely used non-free libraries. In this +case, there is little to gain by limiting the free library to free +software only, so we use the Lesser General Public License. + + In other cases, permission to use a particular library in non-free +programs enables a greater number of people to use a large body of +free software. For example, permission to use the GNU C Library in +non-free programs enables many more people to use the whole GNU +operating system, as well as its variant, the GNU/Linux operating +system. + + Although the Lesser General Public License is Less protective of the +users' freedom, it does ensure that the user of a program that is +linked with the Library has the freedom and the wherewithal to run +that program using a modified version of the Library. + + The precise terms and conditions for copying, distribution and +modification follow. Pay close attention to the difference between a +"work based on the library" and a "work that uses the library". The +former contains code derived from the library, whereas the latter must +be combined with the library in order to run. + + GNU LESSER GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License Agreement applies to any software library or other +program which contains a notice placed by the copyright holder or +other authorized party saying it may be distributed under the terms of +this Lesser General Public License (also called "this License"). +Each licensee is addressed as "you". + + A "library" means a collection of software functions and/or data +prepared so as to be conveniently linked with application programs +(which use some of those functions and data) to form executables. + + The "Library", below, refers to any such software library or work +which has been distributed under these terms. A "work based on the +Library" means either the Library or any derivative work under +copyright law: that is to say, a work containing the Library or a +portion of it, either verbatim or with modifications and/or translated +straightforwardly into another language. (Hereinafter, translation is +included without limitation in the term "modification".) + + "Source code" for a work means the preferred form of the work for +making modifications to it. For a library, complete source code means +all the source code for all modules it contains, plus any associated +interface definition files, plus the scripts used to control compilation +and installation of the library. + + Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running a program using the Library is not restricted, and output from +such a program is covered only if its contents constitute a work based +on the Library (independent of the use of the Library in a tool for +writing it). Whether that is true depends on what the Library does +and what the program that uses the Library does. + + 1. You may copy and distribute verbatim copies of the Library's +complete source code as you receive it, in any medium, provided that +you conspicuously and appropriately publish on each copy an +appropriate copyright notice and disclaimer of warranty; keep intact +all the notices that refer to this License and to the absence of any +warranty; and distribute a copy of this License along with the +Library. + + You may charge a fee for the physical act of transferring a copy, +and you may at your option offer warranty protection in exchange for a +fee. + + 2. You may modify your copy or copies of the Library or any portion +of it, thus forming a work based on the Library, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) The modified work must itself be a software library. + + b) You must cause the files modified to carry prominent notices + stating that you changed the files and the date of any change. + + c) You must cause the whole of the work to be licensed at no + charge to all third parties under the terms of this License. + + d) If a facility in the modified Library refers to a function or a + table of data to be supplied by an application program that uses + the facility, other than as an argument passed when the facility + is invoked, then you must make a good faith effort to ensure that, + in the event an application does not supply such function or + table, the facility still operates, and performs whatever part of + its purpose remains meaningful. + + (For example, a function in a library to compute square roots has + a purpose that is entirely well-defined independent of the + application. Therefore, Subsection 2d requires that any + application-supplied function or table used by this function must + be optional: if the application does not supply it, the square + root function must still compute square roots.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Library, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Library, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote +it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Library. + +In addition, mere aggregation of another work not based on the Library +with the Library (or with a work based on the Library) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may opt to apply the terms of the ordinary GNU General Public +License instead of this License to a given copy of the Library. To do +this, you must alter all the notices that refer to this License, so +that they refer to the ordinary GNU General Public License, version 2, +instead of to this License. (If a newer version than version 2 of the +ordinary GNU General Public License has appeared, then you can specify +that version instead if you wish.) Do not make any other change in +these notices. + + Once this change is made in a given copy, it is irreversible for +that copy, so the ordinary GNU General Public License applies to all +subsequent copies and derivative works made from that copy. + + This option is useful when you wish to copy part of the code of +the Library into a program that is not a library. + + 4. You may copy and distribute the Library (or a portion or +derivative of it, under Section 2) in object code or executable form +under the terms of Sections 1 and 2 above provided that you accompany +it with the complete corresponding machine-readable source code, which +must be distributed under the terms of Sections 1 and 2 above on a +medium customarily used for software interchange. + + If distribution of object code is made by offering access to copy +from a designated place, then offering equivalent access to copy the +source code from the same place satisfies the requirement to +distribute the source code, even though third parties are not +compelled to copy the source along with the object code. + + 5. A program that contains no derivative of any portion of the +Library, but is designed to work with the Library by being compiled or +linked with it, is called a "work that uses the Library". Such a +work, in isolation, is not a derivative work of the Library, and +therefore falls outside the scope of this License. + + However, linking a "work that uses the Library" with the Library +creates an executable that is a derivative of the Library (because it +contains portions of the Library), rather than a "work that uses the +library". The executable is therefore covered by this License. +Section 6 states terms for distribution of such executables. + + When a "work that uses the Library" uses material from a header file +that is part of the Library, the object code for the work may be a +derivative work of the Library even though the source code is not. +Whether this is true is especially significant if the work can be +linked without the Library, or if the work is itself a library. The +threshold for this to be true is not precisely defined by law. + + If such an object file uses only numerical parameters, data +structure layouts and accessors, and small macros and small inline +functions (ten lines or less in length), then the use of the object +file is unrestricted, regardless of whether it is legally a derivative +work. (Executables containing this object code plus portions of the +Library will still fall under Section 6.) + + Otherwise, if the work is a derivative of the Library, you may +distribute the object code for the work under the terms of Section 6. +Any executables containing that work also fall under Section 6, +whether or not they are linked directly with the Library itself. + + 6. As an exception to the Sections above, you may also combine or +link a "work that uses the Library" with the Library to produce a +work containing portions of the Library, and distribute that work +under terms of your choice, provided that the terms permit +modification of the work for the customer's own use and reverse +engineering for debugging such modifications. + + You must give prominent notice with each copy of the work that the +Library is used in it and that the Library and its use are covered by +this License. You must supply a copy of this License. If the work +during execution displays copyright notices, you must include the +copyright notice for the Library among them, as well as a reference +directing the user to the copy of this License. Also, you must do one +of these things: + + a) Accompany the work with the complete corresponding + machine-readable source code for the Library including whatever + changes were used in the work (which must be distributed under + Sections 1 and 2 above); and, if the work is an executable linked + with the Library, with the complete machine-readable "work that + uses the Library", as object code and/or source code, so that the + user can modify the Library and then relink to produce a modified + executable containing the modified Library. (It is understood + that the user who changes the contents of definitions files in the + Library will not necessarily be able to recompile the application + to use the modified definitions.) + + b) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (1) uses at run time a + copy of the library already present on the user's computer system, + rather than copying library functions into the executable, and (2) + will operate properly with a modified version of the library, if + the user installs one, as long as the modified version is + interface-compatible with the version that the work was made with. + + c) Accompany the work with a written offer, valid for at + least three years, to give the same user the materials + specified in Subsection 6a, above, for a charge no more + than the cost of performing this distribution. + + d) If distribution of the work is made by offering access to copy + from a designated place, offer equivalent access to copy the above + specified materials from the same place. + + e) Verify that the user has already received a copy of these + materials or that you have already sent this user a copy. + + For an executable, the required form of the "work that uses the +Library" must include any data and utility programs needed for +reproducing the executable from it. However, as a special exception, +the materials to be distributed need not include anything that is +normally distributed (in either source or binary form) with the major +components (compiler, kernel, and so on) of the operating system on +which the executable runs, unless that component itself accompanies +the executable. + + It may happen that this requirement contradicts the license +restrictions of other proprietary libraries that do not normally +accompany the operating system. Such a contradiction means you cannot +use both them and the Library together in an executable that you +distribute. + + 7. You may place library facilities that are a work based on the +Library side-by-side in a single library together with other library +facilities not covered by this License, and distribute such a combined +library, provided that the separate distribution of the work based on +the Library and of the other library facilities is otherwise +permitted, and provided that you do these two things: + + a) Accompany the combined library with a copy of the same work + based on the Library, uncombined with any other library + facilities. This must be distributed under the terms of the + Sections above. + + b) Give prominent notice with the combined library of the fact + that part of it is a work based on the Library, and explaining + where to find the accompanying uncombined form of the same work. + + 8. You may not copy, modify, sublicense, link with, or distribute +the Library except as expressly provided under this License. Any +attempt otherwise to copy, modify, sublicense, link with, or +distribute the Library is void, and will automatically terminate your +rights under this License. However, parties who have received copies, +or rights, from you under this License will not have their licenses +terminated so long as such parties remain in full compliance. + + 9. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Library or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Library (or any work based on the +Library), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Library or works based on it. + + 10. Each time you redistribute the Library (or any work based on the +Library), the recipient automatically receives a license from the +original licensor to copy, distribute, link with or modify the Library +subject to these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties with +this License. + + 11. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Library at all. For example, if a patent +license would not permit royalty-free redistribution of the Library by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Library. + +If any portion of this section is held invalid or unenforceable under any +particular circumstance, the balance of the section is intended to apply, +and the section as a whole is intended to apply in other circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 12. If the distribution and/or use of the Library is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Library under this License may add +an explicit geographical distribution limitation excluding those countries, +so that distribution is permitted only in or among countries not thus +excluded. In such case, this License incorporates the limitation as if +written in the body of this License. + + 13. The Free Software Foundation may publish revised and/or new +versions of the Lesser General Public License from time to time. +Such new versions will be similar in spirit to the present version, +but may differ in detail to address new problems or concerns. + +Each version is given a distinguishing version number. If the Library +specifies a version number of this License which applies to it and +"any later version", you have the option of following the terms and +conditions either of that version or of any later version published by +the Free Software Foundation. If the Library does not specify a +license version number, you may choose any version ever published by +the Free Software Foundation. + + 14. If you wish to incorporate parts of the Library into other free +programs whose distribution conditions are incompatible with these, +write to the author to ask for permission. For software which is +copyrighted by the Free Software Foundation, write to the Free +Software Foundation; we sometimes make exceptions for this. Our +decision will be guided by the two goals of preserving the free status +of all derivatives of our free software and of promoting the sharing +and reuse of software generally. + + NO WARRANTY + + 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO +WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. +EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR +OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY +KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE +LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME +THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN +WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY +AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU +FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR +CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE +LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING +RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A +FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF +SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH +DAMAGES. + + END OF TERMS AND CONDITIONS diff --git a/README.adoc b/README.adoc new file mode 100644 index 0000000..477569e --- /dev/null +++ b/README.adoc @@ -0,0 +1,22 @@ += uMyo_BLE Library for ESP32 = + +This library allows to receive data from uMyo devices using BLE mode of ESP32 radio, +providing muscle activity readings, roll, pitch, yaw angles and 4-bins spectrum + +== License == + +Copyright (c) Ultimate Robotics. All right reserved. + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA diff --git a/README.md b/README.md new file mode 100644 index 0000000..daa9efc --- /dev/null +++ b/README.md @@ -0,0 +1,23 @@ + +# uMyo Library for Arduino + +This library allows to receive data from uMyo devices using BLE mode of ESP32 radio, +providing muscle activity readings, roll, pitch, yaw angles and 4-bins spectrum + +# License + +Copyright (c) Ultimate Robotics. All right reserved. + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA diff --git a/examples/umyo_IMU/umyo_IMU.ino b/examples/umyo_IMU/umyo_IMU.ino new file mode 100644 index 0000000..a948948 --- /dev/null +++ b/examples/umyo_IMU/umyo_IMU.ino @@ -0,0 +1,31 @@ +/* +Obtaining orientation data from uMyo via BLE on ESP32 +Usage: run the code on ESP32 device + - open Serial Plotter (or Serial Monitor) at 115200 speed + - turn on uMyo device + - switch it into BLE mode if necessary (to switch, press button once or twice, depending on current mode) + - you should see Yaw, Pitch, Roll angles on a plot (or serial output) +*/ + +#include + +void setup() { + Serial.begin(115200); + uMyo.begin(); +} + +void loop() +{ + int dev_count = uMyo.getDeviceCount(); //if more than one device is present, show all of them + for(int d = 0; d < dev_count; d++) + { + Serial.print(uMyo.getYaw(d)); + Serial.print(' '); + Serial.print(uMyo.getPitch(d)); + Serial.print(' '); + Serial.print(uMyo.getRoll(d)); + if(d < dev_count-1) Serial.print(' '); + else Serial.println(); + } + delay(30); +} diff --git a/examples/umyo_LED_strip/umyo_LED_strip.ino b/examples/umyo_LED_strip/umyo_LED_strip.ino new file mode 100644 index 0000000..ac463bc --- /dev/null +++ b/examples/umyo_LED_strip/umyo_LED_strip.ino @@ -0,0 +1,39 @@ +/* +Obtaining muscle data from uMyo via BLE on ESP32 and showing +activity level on a connected WS2812 LED strip +*/ + +#include +#include + +#define NUM_LEDS 16 +#define DATA_PIN 13 + +CRGB leds[NUM_LEDS]; + +void setup() { + Serial.begin(115200); + uMyo.begin(); + FastLED.addLeds(leds, NUM_LEDS); + for(int l = 0; l < NUM_LEDS; l++) + leds[l] = CRGB(0, 0, 0); + FastLED.show(); +} + +void loop() +{ + int dev_count = uMyo.getDeviceCount(); //if more than one device is present, show all of them + float muscle_level = 0; + if(dev_count > 0) muscle_level = uMyo.getMuscleLevel(0); //take data from the 1st connected device if more than 1 is connected + float max_level = 600; //defines sensitivity - the lower is maximum, the less effort is needed to fill all LEDs + int active_leds = NUM_LEDS * muscle_level / max_level; + int brightness = 150; + for(int l = 0; l < NUM_LEDS; l++) + { + if(l < active_leds) leds[l] = CRGB(l * brightness / NUM_LEDS, 0, brightness/2); //blue -> purple red color scale + else leds[l] = CRGB(0, 0, 0); + } + FastLED.show(); + delay(5); +} + diff --git a/examples/umyo_basic/umyo_basic.ino b/examples/umyo_basic/umyo_basic.ino new file mode 100644 index 0000000..bd1b364 --- /dev/null +++ b/examples/umyo_basic/umyo_basic.ino @@ -0,0 +1,27 @@ +/* +Obtaining muscle data from uMyo via BLE on ESP32 +Usage: run the code on ESP32 device + - open Serial Plotter (or Serial Monitor) at 115200 speed + - turn on uMyo device + - switch it into BLE mode if necessary (to switch, press button once or twice, depending on current mode) + - you should see muscle activity chart on a plot (or as serial output) +*/ + +#include + +void setup() { + Serial.begin(115200); + uMyo.begin(); +} + +void loop() +{ + int dev_count = uMyo.getDeviceCount(); //if more than one device is present, show all of them + for(int d = 0; d < dev_count; d++) + { + Serial.print(uMyo.getMuscleLevel(d)); + if(d < dev_count-1) Serial.print(' '); + else Serial.println(); + } + delay(30); +} diff --git a/keywords.txt b/keywords.txt new file mode 100644 index 0000000..d3347ac --- /dev/null +++ b/keywords.txt @@ -0,0 +1,28 @@ +####################################### +# Syntax Coloring Map For Keyboard +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +uMyo KEYWORD2 +begin KEYWORD2 +stop KEYWORD2 +getDeviceCount KEYWORD2 +getBattery KEYWORD2 +getID KEYWORD2 +getDataID KEYWORD2 +getMuscleLevel KEYWORD2 +getSpectrum KEYWORD2 +getRoll KEYWORD2 +getPitch KEYWORD2 +getYaw KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### diff --git a/library.properties b/library.properties new file mode 100644 index 0000000..b39d400 --- /dev/null +++ b/library.properties @@ -0,0 +1,9 @@ +name=uMyo_BLE +version=1.0.0 +author=the_3d6 from Ultimate Robotics +category=Sensors +maintainer=hi@ultimaterobotics.com.ua +sentence=Allows to get data from uMyo devices using ESP32 device via BLE. +paragraph=Receives data from uMyo devices using BLE radio and makes them available +url=https://github.com/ultimaterobotics/uMyo_BLE +architectures=esp32 diff --git a/src/quat_math.cpp b/src/quat_math.cpp new file mode 100644 index 0000000..f71d80e --- /dev/null +++ b/src/quat_math.cpp @@ -0,0 +1,222 @@ +#include "quat_math.h" + +float sqrt_f(float x) +{ + if(x < 0) return -1; + if(x == 0) return 0; + float aa = x; + float bb = 1; + float mult = 1; + int m_iters = 0; + while(aa > 100 && m_iters++ < 20) + { + aa *= 0.01; + mult *= 10; + } + while(aa < 0.01 && m_iters++ < 20) + { + aa *= 100; + mult *= 0.1; + } + for(int nn = 0; nn < 6; ++nn) + { + float avg = 0.5f*(aa+bb); + bb *= aa/avg; + aa = avg; + } + return mult*0.5f*(aa+bb); +} +float sin_f(float v) +{ + float res = 0; + while (v < -3.14159265f) v += 6.28318531f; + while(v > 3.14159265f) v -= 6.28318531f; + + if (v < 0) + { + res = 1.27323954f * v + .405284735f * v * v; + + if (res < 0) + res = .225f * (res *-res - res) + res; + else + res = .225f * (res * res - res) + res; + } + else + { + res = 1.27323954f * v - 0.405284735f * v * v; + + if (res < 0) + res = .225f * (res *-res - res) + res; + else + res = .225f * (res * res - res) + res; + } + return res; +} + +float cos_f(float v) +{ + return sin_f(v+1.57079632f); +} + +float atan2_f(float y, float x) +{ + float ax = x; + float ay = y; + if(ax < 0) ax = -x; + if(ay < 0) ay = -y; + float mn = ax; + float mx = ax; + if(mx < 0.0000001f) return 0; //0,0 - undefined + if(ay > ax) mx = ay; + else mn = ay; + float a = mn / mx; + float s = a*a; + float r = ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a; + if(ay > ax) r = 1.57079637f - r; + if(x < 0) r = 3.1415926f - r; + if(y < 0) r = -r; + return r; +} + +float asin_f(float x) +{ + float sgn = 1; + if(x < 0) sgn = -1, x = -x; + + float x21 = x*2.0f - 1.0f; + float x21_2 = x21*x21; + float x21_3 = x21*x21_2; + float x21_4 = x21*x21_3; + float x21_5 = x21*x21_4; + float x21_6 = x21*x21_5; + float res = -0.09902914f + 1.235307f*x + .2237106f*x21_2 - .1746904f * x21_3 - .42491176f * x21_4 + .30870647f * x21_5 + .4401081f * x21_6; + return sgn * res; +} + +float acos_f(float x) +{ + return 3.1415926*0.5 - asin_f(x); +} + +float q_norm(sQ *q) +{ + return sqrt_f(q->x*q->x + q->y*q->y + q->z*q->z + q->w*q->w); +} + +float v_norm(sV *v) +{ + return sqrt_f(v->x*v->x + v->y*v->y + v->z*v->z); +} + +void q_renorm(sQ *q) +{ + float r = q_norm(q); + if(r > 0) + { + float m = 1.0f / r; + q->x *= m; + q->y *= m; + q->z *= m; + q->w *= m; + } +} + +void v_renorm(sV *v) +{ + float r = v_norm(v); + if(r > 0) + { + float m = 1.0f / r; + v->x *= m; + v->y *= m; + v->z *= m; + } +} + +void q_make_conj(sQ *q, sQ *qc) +{ + qc->x = -q->x; + qc->y = -q->y; + qc->z = -q->z; + qc->w = q->w; +} + +void q_set(sQ *target, sQ *source) +{ + target->x = source->x; + target->y = source->y; + target->z = source->z; + target->w = source->w; +} + +void v_set(sV *target, sV *source) +{ + target->x = source->x; + target->y = source->y; + target->z = source->z; +} + +void q_mult(sQ *q1, sQ *q2, sQ *qr) +{ +// x1 y1 z1 +// x2 y2 z2 +// x y z + qr->w = q1->w*q2->w - (q1->x*q2->x + q1->y*q2->y + q1->z*q2->z); + qr->x = q1->w*q2->x + q2->w*q1->x + q1->y*q2->z - q1->z*q2->y; + qr->y = q1->w*q2->y + q2->w*q1->y + q1->z*q2->x - q1->x*q2->z; + qr->z = q1->w*q2->z + q2->w*q1->z + q1->x*q2->y - q1->y*q2->x; +} + +void rotate_v(sQ *q, sV *v) +{ + sQ r; + r.w = 0; + r.x = v->x; + r.y = v->y; + r.z = v->z; + sQ qc, qq, rq; + q_make_conj(q, &qc); + q_mult(&r, &qc, &qq); + q_mult(q, &qq, &rq); + v->x = rq.x; + v->y = rq.y; + v->z = rq.z; +} + +void qv_mult(sQ *q1, sQ *q2, sQ *res) +{ +// x1 y1 z1 +// x2 y2 z2 +// x y z + res->w = 0; + res->x = q1->y*q2->z - q1->z*q2->y; + res->y = q1->z*q2->x - q1->x*q2->z; + res->z = q1->x*q2->y - q1->y*q2->x; +} + +void v_mult(sV *v1, sV *v2, sV *res) +{ +// x1 y1 z1 +// x2 y2 z2 +// x y z + res->x = v1->y*v2->z - v1->z*v2->y; + res->y = v1->z*v2->x - v1->x*v2->z; + res->z = v1->x*v2->y - v1->y*v2->x; +} +float v_dot(sV *v1, sV *v2) +{ + return v1->x*v2->x + v1->y*v2->y + v1->z*v2->z; +} + +void q_from_vectors(sV *u, sV *v, sQ *res) +{ + float d = v_dot(u, v); + sV w; + v_mult(u, v, &w); + res->w = d + sqrt_f(d*d + v_dot(&w, &w)); + res->x = w.x; + res->y = w.y; + res->z = w.z; + + q_renorm(res); +} \ No newline at end of file diff --git a/src/quat_math.h b/src/quat_math.h new file mode 100644 index 0000000..3e4cdb1 --- /dev/null +++ b/src/quat_math.h @@ -0,0 +1,39 @@ + +#define M_PI 3.1415926 + +typedef struct sQ +{ + float x; + float y; + float z; + float w; +}sQ; + +typedef struct sV +{ + float x; + float y; + float z; +}sV; + + +float q_norm(sQ *q); +float v_norm(sV *v); +void q_renorm(sQ *q); +void v_renorm(sV *v); +void q_make_conj(sQ *q, sQ *qc); +void q_mult(sQ *q1, sQ *q2, sQ *qr); +void q_set(sQ *target, sQ *source); +void rotate_v(sQ *q, sV *v); +void qv_mult(sQ *q1, sQ *q2, sQ *res); +void v_mult(sV *v1, sV *v2, sV *res); +float v_dot(sV *v1, sV *v2); +void v_set(sV *target, sV *source); +void q_from_vectors(sV *u, sV *v, sQ *res); + +float sqrt_f(float x); +float sin_f(float v); +float cos_f(float v); +float asin_f(float x); +float acos_f(float x); +float atan2_f(float y, float x); diff --git a/src/uMyo_BLE.cpp b/src/uMyo_BLE.cpp new file mode 100644 index 0000000..326ea15 --- /dev/null +++ b/src/uMyo_BLE.cpp @@ -0,0 +1,204 @@ +/* + uMyo_BLE.cpp + + Copyright (c) 2022, Ultimate Robotics + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ +#include "uMyo_BLE.h" + +int uMyo_BLE_::scanTime = 5; +BLEScan* uMyo_BLE_::pBLEScan = NULL; + +class UMyoAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks +{ + void onResult(BLEAdvertisedDevice advertisedDevice) + { + uint8_t *pack = advertisedDevice.getPayload(); + int len = advertisedDevice.getPayloadLength(); + int pp = 0; + while(pp < len-5) + { + pp++; + if(pack[pp] != 8) continue; + if(pack[pp+1] != 'u') continue; + if(pack[pp+2] != 'M') continue; + if(pack[pp+3] != 'y') continue; + if(pack[pp+4] != 'o') continue; + while(pack[pp] != 255 && pp < len) pp++; + if(pp >= len) return; + pp++; + + int adc_id = pack[pp++]; + int batt_level = pack[pp++]; + int16_t sp0 = (pack[pp]<<8) | pack[pp+1]; pp += 2; + int16_t sp1 = (pack[pp]<<8) | pack[pp+1]; pp += 2; + int16_t sp2 = (pack[pp]<<8) | pack[pp+1]; pp += 2; + int16_t sp3 = (pack[pp]<<8) | pack[pp+1]; pp += 2; + int16_t qw = (pack[pp]<<8) | pack[pp+1]; pp += 2; + int16_t qx = (pack[pp++]<<8); + int16_t qy = (pack[pp++]<<8); + int16_t qz = (pack[pp++]<<8); + sQ Qsg; + Qsg.w = qw; + Qsg.x = qx; + Qsg.y = qy; + Qsg.z = qz; + q_renorm(&Qsg); + + uint8_t *mac = *advertisedDevice.getAddress().getNative(); + uint32_t unit_id = (mac[1]<<24) | (mac[2]<<16) | (mac[3]<<8) | mac[4]; + uMyo_data *device = uMyo.getDeviceByID(unit_id); + device->last_data_id = adc_id; + device->batt_mv = 2000 + batt_level*10; + device->cur_spectrum[0] = sp0; + device->cur_spectrum[1] = sp1; + device->cur_spectrum[2] = sp2; + device->cur_spectrum[3] = sp3; + device->Qsg = Qsg; + + } + return; + } +}; + +void uMyo_BLE_::rescan(BLEScanResults res) +{ + if(scanTime > 0) pBLEScan->start(scanTime, uMyo_BLE_::rescan, true); +} + +uMyo_BLE_::uMyo_BLE_() +{ + nx.x = 1; ny.x = 0; nz.x = 0; //for angles calculation + nx.y = 0; ny.y = 1; nz.y = 0; + nx.z = 0; ny.z = 0; nz.z = 1; + device_count = 0; +} +void uMyo_BLE_::begin() +{ + scanTime = 5; //seconds + BLEDevice::init(""); + pBLEScan = BLEDevice::getScan(); //create new scan + pBLEScan->setAdvertisedDeviceCallbacks(new UMyoAdvertisedDeviceCallbacks()); + pBLEScan->setActiveScan(true); //active scan uses more power, but get results faster + pBLEScan->setInterval(100); + pBLEScan->setWindow(99); // less or equal setInterval value + pBLEScan->start(scanTime, this->rescan, false); +} +void uMyo_BLE_::pause() +{ + scanTime = 0; //stops rescan +} +void uMyo_BLE_::resume() +{ + scanTime = 5; + pBLEScan->start(scanTime, this->rescan, true); +} + +uint8_t uMyo_BLE_::idToIdx(uint32_t id) +{ + uint32_t ms = millis(); + for(uint8_t u = 0; u < device_count; u++) + { + if(ms - devices[u].last_data_time > 5000) + { + for(int u1 = u+1; u1 < device_count; u1++) + devices[u1-1] = devices[u1]; + device_count--; + } + } + for(uint8_t u = 0; u < device_count; u++) + if(id == devices[u].id) + { + devices[u].last_data_time = ms; + return u; + } + if(device_count < MAX_UMYO_DEVICES) + { + uint8_t u = device_count; + devices[u].id = id; + devices[u].last_data_time = ms; + device_count++; + } + return device_count-1; +} +uMyo_data *uMyo_BLE_::getDeviceByID(uint32_t id) +{ + return devices + idToIdx(id); +} +uint8_t uMyo_BLE_::getDeviceCount() +{ + return device_count; +} +int uMyo_BLE_::getBattery(uint8_t devidx) +{ + if(devidx < device_count) return devices[devidx].batt_mv; + return 0; +} +uint32_t uMyo_BLE_::getID(uint8_t devidx) +{ + if(devidx < device_count) return devices[devidx].id; + return 0; +} +uint8_t uMyo_BLE_::getDataID(uint8_t devidx) +{ + if(devidx < device_count) return devices[devidx].last_data_id; + return 0; +} +float uMyo_BLE_::getMuscleLevel(uint8_t devidx) +{ + if(devidx >= device_count) return 0; + float lvl = devices[devidx].cur_spectrum[2] + 2*devices[devidx].cur_spectrum[3]; + return lvl; +} +void uMyo_BLE_::getSpectrum(uint8_t devidx, float *spectrum) +{ + if(devidx >= device_count) return; + for(uint8_t x = 0; x < 4; x++) spectrum[x] = devices[devidx].cur_spectrum[x]; +} +float uMyo_BLE_::getPitch(uint8_t devidx) +{ + if(devidx >= device_count) return 0; + sV nzg = nz; + sQ Qgs; + Qgs.w = devices[devidx].Qsg.w; + Qgs.x = -devices[devidx].Qsg.x; + Qgs.y = -devices[devidx].Qsg.y; + Qgs.z = -devices[devidx].Qsg.z; + rotate_v(&Qgs, &nzg); + return acos_f(v_dot(&nzg, &ny)); +} +float uMyo_BLE_::getRoll(uint8_t devidx) +{ + if(devidx >= device_count) return 0; + sV nzg = nz; + sQ Qgs; + Qgs.w = devices[devidx].Qsg.w; + Qgs.x = -devices[devidx].Qsg.x; + Qgs.y = -devices[devidx].Qsg.y; + Qgs.z = -devices[devidx].Qsg.z; + rotate_v(&Qgs, &nzg); + return atan2_f(nzg.z, nzg.x); +} +float uMyo_BLE_::getYaw(uint8_t devidx) +{ + if(devidx >= device_count) return 0; + sV nyr = ny; + rotate_v(&devices[devidx].Qsg, &nyr); + return atan2_f(nyr.y, nyr.x); +} +uMyo_BLE_ uMyo; + + diff --git a/src/uMyo_BLE.h b/src/uMyo_BLE.h new file mode 100644 index 0000000..453d798 --- /dev/null +++ b/src/uMyo_BLE.h @@ -0,0 +1,78 @@ +/* + uMyo_BLE.h + + Copyright (c) 2022, Ultimate Robotics + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#ifndef UMYO_BLE_h +#define UMYO_BLE_h + +#include +#include +#include "quat_math.h" +#define MAX_UMYO_DEVICES 12 + +typedef struct uMyo_data +{ + uint32_t id; + int batt_mv; + uint8_t last_data_id; + int16_t cur_spectrum[4]; + uint32_t last_data_time; + sQ Qsg; + int operator=(uMyo_data d2) + { + id = d2.id; + batt_mv = d2.batt_mv; + last_data_id = d2.last_data_id; + last_data_time = d2.last_data_time; + for(int x = 0; x < 4; x++) cur_spectrum[x] = d2.cur_spectrum[x]; + Qsg = d2.Qsg; + return 1; + } +}uMyo_data; + +class uMyo_BLE_ +{ +private: + uMyo_data devices[MAX_UMYO_DEVICES]; + sV nx, ny, nz; + int device_count; + uint8_t idToIdx(uint32_t id); + static int scanTime; + static BLEScan *pBLEScan; + static void rescan(BLEScanResults res); +public: + uMyo_BLE_(void); + void begin(); + void pause(); + void resume(); + uint8_t getDeviceCount(); + int getBattery(uint8_t devidx); + uint32_t getID(uint8_t devidx); + uint8_t getDataID(uint8_t devidx); + float getMuscleLevel(uint8_t devidx); + void getSpectrum(uint8_t devidx, float *spectrum); + void getRawData(uint8_t devidx, int16_t *data); + float getPitch(uint8_t devidx); + float getRoll(uint8_t devidx); + float getYaw(uint8_t devidx); + uMyo_data *getDeviceByID(uint32_t id); +}; +extern uMyo_BLE_ uMyo; + +#endif