LORDTEK LORDTEK / Mbed 2 deprecated MPU6050_Balancing_by_LEDs

Balancimg MPU6050 by LEDs on PWM

Dependencies:   mbed

It is possible to observe the MPU6050 data on serial. If serial termimal is not started then still LED application runs.

There are 4 LEDs on LPC1768. So the first 3 LEDs are assign to accelerometer X, Y and Z. The last LED can drive by user. Default it lights a bit. If you want to turn it off then change the value as "190" to "180" on this line: myled4 = cos(190*2.0*3.13/360) * 0.5 + 0.5; If you want to turn it on (full) then change the value as "190" to "0" on above code line.

helper_3dmath.h@0:9a1aae56ed19, 2013-10-13 (annotated)

Committer:
LORDTEK
Date:
Sun Oct 13 12:01:33 2013 +0000
Revision:
0:9a1aae56ed19
Balancing MPU6050 by LEDs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LORDTEK 0:9a1aae56ed19 1 // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
LORDTEK 0:9a1aae56ed19 2 // 6/5/2012 by Jeff Rowberg <jeff@rowberg.net>
LORDTEK 0:9a1aae56ed19 3 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
LORDTEK 0:9a1aae56ed19 4 //
LORDTEK 0:9a1aae56ed19 5 // Changelog:
LORDTEK 0:9a1aae56ed19 6 // 2012-06-05 - add 3D math helper file to DMP6 example sketch
LORDTEK 0:9a1aae56ed19 7
LORDTEK 0:9a1aae56ed19 8 /* ============================================
LORDTEK 0:9a1aae56ed19 9 I2Cdev device library code is placed under the MIT license
LORDTEK 0:9a1aae56ed19 10 Copyright (c) 2012 Jeff Rowberg
LORDTEK 0:9a1aae56ed19 11
LORDTEK 0:9a1aae56ed19 12 Permission is hereby granted, free of charge, to any person obtaining a copy
LORDTEK 0:9a1aae56ed19 13 of this software and associated documentation files (the "Software"), to deal
LORDTEK 0:9a1aae56ed19 14 in the Software without restriction, including without limitation the rights
LORDTEK 0:9a1aae56ed19 15 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
LORDTEK 0:9a1aae56ed19 16 copies of the Software, and to permit persons to whom the Software is
LORDTEK 0:9a1aae56ed19 17 furnished to do so, subject to the following conditions:
LORDTEK 0:9a1aae56ed19 18
LORDTEK 0:9a1aae56ed19 19 The above copyright notice and this permission notice shall be included in
LORDTEK 0:9a1aae56ed19 20 all copies or substantial portions of the Software.
LORDTEK 0:9a1aae56ed19 21
LORDTEK 0:9a1aae56ed19 22 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
LORDTEK 0:9a1aae56ed19 23 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
LORDTEK 0:9a1aae56ed19 24 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
LORDTEK 0:9a1aae56ed19 25 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LORDTEK 0:9a1aae56ed19 26 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
LORDTEK 0:9a1aae56ed19 27 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
LORDTEK 0:9a1aae56ed19 28 THE SOFTWARE.
LORDTEK 0:9a1aae56ed19 29 ===============================================
LORDTEK 0:9a1aae56ed19 30 */
LORDTEK 0:9a1aae56ed19 31
LORDTEK 0:9a1aae56ed19 32 #ifndef _HELPER_3DMATH_H_
LORDTEK 0:9a1aae56ed19 33 #define _HELPER_3DMATH_H_
LORDTEK 0:9a1aae56ed19 34
LORDTEK 0:9a1aae56ed19 35 class Quaternion {
LORDTEK 0:9a1aae56ed19 36 public:
LORDTEK 0:9a1aae56ed19 37 float w;
LORDTEK 0:9a1aae56ed19 38 float x;
LORDTEK 0:9a1aae56ed19 39 float y;
LORDTEK 0:9a1aae56ed19 40 float z;
LORDTEK 0:9a1aae56ed19 41
LORDTEK 0:9a1aae56ed19 42 Quaternion() {
LORDTEK 0:9a1aae56ed19 43 w = 1.0f;
LORDTEK 0:9a1aae56ed19 44 x = 0.0f;
LORDTEK 0:9a1aae56ed19 45 y = 0.0f;
LORDTEK 0:9a1aae56ed19 46 z = 0.0f;
LORDTEK 0:9a1aae56ed19 47 }
LORDTEK 0:9a1aae56ed19 48
LORDTEK 0:9a1aae56ed19 49 Quaternion(float nw, float nx, float ny, float nz) {
LORDTEK 0:9a1aae56ed19 50 w = nw;
LORDTEK 0:9a1aae56ed19 51 x = nx;
LORDTEK 0:9a1aae56ed19 52 y = ny;
LORDTEK 0:9a1aae56ed19 53 z = nz;
LORDTEK 0:9a1aae56ed19 54 }
LORDTEK 0:9a1aae56ed19 55
LORDTEK 0:9a1aae56ed19 56 Quaternion getProduct(Quaternion q) {
LORDTEK 0:9a1aae56ed19 57 // Quaternion multiplication is defined by:
LORDTEK 0:9a1aae56ed19 58 // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
LORDTEK 0:9a1aae56ed19 59 // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
LORDTEK 0:9a1aae56ed19 60 // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
LORDTEK 0:9a1aae56ed19 61 // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
LORDTEK 0:9a1aae56ed19 62 return Quaternion(
LORDTEK 0:9a1aae56ed19 63 w*q.w - x*q.x - y*q.y - z*q.z, // new w
LORDTEK 0:9a1aae56ed19 64 w*q.x + x*q.w + y*q.z - z*q.y, // new x
LORDTEK 0:9a1aae56ed19 65 w*q.y - x*q.z + y*q.w + z*q.x, // new y
LORDTEK 0:9a1aae56ed19 66 w*q.z + x*q.y - y*q.x + z*q.w); // new z
LORDTEK 0:9a1aae56ed19 67 }
LORDTEK 0:9a1aae56ed19 68
LORDTEK 0:9a1aae56ed19 69 Quaternion getConjugate() {
LORDTEK 0:9a1aae56ed19 70 return Quaternion(w, -x, -y, -z);
LORDTEK 0:9a1aae56ed19 71 }
LORDTEK 0:9a1aae56ed19 72
LORDTEK 0:9a1aae56ed19 73 float getMagnitude() {
LORDTEK 0:9a1aae56ed19 74 return sqrt(w*w + x*x + y*y + z*z);
LORDTEK 0:9a1aae56ed19 75 }
LORDTEK 0:9a1aae56ed19 76
LORDTEK 0:9a1aae56ed19 77 void normalize() {
LORDTEK 0:9a1aae56ed19 78 float m = getMagnitude();
LORDTEK 0:9a1aae56ed19 79 w /= m;
LORDTEK 0:9a1aae56ed19 80 x /= m;
LORDTEK 0:9a1aae56ed19 81 y /= m;
LORDTEK 0:9a1aae56ed19 82 z /= m;
LORDTEK 0:9a1aae56ed19 83 }
LORDTEK 0:9a1aae56ed19 84
LORDTEK 0:9a1aae56ed19 85 Quaternion getNormalized() {
LORDTEK 0:9a1aae56ed19 86 Quaternion r(w, x, y, z);
LORDTEK 0:9a1aae56ed19 87 r.normalize();
LORDTEK 0:9a1aae56ed19 88 return r;
LORDTEK 0:9a1aae56ed19 89 }
LORDTEK 0:9a1aae56ed19 90 };
LORDTEK 0:9a1aae56ed19 91
LORDTEK 0:9a1aae56ed19 92 class VectorInt16 {
LORDTEK 0:9a1aae56ed19 93 public:
LORDTEK 0:9a1aae56ed19 94 int16_t x;
LORDTEK 0:9a1aae56ed19 95 int16_t y;
LORDTEK 0:9a1aae56ed19 96 int16_t z;
LORDTEK 0:9a1aae56ed19 97
LORDTEK 0:9a1aae56ed19 98 VectorInt16() {
LORDTEK 0:9a1aae56ed19 99 x = 0;
LORDTEK 0:9a1aae56ed19 100 y = 0;
LORDTEK 0:9a1aae56ed19 101 z = 0;
LORDTEK 0:9a1aae56ed19 102 }
LORDTEK 0:9a1aae56ed19 103
LORDTEK 0:9a1aae56ed19 104 VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
LORDTEK 0:9a1aae56ed19 105 x = nx;
LORDTEK 0:9a1aae56ed19 106 y = ny;
LORDTEK 0:9a1aae56ed19 107 z = nz;
LORDTEK 0:9a1aae56ed19 108 }
LORDTEK 0:9a1aae56ed19 109
LORDTEK 0:9a1aae56ed19 110 float getMagnitude() {
LORDTEK 0:9a1aae56ed19 111 return sqrt((float)(x*x + y*y + z*z));
LORDTEK 0:9a1aae56ed19 112 }
LORDTEK 0:9a1aae56ed19 113
LORDTEK 0:9a1aae56ed19 114 void normalize() {
LORDTEK 0:9a1aae56ed19 115 float m = getMagnitude();
LORDTEK 0:9a1aae56ed19 116 x /= m;
LORDTEK 0:9a1aae56ed19 117 y /= m;
LORDTEK 0:9a1aae56ed19 118 z /= m;
LORDTEK 0:9a1aae56ed19 119 }
LORDTEK 0:9a1aae56ed19 120
LORDTEK 0:9a1aae56ed19 121 VectorInt16 getNormalized() {
LORDTEK 0:9a1aae56ed19 122 VectorInt16 r(x, y, z);
LORDTEK 0:9a1aae56ed19 123 r.normalize();
LORDTEK 0:9a1aae56ed19 124 return r;
LORDTEK 0:9a1aae56ed19 125 }
LORDTEK 0:9a1aae56ed19 126
LORDTEK 0:9a1aae56ed19 127 void rotate(Quaternion *q) {
LORDTEK 0:9a1aae56ed19 128 // http://www.cprogramming.com/tutorial/3d/quaternions.html
LORDTEK 0:9a1aae56ed19 129 // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
LORDTEK 0:9a1aae56ed19 130 // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
LORDTEK 0:9a1aae56ed19 131 // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
LORDTEK 0:9a1aae56ed19 132
LORDTEK 0:9a1aae56ed19 133 // P_out = q * P_in * conj(q)
LORDTEK 0:9a1aae56ed19 134 // - P_out is the output vector
LORDTEK 0:9a1aae56ed19 135 // - q is the orientation quaternion
LORDTEK 0:9a1aae56ed19 136 // - P_in is the input vector (a*aReal)
LORDTEK 0:9a1aae56ed19 137 // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
LORDTEK 0:9a1aae56ed19 138 Quaternion p(0, x, y, z);
LORDTEK 0:9a1aae56ed19 139
LORDTEK 0:9a1aae56ed19 140 // quaternion multiplication: q * p, stored back in p
LORDTEK 0:9a1aae56ed19 141 p = q -> getProduct(p);
LORDTEK 0:9a1aae56ed19 142
LORDTEK 0:9a1aae56ed19 143 // quaternion multiplication: p * conj(q), stored back in p
LORDTEK 0:9a1aae56ed19 144 p = p.getProduct(q -> getConjugate());
LORDTEK 0:9a1aae56ed19 145
LORDTEK 0:9a1aae56ed19 146 // p quaternion is now [0, x', y', z']
LORDTEK 0:9a1aae56ed19 147 x = p.x;
LORDTEK 0:9a1aae56ed19 148 y = p.y;
LORDTEK 0:9a1aae56ed19 149 z = p.z;
LORDTEK 0:9a1aae56ed19 150 }
LORDTEK 0:9a1aae56ed19 151
LORDTEK 0:9a1aae56ed19 152 VectorInt16 getRotated(Quaternion *q) {
LORDTEK 0:9a1aae56ed19 153 VectorInt16 r(x, y, z);
LORDTEK 0:9a1aae56ed19 154 r.rotate(q);
LORDTEK 0:9a1aae56ed19 155 return r;
LORDTEK 0:9a1aae56ed19 156 }
LORDTEK 0:9a1aae56ed19 157 };
LORDTEK 0:9a1aae56ed19 158
LORDTEK 0:9a1aae56ed19 159 class VectorFloat {
LORDTEK 0:9a1aae56ed19 160 public:
LORDTEK 0:9a1aae56ed19 161 float x;
LORDTEK 0:9a1aae56ed19 162 float y;
LORDTEK 0:9a1aae56ed19 163 float z;
LORDTEK 0:9a1aae56ed19 164
LORDTEK 0:9a1aae56ed19 165 VectorFloat() {
LORDTEK 0:9a1aae56ed19 166 x = 0;
LORDTEK 0:9a1aae56ed19 167 y = 0;
LORDTEK 0:9a1aae56ed19 168 z = 0;
LORDTEK 0:9a1aae56ed19 169 }
LORDTEK 0:9a1aae56ed19 170
LORDTEK 0:9a1aae56ed19 171 VectorFloat(float nx, float ny, float nz) {
LORDTEK 0:9a1aae56ed19 172 x = nx;
LORDTEK 0:9a1aae56ed19 173 y = ny;
LORDTEK 0:9a1aae56ed19 174 z = nz;
LORDTEK 0:9a1aae56ed19 175 }
LORDTEK 0:9a1aae56ed19 176
LORDTEK 0:9a1aae56ed19 177 float getMagnitude() {
LORDTEK 0:9a1aae56ed19 178 return sqrt(x*x + y*y + z*z);
LORDTEK 0:9a1aae56ed19 179 }
LORDTEK 0:9a1aae56ed19 180
LORDTEK 0:9a1aae56ed19 181 void normalize() {
LORDTEK 0:9a1aae56ed19 182 float m = getMagnitude();
LORDTEK 0:9a1aae56ed19 183 x /= m;
LORDTEK 0:9a1aae56ed19 184 y /= m;
LORDTEK 0:9a1aae56ed19 185 z /= m;
LORDTEK 0:9a1aae56ed19 186 }
LORDTEK 0:9a1aae56ed19 187
LORDTEK 0:9a1aae56ed19 188 VectorFloat getNormalized() {
LORDTEK 0:9a1aae56ed19 189 VectorFloat r(x, y, z);
LORDTEK 0:9a1aae56ed19 190 r.normalize();
LORDTEK 0:9a1aae56ed19 191 return r;
LORDTEK 0:9a1aae56ed19 192 }
LORDTEK 0:9a1aae56ed19 193
LORDTEK 0:9a1aae56ed19 194 void rotate(Quaternion *q) {
LORDTEK 0:9a1aae56ed19 195 Quaternion p(0, x, y, z);
LORDTEK 0:9a1aae56ed19 196
LORDTEK 0:9a1aae56ed19 197 // quaternion multiplication: q * p, stored back in p
LORDTEK 0:9a1aae56ed19 198 p = q -> getProduct(p);
LORDTEK 0:9a1aae56ed19 199
LORDTEK 0:9a1aae56ed19 200 // quaternion multiplication: p * conj(q), stored back in p
LORDTEK 0:9a1aae56ed19 201 p = p.getProduct(q -> getConjugate());
LORDTEK 0:9a1aae56ed19 202
LORDTEK 0:9a1aae56ed19 203 // p quaternion is now [0, x', y', z']
LORDTEK 0:9a1aae56ed19 204 x = p.x;
LORDTEK 0:9a1aae56ed19 205 y = p.y;
LORDTEK 0:9a1aae56ed19 206 z = p.z;
LORDTEK 0:9a1aae56ed19 207 }
LORDTEK 0:9a1aae56ed19 208
LORDTEK 0:9a1aae56ed19 209 VectorFloat getRotated(Quaternion *q) {
LORDTEK 0:9a1aae56ed19 210 VectorFloat r(x, y, z);
LORDTEK 0:9a1aae56ed19 211 r.rotate(q);
LORDTEK 0:9a1aae56ed19 212 return r;
LORDTEK 0:9a1aae56ed19 213 }
LORDTEK 0:9a1aae56ed19 214 };
LORDTEK 0:9a1aae56ed19 215
LORDTEK 0:9a1aae56ed19 216 #endif /* _HELPER_3DMATH_H_ */