diff --git a/8266/#8266.org# b/8266/#8266.org# new file mode 100644 index 0000000..d89502e --- /dev/null +++ b/8266/#8266.org# @@ -0,0 +1,25 @@ +#+TITLE: 8266 demo za projekt na pifcampu + +* Arduino intro: + https://create.arduino.cc/projecthub/electropeak/getting-started-w-nodemcu-esp8266-on-arduino-ide-28184f + +* Arduino kbd matrika: + https://www.baldengineer.com/arduino-keyboard-matrix-tutorial.html + +* GY-521 howto + https://create.arduino.cc/projecthub/Nicholas_N/how-to-use-the-accelerometer-gyroscope-gy-521-6dfc19 + + Daljsi tutorial: + https://olivertechnologydevelopment.wordpress.com/2017/08/17/esp8266-sensor-series-gy-521-imu-part-1/ + +* ModriZob + https://randomnerdtutorials.com/esp32-bluetooth-classic-arduino-ide/ + https://github.com/espressif/arduino-esp32/tree/master/libraries/BluetoothSerial + https://randomnerdtutorials.com/esp32-bluetooth-classic-arduino-ide/ + +* OSC + https://github.com/CNMAT/OSC + +* high level MPU-6050 + */ +jjkkjkjk \ No newline at end of file diff --git a/IMU_Zero/IMU_Zero/IMU_Zero.ino b/IMU_Zero/IMU_Zero/IMU_Zero.ino new file mode 100644 index 0000000..8e50b99 --- /dev/null +++ b/IMU_Zero/IMU_Zero/IMU_Zero.ino @@ -0,0 +1,358 @@ +// MPU6050 offset-finder, based on Jeff Rowberg's MPU6050_RAW +// 2016-10-19 by Robert R. Fenichel (bob@fenichel.net) + +// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class +// 10/7/2011 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2019-07-11 - added PID offset generation at begninning Generates first offsets +// - in @ 6 seconds and completes with 4 more sets @ 10 seconds +// - then continues with origional 2016 calibration code. +// 2016-11-25 - added delays to reduce sampling rate to ~200 Hz +// added temporizing printing during long computations +// 2016-10-25 - requires inequality (Low < Target, High > Target) during expansion +// dynamic speed change when closing in +// 2016-10-22 - cosmetic changes +// 2016-10-19 - initial release of IMU_Zero +// 2013-05-08 - added multiple output formats +// - added seamless Fastwire support +// 2011-10-07 - initial release of MPU6050_RAW + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2011 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + + If an MPU6050 + * is an ideal member of its tribe, + * is properly warmed up, + * is at rest in a neutral position, + * is in a location where the pull of gravity is exactly 1g, and + * has been loaded with the best possible offsets, +then it will report 0 for all accelerations and displacements, except for +Z acceleration, for which it will report 16384 (that is, 2^14). Your device +probably won't do quite this well, but good offsets will all get the baseline +outputs close to these target values. + + Put the MPU6050 on a flat and horizontal surface, and leave it operating for +5-10 minutes so its temperature gets stabilized. + + Run this program. A "----- done -----" line will indicate that it has done its best. +With the current accuracy-related constants (NFast = 1000, NSlow = 10000), it will take +a few minutes to get there. + + Along the way, it will generate a dozen or so lines of output, showing that for each +of the 6 desired offsets, it is + * first, trying to find two estimates, one too low and one too high, and + * then, closing in until the bracket can't be made smaller. + + The line just above the "done" line will look something like + [567,567] --> [-1,2] [-2223,-2223] --> [0,1] [1131,1132] --> [16374,16404] [155,156] --> [-1,1] [-25,-24] --> [0,3] [5,6] --> [0,4] +As will have been shown in interspersed header lines, the six groups making up this +line describe the optimum offsets for the X acceleration, Y acceleration, Z acceleration, +X gyro, Y gyro, and Z gyro, respectively. In the sample shown just above, the trial showed +that +567 was the best offset for the X acceleration, -2223 was best for Y acceleration, +and so on. + + The need for the delay between readings (usDelay) was brought to my attention by Nikolaus Doppelhammer. +=============================================== +*/ + +// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files +// for both classes must be in the include path of your project +#include "I2Cdev.h" +#include "MPU6050.h" + +// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation +// is used in I2Cdev.h +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + #include "Wire.h" +#endif + +// class default I2C address is 0x68 +// specific I2C addresses may be passed as a parameter here +// AD0 low = 0x68 (default for InvenSense evaluation board) +// AD0 high = 0x69 +MPU6050 accelgyro; +//MPU6050 accelgyro(0x69); // <-- use for AD0 high + + +const char LBRACKET = '['; +const char RBRACKET = ']'; +const char COMMA = ','; +const char BLANK = ' '; +const char PERIOD = '.'; + +const int iAx = 0; +const int iAy = 1; +const int iAz = 2; +const int iGx = 3; +const int iGy = 4; +const int iGz = 5; + +const int usDelay = 3150; // empirical, to hold sampling to 200 Hz +const int NFast = 1000; // the bigger, the better (but slower) +const int NSlow = 10000; // .. +const int LinesBetweenHeaders = 5; + int LowValue[6]; + int HighValue[6]; + int Smoothed[6]; + int LowOffset[6]; + int HighOffset[6]; + int Target[6]; + int LinesOut; + int N; + +void ForceHeader() + { LinesOut = 99; } + +void GetSmoothed() + { int16_t RawValue[6]; + int i; + long Sums[6]; + for (i = iAx; i <= iGz; i++) + { Sums[i] = 0; } +// unsigned long Start = micros(); + + for (i = 1; i <= N; i++) + { // get sums + accelgyro.getMotion6(&RawValue[iAx], &RawValue[iAy], &RawValue[iAz], + &RawValue[iGx], &RawValue[iGy], &RawValue[iGz]); + if ((i % 500) == 0) + Serial.print(PERIOD); + delayMicroseconds(usDelay); + for (int j = iAx; j <= iGz; j++) + Sums[j] = Sums[j] + RawValue[j]; + } // get sums +// unsigned long usForN = micros() - Start; +// Serial.print(" reading at "); +// Serial.print(1000000/((usForN+N/2)/N)); +// Serial.println(" Hz"); + for (i = iAx; i <= iGz; i++) + { Smoothed[i] = (Sums[i] + N/2) / N ; } + } // GetSmoothed + +void Initialize() + { + // join I2C bus (I2Cdev library doesn't do this automatically) + #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + Wire.begin(); + #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + Fastwire::setup(400, true); + #endif + + Serial.begin(9600); + + // initialize device + Serial.println("Initializing I2C devices..."); + accelgyro.initialize(); + + // verify connection + Serial.println("Testing device connections..."); + Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); + Serial.println("PID tuning Each Dot = 100 readings"); + /*A tidbit on how PID (PI actually) tuning works. + When we change the offset in the MPU6050 we can get instant results. This allows us to use Proportional and + integral of the PID to discover the ideal offsets. Integral is the key to discovering these offsets, Integral + uses the error from set-point (set-point is zero), it takes a fraction of this error (error * ki) and adds it + to the integral value. Each reading narrows the error down to the desired offset. The greater the error from + set-point, the more we adjust the integral value. The proportional does its part by hiding the noise from the + integral math. The Derivative is not used because of the noise and because the sensor is stationary. With the + noise removed the integral value lands on a solid offset after just 600 readings. At the end of each set of 100 + readings, the integral value is used for the actual offsets and the last proportional reading is ignored due to + the fact it reacts to any noise. + */ + accelgyro.CalibrateAccel(6); + accelgyro.CalibrateGyro(6); + Serial.println("\nat 600 Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("700 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("800 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("900 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("1000 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println("\n\n Any of the above offsets will work nice \n\n Lets proof the PID tuning using another method:"); + } // Initialize + +void SetOffsets(int TheOffsets[6]) + { accelgyro.setXAccelOffset(TheOffsets [iAx]); + accelgyro.setYAccelOffset(TheOffsets [iAy]); + accelgyro.setZAccelOffset(TheOffsets [iAz]); + accelgyro.setXGyroOffset (TheOffsets [iGx]); + accelgyro.setYGyroOffset (TheOffsets [iGy]); + accelgyro.setZGyroOffset (TheOffsets [iGz]); + } // SetOffsets + +void ShowProgress() + { if (LinesOut >= LinesBetweenHeaders) + { // show header + Serial.println("\tXAccel\t\t\tYAccel\t\t\t\tZAccel\t\t\tXGyro\t\t\tYGyro\t\t\tZGyro"); + LinesOut = 0; + } // show header + Serial.print(BLANK); + for (int i = iAx; i <= iGz; i++) + { Serial.print(LBRACKET); + Serial.print(LowOffset[i]), + Serial.print(COMMA); + Serial.print(HighOffset[i]); + Serial.print("] --> ["); + Serial.print(LowValue[i]); + Serial.print(COMMA); + Serial.print(HighValue[i]); + if (i == iGz) + { Serial.println(RBRACKET); } + else + { Serial.print("]\t"); } + } + LinesOut++; + } // ShowProgress + +void PullBracketsIn() + { boolean AllBracketsNarrow; + boolean StillWorking; + int NewOffset[6]; + + Serial.println("\nclosing in:"); + AllBracketsNarrow = false; + ForceHeader(); + StillWorking = true; + while (StillWorking) + { StillWorking = false; + if (AllBracketsNarrow && (N == NFast)) + { SetAveraging(NSlow); } + else + { AllBracketsNarrow = true; }// tentative + for (int i = iAx; i <= iGz; i++) + { if (HighOffset[i] <= (LowOffset[i]+1)) + { NewOffset[i] = LowOffset[i]; } + else + { // binary search + StillWorking = true; + NewOffset[i] = (LowOffset[i] + HighOffset[i]) / 2; + if (HighOffset[i] > (LowOffset[i] + 10)) + { AllBracketsNarrow = false; } + } // binary search + } + SetOffsets(NewOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { // closing in + if (Smoothed[i] > Target[i]) + { // use lower half + HighOffset[i] = NewOffset[i]; + HighValue[i] = Smoothed[i]; + } // use lower half + else + { // use upper half + LowOffset[i] = NewOffset[i]; + LowValue[i] = Smoothed[i]; + } // use upper half + } // closing in + ShowProgress(); + } // still working + + } // PullBracketsIn + +void PullBracketsOut() + { boolean Done = false; + int NextLowOffset[6]; + int NextHighOffset[6]; + + Serial.println("expanding:"); + ForceHeader(); + + while (!Done) + { Done = true; + SetOffsets(LowOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { // got low values + LowValue[i] = Smoothed[i]; + if (LowValue[i] >= Target[i]) + { Done = false; + NextLowOffset[i] = LowOffset[i] - 1000; + } + else + { NextLowOffset[i] = LowOffset[i]; } + } // got low values + + SetOffsets(HighOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { // got high values + HighValue[i] = Smoothed[i]; + if (HighValue[i] <= Target[i]) + { Done = false; + NextHighOffset[i] = HighOffset[i] + 1000; + } + else + { NextHighOffset[i] = HighOffset[i]; } + } // got high values + ShowProgress(); + for (int i = iAx; i <= iGz; i++) + { LowOffset[i] = NextLowOffset[i]; // had to wait until ShowProgress done + HighOffset[i] = NextHighOffset[i]; // .. + } + } // keep going + } // PullBracketsOut + +void SetAveraging(int NewN) + { N = NewN; + Serial.print("averaging "); + Serial.print(N); + Serial.println(" readings each time"); + } // SetAveraging + +void setup() + { Initialize(); + for (int i = iAx; i <= iGz; i++) + { // set targets and initial guesses + Target[i] = 0; // must fix for ZAccel + HighOffset[i] = 0; + LowOffset[i] = 0; + } // set targets and initial guesses + Target[iAz] = 16384; + SetAveraging(NFast); + + PullBracketsOut(); + PullBracketsIn(); + + Serial.println("-------------- done --------------"); + } // setup + +void loop() + { + } // loop diff --git a/IMU_ZeroBT/IMU_ZeroBT.ino b/IMU_ZeroBT/IMU_ZeroBT.ino new file mode 100644 index 0000000..d15b239 --- /dev/null +++ b/IMU_ZeroBT/IMU_ZeroBT.ino @@ -0,0 +1,404 @@ +// MPU6050 offset-finder, based on Jeff Rowberg's MPU6050_RAW +// 2016-10-19 by Robert R. Fenichel (bob@fenichel.net) + +// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class +// 10/7/2011 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2019-07-11 - added PID offset generation at begninning Generates first offsets +// - in @ 6 seconds and completes with 4 more sets @ 10 seconds +// - then continues with origional 2016 calibration code. +// 2016-11-25 - added delays to reduce sampling rate to ~200 Hz +// added temporizing printing during long computations +// 2016-10-25 - requires inequality (Low < Target, High > Target) during expansion +// dynamic speed change when closing in +// 2016-10-22 - cosmetic changes +// 2016-10-19 - initial release of IMU_Zero +// 2013-05-08 - added multiple output formats +// - added seamless Fastwire support +// 2011-10-07 - initial release of MPU6050_RAW + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2011 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + + If an MPU6050 + * is an ideal member of its tribe, + * is properly warmed up, + * is at rest in a neutral position, + * is in a location where the pull of gravity is exactly 1g, and + * has been loaded with the best possible offsets, +then it will report 0 for all accelerations and displacements, except for +Z acceleration, for which it will report 16384 (that is, 2^14). Your device +probably won't do quite this well, but good offsets will all get the baseline +outputs close to these target values. + + Put the MPU6050 on a flat and horizontal surface, and leave it operating for +5-10 minutes so its temperature gets stabilized. + + Run this program. A "----- done -----" line will indicate that it has done its best. +With the current accuracy-related constants (NFast = 1000, NSlow = 10000), it will take +a few minutes to get there. + + Along the way, it will generate a dozen or so lines of output, showing that for each +of the 6 desired offsets, it is + * first, trying to find two estimates, one too low and one too high, and + * then, closing in until the bracket can't be made smaller. + + The line just above the "done" line will look something like + [567,567] --> [-1,2] [-2223,-2223] --> [0,1] [1131,1132] --> [16374,16404] [155,156] --> [-1,1] [-25,-24] --> [0,3] [5,6] --> [0,4] +As will have been shown in interspersed header lines, the six groups making up this +line describe the optimum offsets for the X acceleration, Y acceleration, Z acceleration, +X gyro, Y gyro, and Z gyro, respectively. In the sample shown just above, the trial showed +that +567 was the best offset for the X acceleration, -2223 was best for Y acceleration, +and so on. + + The need for the delay between readings (usDelay) was brought to my attention by Nikolaus Doppelhammer. +=============================================== +*/ + +// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files +// for both classes must be in the include path of your project +#include "I2Cdev.h" +#include "MPU6050.h" + +// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation +// is used in I2Cdev.h +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + #include "Wire.h" +#endif + +// class default I2C address is 0x68 +// specific I2C addresses may be passed as a parameter here +// AD0 low = 0x68 (default for InvenSense evaluation board) +// AD0 high = 0x69 +MPU6050 accelgyro; +//MPU6050 accelgyro(0x69); // <-- use for AD0 high + + +#include +#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) +#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it +#endif + +BluetoothSerial SerialBT; + + +const char LBRACKET = '['; +const char RBRACKET = ']'; +const char COMMA = ','; +const char BLANK = ' '; +const char PERIOD = '.'; + +const int iAx = 0; +const int iAy = 1; +const int iAz = 2; +const int iGx = 3; +const int iGy = 4; +const int iGz = 5; + +const int usDelay = 3150; // empirical, to hold sampling to 200 Hz +const int NFast = 1000; // the bigger, the better (but slower) +const int NSlow = 10000; // .. +const int LinesBetweenHeaders = 5; + int LowValue[6]; + int HighValue[6]; + int Smoothed[6]; + int LowOffset[6]; + int HighOffset[6]; + int Target[6]; + int LinesOut; + int N; + +void ForceHeader() + { LinesOut = 99; } + +void GetSmoothed() + { int16_t RawValue[6]; + int i; + long Sums[6]; + for (i = iAx; i <= iGz; i++) + { Sums[i] = 0; } +// unsigned long Start = micros(); + + for (i = 1; i <= N; i++) + { // get sums + accelgyro.getMotion6(&RawValue[iAx], &RawValue[iAy], &RawValue[iAz], + &RawValue[iGx], &RawValue[iGy], &RawValue[iGz]); + if ((i % 500) == 0) + Serial.print(PERIOD); + SerialBT.print(PERIOD); + delayMicroseconds(usDelay); + for (int j = iAx; j <= iGz; j++) + Sums[j] = Sums[j] + RawValue[j]; + } // get sums +// unsigned long usForN = micros() - Start; +// Serial.print(" reading at "); +// Serial.print(1000000/((usForN+N/2)/N)); +// Serial.println(" Hz"); + for (i = iAx; i <= iGz; i++) + { Smoothed[i] = (Sums[i] + N/2) / N ; } + } // GetSmoothed + +void Initialize() + { + // join I2C bus (I2Cdev library doesn't do this automatically) + #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + Wire.begin(); + #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + Fastwire::setup(400, true); + #endif + + Serial.begin(9600); + SerialBT.begin("WaveyWindCalibration"); //Bluetooth device name + + + // initialize device + Serial.println("Initializing I2C devices..."); + SerialBT.println("Initializing I2C devices..."); + accelgyro.initialize(); + + // verify connection + Serial.println("Testing device connections..."); + Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); + Serial.println("PID tuning Each Dot = 100 readings"); + SerialBT.println("Testing device connections..."); + SerialBT.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); + SerialBT.println("PID tuning Each Dot = 100 readings"); + /*A tidbit on how PID (PI actually) tuning works. + When we change the offset in the MPU6050 we can get instant results. This allows us to use Proportional and + integral of the PID to discover the ideal offsets. Integral is the key to discovering these offsets, Integral + uses the error from set-point (set-point is zero), it takes a fraction of this error (error * ki) and adds it + to the integral value. Each reading narrows the error down to the desired offset. The greater the error from + set-point, the more we adjust the integral value. The proportional does its part by hiding the noise from the + integral math. The Derivative is not used because of the noise and because the sensor is stationary. With the + noise removed the integral value lands on a solid offset after just 600 readings. At the end of each set of 100 + readings, the integral value is used for the actual offsets and the last proportional reading is ignored due to + the fact it reacts to any noise. + */ + accelgyro.CalibrateAccel(6); + accelgyro.CalibrateGyro(6); + Serial.println("\nat 600 Readings"); + SerialBT.println("\nat 600 Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + SerialBT.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("700 Total Readings"); + SerialBT.println("700 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + SerialBT.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("800 Total Readings"); + Serial.println("800 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + SerialBT.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("900 Total Readings"); + SerialBT.println("900 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + SerialBT.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("1000 Total Readings"); + SerialBT.println("1000 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println("\n\n Any of the above offsets will work nice \n\n Lets proof the PID tuning using another method:"); + SerialBT.println("\n\n Any of the above offsets will work nice \n\n Lets proof the PID tuning using another method:"); + } // Initialize + +void SetOffsets(int TheOffsets[6]) + { accelgyro.setXAccelOffset(TheOffsets [iAx]); + accelgyro.setYAccelOffset(TheOffsets [iAy]); + accelgyro.setZAccelOffset(TheOffsets [iAz]); + accelgyro.setXGyroOffset (TheOffsets [iGx]); + accelgyro.setYGyroOffset (TheOffsets [iGy]); + accelgyro.setZGyroOffset (TheOffsets [iGz]); + } // SetOffsets + +void ShowProgress() + { if (LinesOut >= LinesBetweenHeaders) + { // show header + Serial.println("\tXAccel\t\t\tYAccel\t\t\t\tZAccel\t\t\tXGyro\t\t\tYGyro\t\t\tZGyro"); + SerialBT.println("\tXAccel\t\t\tYAccel\t\t\t\tZAccel\t\t\tXGyro\t\t\tYGyro\t\t\tZGyro"); + LinesOut = 0; + } // show header + Serial.print(BLANK); + SerialBT.print(BLANK); + for (int i = iAx; i <= iGz; i++) + { Serial.print(LBRACKET); + Serial.print(LowOffset[i]), + Serial.print(COMMA); + Serial.print(HighOffset[i]); + Serial.print("] --> ["); + Serial.print(LowValue[i]); + Serial.print(COMMA); + Serial.print(HighValue[i]); + SerialBT.print(LBRACKET); + SerialBT.print(LowOffset[i]), + SerialBT.print(COMMA); + SerialBT.print(HighOffset[i]); + SerialBT.print("] --> ["); + SerialBT.print(LowValue[i]); + SerialBT.print(COMMA); + SerialBT.print(HighValue[i]); + if (i == iGz) + { + Serial.println(RBRACKET); + SerialBT.println(RBRACKET); + } + else + { + Serial.print("]\t"); + SerialBT.print("]\t"); + } + } + LinesOut++; + } // ShowProgress + +void PullBracketsIn() + { boolean AllBracketsNarrow; + boolean StillWorking; + int NewOffset[6]; + + Serial.println("\nclosing in:"); + SerialBT.println("\nclosing in:"); + AllBracketsNarrow = false; + ForceHeader(); + StillWorking = true; + while (StillWorking) + { StillWorking = false; + if (AllBracketsNarrow && (N == NFast)) + { SetAveraging(NSlow); } + else + { AllBracketsNarrow = true; }// tentative + for (int i = iAx; i <= iGz; i++) + { if (HighOffset[i] <= (LowOffset[i]+1)) + { NewOffset[i] = LowOffset[i]; } + else + { // binary search + StillWorking = true; + NewOffset[i] = (LowOffset[i] + HighOffset[i]) / 2; + if (HighOffset[i] > (LowOffset[i] + 10)) + { AllBracketsNarrow = false; } + } // binary search + } + SetOffsets(NewOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { // closing in + if (Smoothed[i] > Target[i]) + { // use lower half + HighOffset[i] = NewOffset[i]; + HighValue[i] = Smoothed[i]; + } // use lower half + else + { // use upper half + LowOffset[i] = NewOffset[i]; + LowValue[i] = Smoothed[i]; + } // use upper half + } // closing in + ShowProgress(); + } // still working + + } // PullBracketsIn + +void PullBracketsOut() + { boolean Done = false; + int NextLowOffset[6]; + int NextHighOffset[6]; + + Serial.println("expanding:"); + SerialBT.println("expanding:"); + ForceHeader(); + + while (!Done) + { Done = true; + SetOffsets(LowOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { // got low values + LowValue[i] = Smoothed[i]; + if (LowValue[i] >= Target[i]) + { Done = false; + NextLowOffset[i] = LowOffset[i] - 1000; + } + else + { NextLowOffset[i] = LowOffset[i]; } + } // got low values + + SetOffsets(HighOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { // got high values + HighValue[i] = Smoothed[i]; + if (HighValue[i] <= Target[i]) + { Done = false; + NextHighOffset[i] = HighOffset[i] + 1000; + } + else + { NextHighOffset[i] = HighOffset[i]; } + } // got high values + ShowProgress(); + for (int i = iAx; i <= iGz; i++) + { LowOffset[i] = NextLowOffset[i]; // had to wait until ShowProgress done + HighOffset[i] = NextHighOffset[i]; // .. + } + } // keep going + } // PullBracketsOut + +void SetAveraging(int NewN) + { N = NewN; + Serial.print("averaging "); + Serial.print(N); + Serial.println(" readings each time"); + SerialBT.print("averaging "); + SerialBT.print(N); + SerialBT.println(" readings each time"); + } // SetAveraging + +void setup() + { Initialize(); + for (int i = iAx; i <= iGz; i++) + { // set targets and initial guesses + Target[i] = 0; // must fix for ZAccel + HighOffset[i] = 0; + LowOffset[i] = 0; + } // set targets and initial guesses + Target[iAz] = 16384; + SetAveraging(NFast); + + PullBracketsOut(); + PullBracketsIn(); + + SerialBT.println("-------------- done --------------"); + } // setup + +void loop() + { + } // loop diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..be3f7b2 --- /dev/null +++ b/LICENSE @@ -0,0 +1,661 @@ + GNU AFFERO GENERAL PUBLIC LICENSE + Version 3, 19 November 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU Affero General Public License is a free, copyleft license for +software and other kinds of works, specifically designed to ensure +cooperation with the community in the case of network server software. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +our General Public Licenses are intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. + + When we speak of free software, we are referring to freedom, 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 +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + Developers that use our General Public Licenses protect your rights +with two steps: (1) assert copyright on the software, and (2) offer +you this License which gives you legal permission to copy, distribute +and/or modify the software. + + A secondary benefit of defending all users' freedom is that +improvements made in alternate versions of the program, if they +receive widespread use, become available for other developers to +incorporate. Many developers of free software are heartened and +encouraged by the resulting cooperation. However, in the case of +software used on network servers, this result may fail to come about. +The GNU General Public License permits making a modified version and +letting the public access it on a server without ever releasing its +source code to the public. + + The GNU Affero General Public License is designed specifically to +ensure that, in such cases, the modified source code becomes available +to the community. It requires the operator of a network server to +provide the source code of the modified version running there to the +users of that server. Therefore, public use of a modified version, on +a publicly accessible server, gives the public access to the source +code of the modified version. + + An older license, called the Affero General Public License and +published by Affero, was designed to accomplish similar goals. This is +a different license, not a version of the Affero GPL, but Affero has +released a new version of the Affero GPL which permits relicensing under +this license. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU Affero General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If 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 convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Remote Network Interaction; Use with the GNU General Public License. + + Notwithstanding any other provision of this License, if you modify the +Program, your modified version must prominently offer all users +interacting with it remotely through a computer network (if your version +supports such interaction) an opportunity to receive the Corresponding +Source of your version by providing access to the Corresponding Source +from a network server at no charge, through some standard or customary +means of facilitating copying of software. This Corresponding Source +shall include the Corresponding Source for any work covered by version 3 +of the GNU General Public License that is incorporated pursuant to the +following paragraph. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the work with which it is combined will remain governed by version +3 of the GNU General Public License. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU Affero 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 +Program specifies that a certain numbered version of the GNU Affero General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU Affero General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU Affero General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "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 PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM 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 PROGRAM (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 PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program 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 Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If your software can interact with users remotely through a computer +network, you should also make sure that it provides a way for users to +get its source. For example, if your program is a web application, its +interface could display a "Source" link that leads users to an archive +of the code. There are many ways you could offer source, and different +solutions will be better for different programs; see section 13 for the +specific requirements. + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU AGPL, see +. diff --git a/kbd/kbd/kbd.ino b/kbd/kbd/kbd.ino new file mode 100644 index 0000000..34bd5f0 --- /dev/null +++ b/kbd/kbd/kbd.ino @@ -0,0 +1,23 @@ +// Keyboard Matrix Tutorial Example +// baldengineer.com +// CC BY-SA 4.0 + +// JP1 is an input +byte keys[] = {16, 17, 5, 18}; +byte pressed[] = {0, 0, 0, 0}; +byte KEYLEN = 4; + +void setup() { + Serial.begin(9600); + for(int i = 0; i < KEYLEN; i++) { + pinMode(keys[i], INPUT_PULLUP); + } +} +void loop() { + for(int i = 0; i < KEYLEN; i++) { + pressed[i] = !digitalRead(keys[i]); + Serial.print(String(pressed[i]) + " "); + } + Serial.println(); + delay(50); +} diff --git a/kbdtest/kbdtest/kbdtest.ino b/kbdtest/kbdtest/kbdtest.ino new file mode 100644 index 0000000..ac8a7fe --- /dev/null +++ b/kbdtest/kbdtest/kbdtest.ino @@ -0,0 +1,70 @@ +// Keyboard Matrix Tutorial Example +// baldengineer.com +// CC BY-SA 4.0 + +// JP1 is an input +byte rows[] = {16,17}; +const int rowCount = sizeof(rows)/sizeof(rows[0]); + +// JP2 and JP3 are outputs +byte cols[] = {18, 2}; +const int colCount = sizeof(cols)/sizeof(cols[0]); + +byte keys[colCount][rowCount]; + +void setup() { + Serial.begin(9600); + + for(int x=0; x + +const byte ROWS = 2; //four rows +const byte COLS = 2; //three columns +char keys[ROWS][COLS] = { +{'a','b'}, +{'c','d'}, +}; +byte rowPins[ROWS] = {16,17}; //connect to the row pinouts of the kpd +byte colPins[COLS] = {18, 2}; //connect to the column pinouts of the kpd + +Keypad kpd = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS ); + +unsigned long loopCount; +unsigned long startTime; +String msg; + + +void setup() { + Serial.begin(9600); + loopCount = 0; + startTime = millis(); + msg = ""; +} + + +void loop() { + loopCount++; + if ( (millis()-startTime)>5000 ) { + Serial.print("Average loops per second = "); + Serial.println(loopCount/5); + startTime = millis(); + loopCount = 0; + } + + // Fills kpd.key[ ] array with up-to 10 active keys. + // Returns true if there are ANY active keys. + if (kpd.getKeys()) + { + for (int i=0; i +#include +/* +Make an OSC message and send it over serial + */ + +#ifdef BOARD_HAS_USB_SERIAL +#include +SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB ); +#else +#include + SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial +#endif + +/* +#include +#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) +#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it +#endif + +BluetoothSerial SerialBT; +*/ + +const int MPU_addr=0x68; // I2C address of the MPU-6050 + +// Sem dobimo vrednosti +int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; + +void setup() { + Wire.begin(); + SLIPSerial.begin(9600); // set this as high as you can reliably run on your platform + + Wire.begin(21, 22, 100000); // sda, scl, clock speed + //check_I2c(MPU_addr); + Wire.beginTransmission(MPU_addr); + Wire.write(0x6B); // PWR_MGMT_1 register + Wire.write(0); // set to zero (wakes up the MPU-6050) + Wire.endTransmission(true); + delay(30); // Ensure gyro has enough time to power up + /* + SerialBT.begin("MotionGlove"); + */ +} + +void loop() { + Wire.beginTransmission(MPU_addr); + Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) + Wire.endTransmission(false); + Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers + AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) + AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) + AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) + Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) + GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) + GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) + GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) + + OSCMessage msg("/accel/"); + msg.add(AcX); + msg.add(AcY); + msg.add(AcZ); + + SLIPSerial.beginPacket(); + msg.send(SLIPSerial); + SLIPSerial.endPacket(); + msg.empty(); + + OSCMessage gmsg("/gyro/"); + gmsg.add(GyX); + gmsg.add(GyY); + gmsg.add(GyZ); + + SLIPSerial.beginPacket(); + gmsg.send(SLIPSerial); + SLIPSerial.endPacket(); + gmsg.empty(); + + OSCMessage tmsg("/temp/"); + tmsg.add(Tmp); + + SLIPSerial.beginPacket(); + tmsg.send(SLIPSerial); + SLIPSerial.endPacket(); + tmsg.empty(); +} + +byte check_I2c(byte addr){ + Serial.println("Looking for sensor..."); + + // We are using the return value of + // the Write.endTransmisstion to see if + // a device did acknowledge to the address. + byte error; + Wire.beginTransmission(addr); + error = Wire.endTransmission(); + + if (error == 0) { + Serial.print(" Device Found at 0x"); + Serial.println(addr,HEX); + } else { + Serial.print(" No Device Found at 0x"); + Serial.println(addr,HEX); + } + return error; +} diff --git a/osc32final/osc32final.ino b/osc32final/osc32final.ino new file mode 100644 index 0000000..88d0968 --- /dev/null +++ b/osc32final/osc32final.ino @@ -0,0 +1,248 @@ +// ESP32 Dev Module + +#include "Wire.h" +#include "MPU6050_6Axis_MotionApps_V6_12.h" + +#include +#include +/* +Make an OSC message and send it over serial + */ + +#ifdef BOARD_HAS_USB_SERIAL +#include +SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB ); +#else +#include + SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial +#endif + +MPU6050 mpu; + +// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual +// quaternion components in a [w, x, y, z] format (not best for parsing +// on a remote host such as Processing or something though) +//#define OUTPUT_READABLE_QUATERNION + +// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles +// (in degrees) calculated from the quaternions coming from the FIFO. +// Note that Euler angles suffer from gimbal lock (for more info, see +// http://en.wikipedia.org/wiki/Gimbal_lock) +#define OUTPUT_READABLE_EULER + +// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ +// pitch/roll angles (in degrees) calculated from the quaternions coming +// from the FIFO. Note this also requires gravity vector calculations. +// Also note that yaw/pitch/roll angles suffer from gimbal lock (for +// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) +//#define OUTPUT_READABLE_YAWPITCHROLL + +// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration +// components with gravity removed. This acceleration reference frame is +// not compensated for orientation, so +X is always +X according to the +// sensor, just without the effects of gravity. If you want acceleration +// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. +//#define OUTPUT_READABLE_REALACCEL + +// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration +// components with gravity removed and adjusted for the world frame of +// reference (yaw is relative to initial orientation, since no magnetometer +// is present in this case). Could be quite handy in some cases. +#define OUTPUT_READABLE_WORLDACCEL + + +// MPU control/status vars +bool dmpReady = false; // set true if DMP init was successful +uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU +uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) +uint16_t packetSize; // expected DMP packet size (default is 42 bytes) +uint16_t fifoCount; // count of all bytes currently in FIFO +uint8_t fifoBuffer[64]; // FIFO storage buffer + +// orientation/motion vars +Quaternion q; // [w, x, y, z] quaternion container +VectorInt16 aa; // [x, y, z] accel sensor measurements +VectorInt16 gy; // [x, y, z] gyro sensor measurements +VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements +VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements +VectorFloat gravity; // [x, y, z] gravity vector +float euler[3]; // [psi, theta, phi] Euler angle container +float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector + + +// Sem dobimo vrednosti +int16_t AcX,AcY,AcZ; +float GyX, GyY, GyZ; + +// Keys +byte keys[] = {16, 17, 5, 18}; +byte pressed[] = {0, 0, 0, 0}; +byte KEYLEN = 4; + +OSCMessage msg("/accel/"); +OSCMessage gmsg("/gyro/"); +OSCMessage emsg("/error/"); +OSCMessage kmsg("/keys/"); +OSCMessage qmsg("/quaternion/"); + +void setup() { + Wire.begin(); + Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties + SLIPSerial.begin(115200); // set this as high as you can reliably run on your platform + + // Keys + + for(int i = 0; i < KEYLEN; i++) { + pinMode(keys[i], INPUT_PULLUP); + } + + mpu.initialize(); +// setFullScaleGyroRange(MPU6050_GYRO_FS_250); +// setFullScaleAccelRange(MPU6050_ACCEL_FS_2); + + // DMP init + devStatus = mpu.dmpInitialize(); + + // supply your own gyro offsets here, scaled for min sensitivity + // !!! Run Zero IMU to get readings + mpu.setXGyroOffset(76); + mpu.setYGyroOffset(68); + mpu.setZGyroOffset(10); + mpu.setXAccelOffset(-3527); + mpu.setYAccelOffset(-913); + mpu.setZAccelOffset(1027); + + // make sure it worked (returns 0 if so) + if (devStatus == 0) { + // Calibration Time: generate offsets and calibrate our MPU6050 + mpu.CalibrateAccel(6); + mpu.CalibrateGyro(6); + //Serial.println(); + mpu.PrintActiveOffsets(); + // turn on the DMP, now that it's ready + //Serial.println(F("Enabling DMP...")); + mpu.setDMPEnabled(true); + + // set our DMP Ready flag so the main loop() function knows it's okay to use it + //Serial.println(F("DMP ready! Waiting for first interrupt...")); + dmpReady = true; + + // get expected DMP packet size for later comparison + packetSize = mpu.dmpGetFIFOPacketSize(); + } else { + emsg.add("DMP Initialization failed (code " + String(devStatus) + ")"); + + + + SLIPSerial.beginPacket(); + emsg.send(SLIPSerial); + SLIPSerial.endPacket(); + emsg.empty(); + //sendSerial(emsg); + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + } +} + +void loop() { + // if programming failed, don't try to do anything + if (!dmpReady) return; + // read a packet from FIFO + if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet + + +#ifdef OUTPUT_READABLE_QUATERNION + // display quaternion values in easy matrix form: w x y z + mpu.dmpGetQuaternion(&q, fifoBuffer); + + qmsg.add(q.w); + qmsg.add(q.x); + qmsg.add(q.y); + qmsg.add(q.z); + //sendSerial(msg); + SLIPSerial.beginPacket(); + qmsg.send(SLIPSerial); + SLIPSerial.endPacket(); + qmsg.empty(); +#endif + + +#ifdef OUTPUT_READABLE_EULER + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetEuler(euler, &q); + + GyX = euler[0]; + GyY = euler[1]; + GyZ = euler[2]; +#endif + +#ifdef OUTPUT_READABLE_YAWPITCHROLL + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + + GyX = ypr[0]; + GyY = ypr[1]; + GyZ = ypr[2]; +#endif + +#ifdef OUTPUT_READABLE_REALACCEL + // display real acceleration, adjusted to remove gravity + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + AcX = aaReal.x; + AcY = aaReal.y; + AcZ = aaReal.z; +#endif + +#ifdef OUTPUT_READABLE_WORLDACCEL + // display initial world-frame acceleration, adjusted to remove gravity + // and rotated based on known orientation from quaternion + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); + AcX = aaWorld.x; + AcY = aaWorld.y; + AcZ = aaWorld.z; + +#endif + + // Send over serial + msg.add(AcX); + msg.add(AcY); + msg.add(AcZ); + //sendSerial(msg); + SLIPSerial.beginPacket(); + msg.send(SLIPSerial); + SLIPSerial.endPacket(); + msg.empty(); + + gmsg.add(GyX); + gmsg.add(GyY); + gmsg.add(GyZ); + //sendSerial(gmsg); + SLIPSerial.beginPacket(); + gmsg.send(SLIPSerial); + SLIPSerial.endPacket(); + gmsg.empty(); + + // Send keys + for(int i = 0; i < KEYLEN; i++) { + pressed[i] = !digitalRead(keys[i]); + kmsg.add(pressed[i]); + } + //sendSerial(kmsg); + SLIPSerial.beginPacket(); + kmsg.send(SLIPSerial); + SLIPSerial.endPacket(); + kmsg.empty(); + } +} diff --git a/osc32h/osc32h/osc32h.ino b/osc32h/osc32h/osc32h.ino new file mode 100644 index 0000000..f403d11 --- /dev/null +++ b/osc32h/osc32h/osc32h.ino @@ -0,0 +1,70 @@ +// ESP32 Dev Module + +#include "Wire.h" +#include "MPU6050.h" + +#include +#include +/* +Make an OSC message and send it over serial + */ + +#ifdef BOARD_HAS_USB_SERIAL +#include +SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB ); +#else +#include + SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial +#endif + +/* +#include +#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) +#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it +#endif + +BluetoothSerial SerialBT; +*/ + +MPU6050 mpu; + +// Sem dobimo vrednosti +int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; + +void setup() { + Wire.begin(); + SLIPSerial.begin(115200); // set this as high as you can reliably run on your platform + + mpu.initialize(); +// setFullScaleGyroRange(MPU6050_GYRO_FS_250); +// setFullScaleAccelRange(MPU6050_ACCEL_FS_2); + + /* + SerialBT.begin("MotionGlove"); + */ +} + +void loop() { + mpu.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ); + + OSCMessage msg("/accel/"); + msg.add(AcX); + msg.add(AcY); + msg.add(AcZ); + + SLIPSerial.beginPacket(); + msg.send(SLIPSerial); + SLIPSerial.endPacket(); + msg.empty(); + + + OSCMessage gmsg("/gyro/"); + gmsg.add(GyX); + gmsg.add(GyY); + gmsg.add(GyZ); + + SLIPSerial.beginPacket(); + gmsg.send(SLIPSerial); + SLIPSerial.endPacket(); + gmsg.empty(); +} diff --git a/osc32hh/osc32hh/osc32hh.ino b/osc32hh/osc32hh/osc32hh.ino new file mode 100644 index 0000000..6c61c6b --- /dev/null +++ b/osc32hh/osc32hh/osc32hh.ino @@ -0,0 +1,224 @@ +// ESP32 Dev Module + +#include "Wire.h" +#include "MPU6050_6Axis_MotionApps_V6_12.h" + +#include +#include +/* +Make an OSC message and send it over serial + */ + +#ifdef BOARD_HAS_USB_SERIAL +#include +SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB ); +#else +#include + SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial +#endif + +MPU6050 mpu; + +// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual +// quaternion components in a [w, x, y, z] format (not best for parsing +// on a remote host such as Processing or something though) +//#define OUTPUT_READABLE_QUATERNION + +// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles +// (in degrees) calculated from the quaternions coming from the FIFO. +// Note that Euler angles suffer from gimbal lock (for more info, see +// http://en.wikipedia.org/wiki/Gimbal_lock) +#define OUTPUT_READABLE_EULER + +// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ +// pitch/roll angles (in degrees) calculated from the quaternions coming +// from the FIFO. Note this also requires gravity vector calculations. +// Also note that yaw/pitch/roll angles suffer from gimbal lock (for +// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) +//#define OUTPUT_READABLE_YAWPITCHROLL + +// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration +// components with gravity removed. This acceleration reference frame is +// not compensated for orientation, so +X is always +X according to the +// sensor, just without the effects of gravity. If you want acceleration +// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. +//#define OUTPUT_READABLE_REALACCEL + +// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration +// components with gravity removed and adjusted for the world frame of +// reference (yaw is relative to initial orientation, since no magnetometer +// is present in this case). Could be quite handy in some cases. +#define OUTPUT_READABLE_WORLDACCEL + + +// MPU control/status vars +bool dmpReady = false; // set true if DMP init was successful +uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU +uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) +uint16_t packetSize; // expected DMP packet size (default is 42 bytes) +uint16_t fifoCount; // count of all bytes currently in FIFO +uint8_t fifoBuffer[64]; // FIFO storage buffer + +// orientation/motion vars +Quaternion q; // [w, x, y, z] quaternion container +VectorInt16 aa; // [x, y, z] accel sensor measurements +VectorInt16 gy; // [x, y, z] gyro sensor measurements +VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements +VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements +VectorFloat gravity; // [x, y, z] gravity vector +float euler[3]; // [psi, theta, phi] Euler angle container +float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector + + +// Sem dobimo vrednosti +int16_t AcX,AcY,AcZ; +float GyX, GyY, GyZ; + + +OSCMessage msg("/accel/"); +OSCMessage gmsg("/gyro/"); +OSCMessage emsg("/error/"); + +void setup() { + Wire.begin(); + Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties + SLIPSerial.begin(115200); // set this as high as you can reliably run on your platform + + mpu.initialize(); +// setFullScaleGyroRange(MPU6050_GYRO_FS_250); +// setFullScaleAccelRange(MPU6050_ACCEL_FS_2); + + // DMP init + devStatus = mpu.dmpInitialize(); + + // supply your own gyro offsets here, scaled for min sensitivity + // !!! Run Zero IMU to get readings + mpu.setXGyroOffset(76); + mpu.setYGyroOffset(68); + mpu.setZGyroOffset(10); + mpu.setXAccelOffset(-3527); + mpu.setYAccelOffset(-913); + mpu.setZAccelOffset(1027); + + // make sure it worked (returns 0 if so) + if (devStatus == 0) { + // Calibration Time: generate offsets and calibrate our MPU6050 + mpu.CalibrateAccel(6); + mpu.CalibrateGyro(6); + //Serial.println(); + mpu.PrintActiveOffsets(); + // turn on the DMP, now that it's ready + //Serial.println(F("Enabling DMP...")); + mpu.setDMPEnabled(true); + + // set our DMP Ready flag so the main loop() function knows it's okay to use it + //Serial.println(F("DMP ready! Waiting for first interrupt...")); + dmpReady = true; + + // get expected DMP packet size for later comparison + packetSize = mpu.dmpGetFIFOPacketSize(); + } else { + + OSCMessage emsg("/error/"); + emsg.add("DMP Initialization failed (code " + String(devStatus) + ")"); + + SLIPSerial.beginPacket(); + emsg.send(SLIPSerial); + SLIPSerial.endPacket(); + emsg.empty(); + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + } + /* + SerialBT.begin("MotionGlove"); + */ +} + +void sendSerial(OSCMessage msg) { + SLIPSerial.beginPacket(); + msg.send(SLIPSerial); + SLIPSerial.endPacket(); + msg.empty(); +} + +void loop() { + // if programming failed, don't try to do anything + if (!dmpReady) return; + // read a packet from FIFO + if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet + +/* +#ifdef OUTPUT_READABLE_QUATERNION + // display quaternion values in easy matrix form: w x y z + mpu.dmpGetQuaternion(&q, fifoBuffer); + Serial.print("quat\t"); + Serial.print(q.w); + Serial.print("\t"); + Serial.print(q.x); + Serial.print("\t"); + Serial.print(q.y); + Serial.print("\t"); + Serial.println(q.z); +#endif +*/ + +#ifdef OUTPUT_READABLE_EULER + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetEuler(euler, &q); + + GyX = euler[0]/* * 180 / M_PI*/; + GyY = euler[1]/* * 180 / M_PI*/; + GyZ = euler[2]/* * 180 / M_PI*/; +#endif + +#ifdef OUTPUT_READABLE_YAWPITCHROLL + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + + GyX = ypr[0]/* * 180 / M_PI*/; + GyY = ypr[1]/* * 180 / M_PI*/; + GyZ = ypr[2]/* * 180 / M_PI*/; +#endif + +#ifdef OUTPUT_READABLE_REALACCEL + // display real acceleration, adjusted to remove gravity + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + AcX = aaReal.x; + AcY = aaReal.y; + AcZ = aaReal.z; +#endif + +#ifdef OUTPUT_READABLE_WORLDACCEL + // display initial world-frame acceleration, adjusted to remove gravity + // and rotated based on known orientation from quaternion + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); + AcX = aaWorld.x; + AcY = aaWorld.y; + AcZ = aaWorld.z; + +#endif + + // Send over serial + msg.add(AcX); + msg.add(AcY); + msg.add(AcZ); + sendSerial(msg); + + gmsg.add(GyX); + gmsg.add(GyY); + gmsg.add(GyZ); + sendSerial(gmsg); + } +} diff --git a/projekt.org b/projekt.org index 0eef793..70f6bb0 100644 --- a/projekt.org +++ b/projekt.org @@ -1,4 +1,7 @@ -* Wavey Wind, the Modul-air Mitt +#+TITLE: Gibalna rokavička + +* Opis +** Wavey Wind, the Modul-air Mitt Using an ESP32 microcontroller with wifi and bluetooth capability, a gyroscope/accelerometer module and keyboards switches, I will be building an experimental low cost motion "glove" controller with a button for each finger. @@ -10,7 +13,7 @@ "What Can a Body Do?" -* Gibalna rokavička ("Wavey Wind, the Modul-air Mitt") +** Gibalna rokavička ("Wavey Wind, the Modul-air Mitt") Z ESP32 mikrokontrolerjem, ki omogoča wifi in bluetooth povezljivost, modulom z žiroskopom in pospeškomerjem ter tipkovničnimi stikali bom izdelal eksperimentalni nizkocenovni gibalni vmesnik z gumbom za vsak prst. @@ -21,3 +24,49 @@ Rokavica oz. rokavice bodo pošiljale podatke preko OSC (open sound control) protokola, kar odpira tudi možnost rabe za zvočno sintezo/modulacijo ali celo kot splošni vmesnik. "Kaj zmore telo?" + + +* Shematika +** ESP32 <-> MPU-6050 (gyro/accel) + - 21 - SDA (data) + - 22 - SCL (clock) + - 3V3 - VCC (strom) + - GND - GND (ZEMLA) +** key switches + - one pin of each to GND + - other pin of each to 16, 17, 5, 18 + +* Arduino sketch +** calibration + - IMU_Zero +** testing, prototypes + - kbdtest + - keypadtest (kbd matrix test, didn't work) + - proto32, proto32plot, osc32, osc32h, osc32hh +** Final sketch + - osc32final + +* 3d models + - proto1.blend first prototype + - proto2.blend second prototype + +* visual + https://git.kompot.si/g1smo/wavey-wind + + start with `npm i; npm start` + + localhost:6676/ + localhost:6676/test + +** older prototypes + - https://kreten.si/spin/ + - https://kreten.si/spin-space/ + - https://kreten.si/spin-color/ + - https://vis.kreten.si/ + +* TODO + - ergonomic control + - wireless (wifi or bluetooth?) + - ad-hoc connection to use multiple controllers + - better starting parameters + - better documentation diff --git a/proto1.blend b/proto1.blend new file mode 100644 index 0000000..ad8107a Binary files /dev/null and b/proto1.blend differ diff --git a/proto1.stl b/proto1.stl new file mode 100644 index 0000000..2819edd Binary files /dev/null and b/proto1.stl differ diff --git a/proto2.blend b/proto2.blend new file mode 100644 index 0000000..f6e6329 Binary files /dev/null and b/proto2.blend differ diff --git a/proto2.blend1 b/proto2.blend1 new file mode 100644 index 0000000..d826c17 Binary files /dev/null and b/proto2.blend1 differ diff --git a/proto2.stl b/proto2.stl new file mode 100644 index 0000000..a356a8f Binary files /dev/null and b/proto2.stl differ diff --git a/proto32/proto32.ino b/proto32/proto32.ino new file mode 100644 index 0000000..77c3585 --- /dev/null +++ b/proto32/proto32.ino @@ -0,0 +1,95 @@ +// ESP32 Dev Module + +#include "Wire.h" + +/* +#include +#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) +#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it +#endif + +BluetoothSerial SerialBT; +*/ + +const int MPU_addr=0x68; // I2C address of the MPU-6050 + +// Sem dobimo vrednosti +int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; + +void setup() { + Wire.begin(); + Serial.begin(9600); + + // Pocakaj na serijski port + while (!Serial); + delay(1000); + + Serial.println("Helllloooo. ☭"); + + Wire.begin(21, 22, 100000); // sda, scl, clock speed + check_I2c(MPU_addr); + Wire.beginTransmission(MPU_addr); + Wire.write(0x6B); // PWR_MGMT_1 register + Wire.write(0); // set to zero (wakes up the MPU-6050) + Wire.endTransmission(true); + delay(30); // Ensure gyro has enough time to power up + + Serial.println("Here we go!"); + + /* + SerialBT.begin("MotionGlove"); + Serial.print("BT on"); + */ +} + +void loop() { + Wire.beginTransmission(MPU_addr); + Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) + Wire.endTransmission(false); + Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers + AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) + AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) + AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) + Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) + GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) + GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) + GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) + Serial.print("AcX = "); Serial.print(AcX); + Serial.print(" | AcY = "); Serial.print(AcY); + Serial.print(" | AcZ = "); Serial.print(AcZ); + Serial.print(" | Tmp = "); Serial.print(Tmp); + Serial.print(" | GyX = "); Serial.print(GyX); + Serial.print(" | GyY = "); Serial.print(GyY); + Serial.print(" | GyZ = "); Serial.println(GyZ); + Serial.print("\n"); + +/* + SerialBT.write(String("/accel/0/x/" + String(AcX) + "\n")); + SerialBT.write(String("/accel/0/y/" + String(AcY) + "\n")); + SerialBT.write(String("/accel/0/z/" + String(AcZ) + "\n")); + SerialBT.write(String("/gy/0/x/" + String(GyX) + "\n")); + SerialBT.write(String("/gy/0/y/" + String(GyY) + "\n")); + SerialBT.write(String("/gy/0/z/" + String(GyZ) + "\n")); + SerialBT.write(String("/tmp/0/val/" + String(Tmp) + "\n")); +*/ +} + +byte check_I2c(byte addr){ + Serial.println("Looking for sensor..."); + + // We are using the return value of + // the Write.endTransmisstion to see if + // a device did acknowledge to the address. + byte error; + Wire.beginTransmission(addr); + error = Wire.endTransmission(); + + if (error == 0) { + Serial.print(" Device Found at 0x"); + Serial.println(addr,HEX); + } else { + Serial.print(" No Device Found at 0x"); + Serial.println(addr,HEX); + } + return error; +} diff --git a/proto32plot/proto32plot/proto32plot.ino b/proto32plot/proto32plot/proto32plot.ino new file mode 100644 index 0000000..016e3ea --- /dev/null +++ b/proto32plot/proto32plot/proto32plot.ino @@ -0,0 +1,65 @@ +// ESP32 Dev Module + +#include "Wire.h" + +const int MPU_addr=0x68; // I2C address of the MPU-6050 + +// Sem dobimo vrednosti +int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; + +void setup() { + Wire.begin(); + Serial.begin(9600); + + // Pocakaj na serijski port + while (!Serial); + delay(1000); + + Wire.begin(21, 22, 100000); // sda, scl, clock speed + Wire.beginTransmission(MPU_addr); + Wire.write(0x6B); // PWR_MGMT_1 register + Wire.write(0); // set to zero (wakes up the MPU-6050) + Wire.endTransmission(true); + delay(300); // Ensure gyro has enough time to power up + + Serial.print("AcX"); + Serial.print(" "); + Serial.print("AcY"); + Serial.print(" "); + Serial.print("AcZ"); + Serial.print(" "); + Serial.print("Tmp"); + Serial.print(" "); + Serial.print("GyX"); + Serial.print(" "); + Serial.print("GyY"); + Serial.print(" "); + Serial.println("GyZ"); +} + +void loop() { + Wire.beginTransmission(MPU_addr); + Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) + Wire.endTransmission(false); + Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers + AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) + AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) + AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) + Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) + GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) + GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) + GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) + Serial.print(AcX); + Serial.print(" "); + Serial.print(AcY); + Serial.print(" "); + Serial.print(AcZ); + Serial.print(" "); + Serial.print(Tmp); + Serial.print(" "); + Serial.print(GyX); + Serial.print(" "); + Serial.print(GyY); + Serial.print(" "); + Serial.println(GyZ); +} diff --git a/scan32/scan32.ino b/scan32/scan32.ino new file mode 100644 index 0000000..b7c2053 --- /dev/null +++ b/scan32/scan32.ino @@ -0,0 +1,45 @@ +/********* + Rui Santos + Complete project details at https://randomnerdtutorials.com +*********/ + +#include + +void setup() { + Wire.begin(); + Serial.begin(115200); + Serial.println("\nI2C Scanner"); +} + +void loop() { + byte error, address; + int nDevices; + Serial.println("Scanning..."); + nDevices = 0; + for(address = 1; address < 127; address++ ) { + Wire.beginTransmission(address); + error = Wire.endTransmission(); + if (error == 0) { + Serial.print("I2C device found at address 0x"); + if (address<16) { + Serial.print("0"); + } + Serial.println(address,HEX); + nDevices++; + } + else if (error==4) { + Serial.print("Unknow error at address 0x"); + if (address<16) { + Serial.print("0"); + } + Serial.println(address,HEX); + } + } + if (nDevices == 0) { + Serial.println("No I2C devices found\n"); + } + else { + Serial.println("done\n"); + } + delay(5000); +}